AP_Airspeed: use DEVID to maintain lineup of CAN sensors

persist DEVID and use it to ensure that we keep the order of DroneCAN
sensors between boots. It still allows for a sensor to be swapped out
for a new one, while keeping slot of the one that hasn't been removed

libraries/AP_Airspeed/AP_Airspeed.cpp

@@ -464,7 +464,7 @@ void AP_Airspeed::init()
             break;
         case TYPE_UAVCAN:
 #if AP_AIRSPEED_UAVCAN_ENABLED
-            sensor[i] = AP_Airspeed_UAVCAN::probe(*this, i);
+            sensor[i] = AP_Airspeed_UAVCAN::probe(*this, i, uint32_t(param[i].bus_id.get()));
 #endif
             break;
         case TYPE_NMEA_WATER:
@@ -489,7 +489,30 @@ void AP_Airspeed::init()
             num_sensors = i+1;
         }
     }
+
+#if AP_AIRSPEED_UAVCAN_ENABLED
+    // we need a 2nd pass for DroneCAN sensors so we can match order by DEVID
+    // the 2nd pass accepts any devid
+    for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
+        if (sensor[i] == nullptr && (enum airspeed_type)param[i].type.get() == TYPE_UAVCAN) {
+            sensor[i] = AP_Airspeed_UAVCAN::probe(*this, i, 0);
+            if (sensor[i] != nullptr) {
+                num_sensors = i+1;
+            }
+        }
+    }
+#endif // AP_AIRSPEED_UAVCAN_ENABLED
 #endif // HAL_AIRSPEED_PROBE_LIST
+
+    // set DEVID to zero for any sensors not found. This allows backends to order
+    // based on previous value of DEVID. This allows for swapping out sensors
+    for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
+        if (sensor[i] == nullptr) {
+            // note we use set() not set_and_save() to allow a sensor to be temporarily
+            // removed for one boot without losing its slot
+            param[i].bus_id.set(0);
+        }
+    }
 }
 
 // read the airspeed sensor

libraries/AP_Airspeed/AP_Airspeed_Backend.cpp

@@ -54,3 +54,8 @@ bool AP_Airspeed_Backend::bus_is_confgured(void) const
 {
     return frontend.param[instance].bus.configured();
 }
+
+void AP_Airspeed_Backend::set_bus_id(uint32_t id)
+{
+    frontend.param[instance].bus_id.set_and_save(int32_t(id));
+}

libraries/AP_Airspeed/AP_Airspeed_Backend.h

@@ -91,9 +91,7 @@ protected:
     }
 
     // set bus ID of this instance, for ARSPD_DEVID parameters
-    void set_bus_id(uint32_t id) {
+    void set_bus_id(uint32_t id);
-        frontend.param[instance].bus_id.set(int32_t(id));
-    }
 
     enum class DevType {
         SITL     = 0x01,

libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp

@@ -39,7 +39,7 @@ void AP_Airspeed_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
     }
 }
 
-AP_Airspeed_Backend* AP_Airspeed_UAVCAN::probe(AP_Airspeed &_frontend, uint8_t _instance)
+AP_Airspeed_Backend* AP_Airspeed_UAVCAN::probe(AP_Airspeed &_frontend, uint8_t _instance, uint32_t previous_devid)
 {
     WITH_SEMAPHORE(_sem_registry);
 
@@ -47,23 +47,28 @@ AP_Airspeed_Backend* AP_Airspeed_UAVCAN::probe(AP_Airspeed &_frontend, uint8_t _
 
     for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) {
         if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_uavcan != nullptr) {
+            const auto bus_id = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN,
+                                                            _detected_modules[i].ap_uavcan->get_driver_index(),
+                                                            _detected_modules[i].node_id, 0);
+            if (previous_devid != 0 && previous_devid != bus_id) {
+                // match with previous ID only
+                continue;
+            }
             backend = new AP_Airspeed_UAVCAN(_frontend, _instance);
             if (backend == nullptr) {
-                AP::can().log_text(AP_CANManager::LOG_INFO, 
+                AP::can().log_text(AP_CANManager::LOG_INFO,
-                                      LOG_TAG,
+                                   LOG_TAG,
-                                      "Failed register UAVCAN Airspeed Node %d on Bus %d\n",
+                                   "Failed register UAVCAN Airspeed Node %d on Bus %d\n",
-                                      _detected_modules[i].node_id,
+                                   _detected_modules[i].node_id,
-                                      _detected_modules[i].ap_uavcan->get_driver_index());
+                                   _detected_modules[i].ap_uavcan->get_driver_index());
             } else {
                 _detected_modules[i].driver = backend;
-                AP::can().log_text(AP_CANManager::LOG_INFO, 
+                AP::can().log_text(AP_CANManager::LOG_INFO,
-                                      LOG_TAG,
+                                   LOG_TAG,
-                                      "Registered UAVCAN Airspeed Node %d on Bus %d\n",
+                                   "Registered UAVCAN Airspeed Node %d on Bus %d\n",
-                                      _detected_modules[i].node_id,
+                                   _detected_modules[i].node_id,
-                                      _detected_modules[i].ap_uavcan->get_driver_index());
+                                   _detected_modules[i].ap_uavcan->get_driver_index());
-                backend->set_bus_id(AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN,
+                backend->set_bus_id(bus_id);
-                                                                _detected_modules[i].ap_uavcan->get_driver_index(),
-                                                                _detected_modules[i].node_id, 0));
             }
             break;
         }

libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h

@@ -28,7 +28,7 @@ public:
 
     static void subscribe_msgs(AP_UAVCAN* ap_uavcan);
 
-    static AP_Airspeed_Backend* probe(AP_Airspeed &_fronted, uint8_t _instance);
+    static AP_Airspeed_Backend* probe(AP_Airspeed &_fronted, uint8_t _instance, uint32_t previous_devid);
 
 private: