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

AP_AHRS: use the right DCM matrix for right EKF

Browse Source 
this fixes a bug where the EKF2 DCM matrix would be used for
get_dcm_matrix() when EKF1 was active
This commit is contained in:
Andrew Tridgell 2015-11-20 18:34:00 +11:00
parent 01665282f7
commit 7b38f2185f
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -101,9 +101,9 @@ void AP_AHRS_NavEKF::update_EKF1(void)
} }
if (ekf1_started) { if (ekf1_started) {
EKF1.UpdateFilter(); EKF1.UpdateFilter();
EKF1.getRotationBodyToNED(_dcm_matrix);
if (active_EKF_type() == EKF_TYPE1) { if (active_EKF_type() == EKF_TYPE1) {
Vector3f eulers; Vector3f eulers;
EKF1.getRotationBodyToNED(_dcm_matrix);
EKF1.getEulerAngles(eulers); EKF1.getEulerAngles(eulers);
roll = eulers.x; roll = eulers.x;
pitch = eulers.y; pitch = eulers.y;
@ -171,9 +171,9 @@ void AP_AHRS_NavEKF::update_EKF2(void)
} }
if (ekf2_started) { if (ekf2_started) {
EKF2.UpdateFilter(); EKF2.UpdateFilter();
EKF2.getRotationBodyToNED(_dcm_matrix);
if (active_EKF_type() == EKF_TYPE2) { if (active_EKF_type() == EKF_TYPE2) {
Vector3f eulers; Vector3f eulers;
EKF2.getRotationBodyToNED(_dcm_matrix);
EKF2.getEulerAngles(-1,eulers); EKF2.getEulerAngles(-1,eulers);
roll = eulers.x; roll = eulers.x;
pitch = eulers.y; pitch = eulers.y;