GPS NEMA String parser library. Only supports SkyTraq Venus chip at this time.
Dependents: MTDOT-EVB-LinkCheck-AL MTDOT-BOX-EVB-Factory-Firmware-LIB-108 TelitSensorToCloud mDot_sensor_to_cloud ... more
GPSPARSER.cpp@1:58c50bb3c60c, 2015-09-16 (annotated)
- Committer:
- falingtrea
- Date:
- Wed Sep 16 14:01:08 2015 +0000
- Revision:
- 1:58c50bb3c60c
- Parent:
- 0:d2169fecf3fb
- Child:
- 2:fbc098f1e7c0
V1.01 - Changed report rate of GGA and GSA NEMA sentences
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
falingtrea | 0:d2169fecf3fb | 1 | /** |
falingtrea | 0:d2169fecf3fb | 2 | * @file NemaParser.cpp |
falingtrea | 0:d2169fecf3fb | 3 | * @brief NEMA String to Packet Parser - NEMA strings to compact packet data |
falingtrea | 0:d2169fecf3fb | 4 | * @author Tim Barr |
falingtrea | 1:58c50bb3c60c | 5 | * @version 1.01 |
falingtrea | 0:d2169fecf3fb | 6 | * @see http://www.kh-gps.de/nmea.faq |
falingtrea | 0:d2169fecf3fb | 7 | * @see http://www.catb.org/gpsd/NMEA.html |
falingtrea | 0:d2169fecf3fb | 8 | * |
falingtrea | 0:d2169fecf3fb | 9 | * Copyright (c) 2015 |
falingtrea | 0:d2169fecf3fb | 10 | * |
falingtrea | 0:d2169fecf3fb | 11 | * Licensed under the Apache License, Version 2.0 (the "License"); |
falingtrea | 0:d2169fecf3fb | 12 | * you may not use this file except in compliance with the License. |
falingtrea | 0:d2169fecf3fb | 13 | * You may obtain a copy of the License at |
falingtrea | 0:d2169fecf3fb | 14 | * |
falingtrea | 0:d2169fecf3fb | 15 | * http://www.apache.org/licenses/LICENSE-2.0 |
falingtrea | 0:d2169fecf3fb | 16 | * |
falingtrea | 0:d2169fecf3fb | 17 | * Unless required by applicable law or agreed to in writing, software |
falingtrea | 0:d2169fecf3fb | 18 | * distributed under the License is distributed on an "AS IS" BASIS, |
falingtrea | 0:d2169fecf3fb | 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
falingtrea | 0:d2169fecf3fb | 20 | * See the License for the specific language governing permissions and |
falingtrea | 0:d2169fecf3fb | 21 | * limitations under the License. |
falingtrea | 0:d2169fecf3fb | 22 | * |
falingtrea | 1:58c50bb3c60c | 23 | * 09/16/15 TAB V1.01 Changed report rate of GGA and GSA NEMA sentences |
falingtrea | 1:58c50bb3c60c | 24 | * |
falingtrea | 0:d2169fecf3fb | 25 | * TODO: Add speed, compass direction, error (DOP) data. Make init more generic |
falingtrea | 0:d2169fecf3fb | 26 | */ |
falingtrea | 0:d2169fecf3fb | 27 | |
falingtrea | 0:d2169fecf3fb | 28 | #include "GPSPARSER.h" |
falingtrea | 0:d2169fecf3fb | 29 | |
falingtrea | 0:d2169fecf3fb | 30 | using namespace mts; |
falingtrea | 0:d2169fecf3fb | 31 | |
falingtrea | 0:d2169fecf3fb | 32 | GPSPARSER::GPSPARSER(MTSSerial *uart) |
falingtrea | 0:d2169fecf3fb | 33 | : _getSentenceThread(&GPSPARSER::startSentenceThread,this) |
falingtrea | 0:d2169fecf3fb | 34 | { |
falingtrea | 0:d2169fecf3fb | 35 | _gps_uart = uart; |
falingtrea | 0:d2169fecf3fb | 36 | _gps_uart->baud(9600); //set GPS baud rate here |
falingtrea | 0:d2169fecf3fb | 37 | |
falingtrea | 0:d2169fecf3fb | 38 | _gps_latitude.degrees = 0; |
falingtrea | 0:d2169fecf3fb | 39 | _gps_longitude.degrees = 0; |
falingtrea | 0:d2169fecf3fb | 40 | _timestamp.tm_sec = 0; |
falingtrea | 0:d2169fecf3fb | 41 | _timestamp.tm_min = 0; |
falingtrea | 0:d2169fecf3fb | 42 | _timestamp.tm_hour = 0; |
falingtrea | 0:d2169fecf3fb | 43 | _timestamp.tm_mday = 0; |
falingtrea | 0:d2169fecf3fb | 44 | _timestamp.tm_mon = 0; |
falingtrea | 0:d2169fecf3fb | 45 | _timestamp.tm_year = 0; |
falingtrea | 0:d2169fecf3fb | 46 | _timestamp.tm_wday = 0; |
falingtrea | 0:d2169fecf3fb | 47 | _timestamp.tm_yday = 0; |
falingtrea | 0:d2169fecf3fb | 48 | _timestamp.tm_isdst = -1; // time is UTC so no Daylight savings time |
falingtrea | 0:d2169fecf3fb | 49 | |
falingtrea | 0:d2169fecf3fb | 50 | _gps_status = false; |
falingtrea | 0:d2169fecf3fb | 51 | _fix_status = 0; |
falingtrea | 0:d2169fecf3fb | 52 | _num_satellites =0; |
falingtrea | 0:d2169fecf3fb | 53 | _gps_detected = false; |
falingtrea | 0:d2169fecf3fb | 54 | |
falingtrea | 0:d2169fecf3fb | 55 | if (GPSPARSER::initGps() == 0){ |
falingtrea | 0:d2169fecf3fb | 56 | _gps_detected = true; |
falingtrea | 0:d2169fecf3fb | 57 | _getSentenceThread.signal_set(START_THREAD); |
falingtrea | 0:d2169fecf3fb | 58 | printf("Started Nema Sentence Thread\r\n"); |
falingtrea | 0:d2169fecf3fb | 59 | } |
falingtrea | 0:d2169fecf3fb | 60 | else |
falingtrea | 0:d2169fecf3fb | 61 | printf("No GPS detected"); |
falingtrea | 0:d2169fecf3fb | 62 | |
falingtrea | 0:d2169fecf3fb | 63 | return; |
falingtrea | 0:d2169fecf3fb | 64 | } |
falingtrea | 0:d2169fecf3fb | 65 | |
falingtrea | 0:d2169fecf3fb | 66 | GPSPARSER::~GPSPARSER(void){ |
falingtrea | 0:d2169fecf3fb | 67 | if (_gps_detected) |
falingtrea | 0:d2169fecf3fb | 68 | _getSentenceThread.terminate(); |
falingtrea | 0:d2169fecf3fb | 69 | } |
falingtrea | 0:d2169fecf3fb | 70 | |
falingtrea | 0:d2169fecf3fb | 71 | uint8_t GPSPARSER::initGps (void){ |
falingtrea | 0:d2169fecf3fb | 72 | |
falingtrea | 0:d2169fecf3fb | 73 | // this code is specific to the Skytraq Venus GPS chip. This code could be re-written to detect another type of |
falingtrea | 0:d2169fecf3fb | 74 | // GPS device. Maybe read serial port for a specific time and detect $GP from NEMA string |
falingtrea | 0:d2169fecf3fb | 75 | |
falingtrea | 1:58c50bb3c60c | 76 | // Sets Skytraq Venus GPS to output GGA,GSA,GSV every 30 seconds, and RMC every second, no ZDA,GLL,VTG |
falingtrea | 0:d2169fecf3fb | 77 | // setup string for GPS GGA GSA GSV GLL RMC VTG ZDA cksum |
falingtrea | 1:58c50bb3c60c | 78 | char init_gps[16] = {0xA0,0xA1,0x00,0x09,0x08,0x1E,0x1E,0x1E,0x00,0x01,0x00,0x00,0x00,0x17,0x0D,0x0A}; |
falingtrea | 0:d2169fecf3fb | 79 | char chk_char; |
falingtrea | 0:d2169fecf3fb | 80 | uint8_t ret = 0; |
falingtrea | 0:d2169fecf3fb | 81 | |
falingtrea | 0:d2169fecf3fb | 82 | _gps_uart->rxClear(); |
falingtrea | 0:d2169fecf3fb | 83 | printf("Starting initGPS \r\n"); |
falingtrea | 0:d2169fecf3fb | 84 | |
falingtrea | 0:d2169fecf3fb | 85 | _gps_uart->write(init_gps,16); |
falingtrea | 0:d2169fecf3fb | 86 | printf("wrote control data\r\n"); |
falingtrea | 0:d2169fecf3fb | 87 | |
falingtrea | 0:d2169fecf3fb | 88 | do { |
falingtrea | 0:d2169fecf3fb | 89 | osDelay(10); |
falingtrea | 0:d2169fecf3fb | 90 | } while (!_gps_uart->txEmpty()); |
falingtrea | 0:d2169fecf3fb | 91 | |
falingtrea | 0:d2169fecf3fb | 92 | osDelay(15); |
falingtrea | 0:d2169fecf3fb | 93 | |
falingtrea | 0:d2169fecf3fb | 94 | if (_gps_uart->readable()){ |
falingtrea | 0:d2169fecf3fb | 95 | do{ |
falingtrea | 0:d2169fecf3fb | 96 | _gps_uart->read(chk_char,20); |
falingtrea | 0:d2169fecf3fb | 97 | } while ((chk_char != 0xA0) && (!_gps_uart->rxEmpty())); |
falingtrea | 0:d2169fecf3fb | 98 | |
falingtrea | 0:d2169fecf3fb | 99 | printf ("found 0xA0 or rx empty\r\n"); |
falingtrea | 0:d2169fecf3fb | 100 | |
falingtrea | 0:d2169fecf3fb | 101 | if (chk_char == 0xA0){ |
falingtrea | 0:d2169fecf3fb | 102 | _gps_uart->read(chk_char,15); |
falingtrea | 0:d2169fecf3fb | 103 | if (chk_char != 0xA1) { |
falingtrea | 0:d2169fecf3fb | 104 | printf( "initGPS failed no message\r\n"); |
falingtrea | 0:d2169fecf3fb | 105 | ret = 1; |
falingtrea | 0:d2169fecf3fb | 106 | } |
falingtrea | 0:d2169fecf3fb | 107 | else { |
falingtrea | 0:d2169fecf3fb | 108 | printf("message received\r\n"); |
falingtrea | 0:d2169fecf3fb | 109 | _gps_uart->read(chk_char); |
falingtrea | 0:d2169fecf3fb | 110 | _gps_uart->read(chk_char); |
falingtrea | 0:d2169fecf3fb | 111 | printf("message size - %x\r\n",chk_char); |
falingtrea | 0:d2169fecf3fb | 112 | _gps_uart->read(chk_char); |
falingtrea | 0:d2169fecf3fb | 113 | if (chk_char != 0x83){ |
falingtrea | 0:d2169fecf3fb | 114 | printf("initGPS failed not ack message\r\n"); |
falingtrea | 0:d2169fecf3fb | 115 | ret = 1; |
falingtrea | 0:d2169fecf3fb | 116 | } |
falingtrea | 0:d2169fecf3fb | 117 | else { |
falingtrea | 0:d2169fecf3fb | 118 | printf("ACK message received\r\n"); |
falingtrea | 0:d2169fecf3fb | 119 | _gps_uart->read(chk_char); |
falingtrea | 0:d2169fecf3fb | 120 | printf("config message acknowledged - ID - %x\r\n",chk_char); |
falingtrea | 0:d2169fecf3fb | 121 | } |
falingtrea | 0:d2169fecf3fb | 122 | } |
falingtrea | 0:d2169fecf3fb | 123 | } |
falingtrea | 0:d2169fecf3fb | 124 | else { |
falingtrea | 0:d2169fecf3fb | 125 | printf("rx empty \r\n"); |
falingtrea | 0:d2169fecf3fb | 126 | ret = 1; |
falingtrea | 0:d2169fecf3fb | 127 | } |
falingtrea | 0:d2169fecf3fb | 128 | } |
falingtrea | 0:d2169fecf3fb | 129 | else { |
falingtrea | 0:d2169fecf3fb | 130 | printf("No readable characters\r\n"); |
falingtrea | 0:d2169fecf3fb | 131 | ret = 1; |
falingtrea | 0:d2169fecf3fb | 132 | } |
falingtrea | 0:d2169fecf3fb | 133 | |
falingtrea | 0:d2169fecf3fb | 134 | printf("initGps done\r\n"); |
falingtrea | 0:d2169fecf3fb | 135 | return ret; |
falingtrea | 0:d2169fecf3fb | 136 | } |
falingtrea | 0:d2169fecf3fb | 137 | |
falingtrea | 0:d2169fecf3fb | 138 | void GPSPARSER::startSentenceThread(void const *p){ |
falingtrea | 0:d2169fecf3fb | 139 | GPSPARSER *instance = (GPSPARSER*)p; |
falingtrea | 0:d2169fecf3fb | 140 | instance->readNemaSentence(); |
falingtrea | 0:d2169fecf3fb | 141 | } |
falingtrea | 0:d2169fecf3fb | 142 | |
falingtrea | 0:d2169fecf3fb | 143 | void GPSPARSER::readNemaSentence (void){ |
falingtrea | 0:d2169fecf3fb | 144 | char chk_char; |
falingtrea | 0:d2169fecf3fb | 145 | uint8_t calc_cksum; |
falingtrea | 0:d2169fecf3fb | 146 | uint8_t nema_cksum; |
falingtrea | 0:d2169fecf3fb | 147 | char nema_id[2]; |
falingtrea | 0:d2169fecf3fb | 148 | char nema_str[80]; |
falingtrea | 0:d2169fecf3fb | 149 | char cksum_str[2]; |
falingtrea | 0:d2169fecf3fb | 150 | |
falingtrea | 0:d2169fecf3fb | 151 | _getSentenceThread.signal_wait(START_THREAD); |
falingtrea | 0:d2169fecf3fb | 152 | printf("Got thread start\r\n"); |
falingtrea | 0:d2169fecf3fb | 153 | |
falingtrea | 0:d2169fecf3fb | 154 | do { |
falingtrea | 0:d2169fecf3fb | 155 | if (_gps_uart->readable() > 80){ |
falingtrea | 0:d2169fecf3fb | 156 | do{ |
falingtrea | 0:d2169fecf3fb | 157 | _gps_uart->read(chk_char); |
falingtrea | 0:d2169fecf3fb | 158 | } while ((chk_char != '$') && (!_gps_uart->rxEmpty())); |
falingtrea | 0:d2169fecf3fb | 159 | |
falingtrea | 0:d2169fecf3fb | 160 | if (chk_char == '$') { |
falingtrea | 0:d2169fecf3fb | 161 | _gps_uart->read(nema_id,2); |
falingtrea | 0:d2169fecf3fb | 162 | if (strpbrk(nema_id,"GP") != NULL){ |
falingtrea | 0:d2169fecf3fb | 163 | uint8_t i = 0; |
falingtrea | 0:d2169fecf3fb | 164 | calc_cksum = 0x17; // 8 bit XOR of G and P checksum seed |
falingtrea | 0:d2169fecf3fb | 165 | nema_cksum = 0; // initialize nema string checksum |
falingtrea | 0:d2169fecf3fb | 166 | memset(nema_str,0x00,80); // clear nema_str array |
falingtrea | 0:d2169fecf3fb | 167 | do { |
falingtrea | 0:d2169fecf3fb | 168 | _gps_uart->read(chk_char); |
falingtrea | 0:d2169fecf3fb | 169 | if ((chk_char != 0x0D) && (chk_char != '*')){ |
falingtrea | 0:d2169fecf3fb | 170 | nema_str[i++] = chk_char; |
falingtrea | 0:d2169fecf3fb | 171 | calc_cksum ^= chk_char; // 8 bit XOR checksum |
falingtrea | 0:d2169fecf3fb | 172 | } |
falingtrea | 0:d2169fecf3fb | 173 | if (chk_char == '*') { |
falingtrea | 0:d2169fecf3fb | 174 | _gps_uart->read(cksum_str,2); |
falingtrea | 0:d2169fecf3fb | 175 | nema_cksum = (uint8_t)strtoul(cksum_str,NULL,16); |
falingtrea | 0:d2169fecf3fb | 176 | } |
falingtrea | 0:d2169fecf3fb | 177 | } while (( chk_char != 0x0D) && !_gps_uart->rxEmpty()); |
falingtrea | 0:d2169fecf3fb | 178 | |
falingtrea | 0:d2169fecf3fb | 179 | if (nema_cksum == calc_cksum) |
falingtrea | 0:d2169fecf3fb | 180 | if (strncmp (nema_str,"GGA",3) == 0) |
falingtrea | 0:d2169fecf3fb | 181 | parseGGA(nema_str); |
falingtrea | 0:d2169fecf3fb | 182 | else if (strncmp (nema_str,"GSA",3) == 0) |
falingtrea | 0:d2169fecf3fb | 183 | parseGSA(nema_str); |
falingtrea | 0:d2169fecf3fb | 184 | else if (strncmp (nema_str,"GSV",3) == 0) |
falingtrea | 0:d2169fecf3fb | 185 | parseGSV(nema_str); |
falingtrea | 0:d2169fecf3fb | 186 | else if (strncmp (nema_str,"GLL",3) == 0) |
falingtrea | 0:d2169fecf3fb | 187 | parseGLL(nema_str); |
falingtrea | 0:d2169fecf3fb | 188 | else if (strncmp (nema_str,"RMC",3) == 0) |
falingtrea | 0:d2169fecf3fb | 189 | parseRMC(nema_str); |
falingtrea | 0:d2169fecf3fb | 190 | else if (strncmp (nema_str,"VTG",3) == 0) |
falingtrea | 0:d2169fecf3fb | 191 | parseVTG(nema_str); |
falingtrea | 0:d2169fecf3fb | 192 | else if (strncmp (nema_str,"ZDA",3) == 0) |
falingtrea | 0:d2169fecf3fb | 193 | parseZDA(nema_str); |
falingtrea | 0:d2169fecf3fb | 194 | else |
falingtrea | 0:d2169fecf3fb | 195 | printf("Unknown NEMA String Type\r\n"); |
falingtrea | 0:d2169fecf3fb | 196 | else |
falingtrea | 0:d2169fecf3fb | 197 | printf("NEMA String checksum error %x != %x\r\n",nema_cksum,calc_cksum); |
falingtrea | 0:d2169fecf3fb | 198 | } |
falingtrea | 0:d2169fecf3fb | 199 | } |
falingtrea | 0:d2169fecf3fb | 200 | else |
falingtrea | 0:d2169fecf3fb | 201 | printf("RX empty before all data read\r\n"); |
falingtrea | 0:d2169fecf3fb | 202 | } |
falingtrea | 0:d2169fecf3fb | 203 | |
falingtrea | 0:d2169fecf3fb | 204 | osDelay(100); |
falingtrea | 0:d2169fecf3fb | 205 | |
falingtrea | 0:d2169fecf3fb | 206 | } while(true); |
falingtrea | 0:d2169fecf3fb | 207 | |
falingtrea | 0:d2169fecf3fb | 208 | } |
falingtrea | 0:d2169fecf3fb | 209 | |
falingtrea | 0:d2169fecf3fb | 210 | uint8_t GPSPARSER::parseGGA(char *nema_buf){ |
falingtrea | 0:d2169fecf3fb | 211 | char* token_str; |
falingtrea | 0:d2169fecf3fb | 212 | uint8_t ret = 0; |
falingtrea | 0:d2169fecf3fb | 213 | |
falingtrea | 0:d2169fecf3fb | 214 | token_str = strtok(nema_buf, ","); |
falingtrea | 0:d2169fecf3fb | 215 | // skip timestamp |
falingtrea | 0:d2169fecf3fb | 216 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 217 | // skip latitude degrees minutes |
falingtrea | 0:d2169fecf3fb | 218 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 219 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 220 | // skip longitude degree minutes |
falingtrea | 0:d2169fecf3fb | 221 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 222 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 223 | // read fix quality |
falingtrea | 0:d2169fecf3fb | 224 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 225 | _fix_quality = atoi(token_str); |
falingtrea | 0:d2169fecf3fb | 226 | // skip number of satellites and horizontal dilution |
falingtrea | 0:d2169fecf3fb | 227 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 228 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 229 | // read msl altitude in meters |
falingtrea | 0:d2169fecf3fb | 230 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 231 | _msl_altitude = atoi(token_str); |
falingtrea | 0:d2169fecf3fb | 232 | |
falingtrea | 0:d2169fecf3fb | 233 | return ret; |
falingtrea | 0:d2169fecf3fb | 234 | } |
falingtrea | 0:d2169fecf3fb | 235 | |
falingtrea | 0:d2169fecf3fb | 236 | uint8_t GPSPARSER::parseGSA(char *nema_buf){ |
falingtrea | 0:d2169fecf3fb | 237 | char* token_str; |
falingtrea | 0:d2169fecf3fb | 238 | uint8_t ret = 0; |
falingtrea | 0:d2169fecf3fb | 239 | |
falingtrea | 0:d2169fecf3fb | 240 | token_str = strtok(nema_buf, ","); |
falingtrea | 0:d2169fecf3fb | 241 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 242 | // read fix status |
falingtrea | 0:d2169fecf3fb | 243 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 244 | _fix_status = atoi(token_str); |
falingtrea | 0:d2169fecf3fb | 245 | // read satellite PRNs |
falingtrea | 0:d2169fecf3fb | 246 | for (uint8_t i=0;i<12;i++){ |
falingtrea | 0:d2169fecf3fb | 247 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 248 | _satellite_prn[i] = atoi(token_str); |
falingtrea | 0:d2169fecf3fb | 249 | } |
falingtrea | 0:d2169fecf3fb | 250 | return ret; |
falingtrea | 0:d2169fecf3fb | 251 | } |
falingtrea | 0:d2169fecf3fb | 252 | |
falingtrea | 0:d2169fecf3fb | 253 | uint8_t GPSPARSER::parseGSV(char *nema_buf){ |
falingtrea | 0:d2169fecf3fb | 254 | char* token_str; |
falingtrea | 0:d2169fecf3fb | 255 | uint8_t ret = 0; |
falingtrea | 0:d2169fecf3fb | 256 | |
falingtrea | 0:d2169fecf3fb | 257 | token_str = strtok(nema_buf, ","); |
falingtrea | 0:d2169fecf3fb | 258 | // skip number of sentences and sentence number for now |
falingtrea | 0:d2169fecf3fb | 259 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 260 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 261 | // read Number of satellites |
falingtrea | 0:d2169fecf3fb | 262 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 263 | _num_satellites = atoi(token_str); |
falingtrea | 0:d2169fecf3fb | 264 | // add code to read satellite specs if needed |
falingtrea | 0:d2169fecf3fb | 265 | |
falingtrea | 0:d2169fecf3fb | 266 | return ret; |
falingtrea | 0:d2169fecf3fb | 267 | } |
falingtrea | 0:d2169fecf3fb | 268 | |
falingtrea | 0:d2169fecf3fb | 269 | uint8_t GPSPARSER::parseRMC(char *nema_buf){ |
falingtrea | 0:d2169fecf3fb | 270 | char* token_str; |
falingtrea | 0:d2169fecf3fb | 271 | char temp_str[6]; |
falingtrea | 0:d2169fecf3fb | 272 | uint8_t ret = 0; |
falingtrea | 0:d2169fecf3fb | 273 | |
falingtrea | 0:d2169fecf3fb | 274 | token_str = strtok(nema_buf, ","); |
falingtrea | 0:d2169fecf3fb | 275 | // read timestamp |
falingtrea | 0:d2169fecf3fb | 276 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 277 | strncpy(temp_str,token_str,2); |
falingtrea | 0:d2169fecf3fb | 278 | _timestamp.tm_hour = atoi(temp_str); |
falingtrea | 0:d2169fecf3fb | 279 | memset(temp_str,0x00,6); |
falingtrea | 0:d2169fecf3fb | 280 | strncpy(temp_str,token_str+2,2); |
falingtrea | 0:d2169fecf3fb | 281 | _timestamp.tm_min = atoi(temp_str); |
falingtrea | 0:d2169fecf3fb | 282 | memset(temp_str,0x00,6); |
falingtrea | 0:d2169fecf3fb | 283 | strncpy(temp_str,token_str+4,2); |
falingtrea | 0:d2169fecf3fb | 284 | _timestamp.tm_sec = atoi(temp_str); |
falingtrea | 0:d2169fecf3fb | 285 | // set gps_status true = active |
falingtrea | 0:d2169fecf3fb | 286 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 287 | memset(temp_str,0x00,6); |
falingtrea | 0:d2169fecf3fb | 288 | strncpy(temp_str,token_str,1); |
falingtrea | 0:d2169fecf3fb | 289 | if (temp_str[0] == 'A') |
falingtrea | 0:d2169fecf3fb | 290 | _gps_status = true; |
falingtrea | 0:d2169fecf3fb | 291 | else |
falingtrea | 0:d2169fecf3fb | 292 | _gps_status = false; |
falingtrea | 0:d2169fecf3fb | 293 | // read latitude degrees minutes |
falingtrea | 0:d2169fecf3fb | 294 | token_str = strtok(NULL, "."); |
falingtrea | 0:d2169fecf3fb | 295 | memset(temp_str,0x00,6); |
falingtrea | 0:d2169fecf3fb | 296 | strncpy(temp_str,token_str,2); |
falingtrea | 0:d2169fecf3fb | 297 | _gps_latitude.degrees = atoi(temp_str); |
falingtrea | 0:d2169fecf3fb | 298 | memset(temp_str,0x00,6); |
falingtrea | 0:d2169fecf3fb | 299 | strncpy(temp_str,token_str+2,2); |
falingtrea | 0:d2169fecf3fb | 300 | _gps_latitude.minutes = atoi(temp_str); |
falingtrea | 0:d2169fecf3fb | 301 | // read fractional minutes |
falingtrea | 0:d2169fecf3fb | 302 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 303 | _gps_latitude.seconds = atoi(token_str); |
falingtrea | 0:d2169fecf3fb | 304 | // read latitude hemisphere change sign if 'S' |
falingtrea | 0:d2169fecf3fb | 305 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 306 | if (token_str[0] == 'S') |
falingtrea | 0:d2169fecf3fb | 307 | _gps_latitude.degrees *= -1; |
falingtrea | 0:d2169fecf3fb | 308 | // read longitude degree minutes |
falingtrea | 0:d2169fecf3fb | 309 | token_str = strtok(NULL, "."); |
falingtrea | 0:d2169fecf3fb | 310 | memset(temp_str,0x00,6); |
falingtrea | 0:d2169fecf3fb | 311 | strncpy(temp_str,token_str,3); |
falingtrea | 0:d2169fecf3fb | 312 | _gps_longitude.degrees = atoi(temp_str); |
falingtrea | 0:d2169fecf3fb | 313 | memset(temp_str,0x00,6); |
falingtrea | 0:d2169fecf3fb | 314 | strncpy(temp_str,token_str+3,2); |
falingtrea | 0:d2169fecf3fb | 315 | _gps_longitude.minutes = atoi(temp_str); |
falingtrea | 0:d2169fecf3fb | 316 | // read fractional minutes |
falingtrea | 0:d2169fecf3fb | 317 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 318 | _gps_longitude.seconds = atoi(token_str); |
falingtrea | 0:d2169fecf3fb | 319 | // read longitude hemisphere change sign if 'W' |
falingtrea | 0:d2169fecf3fb | 320 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 321 | if (token_str[0] == 'W') |
falingtrea | 0:d2169fecf3fb | 322 | _gps_longitude.degrees *= -1; |
falingtrea | 0:d2169fecf3fb | 323 | // skip speed and track angle |
falingtrea | 0:d2169fecf3fb | 324 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 325 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 326 | // read date |
falingtrea | 0:d2169fecf3fb | 327 | token_str = strtok(NULL, ","); |
falingtrea | 0:d2169fecf3fb | 328 | memset(temp_str,0x00,6); |
falingtrea | 0:d2169fecf3fb | 329 | strncpy(temp_str,token_str,2); |
falingtrea | 0:d2169fecf3fb | 330 | _timestamp.tm_mday = atoi(temp_str); |
falingtrea | 0:d2169fecf3fb | 331 | memset(temp_str,0x00,6); |
falingtrea | 0:d2169fecf3fb | 332 | strncpy(temp_str,token_str+2,2); |
falingtrea | 0:d2169fecf3fb | 333 | _timestamp.tm_mon = atoi(temp_str) - 1; |
falingtrea | 0:d2169fecf3fb | 334 | memset(temp_str,0x00,6); |
falingtrea | 0:d2169fecf3fb | 335 | strncpy(temp_str,token_str+4,2); |
falingtrea | 0:d2169fecf3fb | 336 | _timestamp.tm_year = atoi(temp_str) + 100; |
falingtrea | 0:d2169fecf3fb | 337 | |
falingtrea | 0:d2169fecf3fb | 338 | return ret; |
falingtrea | 0:d2169fecf3fb | 339 | } |
falingtrea | 0:d2169fecf3fb | 340 | |
falingtrea | 0:d2169fecf3fb | 341 | uint8_t GPSPARSER::parseVTG(char *nema_buf){ |
falingtrea | 0:d2169fecf3fb | 342 | uint8_t ret = 0; |
falingtrea | 0:d2169fecf3fb | 343 | printf("ParseVTG****\r\n"); |
falingtrea | 0:d2169fecf3fb | 344 | printf(nema_buf); |
falingtrea | 0:d2169fecf3fb | 345 | printf("\r\n"); |
falingtrea | 0:d2169fecf3fb | 346 | return ret; |
falingtrea | 0:d2169fecf3fb | 347 | } |
falingtrea | 0:d2169fecf3fb | 348 | |
falingtrea | 0:d2169fecf3fb | 349 | uint8_t GPSPARSER::parseGLL(char *nema_buf){ |
falingtrea | 0:d2169fecf3fb | 350 | uint8_t ret = 0; |
falingtrea | 0:d2169fecf3fb | 351 | printf("ParseGLL*****\r\n"); |
falingtrea | 0:d2169fecf3fb | 352 | printf(nema_buf); |
falingtrea | 0:d2169fecf3fb | 353 | printf("\r\n"); |
falingtrea | 0:d2169fecf3fb | 354 | return ret; |
falingtrea | 0:d2169fecf3fb | 355 | } |
falingtrea | 0:d2169fecf3fb | 356 | |
falingtrea | 0:d2169fecf3fb | 357 | uint8_t GPSPARSER::parseZDA(char *nema_buf){ |
falingtrea | 0:d2169fecf3fb | 358 | uint8_t ret = 0; |
falingtrea | 0:d2169fecf3fb | 359 | printf("ParseZDA******\r\n"); |
falingtrea | 0:d2169fecf3fb | 360 | printf(nema_buf); |
falingtrea | 0:d2169fecf3fb | 361 | printf("\r\n"); |
falingtrea | 0:d2169fecf3fb | 362 | return ret; |
falingtrea | 0:d2169fecf3fb | 363 | } |
falingtrea | 0:d2169fecf3fb | 364 | |
falingtrea | 0:d2169fecf3fb | 365 | bool GPSPARSER::gpsDetected(void){ |
falingtrea | 0:d2169fecf3fb | 366 | return _gps_detected; |
falingtrea | 0:d2169fecf3fb | 367 | } |
falingtrea | 0:d2169fecf3fb | 368 | |
falingtrea | 0:d2169fecf3fb | 369 | GPSPARSER::longitude GPSPARSER::getLongitude(void){ |
falingtrea | 0:d2169fecf3fb | 370 | return _gps_longitude; |
falingtrea | 0:d2169fecf3fb | 371 | } |
falingtrea | 0:d2169fecf3fb | 372 | |
falingtrea | 0:d2169fecf3fb | 373 | GPSPARSER::latitude GPSPARSER::getLatitude(void){ |
falingtrea | 0:d2169fecf3fb | 374 | return _gps_latitude; |
falingtrea | 0:d2169fecf3fb | 375 | } |
falingtrea | 0:d2169fecf3fb | 376 | |
falingtrea | 0:d2169fecf3fb | 377 | struct tm GPSPARSER::getTimestamp(void){ |
falingtrea | 0:d2169fecf3fb | 378 | return _timestamp; |
falingtrea | 0:d2169fecf3fb | 379 | } |
falingtrea | 0:d2169fecf3fb | 380 | |
falingtrea | 0:d2169fecf3fb | 381 | bool GPSPARSER::getLockStatus(void){ |
falingtrea | 0:d2169fecf3fb | 382 | return _gps_status; |
falingtrea | 0:d2169fecf3fb | 383 | } |
falingtrea | 0:d2169fecf3fb | 384 | |
falingtrea | 0:d2169fecf3fb | 385 | uint8_t GPSPARSER::getFixStatus(void){ |
falingtrea | 0:d2169fecf3fb | 386 | return _fix_status; |
falingtrea | 0:d2169fecf3fb | 387 | } |
falingtrea | 0:d2169fecf3fb | 388 | |
falingtrea | 0:d2169fecf3fb | 389 | uint8_t GPSPARSER::getFixQuality(void){ |
falingtrea | 0:d2169fecf3fb | 390 | return _fix_quality; |
falingtrea | 0:d2169fecf3fb | 391 | } |
falingtrea | 0:d2169fecf3fb | 392 | |
falingtrea | 0:d2169fecf3fb | 393 | uint8_t GPSPARSER::getNumSatellites(void){ |
falingtrea | 0:d2169fecf3fb | 394 | return _num_satellites; |
falingtrea | 0:d2169fecf3fb | 395 | } |
falingtrea | 0:d2169fecf3fb | 396 | |
falingtrea | 0:d2169fecf3fb | 397 | int16_t GPSPARSER::getAltitude(void){ |
falingtrea | 0:d2169fecf3fb | 398 | return _msl_altitude; |
falingtrea | 0:d2169fecf3fb | 399 | } |