Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: fixed calls to DCM in parent class

Browse Source 
use_compass() and reset() are common to AP_AHRS_DCM and
AP_AHRS_NavEKF. As AP_AHRS_NavEKF is a child of AP_AHRS_DCM, when we
call use_compass() from within AP_AHRS_DCM we actually end up calling
AP_AHRS_NavEKF::use_compass().

This has the effect of disabling the compass in DCM when EKF is active
and EKF has decided not to use the compass. That means that the DCM
yaw (and in fact the whole attitude) can get badly off while EKF is
enabled, making DCM an ineffective fallback if EKF fails.

The fix is to call the specific class versions of use_compass() and
reset()
This commit is contained in:
Andrew Tridgell 2014-10-15 10:12:50 +11:00 committed by Randy Mackay
parent 71722d2e49
commit 0dcf501766
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -143,7 +143,7 @@ AP_AHRS_DCM::check_matrix(void)
{
if (_dcm_matrix.is_nan()) {
//Serial.printf("ERROR: DCM matrix NAN\n");
reset(true);
AP_AHRS_DCM::reset(true);
return;
}
// some DCM matrix values can lead to an out of range error in
@ -161,7 +161,7 @@ AP_AHRS_DCM::check_matrix(void)
// in real trouble. All we can do is reset
//Serial.printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
// _dcm_matrix.c.x);
reset(true);
AP_AHRS_DCM::reset(true);
}
}
}
@ -242,7 +242,7 @@ AP_AHRS_DCM::normalize(void)
// Our solution is blowing up and we will force back
// to last euler angles
_last_failure_ms = hal.scheduler->millis();
reset(true);
AP_AHRS_DCM::reset(true);
}
}
@ -362,7 +362,7 @@ AP_AHRS_DCM::drift_correction_yaw(void)
float yaw_error;
float yaw_deltat;
if (use_compass()) {
if (AP_AHRS_DCM::use_compass()) {
/*
we are using compass for yaw
*/
@ -684,7 +684,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
// reduce the impact of the gps/accelerometers on yaw when we are
// flat, but still allow for yaw correction using the
// accelerometers at high roll angles as long as we have a GPS
if (use_compass()) {
if (AP_AHRS_DCM::use_compass()) {
if (have_gps() && gps_gain == 1.0f) {
error[besti].z *= sinf(fabsf(roll));
} else {