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

AP_NavEKF3: final mag reset at 2.5m

Browse Source
This commit is contained in:
Randy Mackay 2017-07-07 17:05:04 +09:00
parent 3b8038fe27
commit af032414f0
2 changed files with 4 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -56,7 +56,7 @@ void NavEKF3_core::controlMagYawReset()
if (flightResetAllowed && !assume_zero_sideslip()) { if (flightResetAllowed && !assume_zero_sideslip()) {
// check that we have reached a height where ground magnetic interference effects are insignificant // check that we have reached a height where ground magnetic interference effects are insignificant
// and can perform a final reset of the yaw and field states // and can perform a final reset of the yaw and field states
finalResetRequest = (stateStruct.position.z - posDownAtTakeoff) < -5.0f; finalResetRequest = (stateStruct.position.z - posDownAtTakeoff) < -EKF3_MAG_FINAL_RESET_ALT;
// check for increasing height // check for increasing height
bool hgtIncreasing = (posDownAtLastMagReset-stateStruct.position.z) > 0.5f; bool hgtIncreasing = (posDownAtLastMagReset-stateStruct.position.z) > 0.5f;

3
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -62,6 +62,9 @@
#define EKF_TARGET_DT_MS 10.0 #define EKF_TARGET_DT_MS 10.0
#define EKF_TARGET_DT 0.01 #define EKF_TARGET_DT 0.01
// mag fusion final reset altitude (using NED frame so altitude is negative)
#define EKF3_MAG_FINAL_RESET_ALT 2.5f
class AP_AHRS; class AP_AHRS;
class NavEKF3_core class NavEKF3_core