AP_NavEKF3: do not treat a never-written terrain timestamp as fresh

terrain_srtm_alt_ms is zero until the first writeTerrainData, and
imuSampleTime_ms counts from boot, so for the first five seconds of uptime
the age test passes against a terrain altitude that is still zero.
terrain_srtm_alt_valid then reports a terrain height the core has never
received: FuseOptFlow scales flow from it, getHeightControlLimit drops the
optical flow altitude cap, and getFilterStatus counts it towards relative
position validity.

On the ground at boot that costs little, since the value it invents is the
height above an origin the vehicle is sitting on. It matters after an
in-flight watchdog reset, where millis() restarts at zero and the vehicle
comes back armed and flying.

gndHgtValidTime_ms is guarded against exactly this one expression away, so
this is the same idiom rather than a new one. The 5 s is named while it is
being touched, because it is about to have a second user.

No autotest: the window is the first five seconds of uptime and closes
before healthy() opens, so SITL cannot enter it from a cold boot, and the
in-flight reset that reaches it is not something the harness can stage.
This commit is contained in:
Andy Piper
2026-09-15 16:33:29 +09:00
committed by Randy Mackay
parent a2b6d4e693
commit bf08027404
2 changed files with 5 additions and 1 deletions
@@ -308,7 +308,8 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus
#if EK3_FEATURE_OPTFLOW_SRTM
// if ground offset (aka terrainState) is not valid, use SRTM altitude. terrain_srtm_alt
// is positive up from the origin where pd and terrainState above are positive down
terrain_srtm_alt_valid = ((imuSampleTime_ms - terrain_srtm_alt_ms) < 5000);
terrain_srtm_alt_valid = (terrain_srtm_alt_ms != 0) &&
((imuSampleTime_ms - terrain_srtm_alt_ms) < TERRAIN_SRTM_ALT_TIMEOUT_MS);
if (!gndOffsetValid && terrain_srtm_alt_valid) {
heightAboveGndEst = MAX((-pd) - terrain_srtm_alt, rngOnGnd);
}
+3
View File
@@ -71,6 +71,9 @@
// number of seconds a request to reset the yaw to the GSF estimate is active before it times out
#define YAW_RESET_TO_GSF_TIMEOUT_MS 5000
// age at which a terrain altitude from the database is no longer used
#define TERRAIN_SRTM_ALT_TIMEOUT_MS 5000
// accuracy threshold applied to GSF yaw estimate use
#define GSF_YAW_ACCURACY_THRESHOLD_DEG 15.0f