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

AP_NavEKF3: Don't allow range finder use to start if terrain state is stale

Browse Source
This commit is contained in:
Paul Riseborough 2022-11-21 20:26:30 +11:00 committed by Andrew Tridgell
parent b9863d8514
commit 2228937536
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -1089,7 +1089,7 @@ void NavEKF3_core::selectHeightForFusion()
// determine if we are above or below the height switch region // determine if we are above or below the height switch region
ftype rangeMaxUse = 1e-4 * (ftype)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (ftype)frontend->_useRngSwHgt; ftype rangeMaxUse = 1e-4 * (ftype)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (ftype)frontend->_useRngSwHgt;
bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse; bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse;
bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse; bool belowLowerSwHgt = ((terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse) && (imuSampleTime_ms - gndHgtValidTime_ms < 1000);
// If the terrain height is consistent and we are moving slowly, then it can be // If the terrain height is consistent and we are moving slowly, then it can be
// used as a height reference in combination with a range finder // used as a height reference in combination with a range finder