Joe Knapp / lib_gps_fine

Changes Lat & Long Binary variable to be 32 bit integers representing micro-degrees.

Fork of lib_gps by wayne roberts

gps.cpp@6:1a45aa41ec15, 2016-08-15 (annotated)

Committer:
jknapp_smtc
Date:
Mon Aug 15 23:21:44 2016 +0000
Revision:
6:1a45aa41ec15
Parent:
5:f06be7a1f427 
Corrected un-fixed values

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
jknapp_smtc 6:1a45aa41ec15 4 #define LAT_UNFIXED 0x07270e00
jknapp_smtc 6:1a45aa41ec15 5 #define LONG_UNFIXED 0x47868c00
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 2:b531881123bf 12 //InterruptIn pps_pin(PC_5);
dudmuck 0:0bd73064b7f6 13
dudmuck 2:b531881123bf 14
dudmuck 2:b531881123bf 15 GPS::GPS(PinName uart_tx, PinName uart_rx, PinName en) : m_uart(uart_tx, uart_rx), m_en_pin(en)
dudmuck 0:0bd73064b7f6 16 {
dudmuck 1:2f5fbb33ae8b 17 have_fix = false;
dudmuck 1:2f5fbb33ae8b 18 }
dudmuck 0:0bd73064b7f6 19
dudmuck 0:0bd73064b7f6 20 GPS::~GPS()
dudmuck 0:0bd73064b7f6 21 {
dudmuck 0:0bd73064b7f6 22 }
dudmuck 0:0bd73064b7f6 23
dudmuck 2:b531881123bf 24 /*void GPS::pps()
dudmuck 0:0bd73064b7f6 25 {
dudmuck 0:0bd73064b7f6 26 pps_occurred = true;
dudmuck 2:b531881123bf 27 }*/
dudmuck 0:0bd73064b7f6 28
dudmuck 0:0bd73064b7f6 29 void GPS::on_uart_rx()
dudmuck 0:0bd73064b7f6 30 {
dudmuck 0:0bd73064b7f6 31 //uint8_t *len = &rx_buf_lens[rx_bufs_in_idx];
dudmuck 0:0bd73064b7f6 32 uint8_t len = rx_buf_lens[rx_bufs_in_idx];
dudmuck 0:0bd73064b7f6 33 char ch;
dudmuck 0:0bd73064b7f6 34
dudmuck 0:0bd73064b7f6 35 do {
dudmuck 2:b531881123bf 36 ch = m_uart.getc();
dudmuck 0:0bd73064b7f6 37
dudmuck 0:0bd73064b7f6 38 if (ch != '$') {
dudmuck 0:0bd73064b7f6 39 if (len < RX_BUF_SIZE) {
dudmuck 0:0bd73064b7f6 40 rx_bufs[rx_bufs_in_idx][len] = ch;
dudmuck 0:0bd73064b7f6 41 rx_buf_lens[rx_bufs_in_idx]++;
dudmuck 4:b8c049fa7db2 42 } /*else
dudmuck 4:b8c049fa7db2 43 printf("S");*/
dudmuck 4:b8c049fa7db2 44 } /*else if (len != 0) {
dudmuck 0:0bd73064b7f6 45 printf("$ at %d\r\n", len);
dudmuck 4:b8c049fa7db2 46 }*/
dudmuck 0:0bd73064b7f6 47
dudmuck 0:0bd73064b7f6 48 if (ch == '\n') {
dudmuck 0:0bd73064b7f6 49 rx_bufs[rx_bufs_in_idx][len] = 0; // null terminate
dudmuck 0:0bd73064b7f6 50 if (++rx_bufs_in_idx == NUM_RX_BUFS)
dudmuck 0:0bd73064b7f6 51 rx_bufs_in_idx = 0;
dudmuck 0:0bd73064b7f6 52 /* just let it overflow -- if (rx_bufs_in_idx == rx_bufs_out_idx)
dudmuck 0:0bd73064b7f6 53 printf("gps overflow\r\n");*/
dudmuck 0:0bd73064b7f6 54 rx_buf_lens[rx_bufs_in_idx] = 0;
dudmuck 0:0bd73064b7f6 55 }
dudmuck 2:b531881123bf 56 } while (m_uart.readable());
dudmuck 0:0bd73064b7f6 57 }
dudmuck 0:0bd73064b7f6 58
dudmuck 0:0bd73064b7f6 59 void GPS::init()
dudmuck 0:0bd73064b7f6 60 {
dudmuck 0:0bd73064b7f6 61 rx_bufs_in_idx = 0;
dudmuck 0:0bd73064b7f6 62 rx_buf_lens[rx_bufs_in_idx] = 0;
dudmuck 0:0bd73064b7f6 63 rx_bufs_out_idx = 0;
dudmuck 0:0bd73064b7f6 64
dudmuck 0:0bd73064b7f6 65 //gps_uart.baud(57600);
dudmuck 2:b531881123bf 66 m_uart.attach(this, &GPS::on_uart_rx);
dudmuck 0:0bd73064b7f6 67
dudmuck 2:b531881123bf 68 //pps_pin.rise(this, &GPS::pps);
dudmuck 0:0bd73064b7f6 69 }
dudmuck 0:0bd73064b7f6 70
dudmuck 0:0bd73064b7f6 71 void GPS::enable(bool en)
dudmuck 0:0bd73064b7f6 72 {
dudmuck 0:0bd73064b7f6 73 if (en) {
dudmuck 3:03d7275dc4fd 74 if (en_invert)
dudmuck 3:03d7275dc4fd 75 m_en_pin = 0;
dudmuck 3:03d7275dc4fd 76 else
dudmuck 3:03d7275dc4fd 77 m_en_pin = 1;
dudmuck 0:0bd73064b7f6 78 } else {
dudmuck 3:03d7275dc4fd 79 if (en_invert)
dudmuck 3:03d7275dc4fd 80 m_en_pin = 1;
dudmuck 3:03d7275dc4fd 81 else
dudmuck 3:03d7275dc4fd 82 m_en_pin = 0;
dudmuck 0:0bd73064b7f6 83 }
dudmuck 1:2f5fbb33ae8b 84
dudmuck 1:2f5fbb33ae8b 85 // reset current string index
dudmuck 1:2f5fbb33ae8b 86 rx_buf_lens[rx_bufs_in_idx] = 0;
dudmuck 0:0bd73064b7f6 87 }
dudmuck 0:0bd73064b7f6 88
dudmuck 0:0bd73064b7f6 89 bool GPS::enabled()
dudmuck 0:0bd73064b7f6 90 {
dudmuck 3:03d7275dc4fd 91 if (en_invert)
dudmuck 3:03d7275dc4fd 92 return !m_en_pin.read();
dudmuck 3:03d7275dc4fd 93 else
dudmuck 3:03d7275dc4fd 94 return m_en_pin.read();
dudmuck 0:0bd73064b7f6 95 }
dudmuck 0:0bd73064b7f6 96
dudmuck 0:0bd73064b7f6 97 /*char GPS::Nibble2HexChar( char a );
dudmuck 0:0bd73064b7f6 98 {
dudmuck 0:0bd73064b7f6 99 if( a < 10 )
dudmuck 0:0bd73064b7f6 100 {
dudmuck 0:0bd73064b7f6 101 return '0' + a;
dudmuck 0:0bd73064b7f6 102 }
dudmuck 0:0bd73064b7f6 103 else if( a < 16 )
dudmuck 0:0bd73064b7f6 104 {
dudmuck 0:0bd73064b7f6 105 return 'A' + ( a - 10 );
dudmuck 0:0bd73064b7f6 106 }
dudmuck 0:0bd73064b7f6 107 else
dudmuck 0:0bd73064b7f6 108 {
dudmuck 0:0bd73064b7f6 109 return '?';
dudmuck 0:0bd73064b7f6 110 }
dudmuck 0:0bd73064b7f6 111 }*/
dudmuck 0:0bd73064b7f6 112
dudmuck 0:0bd73064b7f6 113 int GPS::NmeaChecksum( char *nmeaStr, uint8_t nmeaStrSize, char * checksum )
dudmuck 0:0bd73064b7f6 114 {
dudmuck 0:0bd73064b7f6 115 int i = 0;
dudmuck 0:0bd73064b7f6 116 uint8_t checkNum = 0;
dudmuck 0:0bd73064b7f6 117
dudmuck 0:0bd73064b7f6 118 // Check input parameters
dudmuck 0:0bd73064b7f6 119 if( ( nmeaStr == NULL ) || ( checksum == NULL ) || ( nmeaStrSize <= 1 ) )
dudmuck 0:0bd73064b7f6 120 {
dudmuck 0:0bd73064b7f6 121 return -1;
dudmuck 0:0bd73064b7f6 122 }
dudmuck 0:0bd73064b7f6 123
dudmuck 0:0bd73064b7f6 124 // Skip the first '$' if necessary
dudmuck 0:0bd73064b7f6 125 if( nmeaStr[i] == '$' )
dudmuck 0:0bd73064b7f6 126 {
dudmuck 0:0bd73064b7f6 127 i += 1;
dudmuck 0:0bd73064b7f6 128 }
dudmuck 0:0bd73064b7f6 129
dudmuck 0:0bd73064b7f6 130 // XOR until '*' or max length is reached
dudmuck 0:0bd73064b7f6 131 while( nmeaStr[i] != '*' )
dudmuck 0:0bd73064b7f6 132 {
dudmuck 0:0bd73064b7f6 133 checkNum ^= nmeaStr[i];
dudmuck 0:0bd73064b7f6 134 i += 1;
dudmuck 0:0bd73064b7f6 135 if( i >= nmeaStrSize )
dudmuck 0:0bd73064b7f6 136 {
dudmuck 0:0bd73064b7f6 137 return -1;
dudmuck 0:0bd73064b7f6 138 }
dudmuck 0:0bd73064b7f6 139 }
dudmuck 0:0bd73064b7f6 140
dudmuck 0:0bd73064b7f6 141 // Convert checksum value to 2 hexadecimal characters
dudmuck 0:0bd73064b7f6 142 /*checksum[0] = Nibble2HexChar( checkNum / 16 ); // upper nibble
dudmuck 0:0bd73064b7f6 143 checksum[1] = Nibble2HexChar( checkNum % 16 ); // lower nibble
dudmuck 0:0bd73064b7f6 144 */
dudmuck 0:0bd73064b7f6 145 sprintf(checksum, "%02X", checkNum);
dudmuck 0:0bd73064b7f6 146
dudmuck 0:0bd73064b7f6 147 return i + 1;
dudmuck 0:0bd73064b7f6 148 }
dudmuck 0:0bd73064b7f6 149
dudmuck 0:0bd73064b7f6 150 bool GPS::NmeaValidateChecksum(int idx)
dudmuck 0:0bd73064b7f6 151 {
dudmuck 0:0bd73064b7f6 152 int checksumIndex;
dudmuck 0:0bd73064b7f6 153 char checksum[4];
dudmuck 0:0bd73064b7f6 154 char *NmeaString = rx_bufs[idx];
dudmuck 0:0bd73064b7f6 155
dudmuck 0:0bd73064b7f6 156 checksumIndex = NmeaChecksum( NmeaString, rx_buf_lens[idx], checksum );
dudmuck 0:0bd73064b7f6 157
dudmuck 0:0bd73064b7f6 158 // could we calculate a verification checksum ?
dudmuck 0:0bd73064b7f6 159 if( checksumIndex < 0 )
dudmuck 0:0bd73064b7f6 160 {
dudmuck 4:b8c049fa7db2 161 //printf("gps:checksumIndex:%d\r\n", checksumIndex);
dudmuck 0:0bd73064b7f6 162 return false;
dudmuck 0:0bd73064b7f6 163 }
dudmuck 0:0bd73064b7f6 164
dudmuck 0:0bd73064b7f6 165 // check if there are enough char in the serial buffer to read checksum
dudmuck 0:0bd73064b7f6 166 if( checksumIndex >= ( rx_buf_lens[idx] - 2 ) )
dudmuck 0:0bd73064b7f6 167 {
dudmuck 4:b8c049fa7db2 168 //printf("gps:checksumIndex:%d\r\n", checksumIndex);
dudmuck 0:0bd73064b7f6 169 return false;
dudmuck 0:0bd73064b7f6 170 }
dudmuck 0:0bd73064b7f6 171
dudmuck 0:0bd73064b7f6 172 // check the checksum
dudmuck 0:0bd73064b7f6 173 if( ( NmeaString[checksumIndex] == checksum[0] ) && ( NmeaString[checksumIndex + 1] == checksum[1] ) )
dudmuck 0:0bd73064b7f6 174 {
dudmuck 0:0bd73064b7f6 175 return true;
dudmuck 0:0bd73064b7f6 176 }
dudmuck 0:0bd73064b7f6 177 else
dudmuck 0:0bd73064b7f6 178 {
dudmuck 0:0bd73064b7f6 179 if (verbose)
dudmuck 0:0bd73064b7f6 180 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 181 return false;
dudmuck 0:0bd73064b7f6 182 }
dudmuck 0:0bd73064b7f6 183 }
dudmuck 0:0bd73064b7f6 184
dudmuck 0:0bd73064b7f6 185 void GPS::ConvertPositionFromStringToNumerical( )
dudmuck 0:0bd73064b7f6 186 {
dudmuck 0:0bd73064b7f6 187 int i;
dudmuck 0:0bd73064b7f6 188
dudmuck 0:0bd73064b7f6 189 double valueTmp1;
dudmuck 0:0bd73064b7f6 190 double valueTmp2;
dudmuck 0:0bd73064b7f6 191 double valueTmp3;
dudmuck 0:0bd73064b7f6 192 double valueTmp4;
dudmuck 0:0bd73064b7f6 193
dudmuck 0:0bd73064b7f6 194 // Convert the latitude from ASCII to uint8_t values
dudmuck 0:0bd73064b7f6 195 for( i = 0 ; i < 10 ; i++ )
dudmuck 0:0bd73064b7f6 196 {
dudmuck 0:0bd73064b7f6 197 NmeaGpsData.NmeaLatitude[i] = NmeaGpsData.NmeaLatitude[i] & 0xF;
dudmuck 0:0bd73064b7f6 198 }
dudmuck 0:0bd73064b7f6 199 // Convert latitude from degree/minute/second (DMS) format into decimal
dudmuck 0:0bd73064b7f6 200 valueTmp1 = ( double )NmeaGpsData.NmeaLatitude[0] * 10.0 + ( double )NmeaGpsData.NmeaLatitude[1];
dudmuck 0:0bd73064b7f6 201 valueTmp2 = ( double )NmeaGpsData.NmeaLatitude[2] * 10.0 + ( double )NmeaGpsData.NmeaLatitude[3];
dudmuck 0:0bd73064b7f6 202 valueTmp3 = ( double )NmeaGpsData.NmeaLatitude[5] * 1000.0 + ( double )NmeaGpsData.NmeaLatitude[6] * 100.0 +
dudmuck 0:0bd73064b7f6 203 ( double )NmeaGpsData.NmeaLatitude[7] * 10.0 + ( double )NmeaGpsData.NmeaLatitude[8];
dudmuck 0:0bd73064b7f6 204
dudmuck 0:0bd73064b7f6 205 Latitude = valueTmp1 + ( ( valueTmp2 + ( valueTmp3 * 0.0001 ) ) / 60.0 );
dudmuck 0:0bd73064b7f6 206
dudmuck 0:0bd73064b7f6 207 if( NmeaGpsData.NmeaLatitudePole[0] == 'S' )
dudmuck 0:0bd73064b7f6 208 {
dudmuck 0:0bd73064b7f6 209 Latitude *= -1;
dudmuck 0:0bd73064b7f6 210 }
dudmuck 0:0bd73064b7f6 211
dudmuck 0:0bd73064b7f6 212 // Convert the longitude from ASCII to uint8_t values
dudmuck 0:0bd73064b7f6 213 for( i = 0 ; i < 10 ; i++ )
dudmuck 0:0bd73064b7f6 214 {
dudmuck 0:0bd73064b7f6 215 NmeaGpsData.NmeaLongitude[i] = NmeaGpsData.NmeaLongitude[i] & 0xF;
dudmuck 0:0bd73064b7f6 216 }
dudmuck 0:0bd73064b7f6 217 // Convert longitude from degree/minute/second (DMS) format into decimal
dudmuck 0:0bd73064b7f6 218 valueTmp1 = ( double )NmeaGpsData.NmeaLongitude[0] * 100.0 + ( double )NmeaGpsData.NmeaLongitude[1] * 10.0 + ( double )NmeaGpsData.NmeaLongitude[2];
dudmuck 0:0bd73064b7f6 219 valueTmp2 = ( double )NmeaGpsData.NmeaLongitude[3] * 10.0 + ( double )NmeaGpsData.NmeaLongitude[4];
dudmuck 0:0bd73064b7f6 220 valueTmp3 = ( double )NmeaGpsData.NmeaLongitude[6] * 1000.0 + ( double )NmeaGpsData.NmeaLongitude[7] * 100;
dudmuck 0:0bd73064b7f6 221 valueTmp4 = ( double )NmeaGpsData.NmeaLongitude[8] * 10.0 + ( double )NmeaGpsData.NmeaLongitude[9];
dudmuck 0:0bd73064b7f6 222
dudmuck 0:0bd73064b7f6 223 Longitude = valueTmp1 + ( valueTmp2 / 60.0 ) + ( ( ( valueTmp3 + valueTmp4 ) * 0.0001 ) / 60.0 );
dudmuck 0:0bd73064b7f6 224
dudmuck 0:0bd73064b7f6 225 if( NmeaGpsData.NmeaLongitudePole[0] == 'W' )
dudmuck 0:0bd73064b7f6 226 {
dudmuck 0:0bd73064b7f6 227 Longitude *= -1;
dudmuck 0:0bd73064b7f6 228 }
dudmuck 0:0bd73064b7f6 229
dudmuck 0:0bd73064b7f6 230 //printf("DD %f, %f\r\n", Latitude, Longitude);
dudmuck 0:0bd73064b7f6 231 }
dudmuck 0:0bd73064b7f6 232
dudmuck 0:0bd73064b7f6 233 void GPS::ConvertPositionIntoBinary( )
dudmuck 0:0bd73064b7f6 234 {
jknapp_smtc 5:f06be7a1f427 235 LatitudeBinary32 = (int)(Latitude * 1e6);
dudmuck 0:0bd73064b7f6 236
jknapp_smtc 5:f06be7a1f427 237 LongitudeBinary32 = (int)(Longitude * 1e6);
dudmuck 1:2f5fbb33ae8b 238
jknapp_smtc 5:f06be7a1f427 239 //printf("binary: %d %d\r\n", LatitudeBinary32, LongitudeBinary32);
jknapp_smtc 6:1a45aa41ec15 240 if (LatitudeBinary32 == LAT_UNFIXED && LongitudeBinary32 == LONG_UNFIXED)
dudmuck 1:2f5fbb33ae8b 241 have_fix = false;
dudmuck 1:2f5fbb33ae8b 242 else
dudmuck 1:2f5fbb33ae8b 243 have_fix = true;
dudmuck 0:0bd73064b7f6 244 }
dudmuck 0:0bd73064b7f6 245
dudmuck 0:0bd73064b7f6 246 int GPS::ParseGPSData(int idx)
dudmuck 0:0bd73064b7f6 247 {
dudmuck 0:0bd73064b7f6 248 uint8_t i, j, fieldSize;
dudmuck 0:0bd73064b7f6 249 char *NmeaString;
dudmuck 0:0bd73064b7f6 250
dudmuck 0:0bd73064b7f6 251 if (NmeaValidateChecksum(idx) == false) {
dudmuck 0:0bd73064b7f6 252 if (verbose)
dudmuck 0:0bd73064b7f6 253 printf("gps:bad nmea checksum:%s\r\n", rx_bufs[rx_bufs_out_idx]);
dudmuck 0:0bd73064b7f6 254 return FAIL;
dudmuck 0:0bd73064b7f6 255 }
dudmuck 0:0bd73064b7f6 256
dudmuck 0:0bd73064b7f6 257 NmeaString = rx_bufs[idx];
dudmuck 1:2f5fbb33ae8b 258
dudmuck 1:2f5fbb33ae8b 259 if (NmeaString[0] == 'P' && NmeaString[1] == 'M' && NmeaString[2] == 'T' && NmeaString[3] == 'K') {
dudmuck 1:2f5fbb33ae8b 260 /* anything useful from PMTK packets? */
dudmuck 1:2f5fbb33ae8b 261 return SUCCESS;
dudmuck 1:2f5fbb33ae8b 262 }
dudmuck 1:2f5fbb33ae8b 263
dudmuck 0:0bd73064b7f6 264 fieldSize = 0;
dudmuck 0:0bd73064b7f6 265 i = 0;
dudmuck 0:0bd73064b7f6 266 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 267 {
dudmuck 0:0bd73064b7f6 268 if( fieldSize > 6 )
dudmuck 0:0bd73064b7f6 269 {
dudmuck 1:2f5fbb33ae8b 270 printf("gps parse:%s\r\n", NmeaString);
dudmuck 0:0bd73064b7f6 271 return FAIL;
dudmuck 0:0bd73064b7f6 272 }
dudmuck 0:0bd73064b7f6 273 }
dudmuck 0:0bd73064b7f6 274
dudmuck 0:0bd73064b7f6 275 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 276 {
dudmuck 0:0bd73064b7f6 277 NmeaGpsData.NmeaDataType[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 278 }
dudmuck 0:0bd73064b7f6 279 // Parse the GPGGA data
dudmuck 0:0bd73064b7f6 280 if( strncmp( ( const char* )NmeaGpsData.NmeaDataType, ( const char* )NmeaDataTypeGPGGA, 5 ) == 0 )
dudmuck 0:0bd73064b7f6 281 {
dudmuck 0:0bd73064b7f6 282 // NmeaUtcTime
dudmuck 0:0bd73064b7f6 283 fieldSize = 0;
dudmuck 0:0bd73064b7f6 284 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 285 {
dudmuck 0:0bd73064b7f6 286 if( fieldSize > 11 )
dudmuck 0:0bd73064b7f6 287 {
dudmuck 0:0bd73064b7f6 288 return FAIL;
dudmuck 0:0bd73064b7f6 289 }
dudmuck 0:0bd73064b7f6 290 }
dudmuck 0:0bd73064b7f6 291 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 292 {
dudmuck 0:0bd73064b7f6 293 NmeaGpsData.NmeaUtcTime[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 294 }
dudmuck 0:0bd73064b7f6 295 // NmeaLatitude
dudmuck 0:0bd73064b7f6 296 fieldSize = 0;
dudmuck 0:0bd73064b7f6 297 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 298 {
dudmuck 0:0bd73064b7f6 299 if( fieldSize > 10 )
dudmuck 0:0bd73064b7f6 300 {
dudmuck 0:0bd73064b7f6 301 return FAIL;
dudmuck 0:0bd73064b7f6 302 }
dudmuck 0:0bd73064b7f6 303 }
dudmuck 0:0bd73064b7f6 304 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 305 {
dudmuck 0:0bd73064b7f6 306 NmeaGpsData.NmeaLatitude[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 307 }
dudmuck 0:0bd73064b7f6 308 // NmeaLatitudePole
dudmuck 0:0bd73064b7f6 309 fieldSize = 0;
dudmuck 0:0bd73064b7f6 310 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 311 {
dudmuck 0:0bd73064b7f6 312 if( fieldSize > 2 )
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.NmeaLatitudePole[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 320 }
dudmuck 0:0bd73064b7f6 321 // NmeaLongitude
dudmuck 0:0bd73064b7f6 322 fieldSize = 0;
dudmuck 0:0bd73064b7f6 323 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 324 {
dudmuck 0:0bd73064b7f6 325 if( fieldSize > 11 )
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.NmeaLongitude[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 333 }
dudmuck 0:0bd73064b7f6 334 // NmeaLongitudePole
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.NmeaLongitudePole[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 346 }
dudmuck 0:0bd73064b7f6 347 // NmeaFixQuality
dudmuck 0:0bd73064b7f6 348 fieldSize = 0;
dudmuck 0:0bd73064b7f6 349 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 350 {
dudmuck 0:0bd73064b7f6 351 if( fieldSize > 2 )
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.NmeaFixQuality[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 359 }
dudmuck 0:0bd73064b7f6 360 // NmeaSatelliteTracked
dudmuck 0:0bd73064b7f6 361 fieldSize = 0;
dudmuck 0:0bd73064b7f6 362 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 363 {
dudmuck 0:0bd73064b7f6 364 if( fieldSize > 3 )
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.NmeaSatelliteTracked[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 372 }
dudmuck 0:0bd73064b7f6 373 // NmeaHorizontalDilution
dudmuck 0:0bd73064b7f6 374 fieldSize = 0;
dudmuck 0:0bd73064b7f6 375 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 376 {
dudmuck 0:0bd73064b7f6 377 if( fieldSize > 6 )
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.NmeaHorizontalDilution[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 385 }
dudmuck 0:0bd73064b7f6 386 // NmeaAltitude
dudmuck 0:0bd73064b7f6 387 fieldSize = 0;
dudmuck 0:0bd73064b7f6 388 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 389 {
dudmuck 0:0bd73064b7f6 390 if( fieldSize > 8 )
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.NmeaAltitude[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 398 }
dudmuck 0:0bd73064b7f6 399 // NmeaAltitudeUnit
dudmuck 0:0bd73064b7f6 400 fieldSize = 0;
dudmuck 0:0bd73064b7f6 401 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 402 {
dudmuck 0:0bd73064b7f6 403 if( fieldSize > 2 )
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.NmeaAltitudeUnit[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 411 }
dudmuck 0:0bd73064b7f6 412 // NmeaHeightGeoid
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.NmeaHeightGeoid[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 424 }
dudmuck 0:0bd73064b7f6 425 // NmeaHeightGeoidUnit
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.NmeaHeightGeoidUnit[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 437 }
dudmuck 0:0bd73064b7f6 438
dudmuck 0:0bd73064b7f6 439 //FormatGpsData( );
dudmuck 0:0bd73064b7f6 440 ConvertPositionFromStringToNumerical( );
dudmuck 0:0bd73064b7f6 441 ConvertPositionIntoBinary( );
dudmuck 0:0bd73064b7f6 442 return SUCCESS;
dudmuck 0:0bd73064b7f6 443 }
dudmuck 0:0bd73064b7f6 444 else if ( strncmp( ( const char* )NmeaGpsData.NmeaDataType, ( const char* )NmeaDataTypeGPRMC, 5 ) == 0 )
dudmuck 0:0bd73064b7f6 445 {
dudmuck 0:0bd73064b7f6 446 // NmeaUtcTime
dudmuck 0:0bd73064b7f6 447 fieldSize = 0;
dudmuck 0:0bd73064b7f6 448 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 449 {
dudmuck 0:0bd73064b7f6 450 if( fieldSize > 11 )
dudmuck 0:0bd73064b7f6 451 {
dudmuck 0:0bd73064b7f6 452 return FAIL;
dudmuck 0:0bd73064b7f6 453 }
dudmuck 0:0bd73064b7f6 454 }
dudmuck 0:0bd73064b7f6 455 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 456 {
dudmuck 0:0bd73064b7f6 457 NmeaGpsData.NmeaUtcTime[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 458 }
dudmuck 0:0bd73064b7f6 459 // NmeaDataStatus
dudmuck 0:0bd73064b7f6 460 fieldSize = 0;
dudmuck 0:0bd73064b7f6 461 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 462 {
dudmuck 0:0bd73064b7f6 463 if( fieldSize > 2 )
dudmuck 0:0bd73064b7f6 464 {
dudmuck 0:0bd73064b7f6 465 return FAIL;
dudmuck 0:0bd73064b7f6 466 }
dudmuck 0:0bd73064b7f6 467 }
dudmuck 0:0bd73064b7f6 468 for( j = 0; j < fieldSize; j++, i++ )
dudmuck 0:0bd73064b7f6 469 {
dudmuck 0:0bd73064b7f6 470 NmeaGpsData.NmeaDataStatus[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 471 }
dudmuck 0:0bd73064b7f6 472 // NmeaLatitude
dudmuck 0:0bd73064b7f6 473 fieldSize = 0;
dudmuck 0:0bd73064b7f6 474 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 475 {
dudmuck 0:0bd73064b7f6 476 if( fieldSize > 10 )
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.NmeaLatitude[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 484 }
dudmuck 0:0bd73064b7f6 485 // NmeaLatitudePole
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.NmeaLatitudePole[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 497 }
dudmuck 0:0bd73064b7f6 498 // NmeaLongitude
dudmuck 0:0bd73064b7f6 499 fieldSize = 0;
dudmuck 0:0bd73064b7f6 500 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 501 {
dudmuck 0:0bd73064b7f6 502 if( fieldSize > 11 )
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.NmeaLongitude[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 510 }
dudmuck 0:0bd73064b7f6 511 // NmeaLongitudePole
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.NmeaLongitudePole[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 523 }
dudmuck 0:0bd73064b7f6 524 // NmeaSpeed
dudmuck 0:0bd73064b7f6 525 fieldSize = 0;
dudmuck 0:0bd73064b7f6 526 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 527 {
dudmuck 0:0bd73064b7f6 528 if( fieldSize > 8 )
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.NmeaSpeed[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 536 }
dudmuck 0:0bd73064b7f6 537 // NmeaDetectionAngle
dudmuck 0:0bd73064b7f6 538 fieldSize = 0;
dudmuck 0:0bd73064b7f6 539 while( NmeaString[i + fieldSize++] != ',' )
dudmuck 0:0bd73064b7f6 540 {
dudmuck 0:0bd73064b7f6 541 if( fieldSize > 8 )
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.NmeaDetectionAngle[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 549 }
dudmuck 0:0bd73064b7f6 550 // NmeaDate
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.NmeaDate[j] = NmeaString[i];
dudmuck 0:0bd73064b7f6 562 }
dudmuck 0:0bd73064b7f6 563
dudmuck 0:0bd73064b7f6 564 //FormatGpsData( );
dudmuck 0:0bd73064b7f6 565 ConvertPositionFromStringToNumerical( );
dudmuck 0:0bd73064b7f6 566 ConvertPositionIntoBinary( );
dudmuck 0:0bd73064b7f6 567 return SUCCESS;
dudmuck 0:0bd73064b7f6 568 }
dudmuck 0:0bd73064b7f6 569 else
dudmuck 0:0bd73064b7f6 570 {
dudmuck 0:0bd73064b7f6 571 return FAIL;
dudmuck 0:0bd73064b7f6 572 }
dudmuck 0:0bd73064b7f6 573 }
dudmuck 0:0bd73064b7f6 574
dudmuck 0:0bd73064b7f6 575 void GPS::service()
dudmuck 0:0bd73064b7f6 576 {
dudmuck 0:0bd73064b7f6 577 while (rx_bufs_in_idx != rx_bufs_out_idx) {
dudmuck 0:0bd73064b7f6 578 //printf("%02d:%s\r\n", rx_buf_lens[rx_bufs_out_idx], rx_bufs[rx_bufs_out_idx]);
dudmuck 0:0bd73064b7f6 579 if (verbose)
dudmuck 0:0bd73064b7f6 580 printf("gps:%s\r\n", rx_bufs[rx_bufs_out_idx]);
dudmuck 0:0bd73064b7f6 581 ParseGPSData(rx_bufs_out_idx);
dudmuck 0:0bd73064b7f6 582 if (++rx_bufs_out_idx == NUM_RX_BUFS)
dudmuck 0:0bd73064b7f6 583 rx_bufs_out_idx = 0;
dudmuck 0:0bd73064b7f6 584 }
dudmuck 0:0bd73064b7f6 585
dudmuck 2:b531881123bf 586 /*if (pps_occurred) {
dudmuck 0:0bd73064b7f6 587 pps_occurred = false;
dudmuck 0:0bd73064b7f6 588 if (verbose)
dudmuck 0:0bd73064b7f6 589 printf("gps:pps\r\n");
dudmuck 2:b531881123bf 590 }*/
dudmuck 0:0bd73064b7f6 591 }
dudmuck 0:0bd73064b7f6 592
dudmuck 0:0bd73064b7f6 593