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
Diff: GPSPARSER.cpp
- Revision:
- 0:d2169fecf3fb
- Child:
- 1:58c50bb3c60c
diff -r 000000000000 -r d2169fecf3fb GPSPARSER.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GPSPARSER.cpp Thu Sep 10 20:03:21 2015 +0000 @@ -0,0 +1,397 @@ +/** + * @file NemaParser.cpp + * @brief NEMA String to Packet Parser - NEMA strings to compact packet data + * @author Tim Barr + * @version 1.0 + * @see http://www.kh-gps.de/nmea.faq + * @see http://www.catb.org/gpsd/NMEA.html + * + * Copyright (c) 2015 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * TODO: Add speed, compass direction, error (DOP) data. Make init more generic + */ + +#include "GPSPARSER.h" + +using namespace mts; + +GPSPARSER::GPSPARSER(MTSSerial *uart) + : _getSentenceThread(&GPSPARSER::startSentenceThread,this) +{ + _gps_uart = uart; + _gps_uart->baud(9600); //set GPS baud rate here + + _gps_latitude.degrees = 0; + _gps_longitude.degrees = 0; + _timestamp.tm_sec = 0; + _timestamp.tm_min = 0; + _timestamp.tm_hour = 0; + _timestamp.tm_mday = 0; + _timestamp.tm_mon = 0; + _timestamp.tm_year = 0; + _timestamp.tm_wday = 0; + _timestamp.tm_yday = 0; + _timestamp.tm_isdst = -1; // time is UTC so no Daylight savings time + + _gps_status = false; + _fix_status = 0; + _num_satellites =0; + _gps_detected = false; + + if (GPSPARSER::initGps() == 0){ + _gps_detected = true; + _getSentenceThread.signal_set(START_THREAD); + printf("Started Nema Sentence Thread\r\n"); + } + else + printf("No GPS detected"); + + return; +} + +GPSPARSER::~GPSPARSER(void){ + if (_gps_detected) + _getSentenceThread.terminate(); +} + +uint8_t GPSPARSER::initGps (void){ + + // this code is specific to the Skytraq Venus GPS chip. This code could be re-written to detect another type of + // GPS device. Maybe read serial port for a specific time and detect $GP from NEMA string + + // Sets Skytraq Venus GPS to output GGA,GSA every 240 seconds, GSV every 30 seconds, and RMC every second, no ZDA,GLL,VTG + // setup string for GPS GGA GSA GSV GLL RMC VTG ZDA cksum + char init_gps[16] = {0xA0,0xA1,0x00,0x09,0x08,0xF0,0xF0,0x1E,0x00,0x01,0x00,0x00,0x00,0x17,0x0D,0x0A}; + char chk_char; + uint8_t ret = 0; + + _gps_uart->rxClear(); + printf("Starting initGPS \r\n"); + + _gps_uart->write(init_gps,16); + printf("wrote control data\r\n"); + + do { + osDelay(10); + } while (!_gps_uart->txEmpty()); + + osDelay(15); + + if (_gps_uart->readable()){ + do{ + _gps_uart->read(chk_char,20); + } while ((chk_char != 0xA0) && (!_gps_uart->rxEmpty())); + + printf ("found 0xA0 or rx empty\r\n"); + + if (chk_char == 0xA0){ + _gps_uart->read(chk_char,15); + if (chk_char != 0xA1) { + printf( "initGPS failed no message\r\n"); + ret = 1; + } + else { + printf("message received\r\n"); + _gps_uart->read(chk_char); + _gps_uart->read(chk_char); + printf("message size - %x\r\n",chk_char); + _gps_uart->read(chk_char); + if (chk_char != 0x83){ + printf("initGPS failed not ack message\r\n"); + ret = 1; + } + else { + printf("ACK message received\r\n"); + _gps_uart->read(chk_char); + printf("config message acknowledged - ID - %x\r\n",chk_char); + } + } + } + else { + printf("rx empty \r\n"); + ret = 1; + } + } + else { + printf("No readable characters\r\n"); + ret = 1; + } + + printf("initGps done\r\n"); + return ret; +} + +void GPSPARSER::startSentenceThread(void const *p){ + GPSPARSER *instance = (GPSPARSER*)p; + instance->readNemaSentence(); +} + +void GPSPARSER::readNemaSentence (void){ + char chk_char; + uint8_t calc_cksum; + uint8_t nema_cksum; + char nema_id[2]; + char nema_str[80]; + char cksum_str[2]; + + _getSentenceThread.signal_wait(START_THREAD); + printf("Got thread start\r\n"); + + do { + if (_gps_uart->readable() > 80){ + do{ + _gps_uart->read(chk_char); + } while ((chk_char != '$') && (!_gps_uart->rxEmpty())); + + if (chk_char == '$') { + _gps_uart->read(nema_id,2); + if (strpbrk(nema_id,"GP") != NULL){ + uint8_t i = 0; + calc_cksum = 0x17; // 8 bit XOR of G and P checksum seed + nema_cksum = 0; // initialize nema string checksum + memset(nema_str,0x00,80); // clear nema_str array + do { + _gps_uart->read(chk_char); + if ((chk_char != 0x0D) && (chk_char != '*')){ + nema_str[i++] = chk_char; + calc_cksum ^= chk_char; // 8 bit XOR checksum + } + if (chk_char == '*') { + _gps_uart->read(cksum_str,2); + nema_cksum = (uint8_t)strtoul(cksum_str,NULL,16); + } + } while (( chk_char != 0x0D) && !_gps_uart->rxEmpty()); + + if (nema_cksum == calc_cksum) + if (strncmp (nema_str,"GGA",3) == 0) + parseGGA(nema_str); + else if (strncmp (nema_str,"GSA",3) == 0) + parseGSA(nema_str); + else if (strncmp (nema_str,"GSV",3) == 0) + parseGSV(nema_str); + else if (strncmp (nema_str,"GLL",3) == 0) + parseGLL(nema_str); + else if (strncmp (nema_str,"RMC",3) == 0) + parseRMC(nema_str); + else if (strncmp (nema_str,"VTG",3) == 0) + parseVTG(nema_str); + else if (strncmp (nema_str,"ZDA",3) == 0) + parseZDA(nema_str); + else + printf("Unknown NEMA String Type\r\n"); + else + printf("NEMA String checksum error %x != %x\r\n",nema_cksum,calc_cksum); + } + } + else + printf("RX empty before all data read\r\n"); + } + + osDelay(100); + + } while(true); + +} + +uint8_t GPSPARSER::parseGGA(char *nema_buf){ + char* token_str; + uint8_t ret = 0; + + token_str = strtok(nema_buf, ","); +// skip timestamp + token_str = strtok(NULL, ","); +// skip latitude degrees minutes + token_str = strtok(NULL, ","); + token_str = strtok(NULL, ","); +// skip longitude degree minutes + token_str = strtok(NULL, ","); + token_str = strtok(NULL, ","); +// read fix quality + token_str = strtok(NULL, ","); + _fix_quality = atoi(token_str); +// skip number of satellites and horizontal dilution + token_str = strtok(NULL, ","); + token_str = strtok(NULL, ","); +// read msl altitude in meters + token_str = strtok(NULL, ","); + _msl_altitude = atoi(token_str); + + return ret; +} + +uint8_t GPSPARSER::parseGSA(char *nema_buf){ + char* token_str; + uint8_t ret = 0; + + token_str = strtok(nema_buf, ","); + token_str = strtok(NULL, ","); +// read fix status + token_str = strtok(NULL, ","); + _fix_status = atoi(token_str); +// read satellite PRNs + for (uint8_t i=0;i<12;i++){ + token_str = strtok(NULL, ","); + _satellite_prn[i] = atoi(token_str); + } + return ret; +} + +uint8_t GPSPARSER::parseGSV(char *nema_buf){ + char* token_str; + uint8_t ret = 0; + + token_str = strtok(nema_buf, ","); +// skip number of sentences and sentence number for now + token_str = strtok(NULL, ","); + token_str = strtok(NULL, ","); +// read Number of satellites + token_str = strtok(NULL, ","); + _num_satellites = atoi(token_str); +// add code to read satellite specs if needed + + return ret; +} + +uint8_t GPSPARSER::parseRMC(char *nema_buf){ + char* token_str; + char temp_str[6]; + uint8_t ret = 0; + + token_str = strtok(nema_buf, ","); +// read timestamp + token_str = strtok(NULL, ","); + strncpy(temp_str,token_str,2); + _timestamp.tm_hour = atoi(temp_str); + memset(temp_str,0x00,6); + strncpy(temp_str,token_str+2,2); + _timestamp.tm_min = atoi(temp_str); + memset(temp_str,0x00,6); + strncpy(temp_str,token_str+4,2); + _timestamp.tm_sec = atoi(temp_str); +// set gps_status true = active + token_str = strtok(NULL, ","); + memset(temp_str,0x00,6); + strncpy(temp_str,token_str,1); + if (temp_str[0] == 'A') + _gps_status = true; + else + _gps_status = false; +// read latitude degrees minutes + token_str = strtok(NULL, "."); + memset(temp_str,0x00,6); + strncpy(temp_str,token_str,2); + _gps_latitude.degrees = atoi(temp_str); + memset(temp_str,0x00,6); + strncpy(temp_str,token_str+2,2); + _gps_latitude.minutes = atoi(temp_str); +// read fractional minutes + token_str = strtok(NULL, ","); + _gps_latitude.seconds = atoi(token_str); +// read latitude hemisphere change sign if 'S' + token_str = strtok(NULL, ","); + if (token_str[0] == 'S') + _gps_latitude.degrees *= -1; +// read longitude degree minutes + token_str = strtok(NULL, "."); + memset(temp_str,0x00,6); + strncpy(temp_str,token_str,3); + _gps_longitude.degrees = atoi(temp_str); + memset(temp_str,0x00,6); + strncpy(temp_str,token_str+3,2); + _gps_longitude.minutes = atoi(temp_str); +// read fractional minutes + token_str = strtok(NULL, ","); + _gps_longitude.seconds = atoi(token_str); +// read longitude hemisphere change sign if 'W' + token_str = strtok(NULL, ","); + if (token_str[0] == 'W') + _gps_longitude.degrees *= -1; +// skip speed and track angle + token_str = strtok(NULL, ","); + token_str = strtok(NULL, ","); +// read date + token_str = strtok(NULL, ","); + memset(temp_str,0x00,6); + strncpy(temp_str,token_str,2); + _timestamp.tm_mday = atoi(temp_str); + memset(temp_str,0x00,6); + strncpy(temp_str,token_str+2,2); + _timestamp.tm_mon = atoi(temp_str) - 1; + memset(temp_str,0x00,6); + strncpy(temp_str,token_str+4,2); + _timestamp.tm_year = atoi(temp_str) + 100; + + return ret; +} + +uint8_t GPSPARSER::parseVTG(char *nema_buf){ + uint8_t ret = 0; + printf("ParseVTG****\r\n"); + printf(nema_buf); + printf("\r\n"); + return ret; +} + +uint8_t GPSPARSER::parseGLL(char *nema_buf){ + uint8_t ret = 0; + printf("ParseGLL*****\r\n"); + printf(nema_buf); + printf("\r\n"); + return ret; +} + +uint8_t GPSPARSER::parseZDA(char *nema_buf){ + uint8_t ret = 0; + printf("ParseZDA******\r\n"); + printf(nema_buf); + printf("\r\n"); + return ret; +} + +bool GPSPARSER::gpsDetected(void){ + return _gps_detected; +} + +GPSPARSER::longitude GPSPARSER::getLongitude(void){ + return _gps_longitude; +} + +GPSPARSER::latitude GPSPARSER::getLatitude(void){ + return _gps_latitude; +} + +struct tm GPSPARSER::getTimestamp(void){ + return _timestamp; +} + +bool GPSPARSER::getLockStatus(void){ + return _gps_status; +} + +uint8_t GPSPARSER::getFixStatus(void){ + return _fix_status; +} + +uint8_t GPSPARSER::getFixQuality(void){ + return _fix_quality; +} + +uint8_t GPSPARSER::getNumSatellites(void){ + return _num_satellites; +} + +int16_t GPSPARSER::getAltitude(void){ + return _msl_altitude; +}