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