aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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) {
EKF1.UpdateFilter();
EKF1.getRotationBodyToNED(_dcm_matrix);
if (active_EKF_type() == EKF_TYPE1) {
Vector3f eulers;
EKF1.getRotationBodyToNED(_dcm_matrix);
EKF1.getEulerAngles(eulers);
roll = eulers.x;
pitch = eulers.y;
@ -171,9 +171,9 @@ void AP_AHRS_NavEKF::update_EKF2(void)
}
if (ekf2_started) {
EKF2.UpdateFilter();
EKF2.getRotationBodyToNED(_dcm_matrix);
if (active_EKF_type() == EKF_TYPE2) {
Vector3f eulers;
EKF2.getRotationBodyToNED(_dcm_matrix);
EKF2.getEulerAngles(-1,eulers);
roll = eulers.x;
pitch = eulers.y;