AP_GPS: add support for auto config Moving Baseline for AP_Periph GPS

libraries/AP_GPS/AP_GPS.cpp

@@ -183,7 +183,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     // @Param: _AUTO_CONFIG
     // @DisplayName: Automatic GPS configuration
     // @Description: Controls if the autopilot should automatically configure the GPS based on the parameters and default settings
-    // @Values: 0:Disables automatic configuration,1:Enable automatic configuration
+    // @Values: 0:Disables automatic configuration,1:Enable automatic configuration,2:Enable automatic configuration for UAVCAN as well
     // @User: Advanced
     AP_GROUPINFO("_AUTO_CONFIG", 13, AP_GPS, _auto_config, 1),
 
@@ -300,7 +300,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
+    // @Bitmask: 0:Use UART2/2nd CAN for moving baseline on ublox/uavcan,,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200
     // @User: Advanced
     AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 0),
 #endif
@@ -642,7 +642,7 @@ void AP_GPS::detect_instance(uint8_t instance)
         _port[instance]->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
         dstate->last_baud_change_ms = now;
 
-        if (_auto_config == GPS_AUTO_CONFIG_ENABLE && new_gps == nullptr) {
+        if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY && new_gps == nullptr) {
             if (_type[instance] == GPS_TYPE_HEMI) {
                 send_blob_start(instance, AP_GPS_NMEA_HEMISPHERE_INIT_STRING, strlen(AP_GPS_NMEA_HEMISPHERE_INIT_STRING));
             } else if ((_type[instance] == GPS_TYPE_UBLOX_RTK_BASE ||
@@ -663,7 +663,7 @@ void AP_GPS::detect_instance(uint8_t instance)
         }
     }
 
-    if (_auto_config == GPS_AUTO_CONFIG_ENABLE && new_gps == nullptr) {
+    if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY && new_gps == nullptr) {
         send_blob_update(instance);
     }
 
@@ -807,7 +807,7 @@ void AP_GPS::update_instance(uint8_t instance)
         return;
     }
 
-    if (_auto_config == GPS_AUTO_CONFIG_ENABLE) {
+    if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY) {
         send_blob_update(instance);
     }
 
@@ -1915,7 +1915,7 @@ bool AP_GPS::is_healthy(uint8_t instance) const
       happens with the RTCMv3 data
      */
     const uint8_t delay_threshold = 2;
-    const float delay_avg_max = _type[instance] == (GPS_TYPE_UBLOX_RTK_ROVER || GPS_TYPE_UAVCAN_RTK_ROVER)?245:215;
+    const float delay_avg_max = ((_type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) || (_type[instance] == GPS_TYPE_UAVCAN_RTK_ROVER))?245:215;
     const GPS_timing &t = timing[instance];
     bool delay_ok = (t.delayed_count < delay_threshold) &&
         t.average_delta_ms < delay_avg_max &&

libraries/AP_GPS/AP_GPS.h

@@ -694,7 +694,8 @@ private:
     // Auto configure types
     enum GPS_AUTO_CONFIG {
         GPS_AUTO_CONFIG_DISABLE = 0,
-        GPS_AUTO_CONFIG_ENABLE  = 1
+        GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY  = 1,
+        GPS_AUTO_CONFIG_ENABLE_ALL = 2,
     };
 
     enum class GPSAutoSwitch {

libraries/AP_GPS/AP_GPS_UAVCAN.cpp

@@ -39,6 +39,21 @@
 
 extern const AP_HAL::HAL& hal;
 
+#define GPS_UAVCAN_DEBUGGING 0
+
+#if GPS_UAVCAN_DEBUGGING
+#if defined(HAL_BUILD_AP_PERIPH)
+ extern "C" {
+   void can_printf(const char *fmt, ...);
+ }
+ # define Debug(fmt, args ...)  do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0)
+#else
+ # define Debug(fmt, args ...)  do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
+#endif
+#else
+ # define Debug(fmt, args ...)
+#endif
+
 #define LOG_TAG "GPS"
 
 UC_REGISTRY_BINDER(FixCb, uavcan::equipment::gnss::Fix);
@@ -58,7 +73,11 @@ HAL_Semaphore AP_GPS_UAVCAN::_sem_registry;
 AP_GPS_UAVCAN::AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_GPS::GPS_Role _role) :
     AP_GPS_Backend(_gps, _state, nullptr),
     role(_role)
-{}
+{
+    param_int_cb = FUNCTOR_BIND_MEMBER(&AP_GPS_UAVCAN::handle_param_get_set_response_int, bool, AP_UAVCAN*, const uint8_t, const char*, int32_t &);
+    param_float_cb = FUNCTOR_BIND_MEMBER(&AP_GPS_UAVCAN::handle_param_get_set_response_float, bool, AP_UAVCAN*, const uint8_t, const char*, float &);
+    param_save_cb = FUNCTOR_BIND_MEMBER(&AP_GPS_UAVCAN::handle_param_save_response, void, AP_UAVCAN*, const uint8_t, bool);
+}
 
 AP_GPS_UAVCAN::~AP_GPS_UAVCAN()
 {
@@ -185,7 +204,7 @@ AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
         return NULL;
     }
     // initialise the backend based on the UAVCAN Moving baseline selection
-    switch (_gps.get_type(found_match)) {
+    switch (_gps.get_type(_state.instance)) {
         case AP_GPS::GPS_TYPE_UAVCAN:
             backend = new AP_GPS_UAVCAN(_gps, _state, AP_GPS::GPS_ROLE_NORMAL);
             break;
@@ -738,9 +757,49 @@ void AP_GPS_UAVCAN::handle_relposheading_msg_trampoline(AP_UAVCAN* ap_uavcan, ui
 }
 #endif
 
+bool AP_GPS_UAVCAN::do_config()
+{
+    AP_UAVCAN *ap_uavcan = _detected_modules[_detected_module].ap_uavcan;
+    if (ap_uavcan == nullptr) {
+        return false;
+    }
+    uint8_t node_id = _detected_modules[_detected_module].node_id;
+    
+    switch(cfg_step) {
+        case STEP_SET_TYPE:
+            ap_uavcan->get_parameter_on_node(node_id, "GPS_TYPE", &param_int_cb);
+            break;
+        case STEP_SET_MB_CAN_TX:
+            if (role != AP_GPS::GPS_Role::GPS_ROLE_NORMAL) {
+                ap_uavcan->get_parameter_on_node(node_id, "GPS_MB_ONLY_PORT", &param_int_cb);
+            } else {
+                cfg_step++;
+            }
+            break;
+        case STEP_SAVE_AND_REBOOT:
+            if (requires_save_and_reboot) {
+                ap_uavcan->save_parameters_on_node(node_id, &param_save_cb);
+            } else {
+                cfg_step++;
+            }
+            break;
+        case STEP_FINISHED:
+            return true;
+        default:
+            break;
+    }
+    return false;
+}
+
 // Consume new data and mark it received
 bool AP_GPS_UAVCAN::read(void)
 {
+    if (gps._auto_config >= AP_GPS::GPS_AUTO_CONFIG_ENABLE_ALL) {
+        if (!do_config()) {
+            return false;
+        }
+    }
+
     WITH_SEMAPHORE(sem);
     if (_new_data) {
         _new_data = false;
@@ -805,4 +864,67 @@ void AP_GPS_UAVCAN::inject_data(const uint8_t *data, uint16_t len)
     }
 }
 
+/*
+    handle param get/set response
+*/
+bool AP_GPS_UAVCAN::handle_param_get_set_response_int(AP_UAVCAN* ap_uavcan, uint8_t node_id, const char* name, int32_t &value)
+{
+    Debug("AP_GPS_UAVCAN: param set/get response from %d %s %ld\n", node_id, name, value);
+    if (strcmp(name, "GPS_TYPE") == 0 && cfg_step == STEP_SET_TYPE) {
+        if (role == AP_GPS::GPS_ROLE_MB_BASE && value != AP_GPS::GPS_TYPE_UBLOX_RTK_BASE) {
+            value = (int32_t)AP_GPS::GPS_TYPE_UBLOX_RTK_BASE;
+            requires_save_and_reboot = true;
+            return true;
+        } else if (role == AP_GPS::GPS_ROLE_MB_ROVER && value != AP_GPS::GPS_TYPE_UBLOX_RTK_ROVER) {
+            value = (int32_t)AP_GPS::GPS_TYPE_UBLOX_RTK_ROVER;
+            requires_save_and_reboot = true;
+            return true;
+        } else {
+            cfg_step++;
+        }
+    }
+
+    if (strcmp(name, "GPS_MB_ONLY_PORT") == 0 && cfg_step == STEP_SET_MB_CAN_TX) {
+        if (gps._driver_options == 0 && value != 1) {
+            // set up so that another CAN port is used for the Moving Baseline Data
+            // setting this value will allow another CAN port to be used as dedicated
+            // line for the Moving Baseline Data
+            value = 1;
+            requires_save_and_reboot = true;
+            return true;
+        } else if (gps._driver_options != 0 && value != 0) {
+            // set up so that all CAN ports are used for the Moving Baseline Data
+            value = 0;
+            requires_save_and_reboot = true;
+            return true;
+        } else {
+            cfg_step++;
+        }
+    }
+    return false;
+}
+
+bool AP_GPS_UAVCAN::handle_param_get_set_response_float(AP_UAVCAN* ap_uavcan, uint8_t node_id, const char* name, float &value)
+{
+    Debug("AP_GPS_UAVCAN: param set/get response from %d %s %f\n", node_id, name, value);
+    return false;
+}
+
+void AP_GPS_UAVCAN::handle_param_save_response(AP_UAVCAN* ap_uavcan, const uint8_t node_id, bool success)
+{
+    Debug("AP_GPS_UAVCAN: param save response from %d %s\n", node_id, success ? "success" : "failure");
+
+    if (cfg_step != STEP_SAVE_AND_REBOOT) {
+        return;
+    }
+
+    if (success) {
+        cfg_step++;
+    }
+    // Also send reboot command
+    // this is ok as we are sending from UAVCAN thread context
+    Debug("AP_GPS_UAVCAN: sending reboot command %d\n", node_id);
+    ap_uavcan->send_reboot_request(node_id);
+}
+
 #endif // HAL_ENABLE_LIBUAVCAN_DRIVERS

libraries/AP_GPS/AP_GPS_UAVCAN.h

@@ -71,7 +71,22 @@ public:
     bool get_RTCMV3(const uint8_t *&data, uint16_t &len) override;
     void clear_RTCMV3() override;
 #endif
+
 private:
+
+    bool param_configured = true;
+    enum config_step {
+        STEP_SET_TYPE = 0,
+        STEP_SET_MB_CAN_TX,
+        STEP_SAVE_AND_REBOOT,
+        STEP_FINISHED
+    };
+    uint8_t cfg_step;
+    bool requires_save_and_reboot;
+
+    // returns true once configuration has finished
+    bool do_config(void);
+
     void handle_fix_msg(const FixCb &cb);
     void handle_fix2_msg(const Fix2Cb &cb);
     void handle_aux_msg(const AuxCb &cb);
@@ -79,8 +94,8 @@ private:
     void handle_status_msg(const StatusCb &cb);
 
 #if GPS_MOVING_BASELINE
-    void handle_moving_baseline_msg(const MovingBaselineDataCb &cb);
+    void handle_moving_baseline_msg(const MovingBaselineDataCb &cb, uint8_t node_id);
-    void handle_relposheading_msg(const RelPosHeadingCb &cb);
+    void handle_relposheading_msg(const RelPosHeadingCb &cb, uint8_t node_id);
 #endif
 
     static bool take_registry();
@@ -120,4 +135,11 @@ private:
     // the role set from GPS_TYPE
     AP_GPS::GPS_Role role;
 
+    FUNCTOR_DECLARE(param_int_cb, bool, AP_UAVCAN*, const uint8_t, const char*, int32_t &);
+    FUNCTOR_DECLARE(param_float_cb, bool, AP_UAVCAN*, const uint8_t, const char*, float &);
+    FUNCTOR_DECLARE(param_save_cb, void, AP_UAVCAN*, const uint8_t, bool);
+
+    bool handle_param_get_set_response_int(AP_UAVCAN* ap_uavcan, const uint8_t node_id, const char* name, int32_t &value);
+    bool handle_param_get_set_response_float(AP_UAVCAN* ap_uavcan, const uint8_t node_id, const char* name, float &value);
+    void handle_param_save_response(AP_UAVCAN* ap_uavcan, const uint8_t node_id, bool success);
 };

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -1866,7 +1866,7 @@ bool AP_GPS_UBLOX::get_lag(float &lag_sec) const
         // always bail out in this case, it's used to indicate we have yet to receive a valid
         // hardware generation, however the user may have inhibited us detecting the generation
         // so if we aren't allowed to do configuration, we will accept this as the default delay
-        return gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_ENABLE;
+        return gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE;
     case UBLOX_5:
     case UBLOX_6:
     default: