Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: add correct_angle_for_orientation to backend

This commit is contained in:
Randy Mackay 2020-09-30 14:13:45 +09:00 committed by Andrew Tridgell
parent cf020e1d84
commit 63a21c6c12
2 changed files with 11 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

@ -229,6 +229,14 @@ void AP_Proximity_Backend::set_status(AP_Proximity::Status status)
state.status = status; state.status = status;
} }
// correct an angle (in degrees) based on the orientation and yaw correction parameters
float AP_Proximity_Backend::correct_angle_for_orientation(float angle_degrees) const
{
const float angle_sign = (frontend.get_orientation(state.instance) == 1) ? -1.0f : 1.0f;
return wrap_360(angle_degrees * angle_sign + frontend.get_yaw_correction(state.instance));
}
// find which sector a given angle falls into
uint8_t AP_Proximity_Backend::convert_angle_to_sector(float angle_degrees) const uint8_t AP_Proximity_Backend::convert_angle_to_sector(float angle_degrees) const
{ {
return wrap_360(angle_degrees + (PROXIMITY_SECTOR_WIDTH_DEG * 0.5f)) / 45.0f; return wrap_360(angle_degrees + (PROXIMITY_SECTOR_WIDTH_DEG * 0.5f)) / 45.0f;

3
libraries/AP_Proximity/AP_Proximity_Backend.h
View File

@ -67,6 +67,9 @@ protected:
// set status and update valid_count // set status and update valid_count
void set_status(AP_Proximity::Status status); void set_status(AP_Proximity::Status status);
// correct an angle (in degrees) based on the orientation and yaw correction parameters
float correct_angle_for_orientation(float angle_degrees) const;
// find which sector a given angle falls into // find which sector a given angle falls into
uint8_t convert_angle_to_sector(float angle_degrees) const; uint8_t convert_angle_to_sector(float angle_degrees) const;