Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -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 7e7f78d4b5
commit d64b2fcbf0
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

@ -61,6 +61,9 @@
#define EKF_TARGET_DT_MS 12 #define EKF_TARGET_DT_MS 12
#define EKF_TARGET_DT 0.012f #define EKF_TARGET_DT 0.012f
// 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