Miguel Luis / lib_gps

GPS handling library

Dependents:   LoRaWAN-NAMote72-Application-Demo-good LoRaWAN-gps-cayenne LoRaWAN-NAMote72-Application-Demo

Fork of lib_gps by wayne roberts

gps.cpp@0:0bd73064b7f6, 2015-03-18 (annotated)

Committer:
dudmuck
Date:
Wed Mar 18 00:55:27 2015 +0000
Revision:
0:0bd73064b7f6
Child:
1:2f5fbb33ae8b
NMEA GPS receiver

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dudmuck 0:0bd73064b7f6 1 #include "mbed.h"
dudmuck 0:0bd73064b7f6 2 #include "gps.h"
dudmuck 0:0bd73064b7f6 3
dudmuck 0:0bd73064b7f6 4 const char NmeaDataTypeGPGGA[] = "GPGGA";
dudmuck 0:0bd73064b7f6 5 //const char NmeaDataTypeGPGSA[] = "GPGSA";
dudmuck 0:0bd73064b7f6 6 //const char NmeaDataTypeGPGSV[] = "GPGSV";
dudmuck 0:0bd73064b7f6 7 const char NmeaDataTypeGPRMC[] = "GPRMC";
dudmuck 0:0bd73064b7f6 8
dudmuck 0:0bd73064b7f6 9 /* Value used for the conversion of the position from DMS to decimal */
dudmuck 0:0bd73064b7f6 10 const int32_t MaxNorthPosition = 8388607; // 2^23 - 1
dudmuck 0:0bd73064b7f6 11 const int32_t MaxSouthPosition = 8388608; // -2^23
dudmuck 0:0bd73064b7f6 12 const int32_t MaxEastPosition = 8388607; // 2^23 - 1
dudmuck 0:0bd73064b7f6 13 const int32_t MaxWestPosition = 8388608; // -2^23
dudmuck 0:0bd73064b7f6 14
dudmuck 0:0bd73064b7f6 15 Serial gps_uart(PB_6, PB_7);
dudmuck 0:0bd73064b7f6 16 DigitalOut gps_en(PB_11);
dudmuck 0:0bd73064b7f6 17 InterruptIn pps_pin(PC_5);
dudmuck 0:0bd73064b7f6 18
dudmuck 0:0bd73064b7f6 19 GPS::GPS()
dudmuck 0:0bd73064b7f6 20 {
dudmuck 0:0bd73064b7f6 21
dudmuck 0:0bd73064b7f6 22 }
dudmuck 0:0bd73064b7f6 23
dudmuck 0:0bd73064b7f6 24 GPS::~GPS()
dudmuck 0:0bd73064b7f6 25 {
dudmuck 0:0bd73064b7f6 26 }
dudmuck 0:0bd73064b7f6 27
dudmuck 0:0bd73064b7f6 28 void GPS::pps()
dudmuck 0:0bd73064b7f6 29 {
dudmuck 0:0bd73064b7f6 30 pps_occurred = true;
dudmuck 0:0bd73064b7f6 31 }
dudmuck 0:0bd73064b7f6 32
dudmuck 0:0bd73064b7f6 33 void GPS::on_uart_rx()
dudmuck 0:0bd73064b7f6 34 {
dudmuck 0:0bd73064b7f6 35 //uint8_t *len = &rx_buf_lens[rx_bufs_in_idx];
dudmuck 0:0bd73064b7f6 36 uint8_t len = rx_buf_lens[rx_bufs_in_idx];
dudmuck 0:0bd73064b7f6 37 char ch;
dudmuck 0:0bd73064b7f6 38
dudmuck 0:0bd73064b7f6 39 do {
dudmuck 0:0bd73064b7f6 40 ch = gps_uart.getc();
dudmuck 0:0bd73064b7f6 41
dudmuck 0:0bd73064b7f6 42 if (ch != '$') {
dudmuck 0:0bd73064b7f6 43 if (len < RX_BUF_SIZE) {
dudmuck 0:0bd73064b7f6 44 rx_bufs[rx_bufs_in_idx][len] = ch;
dudmuck 0:0bd73064b7f6 45 rx_buf_lens[rx_bufs_in_idx]++;
dudmuck 0:0bd73064b7f6 46 } else
dudmuck 0:0bd73064b7f6 47 printf("S");
dudmuck 0:0bd73064b7f6 48 } else if (len != 0) {
dudmuck 0:0bd73064b7f6 49 printf("$ at %d\r\n", len);
dudmuck 0:0bd73064b7f6 50 }
dudmuck 0:0bd73064b7f6 51
dudmuck 0:0bd73064b7f6 52 if (ch == '\n') {
dudmuck 0:0bd73064b7f6 53 rx_bufs[rx_bufs_in_idx][len] = 0; // null terminate
dudmuck 0:0bd73064b7f6 54 if (++rx_bufs_in_idx == NUM_RX_BUFS)
dudmuck 0:0bd73064b7f6 55 rx_bufs_in_idx = 0;
dudmuck 0:0bd73064b7f6 56 /* just let it overflow -- if (rx_bufs_in_idx == rx_bufs_out_idx)
dudmuck 0:0bd73064b7f6 57 printf("gps overflow\r\n");*/
dudmuck 0:0bd73064b7f6 58 rx_buf_lens[rx_bufs_in_idx] = 0;
dudmuck 0:0bd73064b7f6 59 }
dudmuck 0:0bd73064b7f6 60 } while (gps_uart.readable());
dudmuck 0:0bd73064b7f6 61 }
dudmuck 0:0bd73064b7f6 62
dudmuck 0:0bd73064b7f6 63 void GPS::init()
dudmuck 0:0bd73064b7f6 64 {
dudmuck 0:0bd73064b7f6 65 rx_bufs_in_idx = 0;
dudmuck 0:0bd73064b7f6 66 rx_buf_lens[rx_bufs_in_idx] = 0;
dudmuck 0:0bd73064b7f6 67 rx_bufs_out_idx = 0;
dudmuck 0:0bd73064b7f6 68
dudmuck 0:0bd73064b7f6 69 //gps_uart.baud(57600);
dudmuck 0:0bd73064b7f6 70 gps_uart.attach(this, &GPS::on_uart_rx);
dudmuck 0:0bd73064b7f6 71
dudmuck 0:0bd73064b7f6 72 pps_pin.rise(this, &GPS::pps);
dudmuck 0:0bd73064b7f6 73 }
dudmuck 0:0bd73064b7f6 74
dudmuck 0:0bd73064b7f6 75 #define DELAY_MAX 20
dudmuck 0:0bd73064b7f6 76 void GPS::enable(bool en)
dudmuck 0:0bd73064b7f6 77 {
dudmuck 0:0bd73064b7f6 78 if (en) {
dudmuck 0:0bd73064b7f6 79 /* probably better instead to remove 22uF from GPS VCC */
dudmuck 0:0bd73064b7f6 80 volatile int d;
dudmuck 0:0bd73064b7f6 81 volatile int i;
dudmuck 0:0bd73064b7f6 82 //volatile int d_on = 0;
dudmuck 0:0bd73064b7f6 83 for (d = DELAY_MAX; d > 0; d--) {
dudmuck 0:0bd73064b7f6 84 gps_en = 0;
dudmuck 0:0bd73064b7f6 85 /*for (i = 0; i < d_on; i++)
dudmuck 0:0bd73064b7f6 86 asm("nop");
dudmuck 0:0bd73064b7f6 87 if (d & 1)
dudmuck 0:0bd73064b7f6 88 d_on++;*/
dudmuck 0:0bd73064b7f6 89 gps_en = 1;
dudmuck 0:0bd73064b7f6 90 for (i = 0; i < d; i++) {
dudmuck 0:0bd73064b7f6 91 asm("nop");
dudmuck 0:0bd73064b7f6 92 }
dudmuck 0:0bd73064b7f6 93 }
dudmuck 0:0bd73064b7f6 94
dudmuck 0:0bd73064b7f6 95 for (d = 0; d < 10; d++) {
dudmuck 0:0bd73064b7f6 96 volatile int n;
dudmuck 0:0bd73064b7f6 97 for (n = 0; n < 3; n++) {
dudmuck 0:0bd73064b7f6 98 gps_en = 0;
dudmuck 0:0bd73064b7f6 99 for (i = 0; i < d; i++) {
dudmuck 0:0bd73064b7f6 100 asm("nop");
dudmuck 0:0bd73064b7f6 101 }
dudmuck 0:0bd73064b7f6 102 gps_en = 1;
dudmuck 0:0bd73064b7f6 103 asm("nop");
dudmuck 0:0bd73064b7f6 104 asm("nop");
dudmuck 0:0bd73064b7f6 105 asm("nop");
dudmuck 0:0bd73064b7f6 106 asm("nop");
dudmuck 0:0bd73064b7f6 107 }
dudmuck 0:0bd73064b7f6 108 }
dudmuck 0:0bd73064b7f6 109 gps_en = 0;
dudmuck 0:0bd73064b7f6 110 } else {
dudmuck 0:0bd73064b7f6 111 gps_en = 1;
dudmuck 0:0bd73064b7f6 112 }
dudmuck 0:0bd73064b7f6 113 //gps_en = !en;
dudmuck 0:0bd73064b7f6 114 //printf("gps_en:%d\n", en);
dudmuck 0:0bd73064b7f6 115 }
dudmuck 0:0bd73064b7f6 116
dudmuck 0:0bd73064b7f6 117 bool GPS::enabled()
dudmuck 0:0bd73064b7f6 118 {
dudmuck 0:0bd73064b7f6 119 return !gps_en.read();
dudmuck 0:0bd73064b7f6 120 }
dudmuck 0:0bd73064b7f6 121
dudmuck 0:0bd73064b7f6 122 /*char GPS::Nibble2HexChar( char a );
dudmuck 0:0bd73064b7f6 123 {
dudmuck 0:0bd73064b7f6 124 if( a < 10 )
dudmuck 0:0bd73064b7f6 125 {
dudmuck 0:0bd73064b7f6 126 return '0' + a;
dudmuck 0:0bd73064b7f6 127 }
dudmuck 0:0bd73064b7f6 128 else if( a < 16 )
dudmuck 0:0bd73064b7f6 129 {
dudmuck 0:0bd73064b7f6 130 return 'A' + ( a - 10 );
dudmuck 0:0bd73064b7f6 131 }
dudmuck 0:0bd73064b7f6 132 else
dudmuck 0:0bd73064b7f6 133 {
dudmuck 0:0bd73064b7f6 134 return '?';
dudmuck 0:0bd73064b7f6 135 }
dudmuck 0:0bd73064b7f6 136 }*/
dudmuck 0:0bd73064b7f6 137
dudmuck 0:0bd73064b7f6 138 int GPS::NmeaChecksum( char *nmeaStr, uint8_t nmeaStrSize, char * checksum )
dudmuck 0:0bd73064b7f6 139 {
dudmuck 0:0bd73064b7f6 140 int i = 0;
dudmuck 0:0bd73064b7f6 141 uint8_t checkNum = 0;
dudmuck 0:0bd73064b7f6 142
dudmuck 0:0bd73064b7f6 143 // Check input parameters
dudmuck 0:0bd73064b7f6 144 if( ( nmeaStr == NULL ) || ( checksum == NULL ) || ( nmeaStrSize <= 1 ) )
dudmuck 0:0bd73064b7f6 145 {
dudmuck 0:0bd73064b7f6 146 return -1;
dudmuck 0:0bd73064b7f6 147 }
dudmuck 0:0bd73064b7f6 148
dudmuck 0:0bd73064b7f6 149 // Skip the first '$' if necessary
dudmuck 0:0bd73064b7f6 150 if( nmeaStr[i] == '$' )
dudmuck 0:0bd73064b7f6 151 {
dudmuck 0:0bd73064b7f6 152 i += 1;
dudmuck 0:0bd73064b7f6 153 }
dudmuck 0:0bd73064b7f6 154
dudmuck 0:0bd73064b7f6 155 // XOR until '*' or max length is reached
dudmuck 0:0bd73064b7f6 156 while( nmeaStr[i] != '*' )
dudmuck 0:0bd73064b7f6 157 {
dudmuck 0:0bd73064b7f6 158 checkNum ^= nmeaStr[i];
dudmuck 0:0bd73064b7f6 159 i += 1;
dudmuck 0:0bd73064b7f6 160 if( i >= nmeaStrSize )
dudmuck 0:0bd73064b7f6 161 {
dudmuck 0:0bd73064b7f6 162 return -1;
dudmuck 0:0bd73064b7f6 163 }
dudmuck 0:0bd73064b7f6 164 }
dudmuck 0:0bd73064b7f6 165
dudmuck 0:0bd73064b7f6 166 // Convert checksum value to 2 hexadecimal characters
dudmuck 0:0bd73064b7f6 167 /*checksum[0] = Nibble2HexChar( checkNum / 16 ); // upper nibble
dudmuck 0:0bd73064b7f6 168 checksum[1] = Nibble2HexChar( checkNum % 16 ); // lower nibble
dudmuck 0:0bd73064b7f6 169 */
dudmuck 0:0bd73064b7f6 170 sprintf(checksum, "%02X", checkNum);
dudmuck 0:0bd73064b7f6 171
dudmuck 0:0bd73064b7f6 172 return i + 1;
dudmuck 0:0bd73064b7f6 173 }
dudmuck 0:0bd73064b7f6 174
dudmuck 0:0bd73064b7f6 175 bool GPS::NmeaValidateChecksum(int idx)
dudmuck 0:0bd73064b7f6 176 {
dudmuck 0:0bd73064b7f6 177 int checksumIndex;
dudmuck 0:0bd73064b7f6 178 char checksum[4];
dudmuck 0:0bd73064b7f6 179 char *NmeaString = rx_bufs[idx];
dudmuck 0:0bd73064b7f6 180
dudmuck 0:0bd73064b7f6 181 checksumIndex = NmeaChecksum( NmeaString, rx_buf_lens[idx], checksum );
dudmuck 0:0bd73064b7f6 182
dudmuck 0:0bd73064b7f6 183 // could we calculate a verification checksum ?
dudmuck 0:0bd73064b7f6 184 if( checksumIndex < 0 )
dudmuck 0:0bd73064b7f6 185 {
dudmuck 0:0bd73064b7f6 186 printf("gps:checksumIndex:%d\r\n", checksumIndex);
dudmuck 0:0bd73064b7f6 187 return false;
dudmuck 0:0bd73064b7f6 188 }
dudmuck 0:0bd73064b7f6 189
dudmuck 0:0bd73064b7f6 190 // check if there are enough char in the serial buffer to read checksum
dudmuck 0:0bd73064b7f6 191 if( checksumIndex >= ( rx_buf_lens[idx] - 2 ) )
dudmuck 0:0bd73064b7f6 192 {
dudmuck 0:0bd73064b7f6 193 printf("gps:checksumIndex:%d\r\n", checksumIndex);
dudmuck 0:0bd73064b7f6 194 return false;
dudmuck 0:0bd73064b7f6 195 }
dudmuck 0:0bd73064b7f6 196
dudmuck 0:0bd73064b7f6 197 // check the checksum
dudmuck 0:0bd73064b7f6 198 if( ( NmeaString[checksumIndex] == checksum[0] ) && ( NmeaString[checksumIndex + 1] == checksum[1] ) )
dudmuck 0:0bd73064b7f6 199 {
dudmuck 0:0bd73064b7f6 200 return true;
dudmuck 0:0bd73064b7f6 201 }
dudmuck 0:0bd73064b7f6 202 else
dudmuck 0:0bd73064b7f6 203 {
dudmuck 0:0bd73064b7f6 204 if (verbose)
dudmuck 0:0bd73064b7f6 205 printf("gps:checksum fail idx:%d %c%c %c%c\r\n", checksumIndex, checksum[0], checksum[1], NmeaString[checksumIndex], NmeaString[checksumIndex+1]);
dudmuck 0:0bd73064b7f6 206 return false;
dudmuck 0:0bd73064b7f6 207 }
dudmuck 0:0bd73064b7f6 208 }
dudmuck 0:0bd73064b7f6 209
dudmuck 0:0bd73064b7f6 210 void GPS::ConvertPositionFromStringToNumerical( )
dudmuck 0:0bd73064b7f6 211 {
dudmuck 0:0bd73064b7f6 212 int i;
dudmuck 0:0bd73064b7f6 213
dudmuck 0:0bd73064b7f6 214 double valueTmp1;
dudmuck 0:0bd73064b7f6 215 double valueTmp2;
dudmuck 0:0bd73064b7f6 216 double valueTmp3;
dudmuck 0:0bd73064b7f6 217 double valueTmp4;
dudmuck 0:0bd73064b7f6 218
dudmuck 0:0bd73064b7f6 219 // Convert the latitude from ASCII to uint8_t values
dudmuck 0:0bd73064b7f6 220 for( i = 0 ; i < 10 ; i++ )
dudmuck 0:0bd73064b7f6 221 {
dudmuck 0:0bd73064b7f6 222 NmeaGpsData.NmeaLatitude[i] = NmeaGpsData.NmeaLatitude[i] & 0xF;
dudmuck 0:0bd73064b7f6 223 }
dudmuck 0:0bd73064b7f6 224 // Convert latitude from degree/minute/second (DMS) format into decimal
dudmuck 0:0bd73064b7f6 225 valueTmp1 = ( double )NmeaGpsData.NmeaLatitude[0] * 10.0 + ( double )NmeaGpsData.NmeaLatitude[1];
dudmuck 0:0bd73064b7f6 226 valueTmp2 = ( double )NmeaGpsData.NmeaLatitude[2] * 10.0 + ( double )NmeaGpsData.NmeaLatitude[3];
dudmuck 0:0bd73064b7f6 227 valueTmp3 = ( double )NmeaGpsData.NmeaLatitude[5] * 1000.0 + ( double )NmeaGpsData.NmeaLatitude[6] * 100.0 +
dudmuck 0:0bd73064b7f6 228 ( double )NmeaGpsData.NmeaLatitude[7] * 10.0 + ( double )NmeaGpsData.NmeaLatitude[8];
dudmuck 0:0bd73064b7f6 229
dudmuck 0:0bd73064b7f6 230 Latitude = valueTmp1 + ( ( valueTmp2 + ( valueTmp3 * 0.0001 ) ) / 60.0 );
dudmuck 0:0bd73064b7f6 231
dudmuck 0:0bd73064b7f6 232 if( NmeaGpsData.NmeaLatitudePole[0] == 'S' )
dudmuck 0:0bd73064b7f6 233 {
dudmuck 0:0bd73064b7f6 234 Latitude *= -1;
dudmuck 0:0bd73064b7f6 235 }
dudmuck 0:0bd73064b7f6 236
dudmuck 0:0bd73064b7f6 237 // Convert the longitude from ASCII to uint8_t values
dudmuck 0:0bd73064b7f6 238 for( i = 0 ; i < 10 ; i++ )
dudmuck 0:0bd73064b7f6 239 {
dudmuck 0:0bd73064b7f6 240 NmeaGpsData.NmeaLongitude[i] = NmeaGpsData.NmeaLongitude[i] & 0xF;
dudmuck 0:0bd73064b7f6 241 }
dudmuck 0:0bd73064b7f6 242 // Convert longitude from degree/minute/second (DMS) format into decimal
dudmuck 0:0bd73064b7f6 243 valueTmp1 = ( double )NmeaGpsData.NmeaLongitude[0] * 100.0 + ( double )NmeaGpsData.NmeaLongitude[1] * 10.0 + ( double )NmeaGpsData.NmeaLongitude[2];
dudmuck 0:0bd73064b7f6 244 valueTmp2 = ( double )NmeaGpsData.NmeaLongitude[3] * 10.0 + ( double )NmeaGpsData.NmeaLongitude[4];
dudmuck 0:0bd73064b7f6 245 valueTmp3 = ( double )NmeaGpsData.NmeaLongitude[6] * 1000.0 + ( double )NmeaGpsData.NmeaLongitude[7] * 100;
dudmuck 0:0bd73064b7f6 246 valueTmp4 = ( double )NmeaGpsData.NmeaLongitude[8] * 10.0 + ( double )NmeaGpsData.NmeaLongitude[9];
dudmuck 0:0bd73064b7f6 247
dudmuck 0:0bd73064b7f6 248 Longitude = valueTmp1 + ( valueTmp2 / 60.0 ) + ( ( ( valueTmp3 + valueTmp4 ) * 0.0001 ) / 60.0 );
dudmuck 0:0bd73064b7f6 249
dudmuck 0:0bd73064b7f6 250 if( NmeaGpsData.NmeaLongitudePole[0] == 'W' )
dudmuck 0:0bd73064b7f6 251 {
dudmuck 0:0bd73064b7f6 252 Longitude *= -1;
dudmuck 0:0bd73064b7f6 253 }
dudmuck 0:0bd73064b7f6 254
dudmuck 0:0bd73064b7f6 255 //printf("DD %f, %f\r\n", Latitude, Longitude);
dudmuck 0:0bd73064b7f6 256 }
dudmuck 0:0bd73064b7f6 257
dudmuck 0:0bd73064b7f6 258 void GPS::ConvertPositionIntoBinary( )
dudmuck 0:0bd73064b7f6 259 {
dudmuck 0:0bd73064b7f6 260 long double temp;
dudmuck 0:0bd73064b7f6 261
dudmuck 0:0bd73064b7f6 262 if( Latitude >= 0 ) // North
dudmuck 0:0bd73064b7f6 263 {
dudmuck 0:0bd73064b7f6 264 temp = Latitude * MaxNorthPosition;
dudmuck 0:0bd73064b7f6 265 LatitudeBinary = temp / 90;
dudmuck 0:0bd73064b7f6 266 }
dudmuck 0:0bd73064b7f6 267 else // South
dudmuck 0:0bd73064b7f6 268 {
dudmuck 0:0bd73064b7f6 269 temp = Latitude * MaxSouthPosition;
dudmuck 0:0bd73064b7f6 270 LatitudeBinary = temp / 90;
dudmuck 0:0bd73064b7f6 271 }
dudmuck 0:0bd73064b7f6 272
dudmuck 0:0bd73064b7f6 273 if( Longitude >= 0 ) // East
dudmuck 0:0bd73064b7f6 274 {
dudmuck 0:0bd73064b7f6 275 temp = Longitude * MaxEastPosition;
dudmuck 0:0bd73064b7f6 276 LongitudeBinary = temp / 180;
dudmuck 0:0bd73064b7f6 277 }
dudmuck 0:0bd73064b7f6 278 else // West
dudmuck 0:0bd73064b7f6 279 {
dudmuck 0:0bd73064b7f6 280 temp = Longitude * MaxWestPosition;
dudmuck 0:0bd73064b7f6 281 LongitudeBinary = temp / 180;
dudmuck 0:0bd73064b7f6 282 }
dudmuck 0:0bd73064b7f6 283 }
dudmuck 0:0bd73064b7f6 284
dudmuck 0:0bd73064b7f6 285 int GPS::ParseGPSData(int idx)
dudmuck 0:0bd73064b7f6 286 {
dudmuck 0:0bd73064b7f6 287 uint8_t i, j, fieldSize;
dudmuck 0:0bd73064b7f6 288 char *NmeaString;
dudmuck 0:0bd73064b7f6 289
dudmuck 0:0bd73064b7f6 290 if (NmeaValidateChecksum(idx) == false) {
dudmuck 0:0bd73064b7f6 291 if (verbose)
dudmuck 0:0bd73064b7f6 292 printf("gps:bad nmea checksum:%s\r\n", rx_bufs[rx_bufs_out_idx]);
dudmuck 0:0bd73064b7f6 293 return FAIL;
dudmuck 0:0bd73064b7f6 294 }
dudmuck 0:0bd73064b7f6 295
dudmuck 0:0bd73064b7f6 296 NmeaString = rx_bufs[idx];
dudmuck 0:0bd73064b7f6 297 fieldSize = 0;
dudmuck 0:0bd73064b7f6 298 i = 0;
dudmuck 0:0bd73064b7f6 299 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 300 {
dudmuck 0:0bd73064b7f6 301 if( fieldSize > 6 )
dudmuck 0:0bd73064b7f6 302 {
dudmuck 0:0bd73064b7f6 303 printf("gps:fieldSize:%d\r\n", fieldSize);
dudmuck 0:0bd73064b7f6 304 return FAIL;
dudmuck 0:0bd73064b7f6 305 }
dudmuck 0:0bd73064b7f6 306 }
dudmuck 0:0bd73064b7f6 307
dudmuck 0:0bd73064b7f6 308 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 309 {
dudmuck 0:0bd73064b7f6 310 NmeaGpsData.NmeaDataType[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 311 }
dudmuck 0:0bd73064b7f6 312 // Parse the GPGGA data
dudmuck 0:0bd73064b7f6 313 if( strncmp( ( const char* )NmeaGpsData.NmeaDataType, ( const char* )NmeaDataTypeGPGGA, 5 ) == 0 )
dudmuck 0:0bd73064b7f6 314 {
dudmuck 0:0bd73064b7f6 315 // NmeaUtcTime
dudmuck 0:0bd73064b7f6 316 fieldSize = 0;
dudmuck 0:0bd73064b7f6 317 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 318 {
dudmuck 0:0bd73064b7f6 319 if( fieldSize > 11 )
dudmuck 0:0bd73064b7f6 320 {
dudmuck 0:0bd73064b7f6 321 return FAIL;
dudmuck 0:0bd73064b7f6 322 }
dudmuck 0:0bd73064b7f6 323 }
dudmuck 0:0bd73064b7f6 324 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 325 {
dudmuck 0:0bd73064b7f6 326 NmeaGpsData.NmeaUtcTime[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 327 }
dudmuck 0:0bd73064b7f6 328 // NmeaLatitude
dudmuck 0:0bd73064b7f6 329 fieldSize = 0;
dudmuck 0:0bd73064b7f6 330 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 331 {
dudmuck 0:0bd73064b7f6 332 if( fieldSize > 10 )
dudmuck 0:0bd73064b7f6 333 {
dudmuck 0:0bd73064b7f6 334 return FAIL;
dudmuck 0:0bd73064b7f6 335 }
dudmuck 0:0bd73064b7f6 336 }
dudmuck 0:0bd73064b7f6 337 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 338 {
dudmuck 0:0bd73064b7f6 339 NmeaGpsData.NmeaLatitude[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 340 }
dudmuck 0:0bd73064b7f6 341 // NmeaLatitudePole
dudmuck 0:0bd73064b7f6 342 fieldSize = 0;
dudmuck 0:0bd73064b7f6 343 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 344 {
dudmuck 0:0bd73064b7f6 345 if( fieldSize > 2 )
dudmuck 0:0bd73064b7f6 346 {
dudmuck 0:0bd73064b7f6 347 return FAIL;
dudmuck 0:0bd73064b7f6 348 }
dudmuck 0:0bd73064b7f6 349 }
dudmuck 0:0bd73064b7f6 350 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 351 {
dudmuck 0:0bd73064b7f6 352 NmeaGpsData.NmeaLatitudePole[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 353 }
dudmuck 0:0bd73064b7f6 354 // NmeaLongitude
dudmuck 0:0bd73064b7f6 355 fieldSize = 0;
dudmuck 0:0bd73064b7f6 356 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 357 {
dudmuck 0:0bd73064b7f6 358 if( fieldSize > 11 )
dudmuck 0:0bd73064b7f6 359 {
dudmuck 0:0bd73064b7f6 360 return FAIL;
dudmuck 0:0bd73064b7f6 361 }
dudmuck 0:0bd73064b7f6 362 }
dudmuck 0:0bd73064b7f6 363 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 364 {
dudmuck 0:0bd73064b7f6 365 NmeaGpsData.NmeaLongitude[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 366 }
dudmuck 0:0bd73064b7f6 367 // NmeaLongitudePole
dudmuck 0:0bd73064b7f6 368 fieldSize = 0;
dudmuck 0:0bd73064b7f6 369 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 370 {
dudmuck 0:0bd73064b7f6 371 if( fieldSize > 2 )
dudmuck 0:0bd73064b7f6 372 {
dudmuck 0:0bd73064b7f6 373 return FAIL;
dudmuck 0:0bd73064b7f6 374 }
dudmuck 0:0bd73064b7f6 375 }
dudmuck 0:0bd73064b7f6 376 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 377 {
dudmuck 0:0bd73064b7f6 378 NmeaGpsData.NmeaLongitudePole[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 379 }
dudmuck 0:0bd73064b7f6 380 // NmeaFixQuality
dudmuck 0:0bd73064b7f6 381 fieldSize = 0;
dudmuck 0:0bd73064b7f6 382 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 383 {
dudmuck 0:0bd73064b7f6 384 if( fieldSize > 2 )
dudmuck 0:0bd73064b7f6 385 {
dudmuck 0:0bd73064b7f6 386 return FAIL;
dudmuck 0:0bd73064b7f6 387 }
dudmuck 0:0bd73064b7f6 388 }
dudmuck 0:0bd73064b7f6 389 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 390 {
dudmuck 0:0bd73064b7f6 391 NmeaGpsData.NmeaFixQuality[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 392 }
dudmuck 0:0bd73064b7f6 393 // NmeaSatelliteTracked
dudmuck 0:0bd73064b7f6 394 fieldSize = 0;
dudmuck 0:0bd73064b7f6 395 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 396 {
dudmuck 0:0bd73064b7f6 397 if( fieldSize > 3 )
dudmuck 0:0bd73064b7f6 398 {
dudmuck 0:0bd73064b7f6 399 return FAIL;
dudmuck 0:0bd73064b7f6 400 }
dudmuck 0:0bd73064b7f6 401 }
dudmuck 0:0bd73064b7f6 402 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 403 {
dudmuck 0:0bd73064b7f6 404 NmeaGpsData.NmeaSatelliteTracked[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 405 }
dudmuck 0:0bd73064b7f6 406 // NmeaHorizontalDilution
dudmuck 0:0bd73064b7f6 407 fieldSize = 0;
dudmuck 0:0bd73064b7f6 408 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 409 {
dudmuck 0:0bd73064b7f6 410 if( fieldSize > 6 )
dudmuck 0:0bd73064b7f6 411 {
dudmuck 0:0bd73064b7f6 412 return FAIL;
dudmuck 0:0bd73064b7f6 413 }
dudmuck 0:0bd73064b7f6 414 }
dudmuck 0:0bd73064b7f6 415 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 416 {
dudmuck 0:0bd73064b7f6 417 NmeaGpsData.NmeaHorizontalDilution[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 418 }
dudmuck 0:0bd73064b7f6 419 // NmeaAltitude
dudmuck 0:0bd73064b7f6 420 fieldSize = 0;
dudmuck 0:0bd73064b7f6 421 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 422 {
dudmuck 0:0bd73064b7f6 423 if( fieldSize > 8 )
dudmuck 0:0bd73064b7f6 424 {
dudmuck 0:0bd73064b7f6 425 return FAIL;
dudmuck 0:0bd73064b7f6 426 }
dudmuck 0:0bd73064b7f6 427 }
dudmuck 0:0bd73064b7f6 428 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 429 {
dudmuck 0:0bd73064b7f6 430 NmeaGpsData.NmeaAltitude[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 431 }
dudmuck 0:0bd73064b7f6 432 // NmeaAltitudeUnit
dudmuck 0:0bd73064b7f6 433 fieldSize = 0;
dudmuck 0:0bd73064b7f6 434 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 435 {
dudmuck 0:0bd73064b7f6 436 if( fieldSize > 2 )
dudmuck 0:0bd73064b7f6 437 {
dudmuck 0:0bd73064b7f6 438 return FAIL;
dudmuck 0:0bd73064b7f6 439 }
dudmuck 0:0bd73064b7f6 440 }
dudmuck 0:0bd73064b7f6 441 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 442 {
dudmuck 0:0bd73064b7f6 443 NmeaGpsData.NmeaAltitudeUnit[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 444 }
dudmuck 0:0bd73064b7f6 445 // NmeaHeightGeoid
dudmuck 0:0bd73064b7f6 446 fieldSize = 0;
dudmuck 0:0bd73064b7f6 447 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 448 {
dudmuck 0:0bd73064b7f6 449 if( fieldSize > 8 )
dudmuck 0:0bd73064b7f6 450 {
dudmuck 0:0bd73064b7f6 451 return FAIL;
dudmuck 0:0bd73064b7f6 452 }
dudmuck 0:0bd73064b7f6 453 }
dudmuck 0:0bd73064b7f6 454 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 455 {
dudmuck 0:0bd73064b7f6 456 NmeaGpsData.NmeaHeightGeoid[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 457 }
dudmuck 0:0bd73064b7f6 458 // NmeaHeightGeoidUnit
dudmuck 0:0bd73064b7f6 459 fieldSize = 0;
dudmuck 0:0bd73064b7f6 460 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 461 {
dudmuck 0:0bd73064b7f6 462 if( fieldSize > 2 )
dudmuck 0:0bd73064b7f6 463 {
dudmuck 0:0bd73064b7f6 464 return FAIL;
dudmuck 0:0bd73064b7f6 465 }
dudmuck 0:0bd73064b7f6 466 }
dudmuck 0:0bd73064b7f6 467 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 468 {
dudmuck 0:0bd73064b7f6 469 NmeaGpsData.NmeaHeightGeoidUnit[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 470 }
dudmuck 0:0bd73064b7f6 471
dudmuck 0:0bd73064b7f6 472 //FormatGpsData( );
dudmuck 0:0bd73064b7f6 473 ConvertPositionFromStringToNumerical( );
dudmuck 0:0bd73064b7f6 474 ConvertPositionIntoBinary( );
dudmuck 0:0bd73064b7f6 475 return SUCCESS;
dudmuck 0:0bd73064b7f6 476 }
dudmuck 0:0bd73064b7f6 477 else if ( strncmp( ( const char* )NmeaGpsData.NmeaDataType, ( const char* )NmeaDataTypeGPRMC, 5 ) == 0 )
dudmuck 0:0bd73064b7f6 478 {
dudmuck 0:0bd73064b7f6 479 // NmeaUtcTime
dudmuck 0:0bd73064b7f6 480 fieldSize = 0;
dudmuck 0:0bd73064b7f6 481 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 482 {
dudmuck 0:0bd73064b7f6 483 if( fieldSize > 11 )
dudmuck 0:0bd73064b7f6 484 {
dudmuck 0:0bd73064b7f6 485 return FAIL;
dudmuck 0:0bd73064b7f6 486 }
dudmuck 0:0bd73064b7f6 487 }
dudmuck 0:0bd73064b7f6 488 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 489 {
dudmuck 0:0bd73064b7f6 490 NmeaGpsData.NmeaUtcTime[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 491 }
dudmuck 0:0bd73064b7f6 492 // NmeaDataStatus
dudmuck 0:0bd73064b7f6 493 fieldSize = 0;
dudmuck 0:0bd73064b7f6 494 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 495 {
dudmuck 0:0bd73064b7f6 496 if( fieldSize > 2 )
dudmuck 0:0bd73064b7f6 497 {
dudmuck 0:0bd73064b7f6 498 return FAIL;
dudmuck 0:0bd73064b7f6 499 }
dudmuck 0:0bd73064b7f6 500 }
dudmuck 0:0bd73064b7f6 501 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 502 {
dudmuck 0:0bd73064b7f6 503 NmeaGpsData.NmeaDataStatus[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 504 }
dudmuck 0:0bd73064b7f6 505 // NmeaLatitude
dudmuck 0:0bd73064b7f6 506 fieldSize = 0;
dudmuck 0:0bd73064b7f6 507 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 508 {
dudmuck 0:0bd73064b7f6 509 if( fieldSize > 10 )
dudmuck 0:0bd73064b7f6 510 {
dudmuck 0:0bd73064b7f6 511 return FAIL;
dudmuck 0:0bd73064b7f6 512 }
dudmuck 0:0bd73064b7f6 513 }
dudmuck 0:0bd73064b7f6 514 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 515 {
dudmuck 0:0bd73064b7f6 516 NmeaGpsData.NmeaLatitude[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 517 }
dudmuck 0:0bd73064b7f6 518 // NmeaLatitudePole
dudmuck 0:0bd73064b7f6 519 fieldSize = 0;
dudmuck 0:0bd73064b7f6 520 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 521 {
dudmuck 0:0bd73064b7f6 522 if( fieldSize > 2 )
dudmuck 0:0bd73064b7f6 523 {
dudmuck 0:0bd73064b7f6 524 return FAIL;
dudmuck 0:0bd73064b7f6 525 }
dudmuck 0:0bd73064b7f6 526 }
dudmuck 0:0bd73064b7f6 527 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 528 {
dudmuck 0:0bd73064b7f6 529 NmeaGpsData.NmeaLatitudePole[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 530 }
dudmuck 0:0bd73064b7f6 531 // NmeaLongitude
dudmuck 0:0bd73064b7f6 532 fieldSize = 0;
dudmuck 0:0bd73064b7f6 533 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 534 {
dudmuck 0:0bd73064b7f6 535 if( fieldSize > 11 )
dudmuck 0:0bd73064b7f6 536 {
dudmuck 0:0bd73064b7f6 537 return FAIL;
dudmuck 0:0bd73064b7f6 538 }
dudmuck 0:0bd73064b7f6 539 }
dudmuck 0:0bd73064b7f6 540 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 541 {
dudmuck 0:0bd73064b7f6 542 NmeaGpsData.NmeaLongitude[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 543 }
dudmuck 0:0bd73064b7f6 544 // NmeaLongitudePole
dudmuck 0:0bd73064b7f6 545 fieldSize = 0;
dudmuck 0:0bd73064b7f6 546 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 547 {
dudmuck 0:0bd73064b7f6 548 if( fieldSize > 2 )
dudmuck 0:0bd73064b7f6 549 {
dudmuck 0:0bd73064b7f6 550 return FAIL;
dudmuck 0:0bd73064b7f6 551 }
dudmuck 0:0bd73064b7f6 552 }
dudmuck 0:0bd73064b7f6 553 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 554 {
dudmuck 0:0bd73064b7f6 555 NmeaGpsData.NmeaLongitudePole[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 556 }
dudmuck 0:0bd73064b7f6 557 // NmeaSpeed
dudmuck 0:0bd73064b7f6 558 fieldSize = 0;
dudmuck 0:0bd73064b7f6 559 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 560 {
dudmuck 0:0bd73064b7f6 561 if( fieldSize > 8 )
dudmuck 0:0bd73064b7f6 562 {
dudmuck 0:0bd73064b7f6 563 return FAIL;
dudmuck 0:0bd73064b7f6 564 }
dudmuck 0:0bd73064b7f6 565 }
dudmuck 0:0bd73064b7f6 566 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 567 {
dudmuck 0:0bd73064b7f6 568 NmeaGpsData.NmeaSpeed[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 569 }
dudmuck 0:0bd73064b7f6 570 // NmeaDetectionAngle
dudmuck 0:0bd73064b7f6 571 fieldSize = 0;
dudmuck 0:0bd73064b7f6 572 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 573 {
dudmuck 0:0bd73064b7f6 574 if( fieldSize > 8 )
dudmuck 0:0bd73064b7f6 575 {
dudmuck 0:0bd73064b7f6 576 return FAIL;
dudmuck 0:0bd73064b7f6 577 }
dudmuck 0:0bd73064b7f6 578 }
dudmuck 0:0bd73064b7f6 579 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 580 {
dudmuck 0:0bd73064b7f6 581 NmeaGpsData.NmeaDetectionAngle[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 582 }
dudmuck 0:0bd73064b7f6 583 // NmeaDate
dudmuck 0:0bd73064b7f6 584 fieldSize = 0;
dudmuck 0:0bd73064b7f6 585 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 586 {
dudmuck 0:0bd73064b7f6 587 if( fieldSize > 8 )
dudmuck 0:0bd73064b7f6 588 {
dudmuck 0:0bd73064b7f6 589 return FAIL;
dudmuck 0:0bd73064b7f6 590 }
dudmuck 0:0bd73064b7f6 591 }
dudmuck 0:0bd73064b7f6 592 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 593 {
dudmuck 0:0bd73064b7f6 594 NmeaGpsData.NmeaDate[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 595 }
dudmuck 0:0bd73064b7f6 596
dudmuck 0:0bd73064b7f6 597 //FormatGpsData( );
dudmuck 0:0bd73064b7f6 598 ConvertPositionFromStringToNumerical( );
dudmuck 0:0bd73064b7f6 599 ConvertPositionIntoBinary( );
dudmuck 0:0bd73064b7f6 600 return SUCCESS;
dudmuck 0:0bd73064b7f6 601 }
dudmuck 0:0bd73064b7f6 602 else
dudmuck 0:0bd73064b7f6 603 {
dudmuck 0:0bd73064b7f6 604 return FAIL;
dudmuck 0:0bd73064b7f6 605 }
dudmuck 0:0bd73064b7f6 606 }
dudmuck 0:0bd73064b7f6 607
dudmuck 0:0bd73064b7f6 608 void GPS::service()
dudmuck 0:0bd73064b7f6 609 {
dudmuck 0:0bd73064b7f6 610 while (rx_bufs_in_idx != rx_bufs_out_idx) {
dudmuck 0:0bd73064b7f6 611 //printf("%02d:%s\r\n", rx_buf_lens[rx_bufs_out_idx], rx_bufs[rx_bufs_out_idx]);
dudmuck 0:0bd73064b7f6 612 if (verbose)
dudmuck 0:0bd73064b7f6 613 printf("gps:%s\r\n", rx_bufs[rx_bufs_out_idx]);
dudmuck 0:0bd73064b7f6 614 ParseGPSData(rx_bufs_out_idx);
dudmuck 0:0bd73064b7f6 615 if (++rx_bufs_out_idx == NUM_RX_BUFS)
dudmuck 0:0bd73064b7f6 616 rx_bufs_out_idx = 0;
dudmuck 0:0bd73064b7f6 617 }
dudmuck 0:0bd73064b7f6 618
dudmuck 0:0bd73064b7f6 619 if (pps_occurred) {
dudmuck 0:0bd73064b7f6 620 pps_occurred = false;
dudmuck 0:0bd73064b7f6 621 if (verbose)
dudmuck 0:0bd73064b7f6 622 printf("gps:pps\r\n");
dudmuck 0:0bd73064b7f6 623 }
dudmuck 0:0bd73064b7f6 624 }
dudmuck 0:0bd73064b7f6 625
dudmuck 0:0bd73064b7f6 626