AP_Proximity_UAVCAN:修复重启才有数据

libraries/AP_Proximity/AP_Proximity.cpp

@@ -328,7 +328,12 @@ void AP_Proximity::detect_instance(uint8_t instance)
             return;
         }
         break;
-
+#if HAL_WITH_UAVCAN
+ case Type::UAVCAN:
+        state[instance].instance = instance;
+        drivers[instance] = AP_Proximity_UAVCAN::probe(*this, state[instance]);
+        return;
+#endif
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     case Type::SITL:
         state[instance].instance = instance;
@@ -345,10 +350,6 @@ void AP_Proximity::detect_instance(uint8_t instance)
         drivers[instance] = new AP_Proximity_AirSimSITL(*this, state[instance]);
         return;
 #endif
-    case Type::UAVCAN:
-        state[instance].instance = instance;
-        drivers[instance] = AP_Proximity_UAVCAN::probe(*this, state[instance]);
-        return;
     }
 }
 

libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp

@@ -83,7 +83,7 @@ AP_Proximity_Backend *AP_Proximity_UAVCAN::probe(AP_Proximity &_frontend,
     return backend;
 }
 
-AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavcan)
+AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavcan,uint8_t node_id)
 {
     if (ap_uavcan == nullptr)
     {
@@ -93,7 +93,8 @@ AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavca
     for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++)
     {
         if (_detected_modules[i].driver != nullptr &&
-            _detected_modules[i].ap_uavcan == ap_uavcan)
+            _detected_modules[i].ap_uavcan == ap_uavcan&&
+            _detected_modules[i].node_id == node_id)    
         {
             return _detected_modules[i].driver;
         }
@@ -102,7 +103,8 @@ AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavca
     bool detected = false;
     for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++)
     {
-        if (_detected_modules[i].ap_uavcan == ap_uavcan)
+        if (_detected_modules[i].ap_uavcan == ap_uavcan &&
+            _detected_modules[i].node_id == node_id)
         {
             // detected
             detected = true;
@@ -117,6 +119,7 @@ AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavca
             if (_detected_modules[i].ap_uavcan == nullptr)
             {
                 _detected_modules[i].ap_uavcan = ap_uavcan;
+                _detected_modules[i].node_id = node_id;
                 break;
             }
         }
@@ -163,7 +166,7 @@ void AP_Proximity_UAVCAN::handle_proximity_data_trampoline(AP_UAVCAN *ap_uavcan,
 {
     WITH_SEMAPHORE(_sem_registry);
 
-    AP_Proximity_UAVCAN *driver = get_uavcan_backend(ap_uavcan);
+    AP_Proximity_UAVCAN *driver = get_uavcan_backend(ap_uavcan,node_id);
     if (driver == nullptr)
     {
         return;

libraries/AP_Proximity/AP_Proximity_UAVCAN.h

@@ -29,7 +29,7 @@ public:
   //AP_Proximity::Proximity_State _status;
 
 private:
-  static AP_Proximity_UAVCAN *get_uavcan_backend(AP_UAVCAN *ap_uavcan);
+  static AP_Proximity_UAVCAN *get_uavcan_backend(AP_UAVCAN *ap_uavcan,uint8_t node_id);
   // inline void register_sensor()
   // {
   //   //  instance = frontend.register_sensor();