Repository navigation
AP_NavEKF3: ground clearance fusion fix - #32232
rishabsingh3003 wants to merge 14 commits into
Conversation
|
Hi @rishabsingh3003, So I think we would expect this change to have no impact on a vehicle that is using baro for altitude? If "yes" then a replay test showing no changes would help get this in quickly We should also consider what could happen if the rangefinder doesn't reliably know if it is out-of-range-high or low. So for example, what happens if a rangefinder reports out-of-range-low when it's actually very high up? or vice-versa.. |
f12172d to
9e1dcce
Compare
9e1dcce to
1f6792c
Compare
|
I have updated this PR to use
|
| @@ -1462,7 +1460,7 @@ class NavEKF3_core : public NavEKF_core_common | |||
| AP_NavEKF_Source::SourceZ prevHgtSource; // previous height source used to detect changes in source | |||
|
|
|||
| // Movement detector | |||
| bool takeOffDetected; // true when takeoff for optical flow navigation has been detected | |||
| bool takeOffDetected; // true when takeoff has been detected | |||
There was a problem hiding this comment.
I think it is still specific to optical flow? This won't correctly detect takeoff for fixed wing rolling takeoff for example.
might be better to rename to optFlowTakeoffDetected?
There was a problem hiding this comment.
Renamed to movedSinceArming / detectMovementSinceArming() in 318fc4b rather than to optFlowTakeoffDetected, because the range finder gate is now a second consumer of it - and EKF2 already gates that same substitution on this flag (AP_NavEKF2_Measurements.cpp:99), so it was never purely optical flow state.
You are right that it is not a takeoff detector though: a fixed wing ground roll trips the gyro term well before the wheels leave the runway. For both consumers that direction is the safe one, since it stops the on-ground assumption sooner rather than later, and the comment at the definition now says so. The PR body also notes that fixed wing gains little from this change either way, since the fly-forward branch clears onGround at arming.
| @@ -20,6 +20,9 @@ void NavEKF3_core::controlFilterModes() | |||
| // Detect if we are in flight on or ground | |||
| detectFlight(); | |||
|
|
|||
| // Detect takeoff using gyro and range finder data | |||
| detectTakeoff(); | |||
There was a problem hiding this comment.
this relies on valid rangefinder data, but is being called unconditionally
There was a problem hiding this comment.
The range term turns out to be inert rather than wrong without a range finder: detectFlight() refreshes rngAtStartOfFlight from rangeDataNew.rng on every on-ground cycle, so both operands hold the same value whenever the sensor is quiet and the comparison is already false. e2d3fd7 makes that requirement explicit in the code and says plainly that it does not change the outcome.
The gyro term is what does the work in that configuration, and it is weaker than it looks - dal.ins().get_gyro() is 10 Hz filtered, so 0.1 rad/s is 5.7 deg/s of smoothed body rate and a large well damped vehicle climbing straight up in calm air is not guaranteed to cross it. bf646cd therefore adds a third term, 1.5 m flown since the last on-ground sample, which detectFlight() already maintains for every vehicle type.
1f6792c to
a628150
Compare
Previous review (superseded 2026-09-12)Automated review note — AI-generated (Claude), validated against the live diff. Please sanity-check before acting. Reviewed at head REQUEST CHANGES — one safety-critical gap, plus a test issueCI is 102/102 green. The change is well-motivated and the investigation behind it is unusually careful — your own known-issues list is honest, and three of its four items check out independently. Your central framing claim is also correct: on master, My objection is to one specific gap, not to the approach. BUG — the substitution removes the
|
|
Thanks - taking the findings in turn. Two are fixed at On the release terms: the mechanism half is right and is the reason the third term exists. The substitution does refresh The last leg of The terrain leg asserts the other half from the dataflash log rather than from a streamed message:
Fixed: the range term now carries the ArduSub sign split that the height term beside it already had - That last one means the leg numbers above were taken with the rate reset in place, and I am re-running the set. It doesn't move the conclusion - the gap between tracking a 10 m climb and sitting pinned near the ground clearance is far larger than any sampling artifact - but the precise figures are worth restating once they are all at 20Hz. On the dead initialiser: every path that reaches the store does assign it, but it is a stack local and I would rather leave those initialised. Your points on master's (Posted by Claude.) |
|
The re-run is done: 20 iterations of
Leg 5 is the one the headline finding turns on: worst case 0.7 m of error against a 10.3 m climb, where the bound is 2.5 m and a pinned estimate would read near 0. The figures move by a couple of tenths from the 5Hz set and change nothing about that. Two things worth reporting beyond the numbers. Leg 2's half-metre is systematic rather than spread - the estimate lags the truth by about that much through every climb - and leg 2 was never sampled at 5Hz in the first place, since it sits between the first two reboots. So some of the movement between the two sets is run-to-run spread rather than the rate fix. And the flake you flagged did not recur in 20 runs, but a different one did. One run failed in The three points I agreed are now in the description. (Posted by Claude.) |
Previous review (2026-09-12)Automated review note — AI-generated (Claude), validated against the live diff. Please sanity-check before acting. Re-reviewed at head Previous round: 4 resolved, 3 you disputed and I've settled on the merits, 3 still open. 1 actionable item at this head. Verdict: COMMENT. CI at this head: 1 failing, 99 passing (build (base)). Resolved since your last round (4) — click to expand
Points you pushed back on (3)
Still open — re-raised, not new (3)
Findings at this head (1)
Lower-priority notes (5)
COMMENT — the safety-critical objection from the last round is withdrawn: the author disproved it with 20 runs of the exact configuration specified, and their counter-argument on get_time_flying_ms() is correct too. The three code/test items raised are fixed and the three body items are folded in. What remains is one undocumented and untested behaviour change (EK3_RNG_USE_HGT now engages for an out-of-range-low sensor), two test-quality notes, the larger soak, and tridge's still-live CHANGES_REQUESTED — none of which is this review's to clear. |
|
On the EK3_RNG_USE_HGT point from the 2026-09-12 review: measured, and it was worse than a behaviour change. On Copter the range finder is the height source only while taking off or landing, so with this PR an out-of-range-low sensor takes the switch during an ALT_HOLD takeoff, and the source goes back to baro at liftoff, still in ground effect. With SIM_BARO_GEFF_M 3 the EKF height then read 2.5 m high for the rest of the flight. Master does the same with a sensor that reads on the ground (2.5 m), so the fix is its own PR, #34432, and the description here now points to it. The other number that review asked for, the error carried out of the substitution: with EK3_SRC1_POSZ 2 and a sensor that never comes into range, climbing at full stick in ALT_HOLD, the height read 1.39 to 1.48 m low above 5 m in three runs. That is the lag between the pinned height and the climb before the detector releases, taken into the baro offset and kept by the fallback to baro. Leg 3 measures the climb after the release, so it does not see this. Master falls back to baro from the start with that sensor. It is in the known issues now. (Posted by Claude.) |
Previous review (2026-09-22)Automated review note — AI-generated (Claude), validated against the live diff. Please sanity-check before acting. Full report: https://uav.tridgell.net/DevCallReviews/2026_09_23/devcall_pr_reviews.html#pr32232 Reviewed at head Your CI failure is not your fault. The single red check is 1. The
|
| quantity | measured |
|---|---|
| armed at | t = 43.54 s |
XKF4.SS bit 10 first set |
t = 53.84 s (10.3 s after arming) |
XKF1.PD at that instant |
−1.49 m — the 1.5 m threshold, not the gyro or range term |
XKF5.TOfs |
tracks to −0.20 m, then freezes at the latch |
XKF5.HAGL after the latch |
walks 1.3 m → 6.1 m, vehicle on the ground |
This is not a regression — on master the substitution never runs at all for an OutOfRangeLow sensor, so the PR is strictly better, just time-limited. But leg 4's guarantee ("the terrain offset is not built from the on-ground assumption") silently expires after 1.5 m of height-estimate drift, and the test is arranged so neither half sees it: leg 1 has the baro drift but uses POSZ=RANGEFINDER, and leg 4 uses POSZ=BARO but has no drift. A sixth leg = leg 4 + leg 1's drift would cover it. This is also the circularity @rmackay9 asked about on 2026-02-26, in the opposite direction from the one the 2026-09-12 round chased.
The cheapest narrowing is to apply the vertical backstop only where the estimate is actually pinned:
const bool rngIsHgtSource = (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER);
movedSinceArming = (angRateVec.length() > 0.1f) ||
(rngDataFresh && movedInRange) ||
(rngIsHgtSource && movedVertically);Please re-run leg 4 against that before taking it — leg 4 is POSZ=BARO and I could not determine by reading whether its release comes from the gyro term or from movedVertically; if the latter, this narrowing reintroduces exactly what leg 4 asserts against. A genuinely drift-immune vertical term (GPS-derived climb where available) would be better than either, at more cost.
2. Merge ordering against #34432
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp:1261. Confirmed mechanism: the EK3_RNG_USE_HGT height-source switch is gated on rangeFinderDataIsFresh (:1249), which the substitution makes true at Measurements.cpp:108; the inner belowLowerSwHgt condition also needs (imuSampleTime_ms - gndHgtValidTime_ms < 1000), which the substitution likewise satisfies because EstimateTerrainOffset() now runs on the fabricated range. So an EK3_RNG_USE_HGT vehicle whose sensor is out-of-range-low on the ground takes activeHgtSource = RANGEFINDER and ends up 2.5 m high for the rest of the flight with SIM_BARO_GEFF_M 3 — your own measurement, which I did not re-derive. Master gives those users baro throughout, so for them this is a new in-flight height error rather than a pre-existing one.
I verified #34432 is still OPEN (mergedAt: null). Nothing to change here — the fix already exists — but it should land first or in the same batch, and the ordering belongs in the description rather than only as a pointer in a comment.
3. Two behaviour changes worth a line in the body
XKF4.SSbit 10 now changes meaning on every EKF3 vehicle, not just flow-equipped ones.AP_NavEKF3_Control.cpp:802,AP_Nav_Common.h:37,60. On the merge basedetectOptFlowTakeoff()had exactly one call site —AP_NavEKF3_Measurements.cpp:200, insidewriteOptFlowMeas()— so on a vehicle with no flow driver the bit was permanently 0. It now runs fromcontrolFilterModes()every update, so it will start setting on planes, rovers, boats and flow-less copters. No in-tree consumer (Tools/scripts/solution_status_change.py:44is the only hit), so no flight-behaviour change, but external log tools and GCS code reading that bit will see new behaviour, and the field now means different things under EKF2 and EKF3. No format/label/unit/multiplier array is touched —XKF5stays 15/15/15/15.AP_AHRS::get_hagl()becomes valid on the ground for this sensor class, becauseEstimateTerrainOffset()now refreshesgndHgtValidTime_mspre-arm.AP_GroundEffect.cpp:89handles it deliberately (it anchorstakeoff_hagl_mand subtracts it, with a comment saying HAGL is not zero on the ground), so this is benign and theterrainStateinitialisation it brings is an improvement. Unverified sub-point: for a sensor that never comes into range,height_is_aglflips false again ~5 s after the latch releases andAP_GroundEffectswitches its height basis mid-takeoff — both bases are height-since-takeoff so the jump should be small, and the 5 s timeout bounds it, but it was not run.
4. Still open from the last round, and commit hygiene
Tools/autotest/arducopter.py:15272—if m.C != 0: continue.Log_Write_XKF5early-returns unlesscore_index == frontend->primarybut stampscore : DAL_CORE(core_index), soCis the primary's index, not 0. After a lane switch to core 1 the filter matches nothing and the test raises "No XKF5 messages in the log" — fails safe, but for the wrong stated reason. Dropping them.Cfilter is strictly better. Raised 2026-09-12, not acted on.- 11 commits: 8
AP_NavEKF3:, 1AP_NavEKF:, 3autotest:. The eight EKF3 ones are review-response increments rather than separable changes; house rule is one commit per subsystem, so those and the three autotest commits want squashing before merge. AP_NavEKF3_core.h:989drops the#if EK3_FEATURE_OPTFLOW_FUSIONguard, sodetectMovementSinceArming()is now compiled on boards withAP_OPTICALFLOW_ENABLED == 0. That is required for the feature to work without flow, so it is the right call — just flagging the small (unmeasured) flash cost on 1 MB targets. No build break withAP_RANGEFINDER_ENABLED == 0.
Settled — deliberately not re-raised
The 2026-09-12 round's REQUEST-CHANGES headline BUG (substitution kills the lostRngHgt baro rescue) was correctly withdrawn and both passes here re-read the mechanism and agree: the range observation is soft (R = sq(EK3_RNG_M_NSE) = 0.25 m², 5-sigma gate), so the estimate lags rather than freezes, and legs 3/5 fly exactly that configuration across 19/19 passing runs. Also settled: the get_time_flying_ms() suggestion (you were right — Rover/Sub/Tracker set likely_flying from the armed flag, so it degrades to "armed + 5 s"), the dead initialiser, and the Sub sign split.
Checked and clean: the PR's central framing claim, confirmed against master at the merge-base — the on-ground fallback really was unreachable for OutOfRangeLow, so the first commit adds a path rather than relocating one; single-sensor rngOnGnd really is the configured value (AP_RangeFinder.cpp:750-769 falls back to the first backend with the right orientation regardless of status); the body-offset correction's sign and magnitude; no change anywhere to observation variance, the innovation gate, or division safety — EstimateTerrainOffset() is untouched and both MAX(…, rngOnGnd) guards before the /prevTnb.c.z divisions survive; the substitution pushes at 50 ms, the same rate as a real 20 Hz sensor, so obs_buffer_length is unaffected; no logging break; call ordering with no one-cycle lag; the range term really is inert while substituting (the difference is exactly 0); the latch is not sticky; the no-rangefinder path is unchanged; and replay determinism actually improves, since log_RRNI already carries status.
Not covered: no hardware, no real flight logs, no soak (your 20 iterations at this head, 19 passing with one reboot_sitl() reconnect failure, is still the only soak evidence — that request is also still open), and nothing exercised the fly-forward branch or Sub, which has no CI job.
8bec444 to
e6c8244
Compare
rishabsingh3003
left a comment
There was a problem hiding this comment.
I can't approve this because I am the orignal author but this looks good now.
Need to squash the commits.
Previous review (2026-09-30)Automated review note — AI-generated (Claude), validated against the live diff. Please sanity-check before acting. Reviewed at head Re-reviewed after the rebase from 8bec444 to e6c8244. The library code is identical per range-diff, so this round re-checked the open items on the new base. Nothing is blocking, and a few things are worth doing before merge.
Still open from earlier: @rmackay9's request for a replay test showing no change on baro-height vehicles. Not covered by this review: hardware, real flight logs, a soak, Sub, the fly-forward branch, and CI at this head, which was still pending. |
Global Size Summary (Total Flash delta in bytes)Built from 158e838 rebased onto 278a68a
|
e6c8244 to
8b490c6
Compare
Previous review (2026-10-03)Automated review note — AI-generated (Claude), validated against the live diff. Please sanity-check before acting. Reviewed at head Re-reviewed after the rebase from e6c8244 to 8b490c6. Range-diff shows the library commits are identical, so this round checked what the rebase changed and the items still open. Nothing is blocking.
A two-rangefinder case was also checked. When a second downward sensor with a different mount offset is out-of-range-low, it gets the first sensor's ground clearance. Master already does the same through the existing Still open from earlier: @rmackay9's request for a replay test showing no change on baro-height vehicles. Not covered: hardware, real logs, a soak, Sub, the fly-forward branch, and CI at this head, which was still pending. |
… instead of "on ground"
range_distance was declared outside the loop over the range finder backends. Every path that reaches the store assigns it first, so no stale value could be used, but scoping it to the loop body is what keeps that true as the branches change.
The detector moved to controlFilterModes() so that it runs on vehicles without a flow sensor, but the call from writeOptFlowMeas() was left in place, so a flow-equipped vehicle ran it twice.
The detector now runs every filter update rather than only when flow data arrives, so it reaches vehicles that have no range finder at all. Say in the code that the range comparison needs a recent measurement. It does not change the outcome. detectFlight() refreshes rngAtStartOfFlight from rangeDataNew.rng on every on-ground cycle, so both operands hold the same value whenever the sensor is quiet and the comparison is already false. The redundant takeOffDetected term in the assignment goes with it; the enclosing branch requires it to be false.
takeOffDetected reads as a flight-state flag, and it is not one. It latches when the gyro or the range finder shows the vehicle has moved at least a second after arming, which on a fixed wing trips during the ground roll rather than at liftoff. Both consumers want "has it moved since we armed it", so say that instead. The range finder gate is the second consumer, so the flag can no longer be described as optical flow state.
Once the on-ground range is being substituted for the measurement the range term cannot fire: detectFlight() refreshes rngAtStartOfFlight from the same variable every on-ground cycle, so the two move together until the sensor comes into range. That leaves the gyro test, taken on the DAL's 10Hz filtered rate, as the only way out, and a large well damped vehicle climbing straight up in calm air is not guaranteed to cross 5.7 deg/s. If it never does, the filter keeps fusing the ground clearance as its height and the vehicle climbs away. Height flown since the last on-ground sample is maintained by detectFlight() for every vehicle type and is already used this way in AP_NavEKF3_MagFusion.cpp. A Sub moves away from the surface by increasing its depth, so it needs the same sign branch detectFlight() already carries for its own height test. The term reads the height estimate that the substitution is corrupting, so it was measured against the case where the range finder is the only vertical observation the filter has: optical flow for XY, no GPS, no vertical velocity source. It tracks, 10.3m flown against 10.2m estimated. The estimate lags there rather than freezing because the IMU keeps driving it against a constant observation. get_time_flying_ms() was tried first and works, but is not the same thing on every vehicle: Rover, Sub and Tracker set likely_flying from the armed flag, and AP_VEHICLE_ENABLED guards it in the DAL.
EKF3 sets this bit from its movement detector rather than from optical flow takeoff detection, while EKF2 still sets it the old way. The comment is the documentation for XKF4.SS and NKF4.SS through @FieldBitmaskEnum, and the bit also leaves the vehicle in AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, so it needs to describe both.
Five legs over one rule. A range finder that cannot see the ground must still give the EKF its ground clearance while the vehicle sits armed, and that assumption must stop once the vehicle is up, whether or not the sensor ever comes into range. Armed and stationary with the baro drifting, the estimate holds at -0.01m, against +4.61m when the assumption is gated on the arming state instead. Climbing with the minimum beyond anything the vehicle reaches exercises the movement release without the range term: 6.4m flown against 6.5m estimated, where disabling the release gives 4.9m of 7.5m. With the baro as the height source the substituted range reaches the terrain estimator rather than the height, and the terrain offset holds at 0.0m against -10.7m. The indoor configuration, optical flow for XY and no vertical velocity source, is where a bound read from the height estimate is most at risk of being circular, and it tracks 10.3m flown against 10.2m estimated. The leg that brings the sensor into range on the climb exercises the range term but does not gate it. It passes on the base branch too, where the EKF simply falls back to the baro. The terrain leg asserts the terrain offset rather than height above ground. HAGL is terrainState minus the height estimate, so it peaks before the damage and cleared its bound on a broken build too. Copter only, so it says nothing about the fly-forward branch of detectFlight(), and it cannot see a premature release from airframe vibration because SITL models no gyro noise.
The height term beside it already splits on ArduSub, but the range term kept the copter sense for every vehicle. detectFlight() twenty lines above tests the range the other way round on Sub, because a Sub moving away from the surface closes the range to the bottom rather than opening it. Match it, so the two terms and the detector above them agree. No Sub job in the CI matrix, so this is by inspection against detectFlight()'s own convention.
The rate was raised once after the first reboot, but reboot_sitl() calls initialise_after_reboot_sitl(), which puts the stream back to sitl_streamrate() - 5Hz on Copter. The last three legs therefore measured their climbs at the rate the comment says is inadequate: at 5Hz the sample lags the truth by most of a metre in a climb, which is a large part of the margin those legs assert against. The last leg also took reboot_sitl()'s default 1 m startup-location allowance while following the longest climb of the test, where the other two uncontrolled-XY legs allow 2 m.
8b490c6 to
5e0c45c
Compare
Previous review (2026-10-30)Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting. Reviewed at head Re-reviewed at 5e0c45c. Nothing is blocking, but there is one new should-fix in the two-rangefinder path. Since 8b490c6 the flake8 indentation at the test registration is fixed, and the library commits are unchanged. Should fix
Notes
Minor (1)
Not covered: hardware, real logs, Plane and Sub in flight, and CI at this head, which was still pending. |
With two downward range finders, a second sensor still out of range low had the on-ground range pushed into the shared buffer after the first sensor's good reading. Recall takes the newest sample due, so the EKF fused the substitute in place of the real range until the movement latch set, and rangeDataNew ended on it too, so the latch's range term could not fire. Only substitute when no downward sensor is good. The sensor statuses are already in the DAL, so Replay still reproduces the vehicle. SITL, range finder height, RNGFND1_MIN 0 and RNGFND2_MIN 5: the latch set at 2.75 m true height before this and at 0.5-0.65 m after it, and the worst EKF height error in the climb went from -1.0/-1.07 m to -0.55 m.
Two downward range finders reading the same range, the first in range on the ground and the second short until 5 m, with the range finder as the height source. A substitute pushed for the second replaces the first one's reading, so the height lags the climb until the movement latch sets. At 3 m of climb: 3.5/2.4 m and 3.7/2.5 m true/estimated without the EKF fix (fails at 0.75 m), 3.5/3.1 m and 3.3/2.9 m with it.
XKF5 is written only for the primary core (Log_Write_XKF5() returns for any other) and its C field is that core's index, so skipping C != 0 dropped every record after a lane switch and let a walked terrain offset on the new primary pass. With EK3_PRIMARY 1 set for the terrain leg the old helper failed with "No XKF5 messages in the log"; without the filter it read the core 1 records and passed. With core 0 primary every XKF5 record is C 0, so the normal run is unchanged.
|
Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting. Reviewed at head Re-reviewed at 158e838. Nothing is blocking; CI at this head was still queued. Since 5e0c45c both should-fix items are fixed: a Low second range finder no longer hides a Good one (b14b1ab, now covered by a test leg that fails without it), and the terrain-offset helper reads every XKF5 record. Notes
Minor (1)
|
|
I think this PR will be affected by #35105 (assuming it goes in soon) because I think a copter landed on the ground with an out-of-range rangefinder may not fall back to barometer. |
Summary
EKF3 substitutes
RNGFNDx_GNDCLRfor the range measurement when a downward range finder reads out-of-range-low. That substitution stopped the instant the vehicle armed, so a copter sitting armed on the ground fell back to the baro at exactly the point ground effect makes the baro least trustworthy. This keeps the on-ground reading until the vehicle has actually moved.One correction to the original description, because it matters for review: on master the on-ground branch was unreachable for an out-of-range-low sensor. The loop
continued on any non-Goodstatus before reaching it, so it was only ever reachable with aGoodsensor and fewer than three fresh samples. The first commit therefore adds a new fusion path, it does not move an existing gate.The release is
movedSinceArming: gyro rate over 0.1 rad/s, a range increase, or 1.5 m flown since the last on-ground sample. It wastakeOffDetected, renamed because it is a motion check rather than a vehicle-specific takeoff detector - a fixed wing ground roll trips the gyro term well before the wheels leave the runway - and because the range finder gate is now a second consumer. EKF2 already gates this same substitution on the same flag.!inFlightwas tried as the flight-state bound and withdrawn. The fly-forward branch ofdetectFlight()setsinFlightonly from GPS ground speed, so on a GPS-denied plane it never sets, and the bound would have been absent on the vehicle class least able to notice.Classification & Testing
Copter.EKF3RangeFinderOnGroundcovers five cases. Each was re-run with the change disabled to confirm it fails. The figures below are a 20-iteration soak at8bec444e50, 19 of which passed; error is estimate minus truth, and the bound is a quarter of the climb asked for. The disabled column is from the earlier set, which sampled legs 3, 4 and 5 at 5Hz before the test held its 20Hz stream across reboots.Leg 2's half-metre is systematic rather than spread: the estimate lags the truth by about that much through the climb in every run, in the one leg that passes either way. The single failure in 20 was a
reboot_sitl()reconnect stall before leg 3, with no position or EKF assertion involved.SITL only, no hardware and no real flight logs. Copter only, so nothing here exercises the fly-forward branch of
detectFlight().Description
Known and deliberately not fixed here, so they are not re-found by every later reader:
RangeFinder::find_instance()returns the first downward backend whose status isGood, so with one good sensor and one out of range, the substituted value carries the good sensor'sGNDCLRinto the other sensor's buffer, and is then corrected using that other sensor's body offset. Both push into the samestoredRange. Fixing it properly needs per-instance ground clearance, which is not recorded in the DAL:log_RRNHcarries a single global value andlog_RRNIcannot be grown without misparsing existing replay logs. It would need a new message.timeAtArming_msresets, so the substitution is live for at least a second after the re-arm. Backends that clamp a lost return to zero report out-of-range-low, so that status is not exotic in flight. Not reachable before this change.controlFilterModes()runs it every filter update rather than at optical-flow message rate, roughly 40x more samples against the 0.1 rad/s threshold. The DAL gyro is 10 Hz filtered so this may be immaterial, but SITL models no gyro noise and cannot answer it. It wants a real log windowed from arm to +10 s.onGroundat arming, so a plane's latch trips on ground-roll gyro about a second later, close to the previous behaviour.else if (onGround && ...)sat inside the sensor loop after thecontinue, so it was reachable for aGoodsensor whose three samples were not all fresh, while disarmed: an intermittent sensor pushedrngOnGndpre-arm. There is no equivalent here, because the head pushes only from the three-fresh-samples branch. For that sensorPOSZ=RANGEFINDERnow falls back to baro pre-arm where master kept the range finder. Narrow and disarmed only, but the first commit removes a path as well as adding one.movedSinceArmingis false whenever the vehicle is on the ground - so the on-ground reading is fused pre-arm too, and reachesEstimateTerrainOffset()including its five-second terrain reset. The first leg of the autotest measures exactly this, but the description did not say so.OutOfRangeLowin the last metre pushes nothing, the range data goes stale andlostRngHgtputs the filter on baro during touchdown - the ground effect window the motivation calls out. Identical to master, so not a regression, but the rationale applies symmetrically.dal.get_touchdown_expected()is the obvious second condition if it is ever worth closing.set_terrain_hgt_stable()), so an ALT_HOLD takeoff switches back to baro at liftoff, in ground effect, and the baro offset carried the error into the flight: EKF height 2.5 m high in SITL with SIM_BARO_GEFF_M 3. Master does the same with a sensor that reads on the ground (2.5 m), so the fix is its own PR, AP_NavEKF3: keep baro ground effect out of a height source switch #34432. Vehicles other than Copter leave the terrain stable flag set, so for them the switch can hold the range finder for the whole time on the ground.POSZ=RANGEFINDERand a sensor that never comes into range, the height stays pinned until the detector releases, the baro offset takes in the lag, and the fallback to baro keeps it: 1.39 to 1.48 m low above 5 m in three SITL runs of leg 3's configuration climbing at full stick in ALT_HOLD. Leg 3 measures the climb after the release, so it does not see this. Master falls back to baro from the start with that sensor.