diff --git a/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp b/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp index 29840c2c14..c58f22e428 100644 --- a/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp +++ b/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