From c0e6fe70e42534a72adc492b19d19b9a7d077f81 Mon Sep 17 00:00:00 2001
From: binsir <yaozb04@163.com>
Date: Tue, 10 Aug 2021 22:23:37 +0800
Subject: [PATCH] =?UTF-8?q?AP=5FProximity=5FUAVCAN:=E4=BF=AE=E5=A4=8D?=
 =?UTF-8?q?=E9=87=8D=E5=90=AF=E6=89=8D=E6=9C=89=E6=95=B0=E6=8D=AE?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 libraries/AP_Proximity/AP_Proximity.cpp        | 11 ++++++-----
 libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp | 11 +++++++----
 libraries/AP_Proximity/AP_Proximity_UAVCAN.h   |  2 +-
 3 files changed, 14 insertions(+), 10 deletions(-)

diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp
index daaeaa31a4..cf57332dfd 100644
--- a/libraries/AP_Proximity/AP_Proximity.cpp
+++ b/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;
     }
 }
 
diff --git a/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp b/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp
index 15d0d4468d..29840c2c14 100644
--- a/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp
+++ b/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;
diff --git a/libraries/AP_Proximity/AP_Proximity_UAVCAN.h b/libraries/AP_Proximity/AP_Proximity_UAVCAN.h
index 12801ecafd..8918eafbe5 100644
--- a/libraries/AP_Proximity/AP_Proximity_UAVCAN.h
+++ b/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();