fisrt commit

CMakeLists.txt

@@ -0,0 +1,3 @@
+idf_component_register(SRCS "GPS_parser.c"
+                    INCLUDE_DIRS "include"
+                    REQUIRES esp_driver_uart)

GPS_parser.c

@@ -0,0 +1,303 @@
+#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; }

idf_component.yml

@@ -0,0 +1,3 @@
+version: "1.0.0"
+description: "Parser para modulo GPS, compatible con NEO-6M"
+url: "https://dacogogs.duckdns.org/dacowars/libreria_GPS_parser.git"

include/GPS_parser.h

@@ -0,0 +1,136 @@
+#ifndef GPS_PARSER_H
+#define GPS_PARSER_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <esp_timer.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define GPS_PARSER_MAX_FIELD_SIZE 15
+#define GPS_PARSER_MPH_PER_KNOT   1.15077945
+#define GPS_PARSER_MPS_PER_KNOT   0.51444444
+#define GPS_PARSER_KMPH_PER_KNOT  1.852
+#define GPS_PARSER_MILES_PER_METER 0.00062137112
+#define GPS_PARSER_KM_PER_METER    0.001
+#define GPS_PARSER_FEET_PER_METER  3.2808399
+#define GPS_PARSER_EARTH_MEAN_RADIUS 6371009
+
+static inline uint32_t gps_parser_millis(void)
+{
+    return (uint32_t)(esp_timer_get_time() / 1000ULL);
+}
+
+typedef struct {
+    uint16_t deg;
+    uint32_t billionths;
+    bool     negative;
+} gps_raw_degrees_t;
+
+typedef struct {
+    bool valid;
+    bool updated;
+    gps_raw_degrees_t rawLatData;
+    gps_raw_degrees_t rawLngData;
+    gps_raw_degrees_t rawNewLatData;
+    gps_raw_degrees_t rawNewLngData;
+    char fixQuality;
+    char newFixQuality;
+    char fixMode;
+    char newFixMode;
+    uint32_t lastCommitTime;
+} gps_location_t;
+
+typedef struct {
+    bool valid;
+    bool updated;
+    uint32_t date;
+    uint32_t newDate;
+    uint32_t lastCommitTime;
+} gps_date_t;
+
+typedef struct {
+    bool valid;
+    bool updated;
+    uint32_t time;
+    uint32_t newTime;
+    uint32_t lastCommitTime;
+} gps_time_t;
+
+typedef struct {
+    bool valid;
+    bool updated;
+    uint32_t lastCommitTime;
+    int32_t val;
+    int32_t newval;
+} gps_decimal_t;
+
+typedef struct {
+    bool valid;
+    bool updated;
+    uint32_t lastCommitTime;
+    uint32_t val;
+    uint32_t newval;
+} gps_integer_t;
+
+typedef struct {
+    uint8_t parity;
+    bool isChecksumTerm;
+    char term[GPS_PARSER_MAX_FIELD_SIZE + 1];
+    uint8_t curSentenceType;
+    uint8_t curTermNumber;
+    uint8_t curTermOffset;
+    bool sentenceHasFix;
+    gps_location_t location;
+    gps_date_t date;
+    gps_time_t time;
+    gps_decimal_t speed;
+    gps_decimal_t course;
+    gps_decimal_t altitude;
+    gps_integer_t satellites;
+    gps_decimal_t hdop;
+    uint32_t encodedCharCount;
+    uint32_t sentencesWithFixCount;
+    uint32_t failedChecksumCount;
+    uint32_t passedChecksumCount;
+} gps_parser_t;
+
+void gps_parser_init(gps_parser_t *gps);
+bool gps_parser_encode(gps_parser_t *gps, char c);
+
+bool gps_location_is_valid(const gps_location_t *location);
+bool gps_location_is_updated(const gps_location_t *location);
+uint32_t gps_location_age(const gps_location_t *location);
+double gps_location_lat(const gps_location_t *location);
+double gps_location_lng(const gps_location_t *location);
+char gps_location_fix_quality(const gps_location_t *location);
+char gps_location_fix_mode(const gps_location_t *location);
+
+bool gps_date_is_valid(const gps_date_t *date);
+uint16_t gps_date_year(const gps_date_t *date);
+uint8_t gps_date_month(const gps_date_t *date);
+uint8_t gps_date_day(const gps_date_t *date);
+
+bool gps_time_is_valid(const gps_time_t *time);
+uint8_t gps_time_hour(const gps_time_t *time);
+uint8_t gps_time_minute(const gps_time_t *time);
+uint8_t gps_time_second(const gps_time_t *time);
+uint8_t gps_time_centisecond(const gps_time_t *time);
+
+bool gps_decimal_is_valid(const gps_decimal_t *value);
+int32_t gps_decimal_value(const gps_decimal_t *value);
+
+gps_integer_t *gps_get_satellites(gps_parser_t *gps);
+
+uint32_t gps_parser_chars_processed(const gps_parser_t *gps);
+uint32_t gps_parser_sentences_with_fix(const gps_parser_t *gps);
+uint32_t gps_parser_failed_checksum(const gps_parser_t *gps);
+uint32_t gps_parser_passed_checksum(const gps_parser_t *gps);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // GPS_PARSER_H