File content as of revision 10:465f9db415ea:

/**
 * @file    NemaParser.cpp
 * @brief   NEMA String to Packet Parser - NEMA strings to compact packet data
 * @author  Tim Barr
 * @version 1.01
 * @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.
 *
 * 09/16/15 TAB V1.01 Changed report rate of GGA and GSA NEMA sentences
 * 09/22/15 TAB V1.02 Fixed status value for no GPS detected. Increased report rate
 *              of GSV, GGA, and GSA NEMA Sentences
 *
 * TODO: Add speed, compass direction, error (DOP) data. Make init more generic
 */

#include "GPSPARSER.h"

using namespace mts;

GPSPARSER::GPSPARSER(MTSSerial *uart, NCP5623B* led)
    : _getSentenceThread(&GPSPARSER::startSentenceThread,this),
      _led(led),
      _led_state(0),
      _tick_running(false)
{
    _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 = 1;
    _num_satellites =0;
    _gps_detected = false;

    _getSentenceThread.signal_set(START_THREAD);

    return;
}

GPSPARSER::~GPSPARSER(void)
{
    if (_gps_detected)
        _getSentenceThread.terminate();
}

void GPSPARSER::startSentenceThread(void const *p)
{
    GPSPARSER *instance = (GPSPARSER*)p;
    instance->readNemaSentence();
}

void GPSPARSER::readNemaSentence (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,GSV every 10 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,0x0A,0x0A,0x0A,0x00,0x01,0x00,0x00,0x00,0x03,0x0D,0x0A};
    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("GPS starting\r\n");

    _gps_uart->rxClear();
    _gps_uart->write(init_gps, sizeof(init_gps));
    while (! _gps_uart->readable())
        osDelay(100);
    do {
        _gps_uart->read(chk_char);
        //printf("read char %#X\r\n", chk_char);
    } while ((chk_char != 0xA0) && (!_gps_uart->rxEmpty()));
    if (chk_char == 0xA0) {
        _gps_uart->read(chk_char);
        //printf("read char %#X\r\n", chk_char);
        if (chk_char == 0xA1) {
            _gps_uart->read(chk_char);
            //printf("read char %#X\r\n", chk_char);
            _gps_uart->read(chk_char);
            //printf("read char %#X\r\n", chk_char);
            _gps_uart->read(chk_char);
            //printf("read char %#X\r\n", chk_char);
            if (chk_char == 0x83) {
                _gps_detected = true;
            }
        }
    }

    //printf("GPS detected %s\r\n", _gps_detected ? "true" : "false");

    if (! _gps_detected) {
        _fix_status = 0;
        return;
    }

    if (_led) {
        _tick.attach(this, &GPSPARSER::blinker, 0.5);
        _tick_running = true;
    }

    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");
        }

        if (_led) {
            if (_fix_status >= 2) {
                if (_tick_running) {
                    _tick.detach();
                    _tick_running = false;
                }
                _led->setPWM(NCP5623B::LED_3, 8);
            } else {
                if (! _tick_running) {
                    _tick.attach(this, &GPSPARSER::blinker, 0.5);
                    _tick_running = true;
                }
            }
        }

        osDelay(100);

    } while(true);

}

uint8_t GPSPARSER::parseGGA(char *nema_buf)
{
    char* token_str;
    uint8_t ret = 0;

    _mutex.lock();

    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);

    _mutex.unlock();

    return ret;
}

uint8_t GPSPARSER::parseGSA(char *nema_buf)
{
    char* token_str;
    uint8_t ret = 0;

    _mutex.lock();

    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);
    }

    _mutex.unlock();

    return ret;
}

uint8_t GPSPARSER::parseGSV(char *nema_buf)
{
    char* token_str;
    uint8_t ret = 0;

    _mutex.lock();

    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
    
    _mutex.unlock();

    return ret;
}

uint8_t GPSPARSER::parseRMC(char *nema_buf)
{
    char* token_str;
    char temp_str[6];
    uint8_t ret = 0;

    _mutex.lock();

    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;

    _mutex.unlock();

    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)
{
    bool detected;

    _mutex.lock();
    detected =  _gps_detected;
    _mutex.unlock();

    return detected;
}

GPSPARSER::longitude GPSPARSER::getLongitude(void)
{
    longitude lon;

    _mutex.lock();
    lon = _gps_longitude;
    _mutex.unlock();

    return lon;
}

GPSPARSER::latitude GPSPARSER::getLatitude(void)
{
    latitude lat;

    _mutex.lock();
    lat = _gps_latitude;
    _mutex.unlock();

    return lat;
}

struct tm GPSPARSER::getTimestamp(void) {
    struct tm time;

    _mutex.lock();
    time = _timestamp;
    _mutex.unlock();

    return time;
}

bool GPSPARSER::getLockStatus(void)
{
    bool status;

    _mutex.lock();
    status = _gps_status;
    _mutex.unlock();

    return status;
}

uint8_t GPSPARSER::getFixStatus(void)
{
    uint8_t fix;

    _mutex.lock();
    fix = _fix_status;
    _mutex.unlock();

    return fix;
}

uint8_t GPSPARSER::getFixQuality(void)
{
    uint8_t fix;

    _mutex.lock();
    fix = _fix_quality;
    _mutex.unlock();

    return fix;
}

uint8_t GPSPARSER::getNumSatellites(void)
{
    uint8_t sats;

    _mutex.lock();
    sats = _num_satellites;
    _mutex.unlock();

    return sats;
}

int16_t GPSPARSER::getAltitude(void)
{
    int16_t alt;

    _mutex.lock();
    alt = _msl_altitude;
    _mutex.unlock();

    return alt;
}

void GPSPARSER::blinker() {
    _led_state = (_led_state == 0) ? 8 : 0;
    _led->setPWM(NCP5623B::LED_3, _led_state);
}