libreria_GPSparser/GPS_parser.c

#include "GPS_parser.h"
#include <ctype.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#define GPS_SENTENCE_GGA 1
#define GPS_SENTENCE_RMC 2
#define GPS_SENTENCE_OTHER 0

static int32_t gps_parse_decimal(const char *term)
{
    bool negative = (*term == '-');
    if (negative) ++term;
    int32_t ret = 100 * (int32_t)atol(term);
    while (isdigit((unsigned char)*term)) ++term;
    if (*term == '.' && isdigit((unsigned char)term[1])) {
        ret += 10 * (term[1] - '0');
        if (isdigit((unsigned char)term[2]))
            ret += term[2] - '0';
    }
    return negative ? -ret : ret;
}

static void gps_parse_degrees(const char *term, gps_raw_degrees_t *deg)
{
    uint32_t leftOfDecimal = (uint32_t)atol(term);
    uint16_t minutes = (uint16_t)(leftOfDecimal % 100);
    uint32_t multiplier = 10000000UL;
    uint32_t tenMillionthsOfMinutes = minutes * multiplier;

    deg->deg = (uint16_t)(leftOfDecimal / 100);
    while (isdigit((unsigned char)*term)) ++term;
    if (*term == '.') {
        while (isdigit((unsigned char)*++term)) {
            multiplier /= 10;
            tenMillionthsOfMinutes += (*term - '0') * multiplier;
        }
    }
    deg->billionths = (5 * tenMillionthsOfMinutes + 1) / 3;
    deg->negative = false;
}

static void gps_location_commit(gps_location_t *location)
{
    location->rawLatData = location->rawNewLatData;
    location->rawLngData = location->rawNewLngData;
    location->fixQuality = location->newFixQuality;
    location->fixMode = location->newFixMode;
    location->lastCommitTime = gps_parser_millis();
    location->valid = true;
    location->updated = true;
}

static void gps_date_commit(gps_date_t *date)
{
    date->date = date->newDate;
    date->lastCommitTime = gps_parser_millis();
    date->valid = true;
    date->updated = true;
}

static void gps_time_commit(gps_time_t *time)
{
    time->time = time->newTime;
    time->lastCommitTime = gps_parser_millis();
    time->valid = true;
    time->updated = true;
}

static void gps_decimal_commit(gps_decimal_t *value)
{
    value->val = value->newval;
    value->lastCommitTime = gps_parser_millis();
    value->valid = true;
    value->updated = true;
}

static void gps_integer_commit(gps_integer_t *value)
{
    value->val = value->newval;
    value->lastCommitTime = gps_parser_millis();
    value->valid = true;
    value->updated = true;
}

static int gps_from_hex(char c)
{
    if (c >= 'A' && c <= 'F') return c - 'A' + 10;
    if (c >= 'a' && c <= 'f') return c - 'a' + 10;
    if (c >= '0' && c <= '9') return c - '0';
    return 0;
}

static inline bool gps_field_is_numeric(const char *term)
{
    return term[0] != '\0';
}

void gps_parser_init(gps_parser_t *gps)
{
    gps->parity = 0;
    gps->isChecksumTerm = false;
    gps->curSentenceType = GPS_SENTENCE_OTHER;
    gps->curTermNumber = 0;
    gps->curTermOffset = 0;
    gps->sentenceHasFix = false;
    gps->encodedCharCount = 0;
    gps->sentencesWithFixCount = 0;
    gps->failedChecksumCount = 0;
    gps->passedChecksumCount = 0;
    gps->term[0] = '\0';

    memset(&gps->location, 0, sizeof(gps->location));
    memset(&gps->date, 0, sizeof(gps->date));
    memset(&gps->time, 0, sizeof(gps->time));
    memset(&gps->speed, 0, sizeof(gps->speed));
    memset(&gps->course, 0, sizeof(gps->course));
    memset(&gps->altitude, 0, sizeof(gps->altitude));
    memset(&gps->satellites, 0, sizeof(gps->satellites));
    memset(&gps->hdop, 0, sizeof(gps->hdop));
}

static bool gps_end_of_term_handler(gps_parser_t *gps)
{
    if (gps->isChecksumTerm) {
        uint8_t checksum = (uint8_t)(16 * gps_from_hex(gps->term[0]) + gps_from_hex(gps->term[1]));
        if (checksum == gps->parity) {
            gps->passedChecksumCount++;
            if (gps->sentenceHasFix) gps->sentencesWithFixCount++;

            if (gps->curSentenceType == GPS_SENTENCE_RMC) {
                gps_date_commit(&gps->date);
                gps_time_commit(&gps->time);
                if (gps->sentenceHasFix) {
                    gps_location_commit(&gps->location);
                    gps_decimal_commit(&gps->speed);
                    gps_decimal_commit(&gps->course);
                }
            } else if (gps->curSentenceType == GPS_SENTENCE_GGA) {
                gps_time_commit(&gps->time);
                if (gps->sentenceHasFix) {
                    gps_location_commit(&gps->location);
                    gps_decimal_commit(&gps->altitude);
                }
                gps_integer_commit(&gps->satellites);
                gps_decimal_commit(&gps->hdop);
            }
            return true;
        }
        gps->failedChecksumCount++;
        return false;
    }

    if (gps->curTermNumber == 0) {
        if (gps->term[0] == 'G' && strchr("PNABL", gps->term[1]) != NULL) {
            if (!strcmp(gps->term + 2, "RMC")) gps->curSentenceType = GPS_SENTENCE_RMC;
            else if (!strcmp(gps->term + 2, "GGA")) gps->curSentenceType = GPS_SENTENCE_GGA;
            else gps->curSentenceType = GPS_SENTENCE_OTHER;
        } else {
            gps->curSentenceType = GPS_SENTENCE_OTHER;
        }
        return false;
    }

    if (gps->curSentenceType != GPS_SENTENCE_OTHER && gps_field_is_numeric(gps->term)) {
        switch ((gps->curSentenceType << 5) | gps->curTermNumber) {
        case (GPS_SENTENCE_RMC << 5) | 1:
        case (GPS_SENTENCE_GGA << 5) | 1:
            gps->time.newTime = (uint32_t)gps_parse_decimal(gps->term);
            break;
        case (GPS_SENTENCE_RMC << 5) | 2:
            gps->sentenceHasFix = (gps->term[0] == 'A');
            break;
        case (GPS_SENTENCE_RMC << 5) | 3:
        case (GPS_SENTENCE_GGA << 5) | 2:
            gps_parse_degrees(gps->term, &gps->location.rawNewLatData);
            break;
        case (GPS_SENTENCE_RMC << 5) | 4:
        case (GPS_SENTENCE_GGA << 5) | 3:
            gps->location.rawNewLatData.negative = (gps->term[0] == 'S');
            break;
        case (GPS_SENTENCE_RMC << 5) | 5:
        case (GPS_SENTENCE_GGA << 5) | 4:
            gps_parse_degrees(gps->term, &gps->location.rawNewLngData);
            break;
        case (GPS_SENTENCE_RMC << 5) | 6:
        case (GPS_SENTENCE_GGA << 5) | 5:
            gps->location.rawNewLngData.negative = (gps->term[0] == 'W');
            break;
        case (GPS_SENTENCE_RMC << 5) | 7:
            gps->speed.newval = gps_parse_decimal(gps->term);
            break;
        case (GPS_SENTENCE_RMC << 5) | 8:
            gps->course.newval = gps_parse_decimal(gps->term);
            break;
        case (GPS_SENTENCE_RMC << 5) | 9:
            gps->date.newDate = (uint32_t)atol(gps->term);
            break;
        case (GPS_SENTENCE_GGA << 5) | 6:
            gps->sentenceHasFix = (gps->term[0] > '0');
            gps->location.newFixQuality = gps->term[0];
            break;
        case (GPS_SENTENCE_GGA << 5) | 7:
            gps->satellites.newval = (uint32_t)atol(gps->term);
            break;
        case (GPS_SENTENCE_GGA << 5) | 8:
            gps->hdop.newval = gps_parse_decimal(gps->term);
            break;
        case (GPS_SENTENCE_GGA << 5) | 9:
            gps->altitude.newval = gps_parse_decimal(gps->term);
            break;
        case (GPS_SENTENCE_RMC << 5) | 12:
            gps->location.newFixMode = gps->term[0];
            break;
        default:
            break;
        }
    }

    return false;
}

bool gps_parser_encode(gps_parser_t *gps, char c)
{
    gps->encodedCharCount++;

    switch (c) {
    case ',':
        gps->parity ^= (uint8_t)c;
        /* fall through */
    case '\r':
    case '\n':
    case '*':
        if (gps->curTermOffset < sizeof(gps->term)) {
            gps->term[gps->curTermOffset] = '\0';
            bool valid = gps_end_of_term_handler(gps);
            gps->curTermNumber++;
            gps->curTermOffset = 0;
            gps->isChecksumTerm = (c == '*');
            return valid;
        }
        gps->curTermNumber++;
        gps->curTermOffset = 0;
        gps->isChecksumTerm = (c == '*');
        return false;
    case '$':
        gps->curTermNumber = 0;
        gps->curTermOffset = 0;
        gps->parity = 0;
        gps->curSentenceType = GPS_SENTENCE_OTHER;
        gps->isChecksumTerm = false;
        gps->sentenceHasFix = false;
        return false;
    default:
        if (gps->curTermOffset < sizeof(gps->term) - 1) {
            gps->term[gps->curTermOffset++] = c;
        }
        if (!gps->isChecksumTerm) {
            gps->parity ^= (uint8_t)c;
        }
        return false;
    }
}

bool gps_location_is_valid(const gps_location_t *location) { return location->valid; }
bool gps_location_is_updated(const gps_location_t *location) { return location->updated; }
uint32_t gps_location_age(const gps_location_t *location) {
    return location->valid ? gps_parser_millis() - location->lastCommitTime : UINT32_MAX;
}
double gps_location_lat(const gps_location_t *location)
{
    double v = location->rawLatData.deg + location->rawLatData.billionths / 1000000000.0;
    return location->rawLatData.negative ? -v : v;
}
double gps_location_lng(const gps_location_t *location)
{
    double v = location->rawLngData.deg + location->rawLngData.billionths / 1000000000.0;
    return location->rawLngData.negative ? -v : v;
}
char gps_location_fix_quality(const gps_location_t *location) { return location->fixQuality; }
char gps_location_fix_mode(const gps_location_t *location) { return location->fixMode; }

bool gps_date_is_valid(const gps_date_t *date) { return date->valid; }
uint16_t gps_date_year(const gps_date_t *date) { return (uint16_t)(date->date % 100) + 2000; }
uint8_t gps_date_month(const gps_date_t *date) { return (uint8_t)((date->date / 100) % 100); }
uint8_t gps_date_day(const gps_date_t *date) { return (uint8_t)(date->date / 10000); }

bool gps_time_is_valid(const gps_time_t *time) { return time->valid; }
uint8_t gps_time_hour(const gps_time_t *time) { return (uint8_t)(time->time / 1000000); }
uint8_t gps_time_minute(const gps_time_t *time) { return (uint8_t)((time->time / 10000) % 100); }
uint8_t gps_time_second(const gps_time_t *time) { return (uint8_t)((time->time / 100) % 100); }
uint8_t gps_time_centisecond(const gps_time_t *time) { return (uint8_t)(time->time % 100); }

bool gps_decimal_is_valid(const gps_decimal_t *value) { return value->valid; }
int32_t gps_decimal_value(const gps_decimal_t *value) { return value->val; }

gps_integer_t *gps_get_satellites(gps_parser_t *gps) { return &gps->satellites; }

uint32_t gps_parser_chars_processed(const gps_parser_t *gps) { return gps->encodedCharCount; }
uint32_t gps_parser_sentences_with_fix(const gps_parser_t *gps) { return gps->sentencesWithFixCount; }
uint32_t gps_parser_failed_checksum(const gps_parser_t *gps) { return gps->failedChecksumCount; }
uint32_t gps_parser_passed_checksum(const gps_parser_t *gps) { return gps->passedChecksumCount; }