AP_GPS: add support for setting L5 override

libraries/AP_GPS/AP_GPS.cpp

@@ -347,7 +347,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
+    // @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 
     // @User: Advanced
     AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 0),
 

libraries/AP_GPS/AP_GPS.h

@@ -624,6 +624,7 @@ protected:
         UBX_Use115200     = (1U << 2U),
         UAVCAN_MBUseDedicatedBus  = (1 << 3U),
         HeightEllipsoid   = (1U << 4),
+        GPSL5HealthOverride = (1U << 5)
     };
 
     // check if an option is set

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -241,6 +241,18 @@ const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_M10[] {
 };
 
 
+/*
+  config changes for L5 modules
+*/
+const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_L5_ovrd_ena[] {
+    {ConfigKey::CFG_SIGNAL_GPS_L5_ENA, 1},
+    {ConfigKey::CFG_SIGNAL_L5_HEALTH_OVRD, 1},
+};
+
+const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_L5_ovrd_dis[] {
+    {ConfigKey::CFG_SIGNAL_L5_HEALTH_OVRD, 0},
+};
+
 void
 AP_GPS_UBLOX::_request_next_config(void)
 {
@@ -436,7 +448,24 @@ AP_GPS_UBLOX::_request_next_config(void)
         }
         break;
     }
-        
+
+    case STEP_L5: {
+        if (supports_l5 && option_set(AP_GPS::DriverOptions::GPSL5HealthOverride)) {
+            const config_list *list = config_L5_ovrd_ena;
+            const uint8_t list_length = ARRAY_SIZE(config_L5_ovrd_ena);
+            if (!_configure_config_set(list, list_length, CONFIG_L5, UBX_VALSET_LAYER_ALL)) {
+                _next_message--;
+            }
+        } else if (supports_l5 && !option_set(AP_GPS::DriverOptions::GPSL5HealthOverride)) {
+            const config_list *list = config_L5_ovrd_dis;
+            const uint8_t list_length = ARRAY_SIZE(config_L5_ovrd_dis);
+            if (!_configure_config_set(list, list_length, CONFIG_L5, UBX_VALSET_LAYER_ALL)) {
+                _next_message--;
+            }
+        }
+        break;
+    }
+
     default:
         // this case should never be reached, do a full reset if it is hit
         _next_message = STEP_PVT;
@@ -1314,6 +1343,12 @@ AP_GPS_UBLOX::_parse_gps(void)
                     }
                     _hardware_generation = UBLOX_F9;
                 }
+                // check if L1L5 in extension
+                if (memmem(_buffer.mon_ver.extension, sizeof(_buffer.mon_ver.extension), "L1L5", 4) != nullptr) {
+                    supports_l5 = true;
+                    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "u-blox supports L5 Band");
+                    _unconfigured_messages |= CONFIG_L5;
+                }
                 if (strncmp(_version.swVersion, "EXT CORE 4", 10) == 0) {
                     // a M9
                     _hardware_generation = UBLOX_M9;
@@ -1995,7 +2030,8 @@ static const char *reasons[] = {"navigation rate",
                                 "Time mode settings",
                                 "RTK MB",
                                 "TIM TM2",
-                                "M10"};
+                                "M10",
+                                "L5 Enable Disable"};
 
 static_assert((1 << ARRAY_SIZE(reasons)) == CONFIG_LAST, "UBLOX: Missing configuration description");
 

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -100,7 +100,8 @@
 #define CONFIG_RTK_MOVBASE   (1<<17)
 #define CONFIG_TIM_TM2       (1<<18)
 #define CONFIG_M10           (1<<19)
-#define CONFIG_LAST          (1<<20) // this must always be the last bit
+#define CONFIG_L5            (1<<20)
+#define CONFIG_LAST          (1<<21) // this must always be the last bit
 
 #define CONFIG_REQUIRED_INITIAL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_VELNED)
 
@@ -317,6 +318,8 @@ private:
         CFG_SIGNAL_NAVIC_ENA            = 0x10310026,
         CFG_SIGNAL_NAVIC_L5_ENA         = 0x1031001d,
 
+        CFG_SIGNAL_L5_HEALTH_OVRD       = 0x10320001,
+
         // other keys
         CFG_NAVSPG_DYNMODEL             = 0x20110021,
 
@@ -513,7 +516,7 @@ private:
     struct PACKED ubx_mon_ver {
         char swVersion[30];
         char hwVersion[10];
-        char extension; // extensions are not enabled
+        char extension[50]; // extensions are not enabled
     };
     struct PACKED ubx_nav_svinfo_header {
         uint32_t itow;
@@ -738,6 +741,7 @@ private:
         STEP_RTK_MOVBASE, // setup moving baseline
         STEP_TIM_TM2,
         STEP_M10,
+        STEP_L5,
         STEP_LAST
     };
 
@@ -872,7 +876,10 @@ private:
     RTCM3_Parser *rtcm3_parser;
 #endif // GPS_MOVING_BASELINE
 
+    bool supports_l5;
     static const config_list config_M10[];
+    static const config_list config_L5_ovrd_ena[];
+    static const config_list config_L5_ovrd_dis[];
 };
 
 #endif