AP_GPS: add UBX_DebugMessages driver option

GPS_DRV_OPTIONS bit 10 enables F9/HPG debug streams (NAV-SAT, NAV-SIG,
NAV-CLOCK, MON-RF, MON-HW3, MON-COMMS, MON-SPAN) on UART1. The boot
blob switches the receiver to 921600 to fit the extra bandwidth, and
the UART RX buffer floors at 4096 to absorb a full nav epoch in one
burst. Debug message ids are added to the parser as consume-cases so
the unknown-message disable path does not race with the enable list.

Also fixes UBLOX_SET_BINARY_921600 NMEA checksum (was 1F, correct 17)
and verifies all MSGOUT key IDs against u-blox HPG 2.02.

Author: bugobliterator

libraries/AP_GPS/AP_GPS.cpp

@@ -71,7 +71,7 @@
 extern const AP_HAL::HAL &hal;
 
 // baudrates to try to detect GPSes with
-const uint32_t AP_GPS::_baudrates[] = {9600U, 115200U, 4800U, 19200U, 38400U, 57600U, 230400U, 460800U};
+const uint32_t AP_GPS::_baudrates[] = {9600U, 115200U, 4800U, 19200U, 38400U, 57600U, 230400U, 460800U, 921600U};
 #ifndef AP_GPS_UBLOX_DEFAULT_BAUDRATE
 #define AP_GPS_UBLOX_DEFAULT_BAUDRATE 230400
 #endif
@@ -242,7 +242,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     // @Param: _DRV_OPTIONS
     // @DisplayName: driver options
     // @Description: Additional backend specific options
-    // @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200,3:Use dedicated CAN port b/w GPSes for moving baseline,4:Use ellipsoid height instead of AMSL, 5:Override GPS satellite health of L5 band from L1 health, 6:Enable RTCM full parse even for a single channel, 7:Disable automatic full RTCM parsing when RTCM seen on more than one channel
+    // @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200,3:Use dedicated CAN port b/w GPSes for moving baseline,4:Use ellipsoid height instead of AMSL, 5:Override GPS satellite health of L5 band from L1 health, 6:Enable RTCM full parse even for a single channel, 7:Disable automatic full RTCM parsing when RTCM seen on more than one channel, 8:HSI trim using PPS (ublox), 9:HSI trim stats, 10:Enable u-blox debug messages on UART1
     // @User: Advanced
     AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 0),
 
@@ -540,6 +540,12 @@ void AP_GPS::send_blob_start(uint8_t instance)
     const auto type = params[instance].type;
 
 #if AP_GPS_UBLOX_ENABLED
+    if (type == GPS_TYPE_UBLOX && option_set(DriverOptions::UBX_DebugMessages)) {
+        // bump UART1 to 921600 so the extra debug streams fit within nav-rate
+        static const char blob[] = UBLOX_SET_BINARY_921600;
+        send_blob_start(instance, blob, sizeof(blob));
+        return;
+    }
     if (type == GPS_TYPE_UBLOX && option_set(DriverOptions::UBX_Use115200)) {
         static const char blob[] = UBLOX_SET_BINARY_115200;
         send_blob_start(instance, blob, sizeof(blob));
@@ -718,6 +724,16 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance)
         if (type == GPS_TYPE_UBLOX_RTK_BASE) {
             rx_size = 2048;
         }
+#if AP_GPS_UBLOX_ENABLED
+        if ((type == GPS_TYPE_AUTO || type == GPS_TYPE_UBLOX ||
+             type == GPS_TYPE_UBLOX_RTK_BASE || type == GPS_TYPE_UBLOX_RTK_ROVER) &&
+            option_set(DriverOptions::UBX_DebugMessages)) {
+            // F9 debug streams (NAV-SAT/SIG/CLOCK + MON-RF/HW3/COMMS/SPAN)
+            // push several KB per nav epoch in a single burst at 921600 -
+            // give the RX path enough headroom to absorb a full epoch
+            rx_size = MAX(rx_size, (uint16_t)4096);
+        }
+#endif
         _port[instance]->begin(dstate->probe_baud, rx_size, tx_size);
         _port[instance]->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
         dstate->last_baud_change_ms = now;
@@ -768,7 +784,10 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance)
              type == GPS_TYPE_UBLOX) &&
             ((!_auto_config && _baudrates[dstate->current_baud] >= 38400) ||
              (_baudrates[dstate->current_baud] >= 115200 && option_set(DriverOptions::UBX_Use115200)) ||
-             _baudrates[dstate->current_baud] == AP_GPS_UBLOX_DEFAULT_BAUDRATE) &&
+             _baudrates[dstate->current_baud] == AP_GPS_UBLOX_DEFAULT_BAUDRATE ||
+             // also accept 921600 so we can re-detect a u-blox that was
+             // previously configured to 921600 by the debug-msgs option
+             _baudrates[dstate->current_baud] == 921600U) &&
             AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) {
             return new AP_GPS_UBLOX(*this, params[instance], state[instance], _port[instance], GPS_ROLE_NORMAL);
         }

libraries/AP_GPS/AP_GPS.h

@@ -648,6 +648,7 @@ class AP_GPS
         DisableRTCMDecode = (1U << 7),
         HSITrimUsingPPS = (1U << 8),
         HSITrimStats = (1U << 9),
+        UBX_DebugMessages = (1U << 10),
     };
 
     // check if an option is set

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -276,6 +276,34 @@ const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_L5_ovrd_dis[] {
     {ConfigKey::CFG_SIGNAL_L5_HEALTH_OVRD, 0},
 };
 
+/*
+  enable F9 debug message outputs on UART1.
+  NAV-PVT/STATUS/VELNED/DOP/TIMEGPS and RXM-RAWX are excluded - they
+  are managed by the standard rate-config path (CONFIG_RATE_*) and
+  would otherwise race with this list.
+  driven by GPS_DRV_OPTIONS bit 10 (UBX_DebugMessages).
+*/
+const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_F9_debug_uart1[] {
+    { ConfigKey::MSGOUT_UBX_NAV_SAT_UART1,     1},
+    { ConfigKey::MSGOUT_UBX_NAV_CLOCK_UART1,   1},
+    { ConfigKey::MSGOUT_UBX_NAV_SIG_UART1,     1},
+    { ConfigKey::MSGOUT_UBX_MON_RF_UART1,      5},
+    { ConfigKey::MSGOUT_UBX_MON_HW3_UART1,     5},
+    { ConfigKey::MSGOUT_UBX_MON_COMMS_UART1,   5},
+    { ConfigKey::MSGOUT_UBX_MON_SPAN_UART1,    5},
+};
+
+// disable variant - applied when UBX_DebugMessages is cleared
+const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_F9_debug_uart1_dis[] {
+    { ConfigKey::MSGOUT_UBX_NAV_SAT_UART1,     0},
+    { ConfigKey::MSGOUT_UBX_NAV_CLOCK_UART1,   0},
+    { ConfigKey::MSGOUT_UBX_NAV_SIG_UART1,     0},
+    { ConfigKey::MSGOUT_UBX_MON_RF_UART1,      0},
+    { ConfigKey::MSGOUT_UBX_MON_HW3_UART1,     0},
+    { ConfigKey::MSGOUT_UBX_MON_COMMS_UART1,   0},
+    { ConfigKey::MSGOUT_UBX_MON_SPAN_UART1,    0},
+};
+
 void
 AP_GPS_UBLOX::_request_next_config(void)
 {
@@ -497,6 +525,30 @@ AP_GPS_UBLOX::_request_next_config(void)
         break;
     }
 
+    case STEP_F9_DEBUG: {
+        if (supports_F9_config()) {
+            const config_list *list;
+            uint8_t list_length;
+            if (option_set(AP_GPS::DriverOptions::UBX_DebugMessages)) {
+                list = config_F9_debug_uart1;
+                list_length = ARRAY_SIZE(config_F9_debug_uart1);
+                Debug("Enabling F9 debug messages on UART1");
+            } else {
+                list = config_F9_debug_uart1_dis;
+                list_length = ARRAY_SIZE(config_F9_debug_uart1_dis);
+            }
+            // baud switch is handled by AP_GPS::send_blob_start() during
+            // detection - it picks UBLOX_SET_BINARY_921600 when this
+            // option is set, mirroring the moving-baseline 460800 path
+            if (!_configure_config_set(list, list_length, CONFIG_F9_DEBUG, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) {
+                _next_message--;
+            }
+        } else {
+            _unconfigured_messages &= ~CONFIG_F9_DEBUG;
+        }
+        break;
+    }
+
     default:
         // this case should never be reached, do a full reset if it is hit
         _next_message = STEP_PVT;
@@ -1409,9 +1461,17 @@ AP_GPS_UBLOX::_parse_gps(void)
             break;
         case MSG_MON_HW2:
             if (_payload_length == 28) {
-                log_mon_hw2();  
+                log_mon_hw2();
             }
             break;
+        // F9 debug streams (enabled via CONFIG_F9_DEBUG). We don't decode
+        // them but must consume them so the unknown-message default path
+        // does not auto-disable them.
+        case MSG_MON_RF:
+        case MSG_MON_HW3:
+        case MSG_MON_COMMS:
+        case MSG_MON_SPAN:
+            break;
         case MSG_MON_VER: {
             bool check_L1L5 = false;
             _have_version = true;
@@ -1430,6 +1490,8 @@ AP_GPS_UBLOX::_parse_gps(void)
                     if (_hardware_generation != UBLOX_F9) {
                         // need to ensure time mode is correctly setup on F9
                         _unconfigured_messages |= CONFIG_TMODE_MODE;
+                        // F9 supports MSGOUT MSGOUT_UBX_* keys for debug streams
+                        _unconfigured_messages |= CONFIG_F9_DEBUG;
                     }
                     _hardware_generation = UBLOX_F9;
                 }
@@ -1862,6 +1924,13 @@ AP_GPS_UBLOX::_parse_gps(void)
         _configure_message_rate(CLASS_NAV, MSG_NAV_SVINFO, 0);
         break;
         }
+    // F9 debug streams (enabled via CONFIG_F9_DEBUG). We don't decode
+    // them but must consume them so the default path below does not
+    // auto-disable them when the option is on.
+    case MSG_NAV_SAT:
+    case MSG_NAV_SIG:
+    case MSG_NAV_CLOCK:
+        break;
     default:
         Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id);
         if (++_disable_counter == 0) {
@@ -2231,7 +2300,8 @@ static const char *reasons[] = {"navigation rate",
                                 "RTK MB",
                                 "TIM TM2",
                                 "M10",
-                                "L5 Enable Disable"};
+                                "L5 Enable Disable",
+                                "F9 debug messages"};
 
 static_assert((1 << ARRAY_SIZE(reasons)) == CONFIG_LAST, "UBLOX: Missing configuration description");
 

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -55,6 +55,9 @@
 // a variant with 460800 baudrate
 #define UBLOX_SET_BINARY_460800 "\265\142\006\001\003\000\001\006\001\022\117$PUBX,41,1,0023,0001,460800,0*11\r\n"
 
+// a variant with 921600 baudrate
+#define UBLOX_SET_BINARY_921600 "\265\142\006\001\003\000\001\006\001\022\117$PUBX,41,1,0023,0001,921600,0*17\r\n"
+
 #define UBLOX_RXM_RAW_LOGGING 1
 #define UBLOX_MAX_RXM_RAW_SATS 22
 #define UBLOX_MAX_RXM_RAWX_SATS 80
@@ -101,7 +104,8 @@
 #define CONFIG_TIM_TM2       (1<<18)
 #define CONFIG_M10           (1<<19)
 #define CONFIG_L5            (1<<20)
-#define CONFIG_LAST          (1<<21) // this must always be the last bit
+#define CONFIG_F9_DEBUG      (1<<21)
+#define CONFIG_LAST          (1<<22) // this must always be the last bit
 
 #define CONFIG_REQUIRED_INITIAL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_VELNED)
 
@@ -296,6 +300,18 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
         MSGOUT_RTCM_3X_TYPE1230_UART2   = 0x20910305,
         MSGOUT_UBX_NAV_RELPOSNED_UART2  = 0x2091008f,
 
+        // F9 debug message outputs on UART1 (used by CONFIG_F9_DEBUG).
+        // NAV-PVT/STATUS/VELNED/DOP/TIMEGPS and RXM-RAWX are intentionally
+        // omitted - they are owned by the standard rate-config path.
+        // Values cross-checked against u-blox HPG 2.02 interface description.
+        MSGOUT_UBX_NAV_SAT_UART1        = 0x20910016,
+        MSGOUT_UBX_NAV_CLOCK_UART1      = 0x20910066,
+        MSGOUT_UBX_NAV_SIG_UART1        = 0x20910346,
+        MSGOUT_UBX_MON_COMMS_UART1      = 0x20910350,
+        MSGOUT_UBX_MON_HW3_UART1        = 0x20910355,
+        MSGOUT_UBX_MON_RF_UART1         = 0x2091035a,
+        MSGOUT_UBX_MON_SPAN_UART1       = 0x2091038c,
+
         // enable specific signals and constellations
         CFG_SIGNAL_GPS_ENA              = 0x1031001f,
         CFG_SIGNAL_GPS_L1CA_ENA         = 0x10310001,
@@ -701,7 +717,18 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
         MSG_MON_HW = 0x09,
         MSG_MON_HW2 = 0x0B,
         MSG_MON_VER = 0x04,
+        // CLASS_MON debug message ids (consumed silently when GPS_DRV_OPTIONS
+        // bit 10 has them enabled so the unknown-message disable counter
+        // does not race with the F9 debug VALSET path)
+        MSG_MON_SPAN = 0x31,
+        MSG_MON_COMMS = 0x36,
+        MSG_MON_HW3 = 0x37,
+        MSG_MON_RF = 0x38,
         MSG_NAV_SVINFO = 0x30,
+        // CLASS_NAV debug message ids (same rationale as the MON_* group)
+        MSG_NAV_CLOCK = 0x22,
+        MSG_NAV_SAT = 0x35,
+        MSG_NAV_SIG = 0x43,
         MSG_RXM_RAW = 0x10,
         MSG_RXM_RAWX = 0x15,
         MSG_TIM_TM2 = 0x03
@@ -765,6 +792,7 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
         STEP_TIM_TM2,
         STEP_M10,
         STEP_L5,
+        STEP_F9_DEBUG,
         STEP_LAST
     };
 
@@ -816,7 +844,7 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
     bool _cfg_needs_save;
 
     bool noReceivedHdop;
-    
+
     bool havePvtMsg;
 
     bool        _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
@@ -925,6 +953,10 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
     static const config_list config_M10[];
     static const config_list config_L5_ovrd_ena[];
     static const config_list config_L5_ovrd_dis[];
+
+    // F9 debug message MSGOUT enable/disable on UART1
+    static const config_list config_F9_debug_uart1[];
+    static const config_list config_F9_debug_uart1_dis[];
     bool in_safeboot_mode;
 };