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

AP_NavEKF: Add second stage alignment of yaw and earth field states

Browse Source 
Flight tests have shown that the magnetic field distortion associated with flight from steel structures can extend 3m or higher. To counteract this, a second and final yaw and magnetic field alignment has been added which is activated when the height exceeds 5m for the first time.
This commit is contained in:
Paul Riseborough 2015-02-22 17:43:17 +11:00 committed by Andrew Tridgell
parent ed9c05cf2a
commit 70afcd7e70
2 changed files with 13 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -4328,7 +4328,8 @@ void NavEKF::InitialiseVariables()
badMag = false;
badIMUdata = false;
firstArmComplete = false;
finalMagYawInit = false;
firstMagYawInit = false;
secondMagYawInit = false;
storeIndex = 0;
dtIMU = 0;
dt = 0;
@ -4650,10 +4651,16 @@ void NavEKF::performArmingChecks()
ResetHeight();
StoreStatesReset();
} else if (vehicleArmed && !finalMagYawInit && firstArmPosD - state.position.z > 1.5f && !assume_zero_sideslip()) {
// Do a final yaw and earth mag field initialisation when the vehicle has gained 1.5m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff)
} else if (vehicleArmed && !firstMagYawInit && firstArmPosD - state.position.z > 1.5f && !assume_zero_sideslip()) {
// Do the first in-air yaw and earth mag field initialisation when the vehicle has gained 1.5m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff)
// This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values
state.quat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
finalMagYawInit = true;
firstMagYawInit = true;
} else if (vehicleArmed && !secondMagYawInit && firstArmPosD - state.position.z > 5.0f && !assume_zero_sideslip()) {
// Do the second and final yaw and earth mag field initialisation when the vehicle has gained 5.0m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff)
// This second and final correction is needed for flight from large metal structures where the magnetic field distortion can extend up to 5m
state.quat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
secondMagYawInit = true;
}
// Always turn aiding off when the vehicle is disarmed

3
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -604,7 +604,8 @@ private:
bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
float firstArmPosD; // vertical position at the first arming (transition from sttatic mode) after start up
bool firstArmComplete; // true when first transition out of static mode has been performed after start up
bool finalMagYawInit; // true when the final post takeoff initialisation of earth field and yaw angle has been performed
bool firstMagYawInit; // true when the first post takeoff initialisation of earth field and yaw angle has been performed
bool secondMagYawInit; // true when the second post takeoff initialisation of earth field and yaw angle has been performed
bool flowTimeout; // true when optical flow measurements have time out
Vector2f gpsVelGlitchOffset; // Offset applied to the GPS velocity when the gltch radius is being decayed back to zero
bool gpsNotAvailable; // bool true when valid GPS data is not available