Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-09 01:13:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS_view: add rotate method to move from AHRS to AHRS view referance frames

Browse Source
This commit is contained in:
Iampete1 2022-06-16 23:42:39 +01:00 committed by Andrew Tridgell
parent f2a162b963
commit dc0491a8ff
2 changed files with 9 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_AHRS/AP_AHRS_View.cpp
View File

@ -100,3 +100,9 @@ Vector2f AP_AHRS_View::body_to_earth2D(const Vector2f &bf) const
return Vector2f(bf.x * trig.cos_yaw - bf.y * trig.sin_yaw, return Vector2f(bf.x * trig.cos_yaw - bf.y * trig.sin_yaw,
bf.x * trig.sin_yaw + bf.y * trig.cos_yaw); bf.x * trig.sin_yaw + bf.y * trig.cos_yaw);
} }
// Rotate vector from AHRS reference frame to AHRS view reference frame
void AP_AHRS_View::rotate(Vector3f &vec) const
{
vec = rot_view * vec;
}

3
libraries/AP_AHRS/AP_AHRS_View.h
View File

@ -195,6 +195,9 @@ public:
// get pitch trim (deg) // get pitch trim (deg)
float get_pitch_trim() const { return _pitch_trim_deg; } float get_pitch_trim() const { return _pitch_trim_deg; }
// Rotate vector from AHRS reference frame to AHRS view refences frame
void rotate(Vector3f &vec) const;
private: private:
const enum Rotation rotation; const enum Rotation rotation;
AP_AHRS &ahrs; AP_AHRS &ahrs;