nmea.cpp

Go to the documentation of this file.

/**
 * @date Dec 20, 2013
 *
 * @author Andrey Belomutskiy, (c) 2012-2020
 * @author Kot_dnz
 *
 * This file is part of rusEfi - see http://rusefi.com
 *
 * rusEfi is free software; you can redistribute it and/or modify it under the terms of
 * the GNU General Public License as published by the Free Software Foundation; either
 * version 3 of the License, or (at your option) any later version.
 *
 * rusEfi is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without
 * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along with this program.
 * If not, see <http://www.gnu.org/licenses/>.
 *
 * see #testGpsParser
 */
#include "pch.h"
#include "nmea.h"
#include "rtc_helper.h"
 
using namespace rusefi::stringutil;
 
static long hex2int(const char * a, const int len) {
    int i;
    long val = 0;
 
    for (i = 0; i < len; i++)
        if (a[i] <= 57)
            val += (a[i] - 48) * (1 << (4 * (len - 1 - i))); // it's decimal number
        else
            val += (a[i] - 87) * (1 << (4 * (len - 1 - i))); // it's a-f -> work only with low case hex
    return val;
}
 
static float gps_deg_dec(float deg_point) {
    float ddeg;
    float sec = modff(deg_point, &ddeg) * 60;
    int deg = (int) (ddeg / 100);
    int min = (int) (deg_point - (deg * 100));
 
    float absdlat = round(deg * 1000000.);
    float absmlat = round(min * 1000000.);
    float absslat = round(sec * 1000000.);
 
    return round(absdlat + (absmlat / 60) + (absslat / 3600)) / 1000000;
}
 
// Convert lat e lon to decimals (from deg)
static void gps_convert_deg_to_dec(float *latitude, char ns, float *longitude, char we) {
    float lat = (ns == 'N') ? *latitude : -1 * (*latitude);
    float lon = (we == 'E') ? *longitude : -1 * (*longitude);
 
    *latitude = gps_deg_dec(lat);
    *longitude = gps_deg_dec(lon);
}
 
// in string collect all char till comma and convert to float
static int str_till_comma(const char * const a, char * const dStr) {
 
    int i = 0, sLen = strlen(a);
    if (sLen > GPS_MAX_STRING)
        sLen = GPS_MAX_STRING;
 
    while (i < sLen && a[i] != 44) {    // while not comma or end
        dStr[i] = a[i];
        i++;
    }
    dStr[i] = '\0';
    return i;
}
 
/*
GxGGA - name code
Parameter   Value   Unit    Description
UTC                 hhmmss.sss  Universal time coordinated
Lat                 ddmm.mmmm   Latitude
Northing Indicator          N=North, S=South
Lon                 dddmm.mmmm  Longitude
Easting Indicator           E=East, W=West
Status              0           0=Invalid, 1=2D/3D, 2=DGPS, 6=Dead Reckoning
SVs Used            00          Number of SVs used for Navigation
HDOP                99.99       Horizontal Dilution of Precision
Alt (MSL)           m   Altitude (above means sea level)
Unit                M=Meters
Geoid Sep.          m           Geoid Separation = Alt(HAE) - Alt(MSL)
Unit                M=Meters
Age of DGPS Corr    s           Age of Differential Corrections
DGPS Ref Station                ID of DGPS Reference Station
*/
static void nmea_parse_gpgga(char const * const nmea, loc_t *loc) {
    char const * p = (char *)nmea;
    char dStr[GPS_MAX_STRING];
 
    p = strchr(p, ',') + 1;                 //skip time - we read date&time if Valid in GxRMC
 
    p = strchr(p, ',') + 1;                 // in p string started with searching address
    str_till_comma(p, dStr);                // str to float till comma saved modified string
    if (strlen(p) == 0) {
        return;             // if no data in field - empty data - we return
    }
    
    loc->latitude = atoff(dStr);                // fulfil data
 
    p = strchr(p, ',') + 1;                 // see above
    switch (p[0]) {
    case 'N':
        loc->lat = 'N';
        break;
    case 'S':
        loc->lat = 'S';
        break;
    case ',':
        loc->lat = '\0';
        break;
    }
 
    p = strchr(p, ',') + 1;
    str_till_comma(p, dStr);                // str to float till comma saved modified string
    loc->longitude = atoff(dStr);
 
    p = strchr(p, ',') + 1;
    switch (p[0]) {
    case 'W':
        loc->lon = 'W';
        break;
    case 'E':
        loc->lon = 'E';
        break;
    case ',':
        loc->lon = '\0';
        break;
    }
 
    p = strchr(p, ',') + 1;
    str_till_comma(p, dStr);                // str to float till comma saved modified string
    loc->quality = atoi(dStr);
 
    p = strchr(p, ',') + 1;
    str_till_comma(p, dStr);                // str to float till comma saved modified string
    loc->satellites = atoi(dStr);
 
    p = strchr(p, ',') + 1;
 
    p = strchr(p, ',') + 1;
    str_till_comma(p, dStr);                // str to float till comma saved modified string
    loc->altitude = atoff(dStr);
}
 
/*
GxRMC - nmea code
Parameter   Value       Unit            Description
UTC                     hhmmss.sss      Universal time coordinated
Status      V       A=Valid, V=Invalid
Lat         ddmm.mmmm                   Latitude
Northing Indicator          N=North, S=South
Lon         dddmm.mmmm                  Longitude
Easting Indicator           E=East, W=West
SOG                     nots            Speed Over Ground
COG (true)              °               Course Over Ground (true)
Date                    ddmmyy          Universal time coordinated
Magnetic Variation      °               Magnetic Variation
Magnetic Variation          E=East,W=West
Mode Indicator  N       A=Autonomous, D=Differential, E=Dead Reckoning, N=None
Navigational Status         S=Safe C=Caution U=Unsafe V=Not valid
*/
 
static void nmea_parse_gprmc(char const * const nmea, loc_t *loc) {
    char const * p = (char *)nmea;
    char dStr[GPS_MAX_STRING];
    efidatetime_t dt;
 
    p = strchr(p, ',') + 1;                 // read time
    str_till_comma(p, dStr);
    if (strlen(dStr) > 5) {
        dt.hour = str2int(dStr, 2);
        dt.minute = str2int(dStr + 2, 2);
        dt.second = str2int(dStr + 4, 2);
    }
    
    p = strchr(p, ',') + 1;                 // read field Valid status
    str_till_comma(p, dStr);
 
    if (dStr[0] == 'V') {                   // if field is invalid
        loc->quality = 0;
        return;
    }
    
    loc->quality = 4;                       // this is declaration that last receive field VALID
    
    p = strchr(p, ',') + 1;                 // latitude
    str_till_comma(p, dStr);                // str to float till comma saved modified string
    loc->latitude = atoff(dStr);
 
    p = strchr(p, ',') + 1;
    switch (p[0]) {
    case 'N':
        loc->lat = 'N';
        break;
    case 'S':
        loc->lat = 'S';
        break;
    case ',':
        loc->lat = '\0';
        break;
    }
 
    p = strchr(p, ',') + 1;                 // longitude
    str_till_comma(p, dStr);                // str to float till comma saved modified string
    loc->longitude = atoff(dStr);
 
    p = strchr(p, ',') + 1;
    switch (p[0]) {
    case 'W':
        loc->lon = 'W';
        break;
    case 'E':
        loc->lon = 'E';
        break;
    case ',':
        loc->lon = '\0';
        break;
    }
 
    p = strchr(p, ',') + 1;
    str_till_comma(p, dStr);                // str to float till comma saved modified string
    loc->speed = atoff(dStr);
    
    p = strchr(p, ',') + 1;
    str_till_comma(p, dStr);                // str to float till comma saved modified string
    loc->course = atoff(dStr);
 
    p = strchr(p, ',') + 1;                 // read date
    str_till_comma(p, dStr);
    if (strlen(dStr) > 5) {
        dt.day = str2int(dStr, 2);
        dt.month = str2int(dStr + 2, 2);
        dt.year = 100 +                     // we receive -200, but standard wait -1900 = add correction
            str2int(dStr + 4, 2);
    }
 
    if (dt.year > 0 ) {                     // check if date field is valid
        memcpy(&loc->time, &dt, sizeof(dt));
    }
}
 
/**
 * Get the message type (GPGGA, GPRMC, etc..)
 *
 * This function filters out also wrong packages (invalid checksum)
 *
 * @param message The NMEA message
 * @return The type of message if it is valid
 */
nmea_message_type nmea_get_message_type(const char *message) {
    int checksum = nmea_valid_checksum(message);
    if (checksum != _EMPTY) {
        return static_cast<nmea_message_type>(checksum);
    }
 
    if (strstr(message, NMEA_GPGGA_STR) != NULL) {
        return NMEA_GPGGA;
    }
 
    if (strstr(message, NMEA_GPRMC_STR) != NULL) {
        return NMEA_GPRMC;
    }
 
    return NMEA_UNKNOWN;
}
 
int nmea_valid_checksum(char const * message) {
    char p;
    int sum = 0;
    const char* starPtr = strrchr(message, '*');
    if (!starPtr) {
        return NMEA_CHECKSUM_ERR;
    }
    const char* int_message = starPtr + 1;
    long checksum = hex2int(int_message, 2);
 
    ++message;
    while ((p = *message++) != '*') {
        sum ^= p;
    }
 
    if (sum != checksum) {
        return NMEA_CHECKSUM_ERR;
    }
    return _EMPTY;
}
 
// Compute the GPS location using decimal scale
void gps_location(loc_t *coord, char const * const buffer) {
    coord->type = nmea_get_message_type(buffer);
 
    switch (coord->type) {
    case NMEA_GPGGA:
        nmea_parse_gpgga(buffer, coord);
        gps_convert_deg_to_dec(&(coord->latitude), coord->lat, &(coord->longitude), coord->lon);
        break;
    case NMEA_GPRMC:
        nmea_parse_gprmc(buffer, coord);
        break;
    case NMEA_UNKNOWN:
        // unknown message type
        break;
    }
}