From 6fcb9a1cdc4bd8be275811fe47529d3844bf2607 Mon Sep 17 00:00:00 2001
From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>
Date: Wed, 4 Dec 2024 19:46:56 -0800
Subject: [PATCH] AP_GSOF: refactor GSOF to expect packets by ID

Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>
---
 libraries/AP_GSOF/AP_GSOF.cpp         | 22 ++++++++--------------
 libraries/AP_GSOF/AP_GSOF.h           | 17 ++++++++++++-----
 libraries/AP_GSOF/tests/test_gsof.cpp | 13 +++++++++++--
 3 files changed, 31 insertions(+), 21 deletions(-)

diff --git a/libraries/AP_GSOF/AP_GSOF.cpp b/libraries/AP_GSOF/AP_GSOF.cpp
index f60fb37207..c257362746 100644
--- a/libraries/AP_GSOF/AP_GSOF.cpp
+++ b/libraries/AP_GSOF/AP_GSOF.cpp
@@ -28,7 +28,7 @@ do {                                            \
 #endif
 
 int
-AP_GSOF::parse(const uint8_t temp, const uint8_t n_expected)
+AP_GSOF::parse(const uint8_t temp, MsgTypes& message_types)
 {
     // https://receiverhelp.trimble.com/oem-gnss/index.html#API_DataCollectorFormatPacketStructure.html
     switch (msg.state) {
@@ -67,11 +67,10 @@ AP_GSOF::parse(const uint8_t temp, const uint8_t n_expected)
         msg.checksum = temp;
         msg.state = Msg_Parser::State::ENDTX;
         if (msg.checksum == msg.checksumcalc) {
-            const auto n_processed = process_message();
-            if (n_processed != n_expected ) {
+            if (!process_message(message_types)) {
                 return UNEXPECTED_NUM_GSOF_PACKETS;
             }
-            return n_processed;
+            return PARSED_AS_EXPECTED;
         }
         break;
     case Msg_Parser::State::ENDTX:
@@ -83,8 +82,8 @@ AP_GSOF::parse(const uint8_t temp, const uint8_t n_expected)
     return NO_GSOF_DATA;
 }
 
-int
-AP_GSOF::process_message(void)
+bool
+AP_GSOF::process_message(const MsgTypes& expected_msgs)
 {
     if (msg.packettype == 0x40) { // GSOF
         // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25
@@ -98,11 +97,11 @@ AP_GSOF::process_message(void)
 #endif
 
         // This counts up the number of received packets.
-        int valid = 0;
+        MsgTypes parsed_msgs;
 
-        // want 1 2 8 9 12
         for (uint32_t a = 3; a < msg.length; a++) {
             const uint8_t output_type = msg.data[a];
+            parsed_msgs.set(output_type);
             a++;
             const uint8_t output_length = msg.data[a];
             a++;
@@ -111,23 +110,18 @@ AP_GSOF::process_message(void)
             switch (output_type) {
             case POS_TIME:
                 parse_pos_time(a);
-                valid++;
                 break;
             case POS:
                 parse_pos(a);
-                valid++;
                 break;
             case VEL:
                 parse_vel(a);
-                valid++;
                 break;
             case DOP:
                 parse_dop(a);
-                valid++;
                 break;
             case POS_SIGMA:
                 parse_pos_sigma(a);
-                valid++;
                 break;
             default:
                 break;
@@ -136,7 +130,7 @@ AP_GSOF::process_message(void)
             a += output_length - 1u;
         }
 
-        return valid;
+        return parsed_msgs == expected_msgs;
     }
 
     // No GSOF packets.
diff --git a/libraries/AP_GSOF/AP_GSOF.h b/libraries/AP_GSOF/AP_GSOF.h
index 1aeda02564..d7a62d89cb 100644
--- a/libraries/AP_GSOF/AP_GSOF.h
+++ b/libraries/AP_GSOF/AP_GSOF.h
@@ -3,6 +3,7 @@
 #pragma once
 
 #include "AP_GSOF_config.h"
+#include <AP_Common/Bitmask.h>
 
 #if AP_GSOF_ENABLED
 
@@ -12,13 +13,17 @@ public:
 
     static constexpr int NO_GSOF_DATA = 0;
     static constexpr int UNEXPECTED_NUM_GSOF_PACKETS = -1;
+    static constexpr int PARSED_AS_EXPECTED = 1;
+
+    // A type alias for which packets have been parsed.
+    using MsgTypes = Bitmask<70>;
 
     // Parse a single byte into the buffer.
-    // When enough data has arrived, it returns the number of GSOF packets parsed in the GENOUT packet.
+    // When enough data has arrived, it populates a bitmask of which GSOF packets were parsed in the GENOUT packet.
     // If an unexpected number of GSOF packets were parsed, returns UNEXPECTED_NUM_GSOF_PACKETS.
-    // This means there is no fix.
     // If it returns NO_GSOF_DATA, the parser just needs more data.
-    int parse(const uint8_t temp, const uint8_t n_expected) WARN_IF_UNUSED;
+    // Upon parsing the expected message contents, it returns PARSED_AS_EXPECTED.
+    int parse(const uint8_t temp, MsgTypes& message_types) WARN_IF_UNUSED;
 
     // GSOF packet numbers.
     enum msg_idx_t {
@@ -91,8 +96,10 @@ public:
 
 private:
 
-    // Parses current data and returns the number of parsed GSOF messages.
-    int process_message() WARN_IF_UNUSED;
+    // Parses current data.
+    // Returns true if, and only if, the expected packets were parsed.
+    // Unexpected/unparsed data will make this return false.
+    bool process_message(const MsgTypes& expected_msgs) WARN_IF_UNUSED;
 
     void parse_pos_time(uint32_t a);
     void parse_pos(uint32_t a);
diff --git a/libraries/AP_GSOF/tests/test_gsof.cpp b/libraries/AP_GSOF/tests/test_gsof.cpp
index 549b9169c3..ab16c18b08 100644
--- a/libraries/AP_GSOF/tests/test_gsof.cpp
+++ b/libraries/AP_GSOF/tests/test_gsof.cpp
@@ -16,7 +16,8 @@ const AP_HAL::HAL &hal = AP_HAL::get_HAL();
 TEST(AP_GSOF, incomplete_packet)
 {
     AP_GSOF gsof;
-    EXPECT_FALSE(gsof.parse(0, 5));
+    AP_GSOF::MsgTypes expected;
+    EXPECT_FALSE(gsof.parse(0, expected));
 }
 
 TEST(AP_GSOF, packet1)
@@ -27,9 +28,17 @@ TEST(AP_GSOF, packet1)
     AP_GSOF gsof;
     char c = 0;
     bool parsed = false;
+
+    AP_GSOF::MsgTypes expected;
+    expected.set(1);
+    expected.set(2);
+    expected.set(8);
+    expected.set(9);
+    expected.set(12);
+
     while (c != EOF) {
         c = fgetc (fp);
-        parsed |= gsof.parse((uint8_t)c, 5);
+        parsed |= gsof.parse((uint8_t)c, expected);
     }
     
     EXPECT_TRUE(parsed);