Revision 0:d2169fecf3fb, committed 2015-09-10

Comitter:

falingtrea

Date:

Thu Sep 10 20:03:21 2015 +0000

Child:

1:58c50bb3c60c

Commit message:

Initial release. Only supports SkyTraq Venus GPS chip

Changed in this revision

--- /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;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GPSPARSER.h	Thu Sep 10 20:03:21 2015 +0000
@@ -0,0 +1,204 @@
+/**
+ * @file    GpsParser.h
+ * @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
+ * @see     http://www.skytraq.com.tw/products/Venus638LPx_PB_v3.pdf
+ *
+ * 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.
+ *
+ */
+
+#ifndef GPSPARSER_H
+#define GPSPARSER_H
+
+#include "mbed.h"
+#include "rtos.h"
+#include "MTSSerial.h"
+#include <string>
+#include <vector>
+#include <ctime>
+#define START_THREAD 1
+
+using namespace mts;
+
+class GPSPARSER
+{
+public:
+
+    /**
+     * @typedef struct latitude
+     * @brief latitude degrees, minutes, seconds structure
+     * range of degrees : 0 to 90 signed (negative degrees is South)
+     * range of minutes : 0 to 60 unsigned
+     * Range of seconds : 0-9999 unsigned or 1/10000 of a minute.
+     * Multiply by 6 and divide or modulus by 1000 to get seconds and fractional seconds
+     */
+    typedef struct{int8_t degrees; uint8_t minutes; uint16_t seconds;} latitude;
+    
+    /**
+     * @typedef struct longitude
+     * @brief longitude degrees, minutes, seconds structure negative degrees is West
+     * range of degrees : 0 to 180 signed (negative degrees is West)
+     * range of minutes : 0 to 60 unsigned
+     * Range of seconds : 0-9999 unsigned or 1/10000 of a minute.
+     * Multiply by 6 and divide or modulus by 1000 to get seconds and fractional seconds
+     */
+    typedef struct{int16_t degrees; uint8_t minutes; uint16_t seconds;} longitude;
+    
+    /**
+     * @typdef satellite_prn
+     * @brief satellite prn list, up to 12 valid
+     */
+    typedef uint8_t satellite_prn[12];
+
+    /** Create the GPSPARSER object
+     *  @param uart - an MTSSerial object
+     */ 
+    GPSPARSER(MTSSerial *uart);
+
+    /** destroy the GPSPARSER object
+     */ 
+    ~GPSPARSER(void);
+    
+    /** GPS device detected
+     *  @return boolean whether GPS device detected
+     */
+    bool gpsDetected(void);
+
+    /** get longitude structure
+     *  @return last valid NEMA string longitude data
+     */
+    longitude getLongitude(void);
+
+    /** get latitude structure
+     *  @return last valid NEMA string latitude data
+     */
+    latitude getLatitude(void);
+
+    /** get timestamp structure
+     *  @return last valid NEMA string time and date data
+     *  uses tm struc from ctime standard library
+     */
+    struct tm getTimestamp(void);
+
+    /** get GPS Lock status
+     *  @return boolean of last valid NEMA string lock status data
+     */
+    bool getLockStatus(void);
+
+    /** get GPS Fix status
+     *  @return last valid NEMA Fix status data
+     *  1 = no fix
+     *  2 = 2D fix - latitude, longitude, time only valid
+     *  3 = 3D fix - all data valid
+     */
+    uint8_t getFixStatus(void);
+
+    /** get GPS Fix qualty
+     *  @return last valid NEMA string Fix Quality data
+     *  0 = invalid
+     *  1 = GPS fix (SPS)
+     *  2 = DGPS fix
+     *  3 = PPS fix
+     *  4 = Real Time Kinematic
+     *  5 = Float RTK
+     *  6 = estimated (dead reckoning) (2.3 feature)
+     *  7 = Manual input mode
+     *  8 = Simulation mode
+     */
+    uint8_t getFixQuality(void);
+
+    /** get number of visible satellites
+     *  @return last valid NEMA string number of satellites visible
+     */
+    uint8_t getNumSatellites(void);
+
+    /** get calculated altitude
+     *  @return last valid NEMA string altitude data in meters 
+     */
+    int16_t getAltitude(void);
+
+private:
+
+    std::string _nema_buff;
+    bool _gps_detected;
+    latitude _gps_latitude;
+    longitude _gps_longitude;
+    struct tm _timestamp;
+    bool _gps_status;
+    uint8_t _fix_status;
+    uint8_t _fix_quality;
+    uint8_t _num_satellites;
+    satellite_prn _satellite_prn;
+    int16_t _msl_altitude;
+    
+    MTSSerial::MTSSerial *_gps_uart;
+    Thread _getSentenceThread;
+    
+    /** Detect and initialize GPS device
+     *  @return status of initialization
+     *  @note rewrite this code for specific GPS device. This code supports Skytraq Venus chip
+     */
+    uint8_t initGps (void);
+    
+    /** Start sentence parser thread
+     *  
+     */
+    static void startSentenceThread (void const *p);
+
+    /** Read NEMA sentences from specified serial port and call specific sentence parser
+     */
+    void readNemaSentence (void);
+
+    /** Parse GGA NEMA sentence
+     *  @return status of parser attempt
+     */
+    uint8_t parseGGA(char *nema_buf);
+
+    /** Parse GSA NEMA sentence
+     *  @return status of parser attempt
+     */
+    uint8_t parseGSA(char *nema_buf);
+
+    /** Parse GSV NEMA sentence
+     *  @return status of parser attempt
+     */
+    uint8_t parseGSV(char *nema_buf);
+
+    /** Parse RMC NEMA sentence
+     *  @return status of parser attempt
+     */
+    uint8_t parseRMC(char *nema_buf);
+
+    /** Parse VTG NEMA sentence
+     *  @return status of parser attempt
+     */
+    uint8_t parseVTG(char *nema_buf);
+
+    /** Parse GLL NEMA sentence
+     *  @return status of parser attempt
+     */
+    uint8_t parseGLL(char *nema_buff);
+
+    /** Parse ZDA NEMA sentence
+     *  @return status of parser attempt
+     */
+    uint8_t parseZDA(char *nema_buff);
+
+};
+#endif
\ No newline at end of file