Gsof bitmask flexible parser

libraries/AP_Common/Bitmask.h

@@ -53,6 +53,18 @@ class Bitmask {
 
     Bitmask(const Bitmask &other) = delete;
 
+
+    // Construct a bitmask with some bits enabled.
+    template<size_t N>
+    Bitmask(const uint16_t (&enabled_bits)[N]) {
+        clearall();
+        for (size_t i = 0; i < N; ++i) {
+            if (enabled_bits[i] < NUMBITS) {
+                set(enabled_bits[i]);
+            }
+        }
+    }
+
     // set given bitnumber
     void set(uint16_t bit) {
         if (!validate(bit)) {

libraries/AP_GPS/AP_GPS_GSOF.cpp

@@ -22,8 +22,12 @@
 //    param set GPS1_TYPE 11 // GSOF
 //    param set SERIAL3_PROTOCOL 5 // GPS
 //
-//  Pure SITL usage
-//     param set SIM_GPS_TYPE 11 // GSOF
+//  Pure SITL usage:
+//    sim_vehicle.py -v Plane --console --map -DG
+//    param set SIM_GPS1_TYPE 11 // GSOF
+//    param set GPS1_TYPE 11 // GSOF
+//    param set SERIAL3_PROTOCOL 5 // GPS
+
 #define ALLOW_DOUBLE_MATH_FUNCTIONS
 
 #include "AP_GPS.h"
@@ -36,28 +40,25 @@
 
 extern const AP_HAL::HAL& hal;
 
-#define gsof_DEBUGGING 0
-
-#if gsof_DEBUGGING
-# define Debug(fmt, args ...)                  \
-do {                                            \
-    hal.console->printf("%s:%d: " fmt "\n",     \
-                        __FUNCTION__, __LINE__, \
-                        ## args);               \
-    hal.scheduler->delay(1);                    \
-} while(0)
-#else
-# define Debug(fmt, args ...)
-#endif
-
 AP_GPS_GSOF::AP_GPS_GSOF(AP_GPS &_gps,
                          AP_GPS::Params &_params,
                          AP_GPS::GPS_State &_state,
                          AP_HAL::UARTDriver *_port) :
     AP_GPS_Backend(_gps, _params, _state, _port)
 {
+
+    const uint16_t gsofmsgreq[5] = {
+        AP_GSOF::POS_TIME,
+        AP_GSOF::POS,
+        AP_GSOF::VEL,
+        AP_GSOF::DOP,
+        AP_GSOF::POS_SIGMA
+    };
     // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_Overview.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257COverview%257C_____0
+    // The maximum number of outputs allowed with GSOF is 10
     static_assert(ARRAY_SIZE(gsofmsgreq) <= 10, "The maximum number of outputs allowed with GSOF is 10.");
+    requested_msgs = AP_GSOF::MsgTypes(gsofmsgreq);
+
 
     constexpr uint8_t default_com_port = static_cast<uint8_t>(HW_Port::COM2);
     params.com_port.set_default(default_com_port);
@@ -80,38 +81,48 @@ AP_GPS_GSOF::read(void)
 {
     const uint32_t now = AP_HAL::millis();
 
-    if (gsofmsgreq_index < (sizeof(gsofmsgreq))) {
+    if (gsofmsgreq_index < (requested_msgs.count())) {
         const auto com_port = params.com_port.get();
         if (!validate_com_port(com_port)) {
             // The user parameter for COM port is not a valid GSOF port
             return false;
         }
         if (now > gsofmsg_time) {
-            requestGSOF(gsofmsgreq[gsofmsgreq_index], static_cast<HW_Port>(com_port), Output_Rate::FREQ_10_HZ);
+            for (uint16_t i = next_req_gsof; i < requested_msgs.size(); i++){
+                if (requested_msgs.get(i)) {
+                    next_req_gsof = i;
+                    break;
+                }
+            }
+            requestGSOF(next_req_gsof, static_cast<HW_Port>(com_port), Output_Rate::FREQ_10_HZ);
             gsofmsg_time = now + 110;
             gsofmsgreq_index++;
+            next_req_gsof++;
         }
     }
 
-    bool ret = false;
     while (port->available() > 0) {
         const uint8_t temp = port->read();
 #if AP_GPS_DEBUG_LOGGING_ENABLED
         log_data(&temp, 1);
 #endif
-        const int n_gsof_received = parse(temp, ARRAY_SIZE(gsofmsgreq));
-        if(n_gsof_received == UNEXPECTED_NUM_GSOF_PACKETS) {
-            state.status = AP_GPS::NO_FIX;
-            continue;
-        }
-        const bool got_expected_packets = n_gsof_received == ARRAY_SIZE(gsofmsgreq);
-        ret |= got_expected_packets;
-    }
-    if (ret) {
-        pack_state_data();
+        AP_GSOF::MsgTypes parsed;
+        const int parse_status = parse(temp, parsed);
+        if(parse_status == PARSED_GSOF_DATA) {
+            if (parsed.get(AP_GSOF::POS_TIME) &&
+                parsed.get(AP_GSOF::POS) && 
+                parsed.get(AP_GSOF::VEL) && 
+                parsed.get(AP_GSOF::DOP) && 
+                parsed.get(AP_GSOF::POS_SIGMA)
+            )
+            {
+                pack_state_data();
+                return true;
+            }
+        }          
     }
 
-    return ret;
+    return false;
 }
 
 void

libraries/AP_GPS/AP_GPS_GSOF.h

@@ -73,6 +73,7 @@ class AP_GPS_GSOF : public AP_GPS_Backend, public AP_GSOF
     uint8_t packetcount;
     uint32_t gsofmsg_time;
     uint8_t gsofmsgreq_index;
-    const uint8_t gsofmsgreq[5] = {1,2,8,9,12};
+    uint16_t next_req_gsof;
+    AP_GSOF::MsgTypes requested_msgs;
 };
 #endif

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& parsed_msgs)
 {
     // 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 ) {
-                return UNEXPECTED_NUM_GSOF_PACKETS;
+            if (!process_message(parsed_msgs)) {
+                return NO_GSOF_DATA;
             }
-            return n_processed;
+            return PARSED_GSOF_DATA;
         }
         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(MsgTypes& parsed_msgs)
 {
     if (msg.packettype == 0x40) { // GSOF
         // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25
@@ -97,12 +96,9 @@ AP_GSOF::process_message(void)
               pageidx, maxpageidx, trans_number);
 #endif
 
-        // This counts up the number of received packets.
-        int valid = 0;
-
-        // 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 +107,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,11 +127,11 @@ AP_GSOF::process_message(void)
             a += output_length - 1u;
         }
 
-        return valid;
+        return true;
     }
 
     // No GSOF packets.
-    return NO_GSOF_DATA;
+    return false;
 }
 
 void AP_GSOF::parse_pos_time(uint32_t a)

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
 
@@ -11,14 +12,15 @@ class AP_GSOF
 public:
 
     static constexpr int NO_GSOF_DATA = 0;
-    static constexpr int UNEXPECTED_NUM_GSOF_PACKETS = -1;
+    static constexpr int PARSED_GSOF_DATA = 1;
+
+    // A type alias for which packets have been parsed.
+    using MsgTypes = Bitmask<71>;
 
     // Parse a single byte into the buffer.
-    // When enough data has arrived, it returns the number of GSOF packets 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.
+    // When enough data has arrived, it populates a bitmask of which GSOF packets were parsed in the GENOUT packet and returns PARSED_GSOF_DATA.
     // 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;
+    int parse(const uint8_t temp, MsgTypes& parsed_msgs) WARN_IF_UNUSED;
 
     // GSOF packet numbers.
     enum msg_idx_t {
@@ -91,8 +93,10 @@ class AP_GSOF
 
 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(MsgTypes& parsed_msgs) WARN_IF_UNUSED;
 
     void parse_pos_time(uint32_t a);
     void parse_pos(uint32_t a);

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);