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
00001 /** 00002 * @file NemaParser.cpp 00003 * @brief NEMA String to Packet Parser - NEMA strings to compact packet data 00004 * @author Tim Barr 00005 * @version 1.01 00006 * @see http://www.kh-gps.de/nmea.faq 00007 * @see http://www.catb.org/gpsd/NMEA.html 00008 * 00009 * Copyright (c) 2015 00010 * 00011 * Licensed under the Apache License, Version 2.0 (the "License"); 00012 * you may not use this file except in compliance with the License. 00013 * You may obtain a copy of the License at 00014 * 00015 * http://www.apache.org/licenses/LICENSE-2.0 00016 * 00017 * Unless required by applicable law or agreed to in writing, software 00018 * distributed under the License is distributed on an "AS IS" BASIS, 00019 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00020 * See the License for the specific language governing permissions and 00021 * limitations under the License. 00022 * 00023 * 09/16/15 TAB V1.01 Changed report rate of GGA and GSA NEMA sentences 00024 * 09/22/15 TAB V1.02 Fixed status value for no GPS detected. Increased report rate 00025 * of GSV, GGA, and GSA NEMA Sentences 00026 * 00027 * TODO: Add speed, compass direction, error (DOP) data. Make init more generic 00028 */ 00029 00030 #include "GPSPARSER.h" 00031 00032 using namespace mts; 00033 00034 GPSPARSER::GPSPARSER(MTSSerial *uart, NCP5623B* led) 00035 : _getSentenceThread(&GPSPARSER::startSentenceThread,this), 00036 _led(led), 00037 _led_state(0), 00038 _led_state_out(0), 00039 _tick_running(false) 00040 { 00041 _gps_uart = uart; 00042 _gps_uart->baud(9600); //set GPS baud rate here 00043 00044 _gps_latitude.degrees = 0; 00045 _gps_longitude.degrees = 0; 00046 _timestamp.tm_sec = 0; 00047 _timestamp.tm_min = 0; 00048 _timestamp.tm_hour = 0; 00049 _timestamp.tm_mday = 0; 00050 _timestamp.tm_mon = 0; 00051 _timestamp.tm_year = 0; 00052 _timestamp.tm_wday = 0; 00053 _timestamp.tm_yday = 0; 00054 _timestamp.tm_isdst = -1; // time is UTC so no Daylight savings time 00055 00056 _gps_status = false; 00057 _fix_status = 1; 00058 _num_satellites =0; 00059 _gps_detected = false; 00060 00061 _getSentenceThread.signal_set(START_THREAD); 00062 00063 return; 00064 } 00065 00066 GPSPARSER::~GPSPARSER(void) 00067 { 00068 if (_gps_detected) 00069 _getSentenceThread.terminate(); 00070 } 00071 00072 void GPSPARSER::startSentenceThread(void const *p) 00073 { 00074 GPSPARSER *instance = (GPSPARSER*)p; 00075 instance->readNemaSentence(); 00076 } 00077 00078 void GPSPARSER::readNemaSentence (void) 00079 { 00080 // this code is specific to the Skytraq Venus GPS chip. This code could be re-written to detect another type of 00081 // GPS device. Maybe read serial port for a specific time and detect $GP from NEMA string 00082 00083 // Sets Skytraq Venus GPS to output GGA,GSA,GSV every 10 seconds, and RMC every second, no ZDA,GLL,VTG 00084 // setup string for GPS GGA GSA GSV GLL RMC VTG ZDA cksum 00085 char init_gps[16] = {0xA0,0xA1,0x00,0x09,0x08,0x0A,0x0A,0x0A,0x00,0x01,0x00,0x00,0x00,0x03,0x0D,0x0A}; 00086 char chk_char; 00087 uint8_t calc_cksum; 00088 uint8_t nema_cksum; 00089 char nema_id[2]; 00090 char nema_str[80]; 00091 char cksum_str[2]; 00092 00093 _getSentenceThread.signal_wait(START_THREAD); 00094 //printf("GPS starting\r\n"); 00095 for(int i = 0; i < 20; i++){ 00096 _gps_uart->rxClear(); 00097 _gps_uart->write(init_gps, sizeof(init_gps)); 00098 while (! _gps_uart->readable()) 00099 osDelay(100); 00100 do { 00101 _gps_uart->read(chk_char); 00102 //printf("read char %#X\r\n", chk_char); 00103 } while ((chk_char != 0xA0) && (!_gps_uart->rxEmpty())); 00104 if (chk_char == 0xA0) { 00105 _gps_uart->read(chk_char); 00106 //printf("read char %#X\r\n", chk_char); 00107 if (chk_char == 0xA1) { 00108 _gps_uart->read(chk_char); 00109 //printf("read char %#X\r\n", chk_char); 00110 _gps_uart->read(chk_char); 00111 //printf("read char %#X\r\n", chk_char); 00112 _gps_uart->read(chk_char); 00113 //printf("read char %#X\r\n", chk_char); 00114 if (chk_char == 0x83) { 00115 _gps_detected = true; 00116 break; 00117 } 00118 } 00119 } 00120 osDelay(50); 00121 } 00122 //printf("GPS detected %s\r\n", _gps_detected ? "true" : "false"); 00123 00124 if (! _gps_detected) { 00125 _fix_status = 0; 00126 return; 00127 } 00128 00129 if (_led) { 00130 _tick.attach(this, &GPSPARSER::blinker, 0.5); 00131 _tick_running = true; 00132 } 00133 00134 do { 00135 if (_gps_uart->readable() > 80) { 00136 do { 00137 _gps_uart->read(chk_char); 00138 } while ((chk_char != '$') && (!_gps_uart->rxEmpty())); 00139 00140 if (chk_char == '$') { 00141 _gps_uart->read(nema_id,2); 00142 if (strpbrk(nema_id,"GP") != NULL) { 00143 uint8_t i = 0; 00144 calc_cksum = 0x17; // 8 bit XOR of G and P checksum seed 00145 nema_cksum = 0; // initialize nema string checksum 00146 memset(nema_str,0x00,80); // clear nema_str array 00147 do { 00148 _gps_uart->read(chk_char); 00149 if ((chk_char != 0x0D) && (chk_char != '*')) { 00150 nema_str[i++] = chk_char; 00151 calc_cksum ^= chk_char; // 8 bit XOR checksum 00152 } 00153 if (chk_char == '*') { 00154 _gps_uart->read(cksum_str,2); 00155 nema_cksum = (uint8_t)strtoul(cksum_str,NULL,16); 00156 } 00157 } while (( chk_char != 0x0D) && !_gps_uart->rxEmpty()); 00158 00159 if (nema_cksum == calc_cksum) { 00160 if (strncmp (nema_str,"GGA",3) == 0) 00161 parseGGA(nema_str); 00162 else if (strncmp (nema_str,"GSA",3) == 0) 00163 parseGSA(nema_str); 00164 else if (strncmp (nema_str,"GSV",3) == 0) 00165 parseGSV(nema_str); 00166 else if (strncmp (nema_str,"GLL",3) == 0) 00167 parseGLL(nema_str); 00168 else if (strncmp (nema_str,"RMC",3) == 0) 00169 parseRMC(nema_str); 00170 else if (strncmp (nema_str,"VTG",3) == 0) 00171 parseVTG(nema_str); 00172 else if (strncmp (nema_str,"ZDA",3) == 0) 00173 parseZDA(nema_str); 00174 else 00175 printf("Unknown NEMA String Type\r\n"); 00176 } else { 00177 printf("NEMA String checksum error %x != %x\r\n",nema_cksum,calc_cksum); 00178 } 00179 } 00180 } else 00181 printf("RX empty before all data read\r\n"); 00182 } 00183 00184 if (_led) { 00185 if (_fix_status >= 2) { 00186 if (_tick_running) { 00187 _tick.detach(); 00188 _tick_running = false; 00189 } 00190 _led->setPWM(NCP5623B::LED_3, 8); 00191 } else { 00192 if (! _tick_running) { 00193 _tick.attach(this, &GPSPARSER::blinker, 0.5); 00194 _tick_running = true; 00195 } else { 00196 if (_led_state_out != _led_state) 00197 { 00198 _led_state_out = _led_state; 00199 _led->setPWM(NCP5623B::LED_3, _led_state_out); 00200 } 00201 } 00202 } 00203 } 00204 00205 osDelay(100); 00206 00207 } while(true); 00208 00209 } 00210 00211 uint8_t GPSPARSER::parseGGA(char *nema_buf) 00212 { 00213 char* token_str; 00214 uint8_t ret = 0; 00215 00216 _mutex.lock(); 00217 00218 token_str = strtok(nema_buf, ","); 00219 // skip timestamp 00220 token_str = strtok(NULL, ","); 00221 // skip latitude degrees minutes 00222 token_str = strtok(NULL, ","); 00223 token_str = strtok(NULL, ","); 00224 // skip longitude degree minutes 00225 token_str = strtok(NULL, ","); 00226 token_str = strtok(NULL, ","); 00227 // read fix quality 00228 token_str = strtok(NULL, ","); 00229 _fix_quality = atoi(token_str); 00230 // skip number of satellites and horizontal dilution 00231 token_str = strtok(NULL, ","); 00232 token_str = strtok(NULL, ","); 00233 // read msl altitude in meters 00234 token_str = strtok(NULL, ","); 00235 _msl_altitude = atoi(token_str); 00236 00237 _mutex.unlock(); 00238 00239 return ret; 00240 } 00241 00242 uint8_t GPSPARSER::parseGSA(char *nema_buf) 00243 { 00244 char* token_str; 00245 uint8_t ret = 0; 00246 00247 _mutex.lock(); 00248 00249 token_str = strtok(nema_buf, ","); 00250 token_str = strtok(NULL, ","); 00251 // read fix status 00252 token_str = strtok(NULL, ","); 00253 _fix_status = atoi(token_str); 00254 // read satellite PRNs 00255 for (uint8_t i=0; i<12; i++) { 00256 token_str = strtok(NULL, ","); 00257 _satellite_prn[i] = atoi(token_str); 00258 } 00259 00260 _mutex.unlock(); 00261 00262 return ret; 00263 } 00264 00265 uint8_t GPSPARSER::parseGSV(char *nema_buf) 00266 { 00267 char* token_str; 00268 uint8_t ret = 0; 00269 00270 _mutex.lock(); 00271 00272 token_str = strtok(nema_buf, ","); 00273 // skip number of sentences and sentence number for now 00274 token_str = strtok(NULL, ","); 00275 token_str = strtok(NULL, ","); 00276 // read Number of satellites 00277 token_str = strtok(NULL, ","); 00278 _num_satellites = atoi(token_str); 00279 // add code to read satellite specs if needed 00280 00281 _mutex.unlock(); 00282 00283 return ret; 00284 } 00285 00286 uint8_t GPSPARSER::parseRMC(char *nema_buf) 00287 { 00288 char* token_str; 00289 char temp_str[6]; 00290 uint8_t ret = 0; 00291 00292 _mutex.lock(); 00293 00294 token_str = strtok(nema_buf, ","); 00295 // read timestamp 00296 token_str = strtok(NULL, ","); 00297 strncpy(temp_str,token_str,2); 00298 _timestamp.tm_hour = atoi(temp_str); 00299 memset(temp_str,0x00,6); 00300 strncpy(temp_str,token_str+2,2); 00301 _timestamp.tm_min = atoi(temp_str); 00302 memset(temp_str,0x00,6); 00303 strncpy(temp_str,token_str+4,2); 00304 _timestamp.tm_sec = atoi(temp_str); 00305 // set gps_status true = active 00306 token_str = strtok(NULL, ","); 00307 memset(temp_str,0x00,6); 00308 strncpy(temp_str,token_str,1); 00309 if (temp_str[0] == 'A') 00310 _gps_status = true; 00311 else 00312 _gps_status = false; 00313 // read latitude degrees minutes 00314 token_str = strtok(NULL, "."); 00315 memset(temp_str,0x00,6); 00316 strncpy(temp_str,token_str,2); 00317 _gps_latitude.degrees = atoi(temp_str); 00318 memset(temp_str,0x00,6); 00319 strncpy(temp_str,token_str+2,2); 00320 _gps_latitude.minutes = atoi(temp_str); 00321 // read fractional minutes 00322 token_str = strtok(NULL, ","); 00323 _gps_latitude.seconds = atoi(token_str); 00324 // read latitude hemisphere change sign if 'S' 00325 token_str = strtok(NULL, ","); 00326 if (token_str[0] == 'S') 00327 _gps_latitude.degrees *= -1; 00328 // read longitude degree minutes 00329 token_str = strtok(NULL, "."); 00330 memset(temp_str,0x00,6); 00331 strncpy(temp_str,token_str,3); 00332 _gps_longitude.degrees = atoi(temp_str); 00333 memset(temp_str,0x00,6); 00334 strncpy(temp_str,token_str+3,2); 00335 _gps_longitude.minutes = atoi(temp_str); 00336 // read fractional minutes 00337 token_str = strtok(NULL, ","); 00338 _gps_longitude.seconds = atoi(token_str); 00339 // read longitude hemisphere change sign if 'W' 00340 token_str = strtok(NULL, ","); 00341 if (token_str[0] == 'W') 00342 _gps_longitude.degrees *= -1; 00343 // skip speed and track angle 00344 token_str = strtok(NULL, ","); 00345 token_str = strtok(NULL, ","); 00346 // read date 00347 token_str = strtok(NULL, ","); 00348 memset(temp_str,0x00,6); 00349 strncpy(temp_str,token_str,2); 00350 _timestamp.tm_mday = atoi(temp_str); 00351 memset(temp_str,0x00,6); 00352 strncpy(temp_str,token_str+2,2); 00353 _timestamp.tm_mon = atoi(temp_str) - 1; 00354 memset(temp_str,0x00,6); 00355 strncpy(temp_str,token_str+4,2); 00356 _timestamp.tm_year = atoi(temp_str) + 100; 00357 00358 _mutex.unlock(); 00359 00360 return ret; 00361 } 00362 00363 uint8_t GPSPARSER::parseVTG(char *nema_buf) 00364 { 00365 uint8_t ret = 0; 00366 //printf("ParseVTG****\r\n"); 00367 //printf(nema_buf); 00368 //printf("\r\n"); 00369 return ret; 00370 } 00371 00372 uint8_t GPSPARSER::parseGLL(char *nema_buf) 00373 { 00374 uint8_t ret = 0; 00375 //printf("ParseGLL*****\r\n"); 00376 //printf(nema_buf); 00377 //printf("\r\n"); 00378 return ret; 00379 } 00380 00381 uint8_t GPSPARSER::parseZDA(char *nema_buf) 00382 { 00383 uint8_t ret = 0; 00384 //printf("ParseZDA******\r\n"); 00385 //printf(nema_buf); 00386 //printf("\r\n"); 00387 return ret; 00388 } 00389 00390 bool GPSPARSER::gpsDetected(void) 00391 { 00392 bool detected; 00393 00394 _mutex.lock(); 00395 detected = _gps_detected; 00396 _mutex.unlock(); 00397 00398 return detected; 00399 } 00400 00401 GPSPARSER::longitude GPSPARSER::getLongitude(void) 00402 { 00403 longitude lon; 00404 00405 _mutex.lock(); 00406 lon = _gps_longitude; 00407 _mutex.unlock(); 00408 00409 return lon; 00410 } 00411 00412 GPSPARSER::latitude GPSPARSER::getLatitude(void) 00413 { 00414 latitude lat; 00415 00416 _mutex.lock(); 00417 lat = _gps_latitude; 00418 _mutex.unlock(); 00419 00420 return lat; 00421 } 00422 00423 struct tm GPSPARSER::getTimestamp(void) { 00424 struct tm time; 00425 00426 _mutex.lock(); 00427 time = _timestamp; 00428 _mutex.unlock(); 00429 00430 return time; 00431 } 00432 00433 bool GPSPARSER::getLockStatus(void) 00434 { 00435 bool status; 00436 00437 _mutex.lock(); 00438 status = _gps_status; 00439 _mutex.unlock(); 00440 00441 return status; 00442 } 00443 00444 uint8_t GPSPARSER::getFixStatus(void) 00445 { 00446 uint8_t fix; 00447 00448 _mutex.lock(); 00449 fix = _fix_status; 00450 _mutex.unlock(); 00451 00452 return fix; 00453 } 00454 00455 uint8_t GPSPARSER::getFixQuality(void) 00456 { 00457 uint8_t fix; 00458 00459 _mutex.lock(); 00460 fix = _fix_quality; 00461 _mutex.unlock(); 00462 00463 return fix; 00464 } 00465 00466 uint8_t GPSPARSER::getNumSatellites(void) 00467 { 00468 uint8_t sats; 00469 00470 _mutex.lock(); 00471 sats = _num_satellites; 00472 _mutex.unlock(); 00473 00474 return sats; 00475 } 00476 00477 int16_t GPSPARSER::getAltitude(void) 00478 { 00479 int16_t alt; 00480 00481 _mutex.lock(); 00482 alt = _msl_altitude; 00483 _mutex.unlock(); 00484 00485 return alt; 00486 } 00487 00488 void GPSPARSER::blinker() { 00489 _led_state = (_led_state == 0) ? 8 : 0; 00490 } 00491
Generated on Thu Jul 14 2022 20:56:11 by 1.7.2