智也 大野 / Mbed 2 deprecated ANCHOR_Navigation3

目的地へたどり着くアルゴリズム

Dependencies:   MPU9250_SPI TA7291P mbed

HeptaGPS.cpp@0:5fef60d1a47e, 2017-03-17 (annotated)

Committer:
tomoya123
Date:
Fri Mar 17 08:33:50 2017 +0000
Revision:
0:5fef60d1a47e
???????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tomoya123 0:5fef60d1a47e 1 #include "HeptaGPS.h"
tomoya123 0:5fef60d1a47e 2 #include "mbed.h"
tomoya123 0:5fef60d1a47e 3
tomoya123 0:5fef60d1a47e 4 HeptaGPS::HeptaGPS(PinName tx, PinName rx) : gps(tx,rx)
tomoya123 0:5fef60d1a47e 5 {
tomoya123 0:5fef60d1a47e 6 }
tomoya123 0:5fef60d1a47e 7
tomoya123 0:5fef60d1a47e 8 void HeptaGPS::flushSerialBuffer(void)
tomoya123 0:5fef60d1a47e 9 {
tomoya123 0:5fef60d1a47e 10 while (gps.readable())
tomoya123 0:5fef60d1a47e 11 {
tomoya123 0:5fef60d1a47e 12 gps.getc();
tomoya123 0:5fef60d1a47e 13 }
tomoya123 0:5fef60d1a47e 14 return;
tomoya123 0:5fef60d1a47e 15 }
tomoya123 0:5fef60d1a47e 16
tomoya123 0:5fef60d1a47e 17 void HeptaGPS::sensing_u16(char* lad,char* log, int *dsize)
tomoya123 0:5fef60d1a47e 18 {
tomoya123 0:5fef60d1a47e 19 char gph1[8]={0x00},gph2[8]={0x00},gph3[8]={0x00},gph4[8]={0x00},gpt1[8]={0x00},gpt2[8]={0x00},gpt3[8]={0x00},gpt4[8]={0x00};
tomoya123 0:5fef60d1a47e 20 int i=0,j=0;
tomoya123 0:5fef60d1a47e 21 while (gps.readable()){
tomoya123 0:5fef60d1a47e 22 gps.getc();
tomoya123 0:5fef60d1a47e 23 }
tomoya123 0:5fef60d1a47e 24 loop:
tomoya123 0:5fef60d1a47e 25 while(gps.getc()!='$'){}
tomoya123 0:5fef60d1a47e 26 for(j=0;j<5;j++){
tomoya123 0:5fef60d1a47e 27 gps_data[1][j]=gps.getc();
tomoya123 0:5fef60d1a47e 28 }
tomoya123 0:5fef60d1a47e 29 if((gps_data[1][2]==0x52)&(gps_data[1][3]==0x4d)&(gps_data[1][4]==0x43)){
tomoya123 0:5fef60d1a47e 30 for(j=0;j<1;j++){
tomoya123 0:5fef60d1a47e 31 if(j==0){
tomoya123 0:5fef60d1a47e 32 i=0;
tomoya123 0:5fef60d1a47e 33 while((gps_data[j+1][i+5] = gps.getc()) != '\r'){
tomoya123 0:5fef60d1a47e 34 //pc.putc(gps_data[j+1][i+5]);
tomoya123 0:5fef60d1a47e 35 i++;
tomoya123 0:5fef60d1a47e 36 }
tomoya123 0:5fef60d1a47e 37 gps_data[j+1][i+5]='\0';
tomoya123 0:5fef60d1a47e 38 i=0;
tomoya123 0:5fef60d1a47e 39 //pc.printf("\n\r");
tomoya123 0:5fef60d1a47e 40 }
tomoya123 0:5fef60d1a47e 41 else{
tomoya123 0:5fef60d1a47e 42 while(gps.getc()!='$'){}
tomoya123 0:5fef60d1a47e 43 i=0;
tomoya123 0:5fef60d1a47e 44 while((gps_data[j+1][i] = gps.getc()) != '\r'){
tomoya123 0:5fef60d1a47e 45 //pc.putc(gps_data[j+1][i]);
tomoya123 0:5fef60d1a47e 46 i++;
tomoya123 0:5fef60d1a47e 47 }
tomoya123 0:5fef60d1a47e 48 gps_data[j+1][i]='\0';
tomoya123 0:5fef60d1a47e 49 i=0;
tomoya123 0:5fef60d1a47e 50 //pc.printf("\n\r");
tomoya123 0:5fef60d1a47e 51 }
tomoya123 0:5fef60d1a47e 52 }
tomoya123 0:5fef60d1a47e 53 }
tomoya123 0:5fef60d1a47e 54 else
tomoya123 0:5fef60d1a47e 55 {
tomoya123 0:5fef60d1a47e 56 goto loop;
tomoya123 0:5fef60d1a47e 57 }
tomoya123 0:5fef60d1a47e 58 if( sscanf(gps_data[1],"GPRMC,%f,%c,%f,%c,%f,%c,%f",&time,&statas,&hokui,&ns,&tokei,&ew,&vel) >= 1){
tomoya123 0:5fef60d1a47e 59 //hokui
tomoya123 0:5fef60d1a47e 60 d_hokui=int(hokui/100);
tomoya123 0:5fef60d1a47e 61 m_hokui=(hokui-d_hokui*100);
tomoya123 0:5fef60d1a47e 62 //m_hokui=(hokui-d_hokui*100)/60;
tomoya123 0:5fef60d1a47e 63 g_hokui=d_hokui+(hokui-d_hokui*100)/60;
tomoya123 0:5fef60d1a47e 64 sprintf( gph1, "%02X", (char(d_hokui)) & 0xFF);
tomoya123 0:5fef60d1a47e 65 sprintf( gph2, "%02X", (char(m_hokui)) & 0xFF);
tomoya123 0:5fef60d1a47e 66 sprintf( gph3, "%02X", (char((m_hokui-char(m_hokui))*100)) & 0xFF);
tomoya123 0:5fef60d1a47e 67 sprintf( gph4, "%02X", (char(((m_hokui-char(m_hokui))*100-char((m_hokui-char(m_hokui))*100))*100)) & 0xFF);
tomoya123 0:5fef60d1a47e 68
tomoya123 0:5fef60d1a47e 69 //tokei
tomoya123 0:5fef60d1a47e 70 d_tokei=int(tokei/100);
tomoya123 0:5fef60d1a47e 71 m_tokei=(tokei-d_tokei*100);
tomoya123 0:5fef60d1a47e 72 //m_tokei=(tokei-d_tokei*100)/60;
tomoya123 0:5fef60d1a47e 73 g_tokei=d_tokei+(tokei-d_tokei*100)/60;
tomoya123 0:5fef60d1a47e 74 sprintf( gpt1, "%02X", (char(d_tokei)) & 0xFF);
tomoya123 0:5fef60d1a47e 75 sprintf( gpt2, "%02X", (char(m_tokei)) & 0xFF);
tomoya123 0:5fef60d1a47e 76 sprintf( gpt3, "%02X", (char((m_tokei-char(m_tokei))*100)) & 0xFF);
tomoya123 0:5fef60d1a47e 77 sprintf( gpt4, "%02X", (char(((m_tokei-char(m_tokei))*100-char((m_tokei-char(m_tokei))*100))*100)) & 0xFF);
tomoya123 0:5fef60d1a47e 78 lad[0] = gph1[0];
tomoya123 0:5fef60d1a47e 79 lad[1] = gph1[1];
tomoya123 0:5fef60d1a47e 80 lad[2] = gph2[0];
tomoya123 0:5fef60d1a47e 81 lad[3] = gph2[1];
tomoya123 0:5fef60d1a47e 82 lad[4] = gph3[0];
tomoya123 0:5fef60d1a47e 83 lad[5] = gph3[1];
tomoya123 0:5fef60d1a47e 84 lad[6] = gph4[0];
tomoya123 0:5fef60d1a47e 85 lad[7] = gph4[1];
tomoya123 0:5fef60d1a47e 86 log[0] = gpt1[0];
tomoya123 0:5fef60d1a47e 87 log[1] = gpt1[1];
tomoya123 0:5fef60d1a47e 88 log[2] = gpt2[0];
tomoya123 0:5fef60d1a47e 89 log[3] = gpt2[1];
tomoya123 0:5fef60d1a47e 90 log[4] = gpt3[0];
tomoya123 0:5fef60d1a47e 91 log[5] = gpt3[1];
tomoya123 0:5fef60d1a47e 92 log[6] = gpt4[0];
tomoya123 0:5fef60d1a47e 93 log[7] = gpt4[1];
tomoya123 0:5fef60d1a47e 94 }
tomoya123 0:5fef60d1a47e 95 *dsize = 8;
tomoya123 0:5fef60d1a47e 96 }
tomoya123 0:5fef60d1a47e 97
tomoya123 0:5fef60d1a47e 98 void HeptaGPS::sensing(float* lat, float* log )
tomoya123 0:5fef60d1a47e 99 {
tomoya123 0:5fef60d1a47e 100 int i=0,j=0;
tomoya123 0:5fef60d1a47e 101 while (gps.readable())
tomoya123 0:5fef60d1a47e 102 {
tomoya123 0:5fef60d1a47e 103 gps.getc();
tomoya123 0:5fef60d1a47e 104 }
tomoya123 0:5fef60d1a47e 105 loop:
tomoya123 0:5fef60d1a47e 106 while(gps.getc()!='$'){}
tomoya123 0:5fef60d1a47e 107 for(j=0;j<5;j++)
tomoya123 0:5fef60d1a47e 108 {
tomoya123 0:5fef60d1a47e 109 gps_data[1][j]=gps.getc();
tomoya123 0:5fef60d1a47e 110 }
tomoya123 0:5fef60d1a47e 111 if((gps_data[1][2]==0x52)&(gps_data[1][3]==0x4d)&(gps_data[1][4]==0x43))
tomoya123 0:5fef60d1a47e 112 {
tomoya123 0:5fef60d1a47e 113 for(j=0;j<1;j++){
tomoya123 0:5fef60d1a47e 114 if(j==0){
tomoya123 0:5fef60d1a47e 115 i=0;
tomoya123 0:5fef60d1a47e 116 while((gps_data[j+1][i+5] = gps.getc()) != '\r'){
tomoya123 0:5fef60d1a47e 117 //pc.putc(gps_data[j+1][i+5]);
tomoya123 0:5fef60d1a47e 118 i++;
tomoya123 0:5fef60d1a47e 119 }
tomoya123 0:5fef60d1a47e 120 gps_data[j+1][i+5]='\0';
tomoya123 0:5fef60d1a47e 121 i=0;
tomoya123 0:5fef60d1a47e 122 //pc.printf("\n\r");
tomoya123 0:5fef60d1a47e 123 }
tomoya123 0:5fef60d1a47e 124 else{
tomoya123 0:5fef60d1a47e 125 while(gps.getc()!='$'){}
tomoya123 0:5fef60d1a47e 126 i=0;
tomoya123 0:5fef60d1a47e 127 while((gps_data[j+1][i] = gps.getc()) != '\r'){
tomoya123 0:5fef60d1a47e 128 //pc.putc(gps_data[j+1][i]);
tomoya123 0:5fef60d1a47e 129 i++;
tomoya123 0:5fef60d1a47e 130 }
tomoya123 0:5fef60d1a47e 131 gps_data[j+1][i]='\0';
tomoya123 0:5fef60d1a47e 132 i=0;
tomoya123 0:5fef60d1a47e 133 //pc.printf("\n\r");
tomoya123 0:5fef60d1a47e 134 }
tomoya123 0:5fef60d1a47e 135 }
tomoya123 0:5fef60d1a47e 136 }
tomoya123 0:5fef60d1a47e 137 else{
tomoya123 0:5fef60d1a47e 138 goto loop;
tomoya123 0:5fef60d1a47e 139 }
tomoya123 0:5fef60d1a47e 140 if( sscanf(gps_data[1],"GPRMC,%f,%c,%f,%c,%f,%c,%f",&time,&statas,&hokui,&ns,&tokei,&ew,&vel) >= 1){
tomoya123 0:5fef60d1a47e 141 //hokui
tomoya123 0:5fef60d1a47e 142 d_hokui=int(hokui/100);
tomoya123 0:5fef60d1a47e 143 m_hokui=(hokui-d_hokui*100);
tomoya123 0:5fef60d1a47e 144 //m_hokui=(hokui-d_hokui*100)/60;
tomoya123 0:5fef60d1a47e 145 *lat=d_hokui+(hokui-d_hokui*100)/60;
tomoya123 0:5fef60d1a47e 146 //tokei
tomoya123 0:5fef60d1a47e 147 d_tokei=int(tokei/100);
tomoya123 0:5fef60d1a47e 148 m_tokei=(tokei-d_tokei*100);
tomoya123 0:5fef60d1a47e 149 //m_tokei=(tokei-d_tokei*100)/60;
tomoya123 0:5fef60d1a47e 150 *log = d_tokei+(tokei-d_tokei*100)/60;
tomoya123 0:5fef60d1a47e 151 //pc.printf("Lat:%4.6f,Log:%4.6f\n\r",g_hokui,g_tokei);
tomoya123 0:5fef60d1a47e 152 }
tomoya123 0:5fef60d1a47e 153 }