AP_GPS: Support GPS_SBAS_MODE on SBF GPS units

Also swaps to using an AP_Enum for the SBAS type, and fixes up the fact
that the prearm/failure reasons should be using the config step, rather
then the init blob index

libraries/AP_GPS/AP_GPS.h

@@ -533,6 +533,12 @@ public:
     bool get_error_codes(uint8_t instance, uint32_t &error_codes) const;
     bool get_error_codes(uint32_t &error_codes) const { return get_error_codes(primary_instance, error_codes); }
 
+    enum class SBAS_Mode : int8_t {
+        Disabled = 0,
+        Enabled = 1,
+        DoNotChange = 2,
+    };
+
 protected:
 
     // configuration parameters
@@ -543,7 +549,7 @@ protected:
     AP_Int16 _sbp_logmask;
     AP_Int8 _inject_to;
     uint32_t _last_instance_swap_ms;
-    AP_Int8 _sbas_mode;
+    AP_Enum<SBAS_Mode> _sbas_mode;
     AP_Int8 _min_elevation;
     AP_Int8 _raw_data;
     AP_Int8 _gnss_mode[GPS_MAX_RECEIVERS];

libraries/AP_GPS/AP_GPS_SBF.cpp

@@ -54,6 +54,7 @@ do {                                            \
 
 constexpr const char *AP_GPS_SBF::portIdentifiers[];
 constexpr const char* AP_GPS_SBF::_initialisation_blob[];
+constexpr const char* AP_GPS_SBF::sbas_on_blob[];
 
 AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
                        AP_HAL::UARTDriver *_port) :
@@ -117,10 +118,28 @@ AP_GPS_SBF::read(void)
                                 }
                                 break;
                             case Config_State::Blob:
-                                if (asprintf(&config_string,"%s\n", (char *)_initialisation_blob[_init_blob_index]) == -1) {
+                                if (asprintf(&config_string, "%s\n", _initialisation_blob[_init_blob_index]) == -1) {
                                     config_string = nullptr;
                                 }
                                 break;
+                            case Config_State::SBAS:
+                                switch ((AP_GPS::SBAS_Mode)gps._sbas_mode) {
+                                    case AP_GPS::SBAS_Mode::Disabled:
+                                        if (asprintf(&config_string, "%s\n", sbas_off) == -1) {
+                                            config_string = nullptr;
+                                        }
+                                        break;
+                                    case AP_GPS::SBAS_Mode::Enabled:
+                                        if (asprintf(&config_string, "%s\n", sbas_on_blob[_init_blob_index]) == -1) {
+                                            config_string = nullptr;
+                                        }
+                                        break;
+                                    case AP_GPS::SBAS_Mode::DoNotChange:
+                                        config_string = nullptr;
+                                        config_step = Config_State::Complete;
+                                        break;
+                                }
+                                break;
                             case Config_State::Complete:
                               // should never reach here, why search for a config if we have fully configured already
                               INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
@@ -313,6 +332,14 @@ AP_GPS_SBF::parse(uint8_t temp)
                                 case Config_State::Blob:
                                     _init_blob_index++;
                                     if (_init_blob_index >= ARRAY_SIZE(_initialisation_blob)) {
+                                        config_step = Config_State::SBAS;
+                                        _init_blob_index = 0;
+                                    }
+                                    break;
+                                case Config_State::SBAS:
+                                    _init_blob_index++;
+                                    if ((gps._sbas_mode == AP_GPS::SBAS_Mode::Disabled)
+                                        ||_init_blob_index >= ARRAY_SIZE(sbas_on_blob)) {
                                         config_step = Config_State::Complete;
                                     }
                                     break;
@@ -529,15 +556,19 @@ AP_GPS_SBF::process_message(void)
 void AP_GPS_SBF::broadcast_configuration_failure_reason(void) const
 {
     if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE &&
-        _init_blob_index < ARRAY_SIZE(_initialisation_blob)) {
+        config_step != Config_State::Complete) {
-        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %u: SBF is not fully configured (%u/%u)", state.instance + 1,
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %u: SBF is not fully configured (%u/%u/%u/%u)",
-                        _init_blob_index, (unsigned)ARRAY_SIZE(_initialisation_blob));
+                                         state.instance + 1,
+                                         (unsigned)config_step,
+                                         _init_blob_index,
+                                         (unsigned)ARRAY_SIZE(_initialisation_blob),
+                                         (unsigned)ARRAY_SIZE(sbas_on_blob));
     }
 }
 
 bool AP_GPS_SBF::is_configured (void) const {
-    return (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE ||
+    return ((gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE) ||
-             _init_blob_index >= ARRAY_SIZE(_initialisation_blob));
+            (config_step == Config_State::Complete));
 }
 
 bool AP_GPS_SBF::is_healthy (void) const {

libraries/AP_GPS/AP_GPS_SBF.h

@@ -70,6 +70,7 @@ private:
         Baud_Rate,
         SSO,
         Blob,
+        SBAS,
         Complete
     };
     Config_State config_step;
@@ -83,6 +84,12 @@ private:
     GPS_SBF_EXTRA_CONFIG
 #endif
     };
+    static constexpr const char* sbas_off = "sst, -SBAS";
+    static constexpr const char* sbas_on_blob[] = {
+                                                   "snt,+GEOL1+GEOL5",
+                                                   "sst,+SBAS",
+                                                   "ssbc,auto,Operational,MixedSystems,auto",
+                                                  };
     uint32_t _config_last_ack_time;
 
     const char* _port_enable = "\nSSSSSSSSSS\n";

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -255,7 +255,7 @@ AP_GPS_UBLOX::_request_next_config(void)
         }
         break;
     case STEP_POLL_SBAS:
-        if (gps._sbas_mode != 2) {
+        if (gps._sbas_mode != AP_GPS::SBAS_Mode::DoNotChange) {
             _send_message(CLASS_CFG, MSG_CFG_SBAS, nullptr, 0);
         } else {
             _unconfigured_messages &= ~CONFIG_SBAS;
@@ -1016,7 +1016,7 @@ AP_GPS_UBLOX::_parse_gps(void)
 #endif
 
         case MSG_CFG_SBAS:
-            if (gps._sbas_mode != 2) {
+            if (gps._sbas_mode != AP_GPS::SBAS_Mode::DoNotChange) {
 	        Debug("Got SBAS settings %u %u %u 0x%x 0x%x\n", 
                       (unsigned)_buffer.sbas.mode,
                       (unsigned)_buffer.sbas.usage,