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

AP_Proximity: Follow NED convention

This commit is contained in:
Rishabh 2021-02-11 13:06:58 +05:30 committed by Randy Mackay
parent 74bda4952d
commit 8cab737bdd
1 changed files with 4 additions and 4 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

@ -113,16 +113,16 @@ void AP_Proximity_Backend::database_push(float angle, float pitch, float distanc
if (oaDb == nullptr || !oaDb->healthy()) { if (oaDb == nullptr || !oaDb->healthy()) {
return; return;
} }
//Assume object is angle and pitch bearing and distance meters away from the vehicle //Assume object is angle and pitch bearing and distance meters away from the vehicle
Vector3f object_3D; Vector3f object_3D;
object_3D.offset_bearing(wrap_180(angle), wrap_180(pitch), distance); object_3D.offset_bearing(wrap_180(angle), wrap_180(pitch * -1.0f), distance);
const Vector3f rotated_object_3D = body_to_ned * object_3D; const Vector3f rotated_object_3D = body_to_ned * object_3D;
//Calculate the position vector from origin //Calculate the position vector from origin
Vector3f temp_pos = current_pos + rotated_object_3D; Vector3f temp_pos = current_pos + rotated_object_3D;
//Convert the vector to a NEU frame from NED //Convert the vector to a NEU frame from NED
temp_pos.z = temp_pos.z * -1.0f; temp_pos.z = temp_pos.z * -1.0f;
oaDb->queue_push(temp_pos, timestamp_ms, distance); oaDb->queue_push(temp_pos, timestamp_ms, distance);
} }