Skip to content

AP_NavEKF3: ground clearance fusion fix - #32232

Open
rishabsingh3003 wants to merge 14 commits into
ArduPilot:masterfrom
rishabsingh3003:ek3_gnd_clear
Open

rishabsingh3003 wants to merge 14 commits into
ArduPilot:masterfrom
rishabsingh3003:ek3_gnd_clear

Conversation

@rishabsingh3003

@rishabsingh3003 rishabsingh3003 commented Feb 18, 2026 •

Copy link
Copy Markdown
Contributor

Summary

EKF3 substitutes RNGFNDx_GNDCLR for 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-Good status before reaching it, so it was only ever reachable with a Good sensor 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 was takeOffDetected, 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.

!inFlight was tried as the flight-state bound and withdrawn. The fly-forward branch of detectFlight() sets inFlight only 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

  • Checked by a human programmer
  • Automated test(s) verify changes (e.g. unit test, autotest)
  • Tested manually, description below (e.g. SITL)

Copter.EKF3RangeFinderOnGround covers five cases. Each was re-run with the change disabled to confirm it fails. The figures below are a 20-iteration soak at 8bec444e50, 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 error over 19 passing runs fails at disabled (earlier head)
armed and stationary, baro drifting 0.3 m/s -0.01 m, every run 1 m +4.61 m, follows the baro
sensor comes into range on the climb -0.4 to -0.5 m 1.5 m passes either way
sensor never in range +0.1 to +0.3 m 1.5 m 7.5 / 4.9 m
terrain path, baro as the height source 0.0 m, every run -2 m -10.7 m
indoor: flow for XY, no vertical velocity source -0.3 to +0.7 m 2.5 m -

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:

  • Two downward range finders. RangeFinder::find_instance() returns the first downward backend whose status is Good, so with one good sensor and one out of range, the substituted value carries the good sensor's GNDCLR into the other sensor's buffer, and is then corrected using that other sensor's body offset. Both push into the same storedRange. Fixing it properly needs per-instance ground clearance, which is not recorded in the DAL: log_RRNH carries a single global value and log_RRNI cannot be grown without misparsing existing replay logs. It would need a new message.
  • Mid-air disarm and re-arm. A disarm clears the latch and timeAtArming_ms resets, 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.
  • Detector sample rate. Moving the detector to 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.
  • Fixed wing gains little. The fly-forward branch also clears onGround at arming, so a plane's latch trips on ground-roll gyro about a second later, close to the previous behaviour.
  • Master's on-ground fallback went with it. Master's else if (onGround && ...) sat inside the sensor loop after the continue, so it was reachable for a Good sensor whose three samples were not all fresh, while disarmed: an intermittent sensor pushed rngOnGnd pre-arm. There is no equivalent here, because the head pushes only from the three-fresh-samples branch. For that sensor POSZ=RANGEFINDER now 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.
  • The substitution is live while disarmed. The gate has no arming condition - movedSinceArming is false whenever the vehicle is on the ground - so the on-ground reading is fused pre-arm too, and reaches EstimateTerrainOffset() including its five-second terrain reset. The first leg of the autotest measures exactly this, but the description did not say so.
  • The landing half is out of reach of this design. The latch is set for the whole approach, so re-entering OutOfRangeLow in the last metre pushes nothing, the range data goes stale and lostRngHgt puts 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.
  • EK3_RNG_USE_HGT now takes an out-of-range-low sensor. With the range fresh on the ground, the height switch uses it the way master uses a sensor that reads on the ground. On Copter that is only in the takeoff and landing windows (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.
  • The pinned climb is carried into the flight. With POSZ=RANGEFINDER and 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.

@rmackay9

rmackay9 commented Feb 26, 2026 •

Copy link
Copy Markdown
Contributor

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..

@rishabsingh3003

Copy link
Copy Markdown
Contributor Author

I have updated this PR to use takeOffDetected as a metric to use ground clearance fusion fix. Of all the heuristics available in EK3 to determine whether you are in the air, I found this to be the most reliable for this use case.

onGround for copter is essentially just an arming check. it goes false the moment the vehicle arms. So if the copter is sitting on the ground while armed, waiting to take off, onGround is already false, and the rangefinder-ground-clearance fusion stops immediately.

inFlight depends on height measurements (1.5m climb or 0.5m rangefinder increase) to trigger, which creates a circular dependency. It eventually falls back to the 5-second likely_flying timeout, but that's a long time to be fusing a potentially stale value.

takeOffDetected uses gyro rate and rangefinder delta with very low thresholds (0.1 rad/s gyro, 0.1m range change) after a 1-second arming delay. It triggers right when the motors spin up on takeoff, which is exactly the transition point we care about. I've run many replay logs and it consistently fires at the moment of actual takeoff, making it a reliable and direct indicator for this use case without the circular dependency or vehicle-type assumptions of the other flags.

Comment thread libraries/AP_NavEKF3/AP_NavEKF3_core.h Outdated
@@ -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

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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?

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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();

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this relies on valid rangefinder data, but is being called unconditionally

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

@tridge

tridge commented Sep 7, 2026 •

Copy link
Copy Markdown
Contributor

Deprecated — superseded by the updated review further down this thread.

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 28cbfe4adf. Full report: https://uav.tridgell.net/DevCallReviews/2026_09_08_AIReview/devcall_pr_reviews.html#pr32232

REQUEST CHANGES — one safety-critical gap, plus a test issue

CI 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, Measurements.cpp:45-54 reads if ((orientation == ROTATION_PITCH_270) && (status == Good)) { … } else { continue; } with the on-ground substitution at :96 after that continue, so for OutOfRangeLow it was genuinely unreachable. The first commit does add a new fusion path. (It is worth knowing that EKF2 has the identical dead branch — AP_NavEKF2_Measurements.cpp:55-57 vs :99 — so the "EKF2 already does this" precedent in the body is weaker than it reads: nothing in shipped ArduPilot has ever actually fused GNDCLR for an out-of-range-low sensor.)

My objection is to one specific gap, not to the approach.


BUG — the substitution removes the lostRngHgt → baro fallback, and there is no escape from a false hold

Configuration: EK3_SRC1_POSZ = RANGEFINDER, sensor persistently OutOfRangeLow in flight (dark or absorbing surface, water, or a backend that clamps a lost return to zero — a behaviour you list yourself), filtered gyro below 0.1 rad/s. All three release terms at AP_NavEKF3_VehicleStatus.cpp:489-491 are simultaneously dead:

  • gyro — below threshold by assumption;
  • range increase — rangeDataNew.rng is pinned to MAX(median, rngOnGnd) == rngOnGnd by Measurements.cpp:99 while substituting, so rng > rngAtStartOfFlight + 0.1 can never be true. Your own comment at VehicleStatus.cpp:480-482 says as much;
  • movedVertically — reads stateStruct.position.z, which with POSZ=RANGEFINDER is being driven by the substituted measurement. This is the circularity @rmackay9 warned about on 2026-02-26.

On master this configuration self-rescues. Nothing is pushed, rngValidMeaTime_ms goes stale within 500 ms, and lostRngHgt at AP_NavEKF3_PosVelFusion.cpp:1315 forces activeHgtSource = BARO. This PR removes that rescue, because the substitution refreshes rngValidMeaTime_ms at Measurements.cpp:108 every cycle, so the freshness check at PosVelFusion.cpp:1249 never fails.

I looked for a second escape via the innovation path and there isn't one — it makes things worse. After 5000 ms of gate failure hgtTimeout fires and ResetHeight() runs, and ResetHeight() sets stateStruct.position.z = -hgtMea (PosVelFusion.cpp:277), re-anchoring the estimate to the same substituted ground-clearance value. The altitude controller then demands further climb against an underestimated height.

Worth noting how far below the noise floor 0.1 rad/s (= 5.73 °/s) really is here: it is a 10 Hz-LPF'd rate (AP_DAL_InertialSensor.cpp:70-77, hard-coded and deliberately independent of INS_GYRO_FILTER), and EKF3's own on-ground-not-moving check uses radians(3.0f) scaled by EK3_OGNM_TEST_SF (default 2) → 6 °/s. A well-tuned copter holding attitude in calm air sits under that.

Suggested fix — make the third term independent of the fused range, so it cannot be starved by the very measurement it is guarding. Either compute the vertical term from baro (baroDataDelayed.hgt - meaHgtAtTakeOff), or add a fourth, source-independent release that detectFlight() already uses at VehicleStatus.cpp:411:

movedSinceArming = (angRateVec.length() > 0.1f) ||
                   (rngDataFresh && (rangeDataNew.rng > (rngAtStartOfFlight + 0.1f))) ||
                   movedVertically ||
                   (dal.get_time_flying_ms() > 5000);

A plain wall-clock cap on the substitution from timeAtArming_ms would also close it. This is directly the question @rmackay9 asked and never got an answer to, and you have already built the surrounding machinery.

Both review passes reached this independently, from different starting points, and every link in the mechanism was then re-verified against the head sources.


The autotest's own timing mitigation is silently lost after the first reboot

Tools/autotest/arducopter.py:15325 raises LOCAL_POSITION_NED to 20 Hz, with your comment right above it: "the default 5Hz lags the truth sample by most of a metre in a climb." It is applied once, right after the first reboot. The test then reboots three more times (:15371, :15398, :15428) and never reapplies it — and reboot_sitl → reboot_sitl_mav → initialise_after_reboot_sitl() (vehicle_test_suite.py:2661) calls set_streamrate(self.sitl_streamrate()), with Copter's sitl_streamrate() returning 5 (arducopter.py:121). So legs 3, 4 and 5 run at exactly the rate you identified as inadequate. Moving the set_message_rate_hz call into a helper invoked after every reboot fixes it.

Flake risk, and one leg with a budget tighter than the framework's own baseline

The test flies five ALT_HOLD legs with no XY position control and never resets the simulator position — reboot_sitl() asserts position, it does not restore it (the framework says so at vehicle_test_suite.py:10119-10125). Legs 3 and 4 pass startup_location_dist_max=2, but :15428 uses the bare self.reboot_sitl(), whose default is 1 (vehicle_test_suite.py:2595) — and it follows the longest leg (climb to +10…+40 m, hold, land). Copter's own end-of-test allowance is 2 m and its documented baseline for a single takeoff-and-land is ~1.3 m (arducopter.py:104-112). Suggest self.reboot_sitl(startup_location_dist_max=2) at :15428, for consistency with the other two.

This test wants a soak before merge — roughly 100 iterations of Copter.EKF3RangeFinderOnGround, plus the same on Copter.TakeoffGroundEffectAlt/TouchdownGroundEffectAlt, which run adjacent and inherit any leftover displacement. One green CI run does not clear a five-leg uncontrolled-XY test.

Smaller items

  • Master's onGround fallback was deleted outright, not just relocated. Master Measurements.cpp:96-107 had one reachable case: a Good sensor whose three samples aren't all fresh (an intermittent sensor flickering at worse than ~2.5 Hz), while disarmed — it pushed rngOnGnd. There is no head equivalent, so for that sensor POSZ=RANGEFINDER now falls back to baro pre-arm where master kept the rangefinder. Narrow and disarmed-only, but the body says the first commit only adds a path.
  • The substitution is also newly active while disarmed, which the PR doesn't describe. movedSinceArming is false whenever disarmed, so Measurements.cpp:54 runs pre-arm too, and EstimateTerrainOffset() now runs on substituted data (OptFlowFusion.cpp:55), including the hard terrain reset at :118-119. Not obviously wrong, but it deserves a sentence.
  • The Sub branch is inconsistent with the term beside it and has no coverage. VehicleStatus.cpp:483-488 adds an APM_BUILD_ArduSub sign split for the vertical term, but the range term on the next line keeps the copter sign for all vehicles, whereas detectFlight()'s Sub branch uses the opposite sense (:394). There is no Sub job in the CI matrix. Either drop the Sub branch or make the range term Sub-aware — or better, reuse detectFlight()'s existing convention rather than re-deriving it.
  • The landing half is not fixed and cannot be by this design — worth saying in the body. On descent the latch is true for the whole approach, so re-entering OutOfRangeLow in the last metre pushes nothing, lostRngHgt fires, and the filter falls back to baro during touchdown — the exact ground-effect window your motivation calls out. With DISARM_DELAY 10 s there is a further ~10 s on baro afterwards. Identical to master so not a regression, but the rationale applies symmetrically. dal.get_touchdown_expected() would be the natural second condition.
  • Dead initialiser float range_distance = 0.0f; at Measurements.cpp:50 — every path that reaches the store assigns it.
  • Your "40× more samples" worry is overstated: the signal is already 10 Hz-LPF'd, so 10 Hz sampling is near Nyquist for it and 400 Hz buys at most ~2× independent looks at the peak, not 40×.

Checked and clean

movedSinceArming is not a stuck latch — cleared whenever onGround (VehicleStatus.cpp:492-494), and onGround is exactly !motorsArmed in both branches of detectFlight(), which runs immediately before the detector in Control.cpp:21,23, so the clear is same-cycle. InitialiseVariables() doesn't reset it, so an in-flight core re-init leaves it set — the safe direction. Call ordering is right (detector runs before readRangeFinder(), no one-cycle lag). No ABI, wire or logging break: AP_Nav_Common.h is a comment-only diff, TAKEOFF_DETECTED = 1024 and bit 10 are unchanged, so EKF_STATUS_REPORT and XKF4.SS stay compatible — though bit 10 now means different things under EKF2 and EKF3 on the same field, which the wiki/GCS side should know. @tridge's "called in two places" point is genuinely fixed. EK3_FEATURE_OPTFLOW_FUSION-off builds are handled. Median-filter mixing across the substituted/real boundary degrades gracefully. No new parameters. What the autotest does prove was checked against real code and holds: XKF4.SS is format I so 1<<10 is a valid mask, XKF5.TOfs is int16 with a 0.01 multiplier that pymavlink applies, and leg 3 is a genuine positive control for the gyro release.

Two process items, both outside my remit: @tridge's CHANGES_REQUESTED from 2026-03-23 is still live — two of its three points are addressed, but "rename to optFlowTakeoffDetected" was answered by going the other way, which may well be the better call but is his to accept. And @rmackay9's replay-test request is outstanding; the PR's own table shows a −10.7 m terrain-offset delta on one leg, so "no impact on baro vehicles" isn't self-evident.

@andyp1per

Copy link
Copy Markdown
Contributor

Thanks - taking the findings in turn. Two are fixed at 8bec444e50, and I disagree with the headline one.

On the release terms: the mechanism half is right and is the reason the third term exists. The substitution does refresh rngValidMeaTime_ms, so the range term cannot fire while it is running, and the gyro term is on the DAL's 10Hz filtered rate. The comment above the terms says exactly that. Where I don't follow is the step from there to movedVertically being circular.

The last leg of EKF3RangeFinderOnGround flies the configuration in the finding - optical flow for XY, range finder for Z, no GPS, EK3_SRC1_VELZ=0, and RNGFND1_MIN=50 so the sensor never comes into range at any point in the flight. The estimate tracks: 10.3 m climbed against 10.5 m estimated, where the leg fails at 2.5 m of error. The IMU keeps driving position.z against a constant height observation, so it lags harder with no vertical velocity source, but it does not freeze. This isn't a near miss either way - a pinned estimate reads about 0 against a 10 m climb, four times the failure bound.

The terrain leg asserts the other half from the dataflash log rather than from a streamed message: XKF4.SS bit 10 is published, so the latch does release in flight rather than merely being releasable in principle.

get_time_flying_ms() > 5000 was in fact the first thing tried, and it works on Copter. I dropped it because it isn't the same test on every vehicle: Rover, Sub and Tracker set likely_flying from the armed flag, so it reduces to arming plus five seconds, and AP_VEHICLE_ENABLED guards it in the DAL. Height flown since the last on-ground sample is available everywhere, which is why it ended up as the backstop.

Fixed: the range term now carries the ArduSub sign split that the height term beside it already had - detectFlight() twenty lines up tests the range the other way round on Sub, and the two should agree. And the autotest was losing its 20Hz LOCAL_POSITION_NED rate at every reboot, since initialise_after_reboot_sitl() resets it to sitl_streamrate(). The reboots sit inside legs 3, 4 and 5, so those three were sampling at 5Hz. It is now reapplied after every reboot, and the last leg takes the same 2 m startup-location allowance as the other two uncontrolled-XY legs rather than the 1 m default, which is the flake risk you flagged.

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 onGround fallback, the substitution now being live while disarmed, and the landing half being outside what this design can fix are all correct, and belong in the PR body rather than in code. I will fold them in.

(Posted by Claude.)

@andyp1per

Copy link
Copy Markdown
Contributor

The re-run is done: 20 iterations of Copter.EKF3RangeFinderOnGround at 8bec444e50, where the 20Hz stream is now held across all four reboots. Error is estimate minus truth.

leg error over 19 passing runs fails at
armed and stationary, baro drifting 0.3 m/s -0.01 m, every run 1 m
sensor comes into range on the climb -0.4 to -0.5 m 1.5 m
sensor never in range +0.1 to +0.3 m 1.5 m
terrain path, baro as the height source 0.0 m, every run -2 m
indoor, no vertical velocity source -0.3 to +0.7 m 2.5 m

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 reboot_sitl() before leg 3 with "Did not detect reboot", after get_parameter(STAT_BOOTCNT) returned None on a SITL that had restarted and accepted the connection. No position or EKF assertion was involved. Four reboots give this test about four times a normal test's exposure to that, which is an argument for the soak you asked for, but not something this PR can fix.

The three points I agreed are now in the description.

(Posted by Claude.)

@tridge

tridge commented Sep 12, 2026 •

Copy link
Copy Markdown
Contributor

Deprecated — see below for the updated review.

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 8bec444e50 (you were last told 28cbfe4adf). Full report, including everything checked and the independent cross-check: https://uav.tridgell.net/DevCallReviews/followups/2026_09_12_1439/devcall_pr_reviews.html#pr32232

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
  • RESOLVED — Autotest silently lost its 20 Hz LOCAL_POSITION_NED rate after every reboot. arducopter.py:15324-15328 adds reboot_and_restream(), used at :15330, :15376, :15403, :15433 — all four reboots now restream.
  • RESOLVED — Leg 5 used the bare reboot_sitl() with its 1 m default after the longest leg. arducopter.py:15433 now passes startup_location_dist_max=2, matching legs 3 and 4.
  • RESOLVED — Sub branch inconsistent with the term beside it. Commit a3d7fa8 adds movedInRange with the Sub sense at VehicleStatus.cpp:487 (< -0.1f) vs copter at :490 (> 0.1f) — the same senses detectFlight() uses at :394 and :404.
  • RESOLVED — as body text, which is where they belong — Master's onGround fallback was deleted not relocated; the substitution is newly live while disarmed; the landing half is out of reach of this design. All three are now in the PR description's known-issues list, and the author agreed with each on the merits.

Points you pushed back on (3)

  • DISPUTED — author is right, finding WITHDRAWN — Headline BUG: "the substitution removes the lostRngHgt -> baro fallback and there is no escape from a false hold; movedVertically is circular because stateStruct.position.z is driven by the substituted measurement." The mechanism half stands and the author accepts it: the substitution does refresh rngValidMeaTime_ms (AP_NavEKF3_Measurements.cpp:108), which keeps rangeFinderDataIsFresh true (PosVelFusion.cpp:1249) and so kills lostRngHgt (:1315); the range term is inert while substituting; the gyro term is on a 10 Hz-filtered rate. The circularity is real IN FORM: with activeHgtSource == RANGEFINDER, inhibitGndState is set unconditionally (OptFlowFusion.cpp:96-98) so terrainState is frozen, and hgtMea = MAX(rng*c.z, rngOnGnd) - terrainState (PosVelFusion.cpp:1375-1377) is therefore a near-constant. Where the conclusion was wrong is the step from "circular" to "pinned". That observation is SOFT, not a clamp: R = sq(EK3_RNG_M_NSE) = 0.25 m^2 (AP_NavEKF3.cpp:319), gated at 5 sigma (HGT_I_GATE_DEFAULT 500, AP_NavEKF3.cpp:252), against inertial prediction and — in leg 3 — a GPS vertical-velocity observation (EK3_SRC1_VELZ defaults to GPS, AP_NavEKF_Source.cpp:53). The estimate lags rather than freezing, and the lag passes 1.5 m early in a climb, which is exactly what movedVertically needs. The author's leg 3 (POSZ=RANGEFINDER, RNGFND1_MIN=50, sensor never in range) and leg 5 (same plus VELZ=0) fly precisely the configuration specified and track the climb in 19/19 passing runs, against a bound a pinned estimate would miss by 4x. FINDING WITHDRAWN.
  • DISPUTED — author is right — The suggested fourth term dal.get_time_flying_ms() > 5000. Verified their objection: AP_DAL.cpp:55-59 guards it with AP_VEHICLE_ENABLED, and Sub.cpp:339 / Rover.cpp:499 set likely_flying straight from the armed flag, so on those vehicles it degrades to "armed + 5 s". Height-flown is the better backstop. The suggestion was weaker than claimed.
  • DISPUTED — conceded — Dead initialiser float range_distance = 0.0f; (Measurements.cpp:50). The author prefers stack locals initialised. It was never a defect; dropped.

Still open — re-raised, not new (3)

  • STILL OPEN — cosmetic, re-raised — The body still says "roughly 40x more samples against the 0.1 rad/s threshold". The concern is that the figure is configuration-dependent and the samples are heavily correlated, since the DAL gyro is 10 Hz-LPF'd (which the body now says). CORRECTION to this review's own arithmetic, after the cross-check pushed back and was right to: the proposed "~2x independent looks" replacement is NOT justified. AP_DAL_InertialSensor.cpp:68 is a FIRST-ORDER low-pass, not a brick wall, so a 10 Hz component survives attenuated and 400 Hz sampling genuinely does catch peaks that 10 Hz sampling misses (the counter-example run was 0.11sin(2pi*10t), where phase-aligned 10 Hz sampling misses every peak). 400/10 = 40 is the correct count of raw evaluations. So the number in the body is not wrong; what is worth one clause is that those 40 looks are correlated rather than independent, and the ratio depends on the configured loop rate. Cosmetic either way.
  • STILL OPEN — partially addressed, re-raised — Soak. 20 iterations at 8bec444, 19 passing, is real work — but it is not the ~100 asked for, and the adjacent TakeoffGroundEffectAlt / TouchdownGroundEffectAlt were not soaked alongside it. The author's own soak turned up one reboot_sitl() reconnect failure in 20 attempts. Stated precisely, as the cross-check asked: that is an observed failed-attempt fraction of 1/20, not an established per-run flake rate — 20 samples cannot pin a rate — but for a test carrying four reboots it is enough to be worth the larger soak rather than an argument against it. That is a material addition to CI flake budget and argues for the larger soak rather than against it.
  • STILL OPEN — process — tridge's CHANGES_REQUESTED of 2026-03-23 is still the last maintainer review state (verified live). rmackay9's replay-test request is still outstanding.

Findings at this head (1)

  • ISSUE verified — libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp:1261 — the substitution silently enables the EK3_RNG_USE_HGT height-source switch for a sensor that is out-of-range-low on the ground. That branch is gated on rangeFinderDataIsFresh (:1249), which the substitution now makes true at Measurements.cpp:108. On master the sensor's data never reached the buffer (the else { continue; } at master Measurements.cpp:52-54 precedes the on-ground fallback at :96), so rangeFinderDataIsFresh stayed false and the branch was skipped entirely. With this PR, a BARO- or GPS-primary vehicle with EK3_RNG_USE_HGT set will take activeHgtSource = RANGEFINDER (:1297) on the ground and while armed-but-not-yet-moved, fusing the fabricated GNDCLR, and then flip back to BARO via lostRngHgt (:1315) once the latch releases — two extra ResetPositionD(-hgtMea) transitions (:1439-1441) across takeoff, in a feature whose own parameter doc describes it as "designed for vertical takeoff and landing". This is opt-in (EK3_RNG_USE_HGT default -1, AP_NavEKF3.cpp:514) and may well be the intended fix extended to baro vehicles, but the PR body only ever discusses POSZ=RANGEFINDER, and there is no coverage: the existing Copter.EK3_RNG_USE_HGT test (arducopter.py:4925) uses set_analog_rangefinder_parameters(), which sets RNGFND1_MIN: 0 (vehicle_test_suite.py:6273), so the sensor is never OutOfRangeLow. NOT run, so the harm is unconfirmed — but the behaviour change is verified and belongs in the description, and a sixth leg with EK3_SRC1_POSZ=1 + EK3_RNG_USE_HGT set would close it cheaply.
Lower-priority notes (5)
  • verified — Tools/autotest/arducopter.py:15385-15388 (leg 3) and :15408-15410 (leg 4) — nothing in the test bounds the height error accumulated DURING the substitution. Both legs deliberately climb 8-10 m clear before measuring, and the comment at :15381-15384 says why: "the baro offset filter tracks the pinned height, so the error accumulated before the release is carried forward rather than corrected". That is verified — calcFiltBaroOffset() (Measurements.cpp:811, alpha = 0.1 per baro sample) is called whenever activeHgtSource != BARO (PosVelFusion.cpp:1338-1340), so it learns baro - pinned z, and the source-change ResetPositionD(-hgtMea) at :1439-1441 then locks that offset into the state. So the one number that characterises the cost of this change on a never-in-range sensor — peak |true - est| in the window between arming and the latch firing — is the one number the suite is built to exclude. Reporting it (a progress() line, or a generous assertion) would make the trade explicit. Not a defect claim; the magnitude is unconfirmed.
  • verified — Tools/autotest/arducopter.py:15272 — if m.C != 0: continue in assert_ekf_terrain_offset_above(). Log_Write_XKF5 early-returns for any non-primary core (AP_NavEKF3_Logging.cpp:188-191), so XKF5 only ever carries the primary. If a lane switch makes core 1 primary during leg 4, the filter matches nothing and the test raises "No XKF5 messages in the log" — a failure, so it fails safe, but for the wrong stated reason. Dropping the m.C filter is strictly better here.
  • unconfirmed — libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp:489 — the release margin for movedVertically is characterised only at default tuning. It depends on how fast the estimate outruns a soft constant observation, i.e. on EK3_RNG_M_NSE (0.25 m^2 default) and EK3_HGT_I_GATE (5 sigma), and the 20-run soak varies neither. Tightening EK3_RNG_M_NSE both stiffens the pull toward the constant AND narrows the gate that lets the estimate break free, so the net effect is genuinely ambiguous and could not be settled by reading. One extra iteration of leg 5 at EK3_RNG_M_NSE = 0.1 would bound it. Flagged as an unverified sensitivity, not a defect.
  • verified — The five legs use four reboots (:15330, :15376, :15403, :15433), against ~1 for a typical test — the direct cause of the 5% reconnect flake the author measured. RNGFND1_MIN carries no @RebootRequired (AP_RangeFinder_Params.cpp:102-107) and is read live, so at least some leg boundaries may not need a full reboot. The reboots do also serve to start a fresh log (leg 4 needs that) and to reset position, so this is a suggestion to look rather than a claim that all four are removable.
  • verified — Tools/autotest/arducopter.py:15415 vs libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp:492 — the vertical backstop is never tested in isolation. RAISED BY THE CROSS-CHECK. movedVertically is one term of an OR across the gyro, range and vertical-movement terms, so a leg that passes does not establish WHICH term released the latch. Legs 3 and 5 assert the outcome but neither pins the bias-corrected DAL gyro below the 0.1 rad/s threshold for the duration nor records which term fired, and the explicit latch assertion is called only in the BARO-primary leg 4. So the 19/20 passing runs are good evidence that the release happens, and weak evidence that it happens for the reason the design intends. Instrumenting the gyro condition (or logging the deciding term) would close that gap cheaply. Not a flight defect and not a new one — an evidence gap in what the new tests prove.

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.

@andyp1per

Copy link
Copy Markdown
Contributor

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.)

@AP-Review

AP-Review commented Sep 22, 2026 •

Copy link
Copy Markdown

Deprecated — see below for the updated review.

Previous review (2026-09-22)

Automated review note — AI-generated (Claude), validated against the live diff. Please sanity-check before acting.
Verdict: COMMENT

Full report: https://uav.tridgell.net/DevCallReviews/2026_09_23/devcall_pr_reviews.html#pr32232

Reviewed at head 8bec444e50. COMMENT — the change is a genuine improvement over master in every configuration traced, and two independent passes agreed on that. Note up front that @tridge's CHANGES_REQUESTED of 2026-03-23 is still the live review state and @rmackay9's replay-test request from 2026-02-26 is still unanswered; those outrank anything below.

Your CI failure is not your fault. The single red check is build (base) in test_sitl_rover.yml (run 34590189632, job 103233454862). The API returns an empty steps array for it and a wall time of exactly 45m00s (11:37:39 → 12:22:39) — a job that records zero steps never ran its compile. For contrast, a healthy rover build (base) the same day reports 10 steps in 1–7 min, and the identical build (base) job at this same head passed in the copter and plane workflows in 2m33s and 2m49s. Independently, rover and copter both build clean for SITL at this head locally, and a real git merge --no-commit --no-ff into current master (222 commits ahead) auto-merges with no conflicts. A re-run should clear the board.

1. The movedVertically backstop self-releases on baro drift — reproduced in SITL

libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp:489 and :492-494. movedVertically is computed from stateStruct.position.z, so on a baro-primary vehicle (EK3_SRC1_POSZ = 1, the default) the detector releases on baro drift alone while the vehicle is stationary on the ground — turning off the substitution this PR exists to provide, and publishing XKF4.SS bit 10 for a vehicle that has not moved.

Measured, not argued (Copter/ALT_HOLD, RNGFND1_MIN=1.0 so the sensor reads OutOfRangeLow, SIM_BARO_DRIFT 0.3 m/s — leg 1's own value, sitting armed and stationary):

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.SS bit 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 base detectOptFlowTakeoff() had exactly one call site — AP_NavEKF3_Measurements.cpp:200, inside writeOptFlowMeas() — so on a vehicle with no flow driver the bit was permanently 0. It now runs from controlFilterModes() 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:44 is 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 — XKF5 stays 15/15/15/15.
  • AP_AHRS::get_hagl() becomes valid on the ground for this sensor class, because EstimateTerrainOffset() now refreshes gndHgtValidTime_ms pre-arm. AP_GroundEffect.cpp:89 handles it deliberately (it anchors takeoff_hagl_m and subtracts it, with a comment saying HAGL is not zero on the ground), so this is benign and the terrainState initialisation it brings is an improvement. Unverified sub-point: for a sensor that never comes into range, height_is_agl flips false again ~5 s after the latch releases and AP_GroundEffect switches 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_XKF5 early-returns unless core_index == frontend->primary but stamps core : DAL_CORE(core_index), so C is 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 the m.C filter is strictly better. Raised 2026-09-12, not acted on.
  • 11 commits: 8 AP_NavEKF3:, 1 AP_NavEKF:, 3 autotest:. 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:989 drops the #if EK3_FEATURE_OPTFLOW_FUSION guard, so detectMovementSinceArming() is now compiled on boards with AP_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 with AP_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.

@rishabsingh3003 rishabsingh3003 left a comment

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I can't approve this because I am the orignal author but this looks good now.
Need to squash the commits.

@AP-Review

AP-Review commented Sep 30, 2026 •

Copy link
Copy Markdown

Deprecated — see below for the updated review.

Previous review (2026-09-30)

Automated review note — AI-generated (Claude), validated against the live diff. Please sanity-check before acting.
Verdict: COMMENT

Reviewed at head e6c82440f2.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/32232/4.html#pr32232

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.

  1. The movement detector's vertical term still releases on baro drift alone on a baro-primary vehicle that sits armed with an out-of-range-low sensor (VehicleStatus.cpp:489, :494). Two SITL runs at this head released it about 10.5 s after arming with the vehicle stationary, after which HAGL walked from 1.4 to 5.9 m. The narrowing suggested last time (vertical term only when the rangefinder is the height source) was tested and breaks your leg 4 (TOfs -10.5 m), so please don't take it. That suggestion is withdrawn. It isn't a regression against master. It would be good to list it as a known issue and, if possible, add a leg that combines leg 4's setup with leg 1's baro drift.

  2. assert_ekf_terrain_offset_above() skips XKF5 records with C != 0 (arducopter.py:17700). XKF5 is logged only for the primary core, stamped with its real index. So after a lane switch from core 0 to core 1, a bad terrain offset later in the flight passes silently, which I confirmed by running the helper on synthetic records. Dropping the core filter fixes it.

  3. Two known-issues entries in the description are out of date now that AP_NavEKF3: keep baro ground effect out of a height source switch #34432 is in master. With an out-of-range-low sensor and EK3_RNG_USE_HGT, a single SITL run gives worst -0.17 m instead of 2.5 m, and the carried error is now worst -0.73 m / mean -0.13 m above 5 m instead of 1.39-1.48 m low.

  4. It might be worth one line in the description saying that XKF4.SS bit 10 is now set by the movement detector on every EKF3 vehicle, not just ones with optical flow, and that HAGL becomes valid on the ground. MAVLink EKF_STATUS_REPORT doesn't carry that bit, so this only affects log tooling.

  5. As @rishabsingh3003 asked, please squash the 11 commits down to roughly one per subsystem (AP_NavEKF3, AP_NavEKF, autotest). Also, this repo's AGENTS.md asks the PR description to say how AI was used, and it currently doesn't.

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.

@github-actions

github-actions Bot commented Sep 30, 2026 •

Copy link
Copy Markdown

Global Size Summary (Total Flash delta in bytes)

Built from 158e838 rebased onto 278a68a

Board Periph tracker blimp bootloader copter heli iofw plane rover sub
DrotekP3Pro _ _ _ * _ _ _ _ _ _
Durandal _ +284 +284 _ +64 +64 _ +64 +64 +64
Hitec-Airspeed * _ _ _ _ _ _ _ _ _
KakuteF7 _ +284 +284 _ +284 +284 _ +284 +292 +292
MambaH743v4 _ +308 +308 _ +88 +88 _ +88 +88 +88
MatekF405-CAN _ _ _ * _ _ _ _ _ _
MatekF405 _ +284 +284 _ +284 +284 _ +284 +292 +292
MatekH743-bdshot _ +284 +284 _ +64 +64 _ +64 +64 +64
Pixhawk1-1M _ +292 +292 _ +108 +108 _ +108 +104 +116
disco _ +360 +360 _ +84 +80 _ +88 +80 +90
f103-GPS * _ _ _ _ _ _ _ _ _

@AP-Review

AP-Review commented Oct 3, 2026 •

Copy link
Copy Markdown

Deprecated — see below for the updated review.

Previous review (2026-10-03)

Automated review note — AI-generated (Claude), validated against the live diff. Please sanity-check before acting.
Verdict: COMMENT

Reviewed at head 8b490c6438.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/32232/5.html#pr32232

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.

  1. New with the rebase: the self.EKF3RangeFinderOnGround, registration at arducopter.py:19333 is one space deeper than the rest of the list, which master has reindented. flake8 reports E131 there, and the file is AP_FLAKE8_CLEAN, so CI's flake8 step should fail. Dropping one space fixes it.

  2. The movement detector's vertical term (VehicleStatus.cpp:489, :494) still releases on baro drift alone on a baro-primary vehicle sitting armed with an out-of-range-low sensor. A SITL run at this head released it 10.6 s after arming with the vehicle stationary, after which HAGL walked from 1.3 to 6.0 m. It isn't a regression against master, and the earlier narrowing stays withdrawn. A known-issues entry, and ideally a leg combining leg 4's POSZ=BARO with leg 1's drift, would still be good.

  3. assert_ekf_terrain_offset_above() skips XKF5 with C != 0 (arducopter.py:17766). XKF5 is logged only for the primary core under its real index, so after a lane switch a bad terrain offset passes silently. Dropping the core filter fixes it.

  4. The known-issues figures in the description (2.5 m, 1.39-1.48 m) predate AP_NavEKF3: keep baro ground effect out of a height source switch #34432, which is now in master. It would also help to say that XKF4.SS bit 10 is now set by the movement detector on every EKF3 vehicle (Control.cpp:822) and that HAGL becomes valid on the ground. This only affects log tooling.

  5. As @rishabsingh3003 asked, please squash down to roughly one commit per subsystem. This repo's AGENTS.md also asks the description to say how AI was used.

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 MAX(..., rngOnGnd) floor when that sensor reads in range, so this PR doesn't need to change it.

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.

rmackay9 and others added 11 commits October 29, 2026 22:55
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.
@AP-Review

AP-Review commented Oct 30, 2026 •

Copy link
Copy Markdown

Deprecated — see below for the updated review.

Previous review (2026-10-30)

Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting.
Verdict: COMMENT

Reviewed at head 5e0c45cf62.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/32232/6.html#pr32232

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

  • AP_NavEKF3_Measurements.cpp:56: with two downward range finders, a Low second sensor's substituted ground clearance is pushed after a Good first sensor's reading with the same timestamp, and recall keeps the last one, so the real reading is dropped until the movement latch sets. In SITL with range-primary height and RNGFND2_MIN above the climb, the EKF height lagged 2.5 m on takeoff, against 0.6 m with one or two good sensors. Only substitute when no downward sensor is Good that cycle.
  • arducopter.py:17766: XKF5 core filter (still open). Drop if m.C != 0: continue, since XKF5 is logged only for the primary core.

Notes

  • AP_NavEKF3_VehicleStatus.cpp:489: baro drift releases the latch on a stationary baro-primary vehicle (still open). Worth listing under known issues, and ideally adding a test leg with POSZ=BARO plus drift.
  • AP_NavEKF3_Control.cpp:822: description (still open). The known-issue figures predate the merged AP_NavEKF3: keep baro ground effect out of a height source switch #34432 fix, and the description should say that bit 10 now means 'moved since arming' on every EKF3 vehicle. It also reaches the gimbal device's estimator_status.
  • Replay data for @rmackay9's question: merge-base-flown flights replayed at head show no XKF1/2/3 differences. Only XKF4.SS differs, plus terrain/HAGL by at most 0.07 m when a range finder reads below its minimum on the ground. That could go in the description.
Minor (1)
  • Commit stack (still open): still 11 commits as against the squash @rishabsingh3003 asked for, and the description has no AI-assistance statement.

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.
@AP-Review

Copy link
Copy Markdown

Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting.
Verdict: COMMENT

Reviewed at head 158e8386a3.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/32232/7.html#pr32232

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)
  • Commit stack (still open): 14 commits now, against the squash @rishabsingh3003 asked for. If the work was AI-assisted, AGENTS.md also asks the description to say so.

@rmackay9

rmackay9 commented Nov 17, 2026 •

Copy link
Copy Markdown
Contributor

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.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Projects

None yet

Development

Successfully merging this pull request may close these issues.

7 participants