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

AP_Follow: clarify what we're doing when rotating a vector 

odd sort of a transform, make it clear what's happening
This commit is contained in:
Peter Barker 2024-06-06 11:12:12 +10:00 committed by Andrew Tridgell
parent 9c42a5d9dd
commit 500ca22c75
1 changed files with 4 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_Follow/AP_Follow.cpp
View File

@ -492,9 +492,10 @@ bool AP_Follow::get_offsets_ned(Vector3f &offset) const
Vector3f AP_Follow::rotate_vector(const Vector3f &vec, float angle_deg) const Vector3f AP_Follow::rotate_vector(const Vector3f &vec, float angle_deg) const
{ {
// rotate roll, pitch input from north facing to vehicle's perspective // rotate roll, pitch input from north facing to vehicle's perspective
const float cos_yaw = cosf(radians(angle_deg)); Vector3f ret = vec;
const float sin_yaw = sinf(radians(angle_deg)); ret.xy().rotate(radians(angle_deg));
return Vector3f((vec.x * cos_yaw) - (vec.y * sin_yaw), (vec.y * cos_yaw) + (vec.x * sin_yaw), vec.z);
return ret;
} }
// set recorded distance and bearing to target to zero // set recorded distance and bearing to target to zero