AP_GPS: refactor GSOF to expect packets by ID

Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>
ArduPilot · Dec 4, 2024 · b469160 · b469160
1 parent f990cf9
commit b469160
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 23 deletions.
diff --git a/libraries/AP_GPS/AP_GPS_GSOF.cpp b/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,23 @@
 
 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)
 {
+    gsofmsgreq.set(AP_GSOF::POS_TIME);
+    gsofmsgreq.set(AP_GSOF::POS);
+    gsofmsgreq.set(AP_GSOF::VEL);
+    gsofmsgreq.set(AP_GSOF::DOP);
+    gsofmsgreq.set(AP_GSOF::POS_SIGMA);
+
     // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_Overview.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257COverview%257C_____0
-    static_assert(ARRAY_SIZE(gsofmsgreq) <= 10, "The maximum number of outputs allowed with GSOF is 10.");
+    // The maximum number of outputs allowed with GSOF is 10
+    if(gsofmsgreq.count() >= 10) {
+        INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
+    }
 
     constexpr uint8_t default_com_port = static_cast<uint8_t>(HW_Port::COM2);
     params.com_port.set_default(default_com_port);
@@ -80,16 +79,23 @@ AP_GPS_GSOF::read(void)
 {
     const uint32_t now = AP_HAL::millis();
 
-    if (gsofmsgreq_index < (sizeof(gsofmsgreq))) {
+    if (gsofmsgreq_index < (gsofmsgreq.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 < gsofmsgreq.size(); i++){
+                if (gsofmsgreq.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++;
         }
     }
 
@@ -99,12 +105,12 @@ AP_GPS_GSOF::read(void)
 #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) {
+        const int parse_status = parse(temp, gsofmsgreq);
+        if(parse_status == UNEXPECTED_NUM_GSOF_PACKETS) {
             state.status = AP_GPS::NO_FIX;
             continue;
         }
-        const bool got_expected_packets = n_gsof_received == ARRAY_SIZE(gsofmsgreq);
+        const bool got_expected_packets = parse_status == PARSED_AS_EXPECTED;
         ret |= got_expected_packets;
     }
     if (ret) {

diff --git a/libraries/AP_GPS/AP_GPS_GSOF.h b/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 gsofmsgreq; // [5]; = {1,2,8,9,12};
 };
 #endif