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

AP_NavEKF: Added pitch and roll inertial sensor to body trim adjustment

This commit is contained in:
Paul Riseborough 2014-01-17 23:08:14 +11:00 committed by Andrew Tridgell
parent 9d375da550
commit 7fb60812c2
1 changed files with 3 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1985,6 +1985,7 @@ void NavEKF::getEulerAngles(Vector3f &euler) const
{ {
Quaternion q(states[0], states[1], states[2], states[3]); Quaternion q(states[0], states[1], states[2], states[3]);
q.to_euler(&euler.x, &euler.y, &euler.z); q.to_euler(&euler.x, &euler.y, &euler.z);
euler = euler - _ahrs->get_trim();
} }
void NavEKF::getVelNED(Vector3f &vel) const void NavEKF::getVelNED(Vector3f &vel) const
@ -2291,8 +2292,10 @@ void NavEKF::ForceYawAlignment()
void NavEKF::getRotationBodyToNED(Matrix3f &mat) const void NavEKF::getRotationBodyToNED(Matrix3f &mat) const
{ {
Vector3f trim = _ahrs->get_trim();
Quaternion q(states[0], states[1], states[2], states[3]); Quaternion q(states[0], states[1], states[2], states[3]);
q.rotation_matrix(mat); q.rotation_matrix(mat);
mat.rotateXYinv(trim);
} }
void NavEKF::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov) const void NavEKF::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov) const