Merge branch 'zr_rover4.1' of gitee.com:xmzrzk/zr-v4 into zr_rover4.2

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

@@ -24,7 +24,9 @@ HAL_Semaphore AP_Proximity_UAVCAN::_sem_registry;
 UC_REGISTRY_BINDER(ProximityCb, zrzk::equipment::range_sensor::Proximity);
 AP_Proximity_UAVCAN::DetectedModules AP_Proximity_UAVCAN::_detected_modules[] = {0};
 
-AP_Proximity_UAVCAN::AP_Proximity_UAVCAN(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state) : AP_Proximity_Backend(_frontend, _state)
+AP_Proximity_UAVCAN::AP_Proximity_UAVCAN(AP_Proximity &_frontend,
+                                         AP_Proximity::Proximity_State &_state) : 
+                                         AP_Proximity_Backend(_frontend, _state)
 {
 }
 
@@ -83,7 +85,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 +95,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 +105,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 +121,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;
             }
         }
@@ -140,13 +145,13 @@ void AP_Proximity_UAVCAN::update(void)
     WITH_SEMAPHORE(_sem_proximity);
 
     update_sector_data(0, _interim_data.d1_cm);   // d1
-    update_sector_data(45, _interim_data.d8_cm);  // d8
-    update_sector_data(90, _interim_data.d7_cm);  // d7
-    update_sector_data(135, _interim_data.d6_cm); // d6
+    update_sector_data(45, _interim_data.d2_cm);  // d8
+    update_sector_data(90, _interim_data.d3_cm);  // d7
+    update_sector_data(135, _interim_data.d4_cm); // d6
     update_sector_data(180, _interim_data.d5_cm); // d5
-    update_sector_data(225, _interim_data.d4_cm); // d4
-    update_sector_data(270, _interim_data.d3_cm); // d3
-    update_sector_data(315, _interim_data.d2_cm); // d2
+    update_sector_data(225, _interim_data.d6_cm); // d4
+    update_sector_data(270, _interim_data.d7_cm); // d3
+    update_sector_data(315, _interim_data.d8_cm); // d2
 
     if ((_last_distance_received_ms == 0) || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_UAVCAN_TIMEOUT_MS))
     {
@@ -163,7 +168,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;
@@ -176,35 +181,35 @@ void AP_Proximity_UAVCAN::handle_proximity_data(const ProximityCb &cb)
 
     WITH_SEMAPHORE(_sem_proximity);
 
-    _interim_data.d1_cm = handle_data_valid(cb.msg->d0);
-    _interim_data.d2_cm = handle_data_valid(cb.msg->d315);
-    _interim_data.d3_cm = handle_data_valid(cb.msg->d315);
-    _interim_data.d5_cm = handle_data_valid(cb.msg->d180);
-    _interim_data.d6_cm = handle_data_valid(cb.msg->d135);
-    _interim_data.d7_cm = handle_data_valid(cb.msg->d90);
-    _interim_data.d8_cm = handle_data_valid(cb.msg->d45);
-
+    _interim_data.d1_cm = cb.msg->d0;
+    _interim_data.d2_cm = cb.msg->d45;
+    _interim_data.d3_cm = cb.msg->d90;
+    _interim_data.d4_cm = cb.msg->d135;
+    _interim_data.d5_cm = cb.msg->d180;
+    _interim_data.d6_cm = cb.msg->d225;
+    _interim_data.d7_cm = cb.msg->d270;
+    _interim_data.d8_cm = cb.msg->d315;
     _last_distance_received_ms = AP_HAL::millis();
     // _last_sample_time_ms = AP_HAL::millis();
 }
 
-uint16_t AP_Proximity_UAVCAN::handle_data_valid(uint16_t data)
-{
-    if (data == 0xFFFF)
-    {
-        return 0;
-    }
-    else
-    {
-        return data / 10;
-    }
-}
+// uint16_t AP_Proximity_UAVCAN::handle_data_valid(uint16_t data)
+// {
+//     if (data == 0xFFFF)
+//     {
+//         return 0;
+//     }
+//     else
+//     {
+//         return data / 10;
+//     }
+// }
 
 void AP_Proximity_UAVCAN::update_sector_data(int16_t angle_deg, uint16_t distance_cm)
 {
     const uint8_t sector = convert_angle_to_sector(angle_deg);
     _angle[sector] = angle_deg;
-    _distance[sector] = ((float)distance_cm) / 100;
+    _distance[sector] = ((float)distance_cm) / 1000;
     _distance_valid[sector] = distance_cm != 0xffff;
     //_last_distance_received_ms = AP_HAL::millis();
     // update boundary used for avoidance

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();