zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Proximity_UAVCAN:数据修改

zr_rover4.2
binsir 4 years ago
parent
c0e6fe70e4
commit
9c93cb7620
1 changed files with 29 additions and 27 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 56
      libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp

56
libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp
Unescape View File

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

Write Preview
Loading…
Cancel
Save