AP_Proximity_UAVCAN:数据修改

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)
 {
 }
 
@@ -143,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))
     {
@@ -179,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