canuck lehead
/
lib_gps
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
lib_gps
by 
wayne roberts
gps.cpp@6:9dfe135334c9, 2016-03-07 (annotated)
- Committer:
- shaunkrnelson
- Date:
- Mon Mar 07 15:20:16 2016 -0500
- Revision:
- 6:9dfe135334c9
- Parent:
- 4:b8c049fa7db2
- Child:
- 7:44cb21f05dcc
Copying Bug fixes from SMTC_LoRaMote repository
Who changed what in which revision?
| User | Revision | Line number | New 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; |
| shaunkrnelson |
6:9dfe135334c9 | 24 | uart_rx_count = 0; |
| dudmuck | 1:2f5fbb33ae8b | 25 | } |
| dudmuck | 0:0bd73064b7f6 | 26 | |
| dudmuck | 0:0bd73064b7f6 | 27 | GPS::~GPS() |
| dudmuck | 0:0bd73064b7f6 | 28 | { |
| dudmuck | 0:0bd73064b7f6 | 29 | } |
| dudmuck | 0:0bd73064b7f6 | 30 | |
| dudmuck | 2:b531881123bf | 31 | /*void GPS::pps() |
| dudmuck | 0:0bd73064b7f6 | 32 | { |
| dudmuck | 0:0bd73064b7f6 | 33 | pps_occurred = true; |
| dudmuck | 2:b531881123bf | 34 | }*/ |
| dudmuck | 0:0bd73064b7f6 | 35 | |
| dudmuck | 0:0bd73064b7f6 | 36 | void GPS::on_uart_rx() |
| dudmuck | 0:0bd73064b7f6 | 37 | { |
| dudmuck | 0:0bd73064b7f6 | 38 | //uint8_t *len = &rx_buf_lens[rx_bufs_in_idx]; |
| dudmuck | 0:0bd73064b7f6 | 39 | uint8_t len = rx_buf_lens[rx_bufs_in_idx]; |
| dudmuck | 0:0bd73064b7f6 | 40 | char ch; |
| dudmuck | 0:0bd73064b7f6 | 41 | |
| dudmuck | 0:0bd73064b7f6 | 42 | do { |
| dudmuck | 2:b531881123bf | 43 | ch = m_uart.getc(); |
| dudmuck | 0:0bd73064b7f6 | 44 | |
| dudmuck | 0:0bd73064b7f6 | 45 | if (ch != '$') { |
| dudmuck | 0:0bd73064b7f6 | 46 | if (len < RX_BUF_SIZE) { |
| dudmuck | 0:0bd73064b7f6 | 47 | rx_bufs[rx_bufs_in_idx][len] = ch; |
| dudmuck | 0:0bd73064b7f6 | 48 | rx_buf_lens[rx_bufs_in_idx]++; |
| shaunkrnelson |
6:9dfe135334c9 | 49 | } else |
| shaunkrnelson |
6:9dfe135334c9 | 50 | printf("S"); |
| shaunkrnelson |
6:9dfe135334c9 | 51 | } else if (len != 0) { |
| shaunkrnelson |
6:9dfe135334c9 | 52 | if(verbose) |
| shaunkrnelson |
6:9dfe135334c9 | 53 | printf("$ at %d\r\n", len); |
| shaunkrnelson |
6:9dfe135334c9 | 54 | } |
| dudmuck | 0:0bd73064b7f6 | 55 | |
| dudmuck | 0:0bd73064b7f6 | 56 | if (ch == '\n') { |
| dudmuck | 0:0bd73064b7f6 | 57 | rx_bufs[rx_bufs_in_idx][len] = 0; // null terminate |
| shaunkrnelson |
6:9dfe135334c9 | 58 | |
| shaunkrnelson |
6:9dfe135334c9 | 59 | #if 0 |
| shaunkrnelson |
6:9dfe135334c9 | 60 | if(verbose) |
| shaunkrnelson |
6:9dfe135334c9 | 61 | { |
| shaunkrnelson |
6:9dfe135334c9 | 62 | printf("on_uart_rx: buf=%d, len=%d\r\n", rx_bufs_in_idx, len); |
| shaunkrnelson |
6:9dfe135334c9 | 63 | } |
| shaunkrnelson |
6:9dfe135334c9 | 64 | #endif |
| shaunkrnelson |
6:9dfe135334c9 | 65 | |
| dudmuck | 0:0bd73064b7f6 | 66 | if (++rx_bufs_in_idx == NUM_RX_BUFS) |
| dudmuck | 0:0bd73064b7f6 | 67 | rx_bufs_in_idx = 0; |
| dudmuck | 0:0bd73064b7f6 | 68 | /* just let it overflow -- if (rx_bufs_in_idx == rx_bufs_out_idx) |
| dudmuck | 0:0bd73064b7f6 | 69 | printf("gps overflow\r\n");*/ |
| dudmuck | 0:0bd73064b7f6 | 70 | rx_buf_lens[rx_bufs_in_idx] = 0; |
| shaunkrnelson |
6:9dfe135334c9 | 71 | uart_rx_count++; |
| dudmuck | 0:0bd73064b7f6 | 72 | } |
| dudmuck | 2:b531881123bf | 73 | } while (m_uart.readable()); |
| dudmuck | 0:0bd73064b7f6 | 74 | } |
| dudmuck | 0:0bd73064b7f6 | 75 | |
| dudmuck | 0:0bd73064b7f6 | 76 | void GPS::init() |
| dudmuck | 0:0bd73064b7f6 | 77 | { |
| dudmuck | 0:0bd73064b7f6 | 78 | rx_bufs_in_idx = 0; |
| dudmuck | 0:0bd73064b7f6 | 79 | rx_buf_lens[rx_bufs_in_idx] = 0; |
| dudmuck | 0:0bd73064b7f6 | 80 | rx_bufs_out_idx = 0; |
| dudmuck | 0:0bd73064b7f6 | 81 | |
| dudmuck | 0:0bd73064b7f6 | 82 | //gps_uart.baud(57600); |
| dudmuck | 2:b531881123bf | 83 | m_uart.attach(this, &GPS::on_uart_rx); |
| dudmuck | 0:0bd73064b7f6 | 84 | |
| dudmuck | 2:b531881123bf | 85 | //pps_pin.rise(this, &GPS::pps); |
| dudmuck | 0:0bd73064b7f6 | 86 | } |
| dudmuck | 0:0bd73064b7f6 | 87 | |
| dudmuck | 0:0bd73064b7f6 | 88 | void GPS::enable(bool en) |
| dudmuck | 0:0bd73064b7f6 | 89 | { |
| dudmuck | 0:0bd73064b7f6 | 90 | if (en) { |
| dudmuck | 3:03d7275dc4fd | 91 | if (en_invert) |
| dudmuck | 3:03d7275dc4fd | 92 | m_en_pin = 0; |
| dudmuck | 3:03d7275dc4fd | 93 | else |
| dudmuck | 3:03d7275dc4fd | 94 | m_en_pin = 1; |
| dudmuck | 0:0bd73064b7f6 | 95 | } else { |
| dudmuck | 3:03d7275dc4fd | 96 | if (en_invert) |
| dudmuck | 3:03d7275dc4fd | 97 | m_en_pin = 1; |
| dudmuck | 3:03d7275dc4fd | 98 | else |
| dudmuck | 3:03d7275dc4fd | 99 | m_en_pin = 0; |
| dudmuck | 0:0bd73064b7f6 | 100 | } |
| dudmuck | 1:2f5fbb33ae8b | 101 | |
| dudmuck | 1:2f5fbb33ae8b | 102 | // reset current string index |
| dudmuck | 1:2f5fbb33ae8b | 103 | rx_buf_lens[rx_bufs_in_idx] = 0; |
| dudmuck | 0:0bd73064b7f6 | 104 | } |
| dudmuck | 0:0bd73064b7f6 | 105 | |
| dudmuck | 0:0bd73064b7f6 | 106 | bool GPS::enabled() |
| dudmuck | 0:0bd73064b7f6 | 107 | { |
| dudmuck | 3:03d7275dc4fd | 108 | if (en_invert) |
| dudmuck | 3:03d7275dc4fd | 109 | return !m_en_pin.read(); |
| dudmuck | 3:03d7275dc4fd | 110 | else |
| dudmuck | 3:03d7275dc4fd | 111 | return m_en_pin.read(); |
| dudmuck | 0:0bd73064b7f6 | 112 | } |
| dudmuck | 0:0bd73064b7f6 | 113 | |
| dudmuck | 0:0bd73064b7f6 | 114 | /*char GPS::Nibble2HexChar( char a ); |
| dudmuck | 0:0bd73064b7f6 | 115 | { |
| dudmuck | 0:0bd73064b7f6 | 116 | if( a < 10 ) |
| dudmuck | 0:0bd73064b7f6 | 117 | { |
| dudmuck | 0:0bd73064b7f6 | 118 | return '0' + a; |
| dudmuck | 0:0bd73064b7f6 | 119 | } |
| dudmuck | 0:0bd73064b7f6 | 120 | else if( a < 16 ) |
| dudmuck | 0:0bd73064b7f6 | 121 | { |
| dudmuck | 0:0bd73064b7f6 | 122 | return 'A' + ( a - 10 ); |
| dudmuck | 0:0bd73064b7f6 | 123 | } |
| dudmuck | 0:0bd73064b7f6 | 124 | else |
| dudmuck | 0:0bd73064b7f6 | 125 | { |
| dudmuck | 0:0bd73064b7f6 | 126 | return '?'; |
| dudmuck | 0:0bd73064b7f6 | 127 | } |
| dudmuck | 0:0bd73064b7f6 | 128 | }*/ |
| dudmuck | 0:0bd73064b7f6 | 129 | |
| dudmuck | 0:0bd73064b7f6 | 130 | int GPS::NmeaChecksum( char *nmeaStr, uint8_t nmeaStrSize, char * checksum ) |
| dudmuck | 0:0bd73064b7f6 | 131 | { |
| dudmuck | 0:0bd73064b7f6 | 132 | int i = 0; |
| dudmuck | 0:0bd73064b7f6 | 133 | uint8_t checkNum = 0; |
| dudmuck | 0:0bd73064b7f6 | 134 | |
| dudmuck | 0:0bd73064b7f6 | 135 | // Check input parameters |
| dudmuck | 0:0bd73064b7f6 | 136 | if( ( nmeaStr == NULL ) || ( checksum == NULL ) || ( nmeaStrSize <= 1 ) ) |
| dudmuck | 0:0bd73064b7f6 | 137 | { |
| dudmuck | 0:0bd73064b7f6 | 138 | return -1; |
| dudmuck | 0:0bd73064b7f6 | 139 | } |
| dudmuck | 0:0bd73064b7f6 | 140 | |
| dudmuck | 0:0bd73064b7f6 | 141 | // Skip the first '$' if necessary |
| dudmuck | 0:0bd73064b7f6 | 142 | if( nmeaStr[i] == '$' ) |
| dudmuck | 0:0bd73064b7f6 | 143 | { |
| dudmuck | 0:0bd73064b7f6 | 144 | i += 1; |
| dudmuck | 0:0bd73064b7f6 | 145 | } |
| dudmuck | 0:0bd73064b7f6 | 146 | |
| dudmuck | 0:0bd73064b7f6 | 147 | // XOR until '*' or max length is reached |
| dudmuck | 0:0bd73064b7f6 | 148 | while( nmeaStr[i] != '*' ) |
| dudmuck | 0:0bd73064b7f6 | 149 | { |
| dudmuck | 0:0bd73064b7f6 | 150 | checkNum ^= nmeaStr[i]; |
| dudmuck | 0:0bd73064b7f6 | 151 | i += 1; |
| dudmuck | 0:0bd73064b7f6 | 152 | if( i >= nmeaStrSize ) |
| dudmuck | 0:0bd73064b7f6 | 153 | { |
| dudmuck | 0:0bd73064b7f6 | 154 | return -1; |
| dudmuck | 0:0bd73064b7f6 | 155 | } |
| dudmuck | 0:0bd73064b7f6 | 156 | } |
| dudmuck | 0:0bd73064b7f6 | 157 | |
| dudmuck | 0:0bd73064b7f6 | 158 | // Convert checksum value to 2 hexadecimal characters |
| dudmuck | 0:0bd73064b7f6 | 159 | /*checksum[0] = Nibble2HexChar( checkNum / 16 ); // upper nibble |
| dudmuck | 0:0bd73064b7f6 | 160 | checksum[1] = Nibble2HexChar( checkNum % 16 ); // lower nibble |
| dudmuck | 0:0bd73064b7f6 | 161 | */ |
| dudmuck | 0:0bd73064b7f6 | 162 | sprintf(checksum, "%02X", checkNum); |
| dudmuck | 0:0bd73064b7f6 | 163 | |
| dudmuck | 0:0bd73064b7f6 | 164 | return i + 1; |
| dudmuck | 0:0bd73064b7f6 | 165 | } |
| dudmuck | 0:0bd73064b7f6 | 166 | |
| dudmuck | 0:0bd73064b7f6 | 167 | bool GPS::NmeaValidateChecksum(int idx) |
| dudmuck | 0:0bd73064b7f6 | 168 | { |
| dudmuck | 0:0bd73064b7f6 | 169 | int checksumIndex; |
| dudmuck | 0:0bd73064b7f6 | 170 | char checksum[4]; |
| dudmuck | 0:0bd73064b7f6 | 171 | char *NmeaString = rx_bufs[idx]; |
| dudmuck | 0:0bd73064b7f6 | 172 | |
| dudmuck | 0:0bd73064b7f6 | 173 | checksumIndex = NmeaChecksum( NmeaString, rx_buf_lens[idx], checksum ); |
| dudmuck | 0:0bd73064b7f6 | 174 | |
| dudmuck | 0:0bd73064b7f6 | 175 | // could we calculate a verification checksum ? |
| dudmuck | 0:0bd73064b7f6 | 176 | if( checksumIndex < 0 ) |
| dudmuck | 0:0bd73064b7f6 | 177 | { |
| shaunkrnelson |
6:9dfe135334c9 | 178 | if (verbose) |
| shaunkrnelson |
6:9dfe135334c9 | 179 | printf("gps:checksumIndex:%d\r\n", checksumIndex); |
| dudmuck | 0:0bd73064b7f6 | 180 | return false; |
| dudmuck | 0:0bd73064b7f6 | 181 | } |
| dudmuck | 0:0bd73064b7f6 | 182 | |
| dudmuck | 0:0bd73064b7f6 | 183 | // check if there are enough char in the serial buffer to read checksum |
| dudmuck | 0:0bd73064b7f6 | 184 | if( checksumIndex >= ( rx_buf_lens[idx] - 2 ) ) |
| dudmuck | 0:0bd73064b7f6 | 185 | { |
| shaunkrnelson |
6:9dfe135334c9 | 186 | if (verbose) |
| shaunkrnelson |
6:9dfe135334c9 | 187 | printf("gps:checksumIndex:%d\r\n", checksumIndex); |
| dudmuck | 0:0bd73064b7f6 | 188 | return false; |
| dudmuck | 0:0bd73064b7f6 | 189 | } |
| dudmuck | 0:0bd73064b7f6 | 190 | |
| dudmuck | 0:0bd73064b7f6 | 191 | // check the checksum |
| dudmuck | 0:0bd73064b7f6 | 192 | if( ( NmeaString[checksumIndex] == checksum[0] ) && ( NmeaString[checksumIndex + 1] == checksum[1] ) ) |
| dudmuck | 0:0bd73064b7f6 | 193 | { |
| dudmuck | 0:0bd73064b7f6 | 194 | return true; |
| dudmuck | 0:0bd73064b7f6 | 195 | } |
| dudmuck | 0:0bd73064b7f6 | 196 | else |
| dudmuck | 0:0bd73064b7f6 | 197 | { |
| dudmuck | 0:0bd73064b7f6 | 198 | if (verbose) |
| dudmuck | 0:0bd73064b7f6 | 199 | 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 | 200 | return false; |
| dudmuck | 0:0bd73064b7f6 | 201 | } |
| dudmuck | 0:0bd73064b7f6 | 202 | } |
| dudmuck | 0:0bd73064b7f6 | 203 | |
| dudmuck | 0:0bd73064b7f6 | 204 | void GPS::ConvertPositionFromStringToNumerical( ) |
| dudmuck | 0:0bd73064b7f6 | 205 | { |
| dudmuck | 0:0bd73064b7f6 | 206 | int i; |
| dudmuck | 0:0bd73064b7f6 | 207 | |
| dudmuck | 0:0bd73064b7f6 | 208 | double valueTmp1; |
| dudmuck | 0:0bd73064b7f6 | 209 | double valueTmp2; |
| dudmuck | 0:0bd73064b7f6 | 210 | double valueTmp3; |
| dudmuck | 0:0bd73064b7f6 | 211 | double valueTmp4; |
| dudmuck | 0:0bd73064b7f6 | 212 | |
| shaunkrnelson |
6:9dfe135334c9 | 213 | |
| dudmuck | 0:0bd73064b7f6 | 214 | // Convert the latitude from ASCII to uint8_t values |
| dudmuck | 0:0bd73064b7f6 | 215 | for( i = 0 ; i < 10 ; i++ ) |
| dudmuck | 0:0bd73064b7f6 | 216 | { |
| dudmuck | 0:0bd73064b7f6 | 217 | NmeaGpsData.NmeaLatitude[i] = NmeaGpsData.NmeaLatitude[i] & 0xF; |
| dudmuck | 0:0bd73064b7f6 | 218 | } |
| dudmuck | 0:0bd73064b7f6 | 219 | // Convert latitude from degree/minute/second (DMS) format into decimal |
| dudmuck | 0:0bd73064b7f6 | 220 | valueTmp1 = ( double )NmeaGpsData.NmeaLatitude[0] * 10.0 + ( double )NmeaGpsData.NmeaLatitude[1]; |
| dudmuck | 0:0bd73064b7f6 | 221 | valueTmp2 = ( double )NmeaGpsData.NmeaLatitude[2] * 10.0 + ( double )NmeaGpsData.NmeaLatitude[3]; |
| dudmuck | 0:0bd73064b7f6 | 222 | valueTmp3 = ( double )NmeaGpsData.NmeaLatitude[5] * 1000.0 + ( double )NmeaGpsData.NmeaLatitude[6] * 100.0 + |
| dudmuck | 0:0bd73064b7f6 | 223 | ( double )NmeaGpsData.NmeaLatitude[7] * 10.0 + ( double )NmeaGpsData.NmeaLatitude[8]; |
| dudmuck | 0:0bd73064b7f6 | 224 | |
| dudmuck | 0:0bd73064b7f6 | 225 | Latitude = valueTmp1 + ( ( valueTmp2 + ( valueTmp3 * 0.0001 ) ) / 60.0 ); |
| dudmuck | 0:0bd73064b7f6 | 226 | |
| dudmuck | 0:0bd73064b7f6 | 227 | if( NmeaGpsData.NmeaLatitudePole[0] == 'S' ) |
| dudmuck | 0:0bd73064b7f6 | 228 | { |
| dudmuck | 0:0bd73064b7f6 | 229 | Latitude *= -1; |
| dudmuck | 0:0bd73064b7f6 | 230 | } |
| dudmuck | 0:0bd73064b7f6 | 231 | |
| dudmuck | 0:0bd73064b7f6 | 232 | // Convert the longitude from ASCII to uint8_t values |
| dudmuck | 0:0bd73064b7f6 | 233 | for( i = 0 ; i < 10 ; i++ ) |
| dudmuck | 0:0bd73064b7f6 | 234 | { |
| dudmuck | 0:0bd73064b7f6 | 235 | NmeaGpsData.NmeaLongitude[i] = NmeaGpsData.NmeaLongitude[i] & 0xF; |
| dudmuck | 0:0bd73064b7f6 | 236 | } |
| dudmuck | 0:0bd73064b7f6 | 237 | // Convert longitude from degree/minute/second (DMS) format into decimal |
| dudmuck | 0:0bd73064b7f6 | 238 | valueTmp1 = ( double )NmeaGpsData.NmeaLongitude[0] * 100.0 + ( double )NmeaGpsData.NmeaLongitude[1] * 10.0 + ( double )NmeaGpsData.NmeaLongitude[2]; |
| dudmuck | 0:0bd73064b7f6 | 239 | valueTmp2 = ( double )NmeaGpsData.NmeaLongitude[3] * 10.0 + ( double )NmeaGpsData.NmeaLongitude[4]; |
| dudmuck | 0:0bd73064b7f6 | 240 | valueTmp3 = ( double )NmeaGpsData.NmeaLongitude[6] * 1000.0 + ( double )NmeaGpsData.NmeaLongitude[7] * 100; |
| dudmuck | 0:0bd73064b7f6 | 241 | valueTmp4 = ( double )NmeaGpsData.NmeaLongitude[8] * 10.0 + ( double )NmeaGpsData.NmeaLongitude[9]; |
| dudmuck | 0:0bd73064b7f6 | 242 | |
| dudmuck | 0:0bd73064b7f6 | 243 | Longitude = valueTmp1 + ( valueTmp2 / 60.0 ) + ( ( ( valueTmp3 + valueTmp4 ) * 0.0001 ) / 60.0 ); |
| dudmuck | 0:0bd73064b7f6 | 244 | |
| dudmuck | 0:0bd73064b7f6 | 245 | if( NmeaGpsData.NmeaLongitudePole[0] == 'W' ) |
| dudmuck | 0:0bd73064b7f6 | 246 | { |
| dudmuck | 0:0bd73064b7f6 | 247 | Longitude *= -1; |
| dudmuck | 0:0bd73064b7f6 | 248 | } |
| dudmuck | 0:0bd73064b7f6 | 249 | |
| dudmuck | 0:0bd73064b7f6 | 250 | //printf("DD %f, %f\r\n", Latitude, Longitude); |
| dudmuck | 0:0bd73064b7f6 | 251 | } |
| dudmuck | 0:0bd73064b7f6 | 252 | |
| dudmuck | 0:0bd73064b7f6 | 253 | void GPS::ConvertPositionIntoBinary( ) |
| dudmuck | 0:0bd73064b7f6 | 254 | { |
| dudmuck | 0:0bd73064b7f6 | 255 | long double temp; |
| dudmuck | 0:0bd73064b7f6 | 256 | |
| dudmuck | 0:0bd73064b7f6 | 257 | if( Latitude >= 0 ) // North |
| dudmuck | 0:0bd73064b7f6 | 258 | { |
| dudmuck | 0:0bd73064b7f6 | 259 | temp = Latitude * MaxNorthPosition; |
| dudmuck | 0:0bd73064b7f6 | 260 | LatitudeBinary = temp / 90; |
| dudmuck | 0:0bd73064b7f6 | 261 | } |
| dudmuck | 0:0bd73064b7f6 | 262 | else // South |
| dudmuck | 0:0bd73064b7f6 | 263 | { |
| dudmuck | 0:0bd73064b7f6 | 264 | temp = Latitude * MaxSouthPosition; |
| dudmuck | 0:0bd73064b7f6 | 265 | LatitudeBinary = temp / 90; |
| dudmuck | 0:0bd73064b7f6 | 266 | } |
| dudmuck | 0:0bd73064b7f6 | 267 | |
| dudmuck | 0:0bd73064b7f6 | 268 | if( Longitude >= 0 ) // East |
| dudmuck | 0:0bd73064b7f6 | 269 | { |
| dudmuck | 0:0bd73064b7f6 | 270 | temp = Longitude * MaxEastPosition; |
| dudmuck | 0:0bd73064b7f6 | 271 | LongitudeBinary = temp / 180; |
| dudmuck | 0:0bd73064b7f6 | 272 | } |
| dudmuck | 0:0bd73064b7f6 | 273 | else // West |
| dudmuck | 0:0bd73064b7f6 | 274 | { |
| dudmuck | 0:0bd73064b7f6 | 275 | temp = Longitude * MaxWestPosition; |
| dudmuck | 0:0bd73064b7f6 | 276 | LongitudeBinary = temp / 180; |
| dudmuck | 0:0bd73064b7f6 | 277 | } |
| dudmuck | 1:2f5fbb33ae8b | 278 | |
| dudmuck | 1:2f5fbb33ae8b | 279 | //printf("binary: %x %x\r\n", LatitudeBinary, LongitudeBinary); |
| dudmuck | 1:2f5fbb33ae8b | 280 | if (LatitudeBinary == LAT_UNFIXED && LongitudeBinary == LONG_UNFIXED) |
| dudmuck | 1:2f5fbb33ae8b | 281 | have_fix = false; |
| dudmuck | 1:2f5fbb33ae8b | 282 | else |
| dudmuck | 1:2f5fbb33ae8b | 283 | have_fix = true; |
| dudmuck | 0:0bd73064b7f6 | 284 | } |
| dudmuck | 0:0bd73064b7f6 | 285 | |
| dudmuck | 0:0bd73064b7f6 | 286 | int GPS::ParseGPSData(int idx) |
| dudmuck | 0:0bd73064b7f6 | 287 | { |
| dudmuck | 0:0bd73064b7f6 | 288 | uint8_t i, j, fieldSize; |
| dudmuck | 0:0bd73064b7f6 | 289 | char *NmeaString; |
| dudmuck | 0:0bd73064b7f6 | 290 | |
| dudmuck | 0:0bd73064b7f6 | 291 | if (NmeaValidateChecksum(idx) == false) { |
| dudmuck | 0:0bd73064b7f6 | 292 | if (verbose) |
| dudmuck | 0:0bd73064b7f6 | 293 | printf("gps:bad nmea checksum:%s\r\n", rx_bufs[rx_bufs_out_idx]); |
| dudmuck | 0:0bd73064b7f6 | 294 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 295 | } |
| dudmuck | 0:0bd73064b7f6 | 296 | |
| dudmuck | 0:0bd73064b7f6 | 297 | NmeaString = rx_bufs[idx]; |
| dudmuck | 1:2f5fbb33ae8b | 298 | |
| dudmuck | 1:2f5fbb33ae8b | 299 | if (NmeaString[0] == 'P' && NmeaString[1] == 'M' && NmeaString[2] == 'T' && NmeaString[3] == 'K') { |
| dudmuck | 1:2f5fbb33ae8b | 300 | /* anything useful from PMTK packets? */ |
| dudmuck | 1:2f5fbb33ae8b | 301 | return SUCCESS; |
| dudmuck | 1:2f5fbb33ae8b | 302 | } |
| dudmuck | 1:2f5fbb33ae8b | 303 | |
| dudmuck | 0:0bd73064b7f6 | 304 | fieldSize = 0; |
| dudmuck | 0:0bd73064b7f6 | 305 | i = 0; |
| dudmuck | 0:0bd73064b7f6 | 306 | while( NmeaString[i + fieldSize++] != ',' ) |
| dudmuck | 0:0bd73064b7f6 | 307 | { |
| dudmuck | 0:0bd73064b7f6 | 308 | if( fieldSize > 6 ) |
| dudmuck | 0:0bd73064b7f6 | 309 | { |
| dudmuck | 1:2f5fbb33ae8b | 310 | printf("gps parse:%s\r\n", NmeaString); |
| dudmuck | 0:0bd73064b7f6 | 311 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 312 | } |
| dudmuck | 0:0bd73064b7f6 | 313 | } |
| dudmuck | 0:0bd73064b7f6 | 314 | |
| dudmuck | 0:0bd73064b7f6 | 315 | for( j = 0; j < fieldSize; j++, i++ ) |
| dudmuck | 0:0bd73064b7f6 | 316 | { |
| dudmuck | 0:0bd73064b7f6 | 317 | NmeaGpsData.NmeaDataType[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 318 | } |
| dudmuck | 0:0bd73064b7f6 | 319 | // Parse the GPGGA data |
| dudmuck | 0:0bd73064b7f6 | 320 | if( strncmp( ( const char* )NmeaGpsData.NmeaDataType, ( const char* )NmeaDataTypeGPGGA, 5 ) == 0 ) |
| dudmuck | 0:0bd73064b7f6 | 321 | { |
| dudmuck | 0:0bd73064b7f6 | 322 | // NmeaUtcTime |
| dudmuck | 0:0bd73064b7f6 | 323 | fieldSize = 0; |
| dudmuck | 0:0bd73064b7f6 | 324 | while( NmeaString[i + fieldSize++] != ',' ) |
| dudmuck | 0:0bd73064b7f6 | 325 | { |
| dudmuck | 0:0bd73064b7f6 | 326 | if( fieldSize > 11 ) |
| dudmuck | 0:0bd73064b7f6 | 327 | { |
| dudmuck | 0:0bd73064b7f6 | 328 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 329 | } |
| dudmuck | 0:0bd73064b7f6 | 330 | } |
| dudmuck | 0:0bd73064b7f6 | 331 | for( j = 0; j < fieldSize; j++, i++ ) |
| dudmuck | 0:0bd73064b7f6 | 332 | { |
| dudmuck | 0:0bd73064b7f6 | 333 | NmeaGpsData.NmeaUtcTime[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 334 | } |
| dudmuck | 0:0bd73064b7f6 | 335 | // NmeaLatitude |
| dudmuck | 0:0bd73064b7f6 | 336 | fieldSize = 0; |
| dudmuck | 0:0bd73064b7f6 | 337 | while( NmeaString[i + fieldSize++] != ',' ) |
| dudmuck | 0:0bd73064b7f6 | 338 | { |
| dudmuck | 0:0bd73064b7f6 | 339 | if( fieldSize > 10 ) |
| dudmuck | 0:0bd73064b7f6 | 340 | { |
| dudmuck | 0:0bd73064b7f6 | 341 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 342 | } |
| dudmuck | 0:0bd73064b7f6 | 343 | } |
| shaunkrnelson |
6:9dfe135334c9 | 344 | if(fieldSize == 0) |
| shaunkrnelson |
6:9dfe135334c9 | 345 | return FAIL; |
| shaunkrnelson |
6:9dfe135334c9 | 346 | |
| dudmuck | 0:0bd73064b7f6 | 347 | for( j = 0; j < fieldSize; j++, i++ ) |
| dudmuck | 0:0bd73064b7f6 | 348 | { |
| dudmuck | 0:0bd73064b7f6 | 349 | NmeaGpsData.NmeaLatitude[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 350 | } |
| dudmuck | 0:0bd73064b7f6 | 351 | // NmeaLatitudePole |
| dudmuck | 0:0bd73064b7f6 | 352 | fieldSize = 0; |
| dudmuck | 0:0bd73064b7f6 | 353 | while( NmeaString[i + fieldSize++] != ',' ) |
| dudmuck | 0:0bd73064b7f6 | 354 | { |
| dudmuck | 0:0bd73064b7f6 | 355 | if( fieldSize > 2 ) |
| dudmuck | 0:0bd73064b7f6 | 356 | { |
| dudmuck | 0:0bd73064b7f6 | 357 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 358 | } |
| dudmuck | 0:0bd73064b7f6 | 359 | } |
| shaunkrnelson |
6:9dfe135334c9 | 360 | |
| shaunkrnelson |
6:9dfe135334c9 | 361 | if(fieldSize == 0) |
| shaunkrnelson |
6:9dfe135334c9 | 362 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 363 | for( j = 0; j < fieldSize; j++, i++ ) |
| dudmuck | 0:0bd73064b7f6 | 364 | { |
| dudmuck | 0:0bd73064b7f6 | 365 | NmeaGpsData.NmeaLatitudePole[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 366 | } |
| dudmuck | 0:0bd73064b7f6 | 367 | // NmeaLongitude |
| dudmuck | 0:0bd73064b7f6 | 368 | fieldSize = 0; |
| dudmuck | 0:0bd73064b7f6 | 369 | while( NmeaString[i + fieldSize++] != ',' ) |
| dudmuck | 0:0bd73064b7f6 | 370 | { |
| dudmuck | 0:0bd73064b7f6 | 371 | if( fieldSize > 11 ) |
| dudmuck | 0:0bd73064b7f6 | 372 | { |
| dudmuck | 0:0bd73064b7f6 | 373 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 374 | } |
| dudmuck | 0:0bd73064b7f6 | 375 | } |
| shaunkrnelson |
6:9dfe135334c9 | 376 | if(fieldSize == 0) |
| shaunkrnelson |
6:9dfe135334c9 | 377 | return FAIL; |
| shaunkrnelson |
6:9dfe135334c9 | 378 | |
| dudmuck | 0:0bd73064b7f6 | 379 | for( j = 0; j < fieldSize; j++, i++ ) |
| dudmuck | 0:0bd73064b7f6 | 380 | { |
| dudmuck | 0:0bd73064b7f6 | 381 | NmeaGpsData.NmeaLongitude[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 382 | } |
| dudmuck | 0:0bd73064b7f6 | 383 | // NmeaLongitudePole |
| dudmuck | 0:0bd73064b7f6 | 384 | fieldSize = 0; |
| dudmuck | 0:0bd73064b7f6 | 385 | while( NmeaString[i + fieldSize++] != ',' ) |
| dudmuck | 0:0bd73064b7f6 | 386 | { |
| dudmuck | 0:0bd73064b7f6 | 387 | if( fieldSize > 2 ) |
| dudmuck | 0:0bd73064b7f6 | 388 | { |
| dudmuck | 0:0bd73064b7f6 | 389 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 390 | } |
| dudmuck | 0:0bd73064b7f6 | 391 | } |
| shaunkrnelson |
6:9dfe135334c9 | 392 | |
| shaunkrnelson |
6:9dfe135334c9 | 393 | if(fieldSize == 0) |
| shaunkrnelson |
6:9dfe135334c9 | 394 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 395 | for( j = 0; j < fieldSize; j++, i++ ) |
| dudmuck | 0:0bd73064b7f6 | 396 | { |
| dudmuck | 0:0bd73064b7f6 | 397 | NmeaGpsData.NmeaLongitudePole[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 398 | } |
| dudmuck | 0:0bd73064b7f6 | 399 | // NmeaFixQuality |
| 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.NmeaFixQuality[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 411 | } |
| dudmuck | 0:0bd73064b7f6 | 412 | // NmeaSatelliteTracked |
| dudmuck | 0:0bd73064b7f6 | 413 | fieldSize = 0; |
| dudmuck | 0:0bd73064b7f6 | 414 | while( NmeaString[i + fieldSize++] != ',' ) |
| dudmuck | 0:0bd73064b7f6 | 415 | { |
| dudmuck | 0:0bd73064b7f6 | 416 | if( fieldSize > 3 ) |
| 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.NmeaSatelliteTracked[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 424 | } |
| dudmuck | 0:0bd73064b7f6 | 425 | // NmeaHorizontalDilution |
| dudmuck | 0:0bd73064b7f6 | 426 | fieldSize = 0; |
| dudmuck | 0:0bd73064b7f6 | 427 | while( NmeaString[i + fieldSize++] != ',' ) |
| dudmuck | 0:0bd73064b7f6 | 428 | { |
| dudmuck | 0:0bd73064b7f6 | 429 | if( fieldSize > 6 ) |
| 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.NmeaHorizontalDilution[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 437 | } |
| dudmuck | 0:0bd73064b7f6 | 438 | // NmeaAltitude |
| 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.NmeaAltitude[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 450 | } |
| dudmuck | 0:0bd73064b7f6 | 451 | // NmeaAltitudeUnit |
| 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.NmeaAltitudeUnit[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 463 | } |
| dudmuck | 0:0bd73064b7f6 | 464 | // NmeaHeightGeoid |
| dudmuck | 0:0bd73064b7f6 | 465 | fieldSize = 0; |
| dudmuck | 0:0bd73064b7f6 | 466 | while( NmeaString[i + fieldSize++] != ',' ) |
| dudmuck | 0:0bd73064b7f6 | 467 | { |
| dudmuck | 0:0bd73064b7f6 | 468 | if( fieldSize > 8 ) |
| dudmuck | 0:0bd73064b7f6 | 469 | { |
| dudmuck | 0:0bd73064b7f6 | 470 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 471 | } |
| dudmuck | 0:0bd73064b7f6 | 472 | } |
| dudmuck | 0:0bd73064b7f6 | 473 | for( j = 0; j < fieldSize; j++, i++ ) |
| dudmuck | 0:0bd73064b7f6 | 474 | { |
| dudmuck | 0:0bd73064b7f6 | 475 | NmeaGpsData.NmeaHeightGeoid[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 476 | } |
| dudmuck | 0:0bd73064b7f6 | 477 | // NmeaHeightGeoidUnit |
| dudmuck | 0:0bd73064b7f6 | 478 | fieldSize = 0; |
| dudmuck | 0:0bd73064b7f6 | 479 | while( NmeaString[i + fieldSize++] != ',' ) |
| dudmuck | 0:0bd73064b7f6 | 480 | { |
| dudmuck | 0:0bd73064b7f6 | 481 | if( fieldSize > 2 ) |
| dudmuck | 0:0bd73064b7f6 | 482 | { |
| dudmuck | 0:0bd73064b7f6 | 483 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 484 | } |
| dudmuck | 0:0bd73064b7f6 | 485 | } |
| dudmuck | 0:0bd73064b7f6 | 486 | for( j = 0; j < fieldSize; j++, i++ ) |
| dudmuck | 0:0bd73064b7f6 | 487 | { |
| dudmuck | 0:0bd73064b7f6 | 488 | NmeaGpsData.NmeaHeightGeoidUnit[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 489 | } |
| dudmuck | 0:0bd73064b7f6 | 490 | |
| dudmuck | 0:0bd73064b7f6 | 491 | //FormatGpsData( ); |
| dudmuck | 0:0bd73064b7f6 | 492 | ConvertPositionFromStringToNumerical( ); |
| dudmuck | 0:0bd73064b7f6 | 493 | ConvertPositionIntoBinary( ); |
| shaunkrnelson |
6:9dfe135334c9 | 494 | // printf("GPGAA: %s\r\n",NmeaString); |
| dudmuck | 0:0bd73064b7f6 | 495 | return SUCCESS; |
| dudmuck | 0:0bd73064b7f6 | 496 | } |
| dudmuck | 0:0bd73064b7f6 | 497 | else if ( strncmp( ( const char* )NmeaGpsData.NmeaDataType, ( const char* )NmeaDataTypeGPRMC, 5 ) == 0 ) |
| dudmuck | 0:0bd73064b7f6 | 498 | { |
| dudmuck | 0:0bd73064b7f6 | 499 | // NmeaUtcTime |
| dudmuck | 0:0bd73064b7f6 | 500 | fieldSize = 0; |
| dudmuck | 0:0bd73064b7f6 | 501 | while( NmeaString[i + fieldSize++] != ',' ) |
| dudmuck | 0:0bd73064b7f6 | 502 | { |
| dudmuck | 0:0bd73064b7f6 | 503 | if( fieldSize > 11 ) |
| dudmuck | 0:0bd73064b7f6 | 504 | { |
| dudmuck | 0:0bd73064b7f6 | 505 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 506 | } |
| dudmuck | 0:0bd73064b7f6 | 507 | } |
| dudmuck | 0:0bd73064b7f6 | 508 | for( j = 0; j < fieldSize; j++, i++ ) |
| dudmuck | 0:0bd73064b7f6 | 509 | { |
| dudmuck | 0:0bd73064b7f6 | 510 | NmeaGpsData.NmeaUtcTime[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 511 | } |
| dudmuck | 0:0bd73064b7f6 | 512 | // NmeaDataStatus |
| dudmuck | 0:0bd73064b7f6 | 513 | fieldSize = 0; |
| dudmuck | 0:0bd73064b7f6 | 514 | while( NmeaString[i + fieldSize++] != ',' ) |
| dudmuck | 0:0bd73064b7f6 | 515 | { |
| dudmuck | 0:0bd73064b7f6 | 516 | if( fieldSize > 2 ) |
| dudmuck | 0:0bd73064b7f6 | 517 | { |
| dudmuck | 0:0bd73064b7f6 | 518 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 519 | } |
| dudmuck | 0:0bd73064b7f6 | 520 | } |
| shaunkrnelson |
6:9dfe135334c9 | 521 | if(fieldSize == 0) |
| shaunkrnelson |
6:9dfe135334c9 | 522 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 523 | for( j = 0; j < fieldSize; j++, i++ ) |
| dudmuck | 0:0bd73064b7f6 | 524 | { |
| dudmuck | 0:0bd73064b7f6 | 525 | NmeaGpsData.NmeaDataStatus[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 526 | } |
| shaunkrnelson |
6:9dfe135334c9 | 527 | |
| shaunkrnelson |
6:9dfe135334c9 | 528 | if(NmeaGpsData.NmeaDataStatus[0] != 'A') |
| shaunkrnelson |
6:9dfe135334c9 | 529 | return FAIL; |
| shaunkrnelson |
6:9dfe135334c9 | 530 | // |
| dudmuck | 0:0bd73064b7f6 | 531 | // NmeaLatitude |
| dudmuck | 0:0bd73064b7f6 | 532 | fieldSize = 0; |
| dudmuck | 0:0bd73064b7f6 | 533 | while( NmeaString[i + fieldSize++] != ',' ) |
| dudmuck | 0:0bd73064b7f6 | 534 | { |
| dudmuck | 0:0bd73064b7f6 | 535 | if( fieldSize > 10 ) |
| dudmuck | 0:0bd73064b7f6 | 536 | { |
| dudmuck | 0:0bd73064b7f6 | 537 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 538 | } |
| dudmuck | 0:0bd73064b7f6 | 539 | } |
| shaunkrnelson |
6:9dfe135334c9 | 540 | |
| shaunkrnelson |
6:9dfe135334c9 | 541 | if(fieldSize == 0) |
| shaunkrnelson |
6:9dfe135334c9 | 542 | return FAIL; |
| shaunkrnelson |
6:9dfe135334c9 | 543 | |
| dudmuck | 0:0bd73064b7f6 | 544 | for( j = 0; j < fieldSize; j++, i++ ) |
| dudmuck | 0:0bd73064b7f6 | 545 | { |
| dudmuck | 0:0bd73064b7f6 | 546 | NmeaGpsData.NmeaLatitude[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 547 | } |
| dudmuck | 0:0bd73064b7f6 | 548 | // NmeaLatitudePole |
| dudmuck | 0:0bd73064b7f6 | 549 | fieldSize = 0; |
| dudmuck | 0:0bd73064b7f6 | 550 | while( NmeaString[i + fieldSize++] != ',' ) |
| dudmuck | 0:0bd73064b7f6 | 551 | { |
| dudmuck | 0:0bd73064b7f6 | 552 | if( fieldSize > 2 ) |
| dudmuck | 0:0bd73064b7f6 | 553 | { |
| dudmuck | 0:0bd73064b7f6 | 554 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 555 | } |
| dudmuck | 0:0bd73064b7f6 | 556 | } |
| shaunkrnelson |
6:9dfe135334c9 | 557 | if(fieldSize == 0) |
| shaunkrnelson |
6:9dfe135334c9 | 558 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 559 | for( j = 0; j < fieldSize; j++, i++ ) |
| dudmuck | 0:0bd73064b7f6 | 560 | { |
| dudmuck | 0:0bd73064b7f6 | 561 | NmeaGpsData.NmeaLatitudePole[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 562 | } |
| dudmuck | 0:0bd73064b7f6 | 563 | // NmeaLongitude |
| dudmuck | 0:0bd73064b7f6 | 564 | fieldSize = 0; |
| dudmuck | 0:0bd73064b7f6 | 565 | while( NmeaString[i + fieldSize++] != ',' ) |
| dudmuck | 0:0bd73064b7f6 | 566 | { |
| dudmuck | 0:0bd73064b7f6 | 567 | if( fieldSize > 11 ) |
| dudmuck | 0:0bd73064b7f6 | 568 | { |
| dudmuck | 0:0bd73064b7f6 | 569 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 570 | } |
| dudmuck | 0:0bd73064b7f6 | 571 | } |
| shaunkrnelson |
6:9dfe135334c9 | 572 | if(fieldSize == 0) |
| shaunkrnelson |
6:9dfe135334c9 | 573 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 574 | for( j = 0; j < fieldSize; j++, i++ ) |
| dudmuck | 0:0bd73064b7f6 | 575 | { |
| dudmuck | 0:0bd73064b7f6 | 576 | NmeaGpsData.NmeaLongitude[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 577 | } |
| dudmuck | 0:0bd73064b7f6 | 578 | // NmeaLongitudePole |
| dudmuck | 0:0bd73064b7f6 | 579 | fieldSize = 0; |
| dudmuck | 0:0bd73064b7f6 | 580 | while( NmeaString[i + fieldSize++] != ',' ) |
| dudmuck | 0:0bd73064b7f6 | 581 | { |
| dudmuck | 0:0bd73064b7f6 | 582 | if( fieldSize > 2 ) |
| dudmuck | 0:0bd73064b7f6 | 583 | { |
| dudmuck | 0:0bd73064b7f6 | 584 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 585 | } |
| dudmuck | 0:0bd73064b7f6 | 586 | } |
| shaunkrnelson |
6:9dfe135334c9 | 587 | if(fieldSize == 0) |
| shaunkrnelson |
6:9dfe135334c9 | 588 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 589 | for( j = 0; j < fieldSize; j++, i++ ) |
| dudmuck | 0:0bd73064b7f6 | 590 | { |
| dudmuck | 0:0bd73064b7f6 | 591 | NmeaGpsData.NmeaLongitudePole[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 592 | } |
| dudmuck | 0:0bd73064b7f6 | 593 | // NmeaSpeed |
| dudmuck | 0:0bd73064b7f6 | 594 | fieldSize = 0; |
| dudmuck | 0:0bd73064b7f6 | 595 | while( NmeaString[i + fieldSize++] != ',' ) |
| dudmuck | 0:0bd73064b7f6 | 596 | { |
| dudmuck | 0:0bd73064b7f6 | 597 | if( fieldSize > 8 ) |
| dudmuck | 0:0bd73064b7f6 | 598 | { |
| dudmuck | 0:0bd73064b7f6 | 599 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 600 | } |
| dudmuck | 0:0bd73064b7f6 | 601 | } |
| dudmuck | 0:0bd73064b7f6 | 602 | for( j = 0; j < fieldSize; j++, i++ ) |
| dudmuck | 0:0bd73064b7f6 | 603 | { |
| dudmuck | 0:0bd73064b7f6 | 604 | NmeaGpsData.NmeaSpeed[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 605 | } |
| dudmuck | 0:0bd73064b7f6 | 606 | // NmeaDetectionAngle |
| dudmuck | 0:0bd73064b7f6 | 607 | fieldSize = 0; |
| dudmuck | 0:0bd73064b7f6 | 608 | while( NmeaString[i + fieldSize++] != ',' ) |
| dudmuck | 0:0bd73064b7f6 | 609 | { |
| dudmuck | 0:0bd73064b7f6 | 610 | if( fieldSize > 8 ) |
| dudmuck | 0:0bd73064b7f6 | 611 | { |
| dudmuck | 0:0bd73064b7f6 | 612 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 613 | } |
| dudmuck | 0:0bd73064b7f6 | 614 | } |
| dudmuck | 0:0bd73064b7f6 | 615 | for( j = 0; j < fieldSize; j++, i++ ) |
| dudmuck | 0:0bd73064b7f6 | 616 | { |
| dudmuck | 0:0bd73064b7f6 | 617 | NmeaGpsData.NmeaDetectionAngle[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 618 | } |
| dudmuck | 0:0bd73064b7f6 | 619 | // NmeaDate |
| dudmuck | 0:0bd73064b7f6 | 620 | fieldSize = 0; |
| dudmuck | 0:0bd73064b7f6 | 621 | while( NmeaString[i + fieldSize++] != ',' ) |
| dudmuck | 0:0bd73064b7f6 | 622 | { |
| dudmuck | 0:0bd73064b7f6 | 623 | if( fieldSize > 8 ) |
| dudmuck | 0:0bd73064b7f6 | 624 | { |
| dudmuck | 0:0bd73064b7f6 | 625 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 626 | } |
| dudmuck | 0:0bd73064b7f6 | 627 | } |
| dudmuck | 0:0bd73064b7f6 | 628 | for( j = 0; j < fieldSize; j++, i++ ) |
| dudmuck | 0:0bd73064b7f6 | 629 | { |
| dudmuck | 0:0bd73064b7f6 | 630 | NmeaGpsData.NmeaDate[j] = NmeaString[i]; |
| dudmuck | 0:0bd73064b7f6 | 631 | } |
| dudmuck | 0:0bd73064b7f6 | 632 | |
| dudmuck | 0:0bd73064b7f6 | 633 | //FormatGpsData( ); |
| dudmuck | 0:0bd73064b7f6 | 634 | ConvertPositionFromStringToNumerical( ); |
| dudmuck | 0:0bd73064b7f6 | 635 | ConvertPositionIntoBinary( ); |
| shaunkrnelson |
6:9dfe135334c9 | 636 | // printf("GPMRC: %s\r\n", NmeaString); |
| shaunkrnelson |
6:9dfe135334c9 | 637 | // printf("ParseGPSData: string=%s, lat=%f, long=%f\r\n",NmeaString, Latitude, Longitude); |
| dudmuck | 0:0bd73064b7f6 | 638 | return SUCCESS; |
| dudmuck | 0:0bd73064b7f6 | 639 | } |
| dudmuck | 0:0bd73064b7f6 | 640 | else |
| dudmuck | 0:0bd73064b7f6 | 641 | { |
| dudmuck | 0:0bd73064b7f6 | 642 | return FAIL; |
| dudmuck | 0:0bd73064b7f6 | 643 | } |
| dudmuck | 0:0bd73064b7f6 | 644 | } |
| dudmuck | 0:0bd73064b7f6 | 645 | |
| dudmuck | 0:0bd73064b7f6 | 646 | void GPS::service() |
| dudmuck | 0:0bd73064b7f6 | 647 | { |
| dudmuck | 0:0bd73064b7f6 | 648 | while (rx_bufs_in_idx != rx_bufs_out_idx) { |
| dudmuck | 0:0bd73064b7f6 | 649 | ParseGPSData(rx_bufs_out_idx); |
| dudmuck | 0:0bd73064b7f6 | 650 | if (++rx_bufs_out_idx == NUM_RX_BUFS) |
| dudmuck | 0:0bd73064b7f6 | 651 | rx_bufs_out_idx = 0; |
| dudmuck | 0:0bd73064b7f6 | 652 | } |
| dudmuck | 0:0bd73064b7f6 | 653 | |
| dudmuck | 2:b531881123bf | 654 | /*if (pps_occurred) { |
| dudmuck | 0:0bd73064b7f6 | 655 | pps_occurred = false; |
| dudmuck | 0:0bd73064b7f6 | 656 | if (verbose) |
| dudmuck | 0:0bd73064b7f6 | 657 | printf("gps:pps\r\n"); |
| dudmuck | 2:b531881123bf | 658 | }*/ |
| dudmuck | 0:0bd73064b7f6 | 659 | } |
| dudmuck | 0:0bd73064b7f6 | 660 | |
| dudmuck | 0:0bd73064b7f6 | 661 |