GPS
Dependents: CameraC1098_GPS_different_Lib
HeptaGPS.cpp@0:1be20a74923d, 2017-08-03 (annotated)
- Committer:
- hepta2ume
- Date:
- Thu Aug 03 04:28:24 2017 +0000
- Revision:
- 0:1be20a74923d
test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
hepta2ume | 0:1be20a74923d | 1 | #include "HeptaGPS.h" |
hepta2ume | 0:1be20a74923d | 2 | #include "mbed.h" |
hepta2ume | 0:1be20a74923d | 3 | |
hepta2ume | 0:1be20a74923d | 4 | HeptaGPS::HeptaGPS(PinName tx, PinName rx) : gps(tx,rx) |
hepta2ume | 0:1be20a74923d | 5 | { |
hepta2ume | 0:1be20a74923d | 6 | } |
hepta2ume | 0:1be20a74923d | 7 | void HeptaGPS::baud(int rate) |
hepta2ume | 0:1be20a74923d | 8 | { |
hepta2ume | 0:1be20a74923d | 9 | gps.baud(rate); |
hepta2ume | 0:1be20a74923d | 10 | } |
hepta2ume | 0:1be20a74923d | 11 | char HeptaGPS::getc() |
hepta2ume | 0:1be20a74923d | 12 | { |
hepta2ume | 0:1be20a74923d | 13 | c = gps.getc(); |
hepta2ume | 0:1be20a74923d | 14 | return c; |
hepta2ume | 0:1be20a74923d | 15 | } |
hepta2ume | 0:1be20a74923d | 16 | int HeptaGPS::readable() |
hepta2ume | 0:1be20a74923d | 17 | { |
hepta2ume | 0:1be20a74923d | 18 | i = gps.readable(); |
hepta2ume | 0:1be20a74923d | 19 | return i; |
hepta2ume | 0:1be20a74923d | 20 | } |
hepta2ume | 0:1be20a74923d | 21 | void HeptaGPS::flushSerialBuffer(void) |
hepta2ume | 0:1be20a74923d | 22 | { |
hepta2ume | 0:1be20a74923d | 23 | ite = 0; |
hepta2ume | 0:1be20a74923d | 24 | while (gps.readable()) |
hepta2ume | 0:1be20a74923d | 25 | { |
hepta2ume | 0:1be20a74923d | 26 | gps.getc(); |
hepta2ume | 0:1be20a74923d | 27 | ite++; |
hepta2ume | 0:1be20a74923d | 28 | if(ite==100){break;}; |
hepta2ume | 0:1be20a74923d | 29 | } |
hepta2ume | 0:1be20a74923d | 30 | return; |
hepta2ume | 0:1be20a74923d | 31 | } |
hepta2ume | 0:1be20a74923d | 32 | void HeptaGPS::gga_sensing(float *time, float *latitude, char *ns, float *longitude, char *ew, int *quality, int *stnum, float *hacu, float *altitude, char *aunit, int *gps_check) |
hepta2ume | 0:1be20a74923d | 33 | { |
hepta2ume | 0:1be20a74923d | 34 | int ite = 0; |
hepta2ume | 0:1be20a74923d | 35 | while(gps.getc()!='$'){ |
hepta2ume | 0:1be20a74923d | 36 | ite++; |
hepta2ume | 0:1be20a74923d | 37 | if(ite==10000) break; |
hepta2ume | 0:1be20a74923d | 38 | } |
hepta2ume | 0:1be20a74923d | 39 | for(int i=0; i<5; i++){ |
hepta2ume | 0:1be20a74923d | 40 | msg[i] = gps.getc(); |
hepta2ume | 0:1be20a74923d | 41 | } |
hepta2ume | 0:1be20a74923d | 42 | if((msg[2]=='G')&(msg[3]=='G')&(msg[4]=='A')){ |
hepta2ume | 0:1be20a74923d | 43 | for(int j=0; j<6; j++){ |
hepta2ume | 0:1be20a74923d | 44 | if(j==0){ |
hepta2ume | 0:1be20a74923d | 45 | for(int i=5; i<256; i++){ |
hepta2ume | 0:1be20a74923d | 46 | msg[i] = gps.getc(); |
hepta2ume | 0:1be20a74923d | 47 | if(msg[i] == '\r') { |
hepta2ume | 0:1be20a74923d | 48 | msg[i] = 0; |
hepta2ume | 0:1be20a74923d | 49 | break; |
hepta2ume | 0:1be20a74923d | 50 | } |
hepta2ume | 0:1be20a74923d | 51 | } |
hepta2ume | 0:1be20a74923d | 52 | }else{ |
hepta2ume | 0:1be20a74923d | 53 | for(int i=0; i<256; i++){ |
hepta2ume | 0:1be20a74923d | 54 | msgd[i] = gps.getc(); |
hepta2ume | 0:1be20a74923d | 55 | if(msgd[i] == '\r') { |
hepta2ume | 0:1be20a74923d | 56 | break; |
hepta2ume | 0:1be20a74923d | 57 | } |
hepta2ume | 0:1be20a74923d | 58 | } |
hepta2ume | 0:1be20a74923d | 59 | if((msgd[4]=='V')&(msgd[5]=='T')&(msgd[6]=='G')){ |
hepta2ume | 0:1be20a74923d | 60 | break; |
hepta2ume | 0:1be20a74923d | 61 | } |
hepta2ume | 0:1be20a74923d | 62 | } |
hepta2ume | 0:1be20a74923d | 63 | } |
hepta2ume | 0:1be20a74923d | 64 | if(sscanf(msg, "GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c", time, latitude, ns, longitude, ew, quality, stnum, hacu, altitude, aunit) >= 1) { |
hepta2ume | 0:1be20a74923d | 65 | if(!(quality)) { |
hepta2ume | 0:1be20a74923d | 66 | //latitude(unit transformation) |
hepta2ume | 0:1be20a74923d | 67 | *latitude=int(*latitude/100)+(*latitude-int(*latitude/100)*100)/60; |
hepta2ume | 0:1be20a74923d | 68 | //longitude(unit transformation) |
hepta2ume | 0:1be20a74923d | 69 | *longitude = int(*longitude/100)+(*longitude-int(*longitude/100)*100)/60; |
hepta2ume | 0:1be20a74923d | 70 | *gps_check = 0; |
hepta2ume | 0:1be20a74923d | 71 | } else { |
hepta2ume | 0:1be20a74923d | 72 | //latitude(unit transformation) |
hepta2ume | 0:1be20a74923d | 73 | *latitude=int(*latitude/100)+(*latitude-int(*latitude/100)*100)/60; |
hepta2ume | 0:1be20a74923d | 74 | //longitude(unit transformation) |
hepta2ume | 0:1be20a74923d | 75 | *longitude = int(*longitude/100)+(*longitude-int(*longitude/100)*100)/60; |
hepta2ume | 0:1be20a74923d | 76 | *gps_check = 1; |
hepta2ume | 0:1be20a74923d | 77 | } |
hepta2ume | 0:1be20a74923d | 78 | } |
hepta2ume | 0:1be20a74923d | 79 | else{ |
hepta2ume | 0:1be20a74923d | 80 | printf("No Data"); |
hepta2ume | 0:1be20a74923d | 81 | *gps_check = 2; |
hepta2ume | 0:1be20a74923d | 82 | } |
hepta2ume | 0:1be20a74923d | 83 | } |
hepta2ume | 0:1be20a74923d | 84 | else{ |
hepta2ume | 0:1be20a74923d | 85 | *gps_check = 3; |
hepta2ume | 0:1be20a74923d | 86 | } |
hepta2ume | 0:1be20a74923d | 87 | } |
hepta2ume | 0:1be20a74923d | 88 | void HeptaGPS::lat_log_sensing_u16(char *lat, char *log, int *dsize) |
hepta2ume | 0:1be20a74923d | 89 | { |
hepta2ume | 0:1be20a74923d | 90 | 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}; |
hepta2ume | 0:1be20a74923d | 91 | int i=0,j=0; |
hepta2ume | 0:1be20a74923d | 92 | while (gps.readable()){ |
hepta2ume | 0:1be20a74923d | 93 | gps.getc(); |
hepta2ume | 0:1be20a74923d | 94 | } |
hepta2ume | 0:1be20a74923d | 95 | loop: |
hepta2ume | 0:1be20a74923d | 96 | while(gps.getc()!='$'){} |
hepta2ume | 0:1be20a74923d | 97 | for(j=0;j<5;j++){ |
hepta2ume | 0:1be20a74923d | 98 | gps_data[1][j]=gps.getc(); |
hepta2ume | 0:1be20a74923d | 99 | } |
hepta2ume | 0:1be20a74923d | 100 | if((gps_data[1][2]==0x52)&(gps_data[1][3]==0x4d)&(gps_data[1][4]==0x43)){ |
hepta2ume | 0:1be20a74923d | 101 | for(j=0;j<1;j++){ |
hepta2ume | 0:1be20a74923d | 102 | if(j==0){ |
hepta2ume | 0:1be20a74923d | 103 | i=0; |
hepta2ume | 0:1be20a74923d | 104 | while((gps_data[j+1][i+5] = gps.getc()) != '\r'){ |
hepta2ume | 0:1be20a74923d | 105 | //pc.putc(gps_data[j+1][i+5]); |
hepta2ume | 0:1be20a74923d | 106 | i++; |
hepta2ume | 0:1be20a74923d | 107 | } |
hepta2ume | 0:1be20a74923d | 108 | gps_data[j+1][i+5]='\0'; |
hepta2ume | 0:1be20a74923d | 109 | i=0; |
hepta2ume | 0:1be20a74923d | 110 | //pc.printf("\n\r"); |
hepta2ume | 0:1be20a74923d | 111 | } |
hepta2ume | 0:1be20a74923d | 112 | else{ |
hepta2ume | 0:1be20a74923d | 113 | while(gps.getc()!='$'){} |
hepta2ume | 0:1be20a74923d | 114 | i=0; |
hepta2ume | 0:1be20a74923d | 115 | while((gps_data[j+1][i] = gps.getc()) != '\r'){ |
hepta2ume | 0:1be20a74923d | 116 | //pc.putc(gps_data[j+1][i]); |
hepta2ume | 0:1be20a74923d | 117 | i++; |
hepta2ume | 0:1be20a74923d | 118 | } |
hepta2ume | 0:1be20a74923d | 119 | gps_data[j+1][i]='\0'; |
hepta2ume | 0:1be20a74923d | 120 | i=0; |
hepta2ume | 0:1be20a74923d | 121 | //pc.printf("\n\r"); |
hepta2ume | 0:1be20a74923d | 122 | } |
hepta2ume | 0:1be20a74923d | 123 | } |
hepta2ume | 0:1be20a74923d | 124 | } |
hepta2ume | 0:1be20a74923d | 125 | else |
hepta2ume | 0:1be20a74923d | 126 | { |
hepta2ume | 0:1be20a74923d | 127 | goto loop; |
hepta2ume | 0:1be20a74923d | 128 | } |
hepta2ume | 0:1be20a74923d | 129 | if( sscanf(gps_data[1],"GPRMC,%f,%c,%f,%c,%f,%c,%f",&time,&statas,&hokui,&ns,&tokei,&ew,&vel) >= 1){ |
hepta2ume | 0:1be20a74923d | 130 | //hokui |
hepta2ume | 0:1be20a74923d | 131 | d_hokui=int(hokui/100); |
hepta2ume | 0:1be20a74923d | 132 | m_hokui=(hokui-d_hokui*100); |
hepta2ume | 0:1be20a74923d | 133 | //m_hokui=(hokui-d_hokui*100)/60; |
hepta2ume | 0:1be20a74923d | 134 | g_hokui=d_hokui+(hokui-d_hokui*100)/60; |
hepta2ume | 0:1be20a74923d | 135 | sprintf( gph1, "%02X", (char(d_hokui)) & 0xFF); |
hepta2ume | 0:1be20a74923d | 136 | sprintf( gph2, "%02X", (char(m_hokui)) & 0xFF); |
hepta2ume | 0:1be20a74923d | 137 | sprintf( gph3, "%02X", (char((m_hokui-char(m_hokui))*100)) & 0xFF); |
hepta2ume | 0:1be20a74923d | 138 | sprintf( gph4, "%02X", (char(((m_hokui-char(m_hokui))*100-char((m_hokui-char(m_hokui))*100))*100)) & 0xFF); |
hepta2ume | 0:1be20a74923d | 139 | |
hepta2ume | 0:1be20a74923d | 140 | //tokei |
hepta2ume | 0:1be20a74923d | 141 | d_tokei=int(tokei/100); |
hepta2ume | 0:1be20a74923d | 142 | m_tokei=(tokei-d_tokei*100); |
hepta2ume | 0:1be20a74923d | 143 | //m_tokei=(tokei-d_tokei*100)/60; |
hepta2ume | 0:1be20a74923d | 144 | g_tokei=d_tokei+(tokei-d_tokei*100)/60; |
hepta2ume | 0:1be20a74923d | 145 | sprintf( gpt1, "%02X", (char(d_tokei)) & 0xFF); |
hepta2ume | 0:1be20a74923d | 146 | sprintf( gpt2, "%02X", (char(m_tokei)) & 0xFF); |
hepta2ume | 0:1be20a74923d | 147 | sprintf( gpt3, "%02X", (char((m_tokei-char(m_tokei))*100)) & 0xFF); |
hepta2ume | 0:1be20a74923d | 148 | sprintf( gpt4, "%02X", (char(((m_tokei-char(m_tokei))*100-char((m_tokei-char(m_tokei))*100))*100)) & 0xFF); |
hepta2ume | 0:1be20a74923d | 149 | lat[0] = gph1[0]; |
hepta2ume | 0:1be20a74923d | 150 | lat[1] = gph1[1]; |
hepta2ume | 0:1be20a74923d | 151 | lat[2] = gph2[0]; |
hepta2ume | 0:1be20a74923d | 152 | lat[3] = gph2[1]; |
hepta2ume | 0:1be20a74923d | 153 | lat[4] = gph3[0]; |
hepta2ume | 0:1be20a74923d | 154 | lat[5] = gph3[1]; |
hepta2ume | 0:1be20a74923d | 155 | lat[6] = gph4[0]; |
hepta2ume | 0:1be20a74923d | 156 | lat[7] = gph4[1]; |
hepta2ume | 0:1be20a74923d | 157 | log[0] = gpt1[0]; |
hepta2ume | 0:1be20a74923d | 158 | log[1] = gpt1[1]; |
hepta2ume | 0:1be20a74923d | 159 | log[2] = gpt2[0]; |
hepta2ume | 0:1be20a74923d | 160 | log[3] = gpt2[1]; |
hepta2ume | 0:1be20a74923d | 161 | log[4] = gpt3[0]; |
hepta2ume | 0:1be20a74923d | 162 | log[5] = gpt3[1]; |
hepta2ume | 0:1be20a74923d | 163 | log[6] = gpt4[0]; |
hepta2ume | 0:1be20a74923d | 164 | log[7] = gpt4[1]; |
hepta2ume | 0:1be20a74923d | 165 | } |
hepta2ume | 0:1be20a74923d | 166 | *dsize = 8; |
hepta2ume | 0:1be20a74923d | 167 | } |