From 252a3f9d3bea216530da3fdafb6f15e2c32fb84b Mon Sep 17 00:00:00 2001
From: "roman.storozhenko" <roman.storozhenko@pixelsmatter.com>
Date: Mon, 23 Nov 2020 16:25:03 +0200
Subject: [PATCH] Add GBS parser

Signed-off-by: roman.storozhenko <roman.storozhenko@pixelsmatter.com>
---
 minmea.c | 32 ++++++++++++++++++++++++++++----
 minmea.h | 17 +++++++++++++++--
 tests.c  | 23 +++++++++++++++++++++++
 3 files changed, 66 insertions(+), 6 deletions(-)

diff --git a/minmea.c b/minmea.c
index 32f7881..98bc215 100644
--- a/minmea.c
+++ b/minmea.c
@@ -358,18 +358,20 @@ enum minmea_sentence_id minmea_sentence_id(const char *sentence, bool strict)
     if (!minmea_scan(sentence, "t", type))
         return MINMEA_INVALID;
 
-    if (!strcmp(type+2, "RMC"))
-        return MINMEA_SENTENCE_RMC;
+    if (!strcmp(type+2, "GBS"))
+        return MINMEA_SENTENCE_GBS;
     if (!strcmp(type+2, "GGA"))
         return MINMEA_SENTENCE_GGA;
-    if (!strcmp(type+2, "GSA"))
-        return MINMEA_SENTENCE_GSA;
     if (!strcmp(type+2, "GLL"))
         return MINMEA_SENTENCE_GLL;
+    if (!strcmp(type+2, "GSA"))
+        return MINMEA_SENTENCE_GSA;
     if (!strcmp(type+2, "GST"))
         return MINMEA_SENTENCE_GST;
     if (!strcmp(type+2, "GSV"))
         return MINMEA_SENTENCE_GSV;
+    if (!strcmp(type+2, "RMC"))
+        return MINMEA_SENTENCE_RMC;
     if (!strcmp(type+2, "VTG"))
         return MINMEA_SENTENCE_VTG;
     if (!strcmp(type+2, "ZDA"))
@@ -378,6 +380,28 @@ enum minmea_sentence_id minmea_sentence_id(const char *sentence, bool strict)
     return MINMEA_UNKNOWN;
 }
 
+bool minmea_parse_gbs(struct minmea_sentence_gbs *frame, const char *sentence)
+{
+    // $GNGBS,170556.00,3.0,2.9,8.3,,,,*5C
+    char type[6];
+    if (!minmea_scan(sentence, "tTfffdfff",
+            type,
+            &frame->time,
+            &frame->err_latitude,
+            &frame->err_longitude,
+            &frame->err_altitude,
+            &frame->svid,
+            &frame->prob,
+            &frame->bias,
+            &frame->stddev
+            ))
+        return false;
+    if (strcmp(type+2, "GBS"))
+        return false;
+
+    return true;
+}
+
 bool minmea_parse_rmc(struct minmea_sentence_rmc *frame, const char *sentence)
 {
     // $GPRMC,081836,A,3751.65,S,14507.36,E,000.0,360.0,130998,011.3,E*62
diff --git a/minmea.h b/minmea.h
index eb4202f..91568f7 100644
--- a/minmea.h
+++ b/minmea.h
@@ -28,12 +28,13 @@ extern "C" {
 enum minmea_sentence_id {
     MINMEA_INVALID = -1,
     MINMEA_UNKNOWN = 0,
-    MINMEA_SENTENCE_RMC,
+    MINMEA_SENTENCE_GBS,
     MINMEA_SENTENCE_GGA,
-    MINMEA_SENTENCE_GSA,
     MINMEA_SENTENCE_GLL,
+    MINMEA_SENTENCE_GSA,
     MINMEA_SENTENCE_GST,
     MINMEA_SENTENCE_GSV,
+    MINMEA_SENTENCE_RMC,
     MINMEA_SENTENCE_VTG,
     MINMEA_SENTENCE_ZDA,
 };
@@ -56,6 +57,17 @@ struct minmea_time {
     int microseconds;
 };
 
+struct minmea_sentence_gbs {
+    struct minmea_time time;
+    struct minmea_float err_latitude;
+    struct minmea_float err_longitude;
+    struct minmea_float err_altitude;
+    int svid;
+    struct minmea_float prob;
+    struct minmea_float bias;
+    struct minmea_float stddev;
+};
+
 struct minmea_sentence_rmc {
     struct minmea_time time;
     bool valid;
@@ -199,6 +211,7 @@ bool minmea_scan(const char *sentence, const char *format, ...);
 /*
  * Parse a specific type of sentence. Return true on success.
  */
+bool minmea_parse_gbs(struct minmea_sentence_gbs *frame, const char *sentence);
 bool minmea_parse_rmc(struct minmea_sentence_rmc *frame, const char *sentence);
 bool minmea_parse_gga(struct minmea_sentence_gga *frame, const char *sentence);
 bool minmea_parse_gsa(struct minmea_sentence_gsa *frame, const char *sentence);
diff --git a/tests.c b/tests.c
index 7a31934..1c8e92f 100644
--- a/tests.c
+++ b/tests.c
@@ -41,6 +41,7 @@ static const char *valid_sentences_checksum[] = {
     "$GPVTG,,T,,M,0.016,N,0.030,K,A*27",
     "$GPGST,024603.00,3.2,6.6,4.7,47.3,5.8,5.6,22.0*58",
     "$GPZDA,160012.71,11,03,2004,-1,00*7D",
+    "$GNGBS,170556.00,3.0,2.9,8.3,,,,*5C",
     NULL,
 };
 
@@ -465,6 +466,27 @@ START_TEST(test_minmea_scan_complex3)
 }
 END_TEST
 
+START_TEST(test_minmea_parse_gbs1)
+{
+    const char *sentence = "$GNGBS,170556.00,3.0,2.9,8.3,,,,";
+    struct minmea_sentence_gbs frame = {};
+    static const struct minmea_sentence_gbs expected = {
+	    .time = { 17, 5, 56, 0 },
+	    .err_latitude = { 30, 10 },
+	    .err_longitude = { 29, 10 },
+	    .err_altitude = { 83, 10 },
+	    .svid = 0,
+	    .prob = { 0, 0 },
+	    .bias = { 0, 0 },
+	    .stddev = { 0, 0 },
+    };
+    ck_assert(minmea_check(sentence, false) == true);
+    ck_assert(minmea_check(sentence, true) == false);
+    ck_assert(minmea_parse_gbs(&frame, sentence) == true);
+    ck_assert(!memcmp(&frame, &expected, sizeof(frame)));
+}
+END_TEST
+
 START_TEST(test_minmea_parse_rmc1)
 {
     const char *sentence = "$GPRMC,081836.75,A,3751.65,S,14507.36,E,000.0,360.0,130998,011.3,E";
@@ -1062,6 +1084,7 @@ static Suite *minmea_suite(void)
     suite_add_tcase(s, tc_scan);
 
     TCase *tc_parse = tcase_create("minmea_parse");
+    tcase_add_test(tc_parse, test_minmea_parse_gbs1);
     tcase_add_test(tc_parse, test_minmea_parse_rmc1);
     tcase_add_test(tc_parse, test_minmea_parse_rmc2);
     tcase_add_test(tc_parse, test_minmea_parse_gga1);