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@5:eb5ab720e3f9, 2016-12-07 (annotated)

Committer:
mluis
Date:
Wed Dec 07 14:57:08 2016 +0000
Revision:
5:eb5ab720e3f9
Parent:
4:b8c049fa7db2 
Updated due to latest mbed library versions changes

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