aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_AHRS: restore DCM attitude before update()

Browse Source 
The DCM drift correction code uses the current attitude to calculate
error values to update its gyro drift correction. If we were using EKF
then without this patch the DCM code running as an alternative AHRS
source would be using the EKF attitude for calculating the error
value, leading to very bad gyro drift estimation
This commit is contained in:
Andrew Tridgell 2014-10-15 11:15:33 +11:00 committed by Randy Mackay
parent 0dcf501766
commit f61ae9e9e5
1 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -52,6 +52,16 @@ const Vector3f &AP_AHRS_NavEKF::get_gyro_drift(void) const
void AP_AHRS_NavEKF::update(void)
{
// we need to restore the old DCM attitude values as these are
// used internally in DCM to calculate error values for gyro drift
// correction
roll = _dcm_attitude.x;
pitch = _dcm_attitude.y;
yaw = _dcm_attitude.z;
roll_sensor = degrees(roll)*100;
pitch_sensor = degrees(pitch)*100;
yaw_sensor = degrees(yaw)*100;
AP_AHRS_DCM::update();
// keep DCM attitude available for get_secondary_attitude()