Skip to content

AP_NavEKF3: keep optical flow nav alive above the rangefinder range - #33585

Open
andyp1per wants to merge 27 commits into
ArduPilot:masterfrom
andyp1per:pr-optflow-flat-ground
Open

andyp1per wants to merge 27 commits into
ArduPilot:masterfrom
andyp1per:pr-optflow-flat-ground

Conversation

@andyp1per

@andyp1per andyp1per commented Jun 27, 2026 •

Copy link
Copy Markdown
Contributor

Summary

If you fly a copter on optical flow above what its range finder can see, the EKF drops relative position about 5 s later and, with an EKF origin, the failsafe lands the vehicle, although flow is still being fused. Today the optical flow height limit keeps most vehicles below that height, which is why the limit cannot be lifted (#34380) without this.

This keeps relative position valid above the range, by default as asked for in review. Where terrain data reaches the EKF, the ground follows the terrain database anchored to the height the range finder last measured, so an error in the origin altitude or the database's absolute height cancels. Without terrain data the ground is held at its last measured height, within 10 x the height above ground of where it was last known; after that the failsafe acts as today. Range finder height (EK3_SRCn_POSZ 2) is now covered too, without taking a return from something passed over as the height.

Stacks on #35014 (the terrain state carried across vertical datum resets): its five commits come first, and this PR starts at "AP_DAL: log every terrain height write".

The evidence is SITL and Replay. The flights so far ran EK3_OPTIONS bit 2, which bypasses the anchor; replayed with the anchor, 16 of them gave the same flow velocity within 0.02 m/s, as the database sat within about 0.4 m of the range-measured ground. Cancelling a larger origin error is shown in SITL only. Replayed with range finder height, a flight above its 15 m range finder now keeps relative position; as flown, every record is byte-identical to the previous head.

Classification & Testing (check all that apply and add your own)

  • Automated test(s) verify changes (e.g. unit test, autotest)
  • Tested manually, description below (e.g. SITL)
  • Logs available on request

SITL, flow-only copter with no GPS, 8 m range finder, 1 km traverses at 5 m/s from 35 m over simulated terrain; position error against truth at the end. Closed loop, so runs differ by a few metres; the first two columns were run on 2026-09-16 on the design before this one.

ground under the traverse flat ground alone terrain database used directly this PR
flat 14-15 m - 4.2-4.8 m
rising 16 m 177-179 m 10 m 5.5-11.5 m
falling 14 m 155-159 m 24 m 7.7-7.9 m
off a slope from 6 m 940-950 m 114 m 28.6-30.9 m
rising, origin 30 m low - fallback refused 7.0-12.3 m
any, no terrain data unbounded - ends after about 370 m with 2.2-2.4 m error, then failsafe

EK3_OptflowAboveRangefinder covers the climb out, the cases the checks refuse, the distance bound and short returns from an obstacle. EK3_OptflowAnchoredTerrain checks the EKF's speed against truth over 8 m of rising terrain with the origin 30 m low (within 1%, 0.82 with the anchor removed) and the ground held when the anchor ends (1.2-1.5 m error, 7.9 m without). EK3_OptflowAboveRangefinderRangeHeight does the same with range finder height. Per-leg detail is below.

Description

Detail for reviewers, not needed to review the change in outline.

Tests in detail: EK3_OptflowAboveRangefinder covers the climb out, each case the checks refuse, bit 2, no carry-over to the next flight, the distance bound (dropped at about 210 m at 21 m height, and after 300 m on GPS), LOITER surviving above the range with an origin, short returns from an obstacle, and an obstacle followed by readings the terrain gate rejects (held 3.7 s before the timing fix, 0.2-0.4 s after). EK3_OptflowAboveRangefinderRangeHeight does the same with EK3_SRC1_POSZ 2: relative position held above an 8 m range finder (lost at about 8 m without this), a 1 s return of 2.5-3.5 m at 20 m moving the height under 0.1 m against truth (18.7 m without the hold), the range finder taking the height back in range (a 5 m baro drift leaves the vehicle where it is), and a later loss there refused. EK3_OptflowAnchoredTerrain flies 8 m of rising terrain with the origin 30 m low: the EKF's speed is within 1% of truth, against 0.82 of it with the anchor taken out of FuseOptFlow(), and height above ground within 0.3 m; then bit 2 is set in flight and the ground held is the last anchored one, 1.2-1.5 m worst error against 7.9 m without that hand-over. Each leg was shown to fail with its fix removed on 2026-10-01; the anchor and hand-over mutations were re-run on this design, and the timing fix's test fails without it at 3.7 s. On 2026-11-13 the range finder height legs, the anchor hand-over flown without flow, a descent below the height limit with the range finder lost after a climb out, and a brief return timed on the clock rather than on the range samples were each shown to fail with their fix removed.

This is EKF3, plus an AP_DAL fix and AP_NavEKF's optflow_enabled(). The AP_DAL commit logs every terrain height write rather than only changes: EKF3 judges terrain data stale by when it last arrived, so over level ground the replayed filter dropped it after 5 s where the vehicle did not. That is a Replay fix for bit 2 on master as well.

EK3_OPTIONS bit 2 keeps its behaviour: the database height used directly, including with no height source, and the height limit lifted while terrain data is available. Without it, terrain data now reaches the cores for anchoring only. An anchor measured with no terrain data available is discarded, so the offset is never carried to a place it was not measured. Anchoring only runs with optical flow in a source set, so vehicles without it are unchanged.

FuseOptFlow() chooses the anchored height beside bit 2's database height and the AGL KF's. The flow speed limit and nav gain scaler, the flow height limit, the flat-ground checks and XKF5's HAGL and TOfs read the same height through flowTerrainState(), so they agree with the height flow fusion uses when neither bit 2 nor the AGL KF supplies its own, and TOfs - HAGL is still the vertical position. Nothing writes the terrain state while the anchor is in use; when the anchor ends without a range return (bit 2 set in flight, a disarm, flow removed from the source sets, the range finder becoming the height source) the last anchored ground is copied into it once, and an undone brief return puts back the anchored ground saved when it began. Replay of a flight with the anchor in use for 125 s gives every EKF record byte-identical to the previous design, in which the anchor wrote the terrain state.

Range finder status cannot tell a climb out of range from a failure: most lidar drivers report a lost return as out of range high at any height, and some report no data above their reach. The height gate and the last-good-reading check do it instead, with terrain data or without, so anchored terrain cannot hide a range finder that fails just above the limit. A range finder whose real reach is under 0.7 x its configured maximum gets no fallback, and one under 2.9 m, whose limit sits at its 1 m floor, never does.

A return from a roof or tree flown over above the range is not the ground. Without the brief-return handling the terrain estimator's 5 s reset takes the first return after a gap as the ground, so flow is scaled by the obstacle's height and the vehicle flies faster than it thinks. A return lasting 2 s is taken as the ground; a shorter one is undone 500 ms after it stops, putting back the estimate saved when it began, and one that breaks off with the vehicle under the height limit over the held ground is a descent and is kept. 2 s is a judgement, not a measurement: long enough for a roof or tree passed at flying speed to end first, short enough that a real descent into range is taken up quickly. This is the problem AP_SurfaceDistance's glitch handling solves for the vehicle's target, but that sits above AHRS and reads the range finder outside the DAL, so the EKF cannot use it. The returns are timed on readings fused into the terrain estimator, so readings the terrain gate rejects end a return rather than extend it; timed on every valid reading, an obstacle followed by gate-rejected readings of the ground below was held as the ground and relative position was lost.

The 10 x bound is where flat ground stops being trusted without terrain data. It comes from SITL, not from a flight: no flight has reached it. On the 2026-09-16 code without it, a traverse off a slope from 6 m flew 9.7 m/s on a 5 m/s command and ended 940 m out with nothing reported; with it the flight ends with 2-3 m of error and the failsafe acts as today. A fixed factor could instead follow EK3_TERR_GRAD (at its default of 0.1, 10 x the height is where that gradient puts the ground a whole height away), or flat ground without terrain data could go back to being opt-in. With anchored terrain the ground is known continuously and the bound does not apply.

Anchored terrain is only as accurate as the database's shape. With SITL ground flat and the database's real relief as a known error, position drifted by about 0.4 x that error over the height above ground. In flight logs the database's error changed by 1.6 m (90th percentile) between points within 50 m and 3.4 m between points 50-100 m apart; 3.4 m is 30% of 11 m, where it makes the drift about 12%. Nothing can be done about that in flight, so the EKF warns at each arming when optical flow is configured and the downward range finder's maximum is under 11 m, unless it is under 2.86 m, where there is no flat-ground fallback to drift.

The anchored terrain takes over once the offset has been stale for 5 s, so the height flow is scaled by holds through those 5 s and then steps by whatever the database says the ground did meanwhile. A vertical datum reset moves the anchor and the saved estimate as #35014 moves the terrain state.

With the range finder as the height source the terrain estimator is skipped, so no ground offset is estimated. The ground counts as measured instead in each step whose range height passes its innovation check, which records where it was last known and the last good reading the climb-out check reads. gndHgtValidTime_ms is left alone, so EKF_POS_VERT_AGL and getHAGL() read as before, and flat ground engages in the step the height falls back to baro. While it is engaged the range finder does not take the height back: taking it resets the height to the first reading, which can be a roof passed over. The fallback ends once the terrain estimator has fused range readings for 2 s and the range finder takes the height then, so after a climb out of range it comes back at least 2 s late. The anchor is never taken from range height, so with terrain data such a vehicle falls back to flat ground. Under flow navigation the hold also means the terrain estimator measures the ground, which ends an anchor, before the range finder takes the height; the hand-over of the anchored ground in selectHeightForFusion() is reached in a source set without flow, and EK3_OptflowAnchorToRangeHeight flies both (without the hand-over the height steps by the 8 m the ground rose). On its own this PR leaves the height limit in place everywhere; #34380 lifts it where the fallback can take over.

With bit 3 set, the AGL KF owns the flow scale height for about 5 s after the range finder stops and freezes meanwhile, so expect about 5 s of flow scaled low at the crossing. That is existing bit 3 behaviour.

Known limits. The database is looked up at the primary core's position, so a flow lane that has drifted from it gets the ground of the wrong place; bit 2 does the same. A horizontal position reset does not move the point the ground was last known at, so the jump counts towards the 10 x bound. With two downward range finders the last good reading is compared against the reach of the first one reporting, which need not be the one that read it. Flash cost was about 1.8 kB on Pixhawk1-1M at an earlier head that included #35014's carry.

@Hwurzburg Hwurzburg added the WikiNeeded needs wiki update label Jul 27, 2026
@andyp1per
andyp1per force-pushed the pr-optflow-flat-ground branch 2 times, most recently from 072a072 to b4e5e1c Compare September 4, 2026 08:56
@tridge

tridge commented Sep 4, 2026 •

Copy link
Copy Markdown
Contributor

Note

Superseded. This review was for head b4e5e1cba9. The current review is at
#33585 (comment) (head 18a2bbe7b4),
which raises the verdict from REQUEST CHANGES to COMMENT.

Original review (superseded, kept for reference)

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_05/devcall_pr_reviews.html#pr33585

Reviewed at head b4e5e1cba9. Open since June with no comments or reviews, so apologies this is the first feedback. REQUEST CHANGES — the idea is sound but the new authorisation guard doesn't test what it's meant to.

The gate doesn't mean what it says

1. gndHgtValidTime_ms != 0 is a permanent latch, not a freshness test. flatGroundAssumed() is the only consumer in the file that tests that stamp for non-zero; every other one tests an age — OptFlowFusion.cpp:42 and :122 both use < 5000, PosVelFusion.cpp:1380 uses < 1000. The stamp is written at OptFlowFusion 168 and 251 and cleared only at AP_NavEKF3_core.cpp:216, i.e. full EKF init.

So once terrain height has been valid once in a flight, the flat-ground authorisation never expires — it still holds an hour later with the rangefinder long out of range, which is exactly the regime the option exists for. A freshness window matching its neighbours would make it say what the comment says.

2. And !hgtTimeout is satisfied by a synthetic observation. With EK3_SRCn_POSZ = NONE, PosVelFusion.cpp:1537-1541 fuses a constant height of zero at 14 Hz with fuseHgtData = true, which stamps lastHgtPassTime_ms — so hgtTimeout can never fire in that configuration. The flow scale height then follows a position.z pinned to a synthetic zero. Both halves of the guard pass in a configuration with no real height source at all.

Two features, one title

The body describes only bit 5, but the five commits split cleanly into two independent features:

  • b04875313be + 37e08cbbf8d + 7500ba77dd1 — AglKfVelForVelD (EK3_OPTIONS bit 4), the new parameter EK3_AGL_VD_SPD, a change to the AGL KF velocity decay condition, and its own autotest
  • ea57ce72fef + b4e5e1cba91 — OptflowAssumeFlatGnd (bit 5) and its autotest

Two separately-testable EKF options and a new parameter, with the description covering one. Splitting would let bit 4 land on its own merits; it looks the less contentious of the two.

On the parameter index

EK3_AGL_VD_SPD is 14 characters, index 12 is the next free slot (master's var_info2 stops at 11), metadata is complete, and the default -1 with both bits off means stored configurations upgrade inert. All good.

But index 12 is contested. #32471, also open, carries the comment // index 12 was ABIAS_HVR_Z, moved to INS_ACC_VRFB_Z — it retires 12. Whichever of the two merges second inherits the other's meaning for that index, and anyone who flew a build of #32471's branch would have a stored EK3_ABIAS_HVR_Z that then loads into EK3_AGL_VD_SPD. Neither PR mentions the other. (#33484 took 13 and deliberately left 12 free, so it's already anticipating you.)

One more interaction

A cross-check pass noted that bit 3 (AglKfForOptflow) overrides the flat-ground scale while aglKfValid, but after range loss that KF washes its vertical velocity toward zero — so bits 3 and 5 set together give a scale height that decays rather than holding flat. The two options interact and neither description mentions the other.

Credit where it's due: the autotest asserts on EKF_POS_HORIZ_REL, which is the flag the EKF failsafe actually reads, and flies the same climb twice with the bit clear and set so the flat-ground path is the only difference between the legs. That's the right thing to assert on.

@andyp1per

Copy link
Copy Markdown
Contributor Author

Thanks - the guard was wrong in both halves and is fixed.

The stamp test. Right that it was a latch. A freshness window is not the fix though: the option exists precisely because the terrain offset has gone stale, so any age test its neighbours would recognise makes it a no-op. What the guard is trying to say is "a terrain offset was measured during this flight", and that is what it now says - a gndOffsetMeasured flag that latches when gndOffsetValid is true in flight and clears whenever the vehicle is not in flight, so it cannot carry an offset from an earlier flight flown somewhere else.

POSZ = NONE. Worse than the flag suggests: the flow scale height is terrainState - position.z, and with a constant zero fused at 14 Hz position.z carries no height at all, so the assumption would freeze against nothing. The guard now also requires activeHgtSource != SourceZ::NONE.

Both have autotest legs, and both legs fail against the previous guard: one flies, lands, puts the ground out of the rangefinder's reach and flies again, expecting the assumption not to carry over; the other runs with EK3_SRC1_POSZ = 0 and expects it refused.

Bits 3 and 5. The cross-check is right and it is worth a number. On the autotest climb with both bits set, measured from the moment the rangefinder stops fusing:

t true AGL terrainState-pd AGL KF H AGL KF valid
+0 s 7.9 7.68 7.50 1
+2 s 10.2 10.18 9.46 1
+4 s 12.7 12.70 10.34 1
+6 s 15.2 15.21 10.70 0

The AGL KF freezes because with no rangefinder its velocity is washed to zero, then hands back to terrainState - pd in one step when it times out at 5 s. That is existing bit 3 behaviour - today navigation simply dies at the same moment, so nothing downstream sees the step - and bit 5 is what keeps flying through it. So with both bits set, expect about 5 s of flow velocity scaled low at the ceiling crossing, and correct after. I have not changed it here: making the AGL KF yield earlier means telling a climb out of range apart from a dropout it is supposed to bridge (indoors it bridges a quarter of the flight), which belongs with bit 3. Both bits are now in the autotest, asserting relative position survives with them set together, and the description will say so.

Two features. They are two PRs - #33478 is open and carries the first three commits (bit 4, EK3_AGL_VD_SPD, the decay fix and its autotest); this one is stacked on it and is the last two. That was the last line of the description and is now the second. GitHub will not base a PR on a fork branch, so the base PR's commits show up here whatever I do.

Index 12. A collision between two of my own branches rather than anything released - index 12 has never been used in EKF3's var_info2 upstream, so only someone who flew #32471's branch has a stored EK3_ABIAS_HVR_Z. EK3_AGL_VD_SPD moves to 15 on #33478 (13 is #33484's, 14 is #33507's) and 12 stays retired.

All of the above is written and tested locally; it lands on the branches with the next push.

(Investigated and written by Claude, reviewed by me before posting.)

@rmackay9

rmackay9 commented Sep 4, 2026 •

Copy link
Copy Markdown
Contributor

This is a bit similar to what I added for high-altitude optical flow (see PR #30718, see wiki here) where I added a new EK3_OPTIONS bit "Optflow may use terrain alt".

I wonder why we would want to assume flat earth when instead we could use the terrain altitude? I suspect the answer is because the vehicle may not know its location (e.g. non-GPS navigation) or just as a fall back in case it can't get the terrain altitude from the database. In which case I somewhat wonder if we should just combine the option bits together. It's enough of an edge case that I'd be happy with it being combined and I suspect users would too. It's basically a "do your best!" kind of option.

@andyp1per
andyp1per force-pushed the pr-optflow-flat-ground branch from b4e5e1c to 18a2bbe Compare September 5, 2026 08:16
@andyp1per

Copy link
Copy Markdown
Contributor Author

Correcting my own comment above, and reporting what a self-review pass found on top of it.

The latch I described was still wrong. I said it latches "when gndOffsetValid is true in flight". gndOffsetValid is true by fiat whenever the rangefinder is the height source (OptFlowFusion.cpp:42), and in exactly that state inhibitGndState stops the terrain estimator ever updating gndHgtValidTime_ms - so the latch could fire without a terrain offset having been measured at all, which is the thing it exists to require. It now keys on the measurement timestamp directly. A second path found independently: gndHgtValidTime_ms starts at zero, so (imuSampleTime_ms - 0) < 5000 is true for the first five seconds after boot; the non-zero test closes that too.

One consequence worth stating plainly. EKF3 cannot tell a landing from a disarm on a copter - onGround is the armed flag inverted - so scoping the authorisation to a flight means a mid-air re-arm drops it until the terrain offset is measured again. The old permanent latch survived that case. I think flight scoping is still right, because the case it prevents (an offset measured before an earlier landing somewhere else) is the common one and a mid-air re-arm is not, but it is a real trade and not a free win.

On bits 3 and 5, a second look agreed with the first and put numbers on it from the code that match what I measured from the log. I have left bit 3 alone and documented the transient instead; the autotest leg I had added for the combination is gone, because flatGroundAssumed() does not reference the AGL KF and by the time the leg sampled the flag the AGL KF had been invalid for the same five seconds - it could not have failed differently from the leg above it. A test that cannot fail is worse than no test.

The autotest could have passed for free. delay_sim_time() does not drain the link and assert_receive_message returns the oldest queued message, so the eight second wait for the terrain offset to go stale could be sampled from before it went stale. Every wait is now on EKF_POS_VERT_AGL, which is gndOffsetValid published, and every leg checks the flag set as well as clear so that losing flow aiding cannot satisfy the negative half.

One fix landed on #33478 instead, since it is in this PR's diff and a reviewer will read it: the bad-IMU aliasing check at PosVelFusion.cpp runs on gpsDataDelayed.have_vz, which keeps its last recalled value, so it could run on the AGL KF velocity during the very GPS outage that let that fusion engage - and with R_OBS[2] replaced by the AGL KF variance its threshold is an order of magnitude too tight. It is excluded now. Worth noting the obvious one-line version of that fix is wrong: fuse_gps_vz also drives imax for the velocity consistency test, so narrowing it there would have dropped velD out of that test entirely.

The covariance question that came up alongside this is now measured rather than argued, on #33478's side. XKV1.V06/V09 already log P[6][6]/P[9][9], so a four leg SITL sweep settles it: the velD fusion collapses velD variance 14x and posD variance 10.3x at default process noise, which independently reproduces the 11x-17x measured from flight logs, so that trade is real and the size it was documented as. The predicted fall below VEL_STATE_MIN_VARIANCE into the covariance reset cycle did not happen - closest approach 1.9x above the clip, no samples at it.

(Investigated and written by Claude, reviewed by me before posting.)

@tridge

tridge commented Sep 5, 2026 •

Copy link
Copy Markdown
Contributor

Note

Superseded. This review was for head 18a2bbe7b4. The current review is at
#33585 (comment) (head 62a3fbeaba),
which moves the verdict from COMMENT to REQUEST CHANGES on a measured flow-scale-height error.

Original review (superseded, kept for reference)

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

Reviewed at head 18a2bbe7b4.

Full report: https://uav.tridgell.net/DevCallReviews/2026_09_05/devcall_pr_reviews.html#pr33585

COMMENT — up from REQUEST CHANGES

Every finding from my previous round is either resolved in the new code or answered on the merits, and where you pushed back you were right. What is left is one stale commit message, one status-flag consistency gap with low practical impact, and two documentation precision points. Nothing here should block merge once #33478 lands first and CI is green.

Worth stating up front, because it reframes the risk: the bit-5 change is 29 insertions across 7 files — GitHub folds in the three commits belonging to the stacked #33478, which is what makes the diff look like 497 lines. And it is status-only: FuseOptFlow is untouched, and I confirmed flow fusion already runs above the rangefinder's range on master (AP_NavEKF3_OptFlowFusion.cpp:314, :696 — no gndOffsetValid gate anywhere in that path). So this changes what the EKF reports, not what it estimates.

Previous round

1. gndHgtValidTime_ms != 0 is a permanent latch — RESOLVED, and my proposed remedy was wrong. You disputed the fix rather than the finding, and you are right: a freshness window would make the option a no-op, because the option exists precisely for the regime where the terrain offset has gone stale. Dropping that half.

The latch is now flight-scoped (OptFlowFusion.cpp:43-49), which I verified against detectFlight() — for a non-fly-forward vehicle inFlight is cleared only by !motorsArmed and never again while armed, so the flag is exactly the per-arm-cycle latch the comment now claims. Your own correction also checks out: keying on gndOffsetValid would have been wrong, because it is true by fiat for a rangefinder height source, and in that state EstimateTerrainOffset sets inhibitGndState = true so gndHgtValidTime_ms is never stamped. Keying on the timestamp is the right choice.

2. !hgtTimeout satisfied by the synthetic zero with POSZ = NONE — RESOLVED. flatGroundAssumed() (Control.cpp:759-764) now adds activeHgtSource != SourceZ::NONE. PosVelFusion.cpp:1375-1378 sets NONE exactly when getPosZSource() == NONE, and that is the same branch that reaches the constant-zero fusion — so the exclusion is airtight. The autotest leg at arducopter.py:2232 covers it.

3. Two features, one title — RESOLVED as far as GitHub allows. The PR body now says #33478 carries the first three commits. That is the right answer; the stacking is a GitHub limitation, not an authoring choice.

4. Parameter index 12 contested — RESOLVED. AP_NavEKF3.cpp:781 is now index 15. var_info2 on master stops at 11, 13/14 are claimed by #33484/#33507, and 12 stays retired for #32471. Name is 14 chars, metadata complete, default -1 inert; param_parse and param-file-validation both pass.

5. Bits 3 and 5 interact — DISPUTED, you are right, and the disposition is better than a fix. You agreed with the mechanism, measured it in SITL, chose not to change bit 3, and documented the ~5 s window in both the @Description and the commit message. Making the AGL KF yield earlier would require distinguishing a climb out of range from a dropout it is meant to bridge, which genuinely belongs with bit 3. One consequence for the record: during that window bit 3 overrides the flow scale height while getEkfControlLimits still uses terrainState - pd, so the flow scaling and the nav gain scaler use different heights. Pre-existing bit-3 behaviour, not introduced here.

Credit where it is due on the autotest: drain_mav() before assert_receive_message (arducopter.py:2183-2187) closes the stale-queued-message hole you found yourself, and waiting on EKF_POS_VERT_AGL rather than a fixed delay makes the stale-offset wait observable rather than a race.

New

1. The head commit message describes an autotest leg that is not in the code. It says "Three further legs cover the authorisation", including a bit-3 combination leg. There are only two — :2232 (POSZ None) and :2265 (no carry-over). You explained the removal in the thread, but the commit message was not updated. The PR body is correct; only the message is stale, and it is the message that survives in the tree.

2. status.flags.dead_reckoning was not updated alongside canDeadReckon. The commit message says "the same condition is used for canDeadReckon", which is true of readGpsData() (Measurements.cpp:661) but not of Control.cpp:813, which still uses bare gndOffsetValid. So on the flat-ground path the !(...) term stays true and dead_reckoning can be reported set while horiz_pos_rel is also set — the two notions of "am I dead reckoning" disagree. Practical impact is small, since it also needs doingWindRelNav and a copter on flow alone never hits it. But if the intent is that the flat-ground assumption counts as flow nav, :813 should use optflow_gnd_offset, already computed two lines up. (Note the two also use different doingFlowNav definitions — measurement freshness vs fusion success — which is pre-existing, but this PR makes both lines matter in the same regime.)

3. The option is silently inert with EK3_SRCn_POSZ = RANGEFINDER. With the rangefinder as height source, EstimateTerrainOffset takes the inhibitGndState = true branch unconditionally (OptFlowFusion.cpp:104-106), so gndHgtValidTime_ms is never stamped, so gndOffsetMeasured never latches. When the vehicle then climbs out of range, lostRngHgt drops the source to BARO and there is still no measured offset to latch. Net: the bit does nothing for that configuration, which is a plausible indoor flow setup. Not a bug — the offset genuinely was never measured — but the @Description should say so, because the user gets no behaviour change and no diagnostic.

4. "a terrain offset the range finder measured" is imprecise. gndHgtValidTime_ms is stamped in two places: the rangefinder fusion (:175) and the flow-derived terrain fusion (:258). In practice the second needs a GPS-aided vehicle above 5 m/s, so it is not reachable in the flow-only case this targets — but "a terrain offset measured since the vehicle was last disarmed" would be accurate, and dropping "the range finder" avoids the question.

5. Quantified, because it supports your framing. The flow observation is an angular rate v/H; R_LOS carries no height term and the assumed height enters as a divisor of the whole Jacobian rather than as an uncertain state. So the fixed point is v_est = v_true · H_assumed / H_true — a pure multiplicative scale error, not a divergence:

case                                   H_assumed  H_true   v_est/v_true
0.5 m baro drift at the test ceiling      21.5      21.0    1.024 (+2.4%)
ground drops 10 m (out over an edge)      21.0      31.0    0.677 (-32%)
ground rises 5 m (over a roof)            21.0      16.0    1.313 (+31%)

Baro drift is a non-issue at these heights; the hazard is entirely the flat-ground assumption being violated, exactly as your description says. But no gate catches it — at the fixed point the innovation is zero by construction, so flowTestRatio stays near zero and the < 1.0f gate never fires, and the 5 s flowFusionTimeout backstop needs rejection so never trips either. Not introduced here (the same is true of bit 2 and of the in-range case), but it makes the "only safe over flat ground" warning load-bearing rather than advisory — I would make it the first sentence of the bit-5 text rather than the fourth.

For the record I also confirmed the trade is not circular: terrainState is frozen (EstimateTerrainOffset is not called with _flowUse == FLOW_USE_NAV and no range data) and stateStruct.position.z comes from baro, which flow does not observe.

6. Two smaller ones. #34292 (FLOW_HGT_MIN) inserts a block immediately adjacent to your new hunk, so the two will conflict textually — but semantically they are disjoint (that one guards the bottom of the range with a live rangefinder, this one the top with none), so it is a trivial rebase for whichever merges second. And the bit-5 text is 1018 characters of a 2623-character @Description — more than a third of the whole OPTIONS description for one bit, including operational guidance that reads like wiki material. No flash cost, so purely a UX call.

Thread

@rmackay9's question from 2026-09-04T23:04 is still unanswered and is the one open design question: why assume flat earth when bit 2 can use terrain-database altitude, and should the two be combined? The PR body gives half the answer — they are independent, and bit 2 needs a known position plus database coverage that a non-GPS indoor vehicle has neither of — which is worth saying in the thread rather than only in the commit message. On combining: updateFilterStatus already ORs them, so a single "do your best" bit would be a small change and would remove a genuinely confusing pair of adjacent options. Maintainer call, not a code defect, but better settled before merge than after.

Your 08:16 self-correction identified the gndOffsetValid-vs-timestamp error in your own earlier reply and fixed it; I confirmed it independently. One thing you noted and did not fix, which I agree should stay unfixed here: gndOffsetValid at OptFlowFusion.cpp:42 has no != 0 guard, so it is spuriously true for the first 5 s after EKF init. The new code does guard it; fixing the old one would change behaviour for every existing user and does not belong in this PR.

CI: 35 pass, 65 pending, 0 fail. Every SITL autotest job and all the constrained-board builds were still pending, so I cannot say the copter tests pass — including the new EK3_OptflowAssumeFlatGnd test itself. An independent cold review returned APPROVE with no findings.

@andyp1per
andyp1per force-pushed the pr-optflow-flat-ground branch from 18a2bbe to 62a3fbe Compare September 5, 2026 14:38
@andyp1per

Copy link
Copy Markdown
Contributor Author

Good catch, and you have found a hunk I dropped when splitting this out of my own branch rather than a deliberate design.

You are right about the motivation. The reason terrain altitude does not solve it is more absolute than "may not know its location": AP_Terrain::update() only calls writeTerrainAMSL() when ahrs.get_location() succeeds, so a GPS-denied vehicle never has terrain data written at all, whatever the option bits say. Indoors there is nothing to prefer. Outdoors with a position there is, and it should be preferred - which is what bit 2 already does.

The "set bit 2 as well" line in my description was the tell. NavEKF3::writeTerrainData() forwards to the cores only under bit 2, so with bit 5 alone terrain_srtm_alt_valid could never become true and the flat assumption was the only path even where the database had coverage. My branch widens that gate to either option, with the comment "so the flat-ground fallback automatically prefers the terrain database when coverage exists"; I lost it in the split. Restored, so bit 5 alone is now the "do your best" behaviour you are describing: SRTM where there is coverage, flat ground otherwise, and no second bit to set. The existing height-above-ground hierarchy in FuseOptFlow already does the choosing.

On merging the two bits into one, I would rather you decide, because it changes your option rather than mine. Today bit 2 over terrain without coverage drops relative position and the vehicle failsafes. Merged, it would instead keep navigating on a terrain offset frozen at the takeoff point, and over rising ground that scales the flow velocity wrong in the direction that makes the vehicle drift rather than stop - which is why bit 5 is documented as only safe over flat ground such as large indoor spaces. That is a real difference outdoors even if it never shows up indoors. If you are happy that "do your best" is the right contract for bit 2 as well, I will merge them and drop bit 5; if you would rather bit 2 kept the option of failing cleanly, the two stay separate and bit 5 is the permissive one.

Either way the "set both bits" awkwardness is gone.

There is now an autotest leg for it, which there was not before: with only this option set and EK3_SRC1_POSZ = None the flat-ground path is refused, so relative position staying valid can only come from the terrain altitude reaching the core. It fails if the gate is narrowed back to bit 2.

(Investigated and written by Claude, reviewed by me before posting.)

@tridge

tridge commented Sep 5, 2026 •

Copy link
Copy Markdown
Contributor

Deprecated — see below for the updated review.

Previous review (2026-09-05)

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

Re-reviewed at head 62a3fbeaba. My earlier comment above is superseded.

Follow-up report: https://uav.tridgell.net/DevCallReviews/followups/2026_09_06_0115/devcall_pr_reviews.html#pr33585

REQUEST CHANGES — down from COMMENT, on a measurement

@rmackay9's question is now answered, and answered in code, which is the right way to answer it. Forwarding terrain to the cores when either bit 2 or bit 5 is set (AP_NavEKF3.cpp:1790-1793) means the database is preferred wherever there is coverage and the flat-ground assumption becomes the fallback, without the user needing to know to set both bits. Your reply in the thread also holds up: AP_AHRS::writeTerrainAMSL returns early unless state.origin_ok (AP_AHRS.cpp:2002), so a GPS-denied vehicle genuinely has no terrain to prefer whatever the bits say.

What moves the verdict is that widening the gate lets bit 5 reach the SRTM path with none of the authorisation flatGroundAssumed() imposes — and an instrumented SITL run in exactly the configuration the new autotest leg blesses measures what that costs.

The blocker

flatGroundAssumed() (AP_NavEKF3_Control.cpp:759-764) requires all three of gndOffsetMeasured, !hgtTimeout and activeHgtSource != SourceZ::NONE. The status line at :790 is a plain OR:

const bool optflow_gnd_offset = gndOffsetValid || terrain_srtm_alt_valid || flatGndAssumed;

so terrain_srtm_alt_valid alone satisfies it. That was already true for bit 2 — what is new is that bit 5 now switches the forwarding on by itself, so setting only bit 5 gets a path that skips the guard bit 5's own helper applies.

The activeHgtSource != NONE condition matters just as much there, because both paths use the same pd. OptFlowFusion.cpp:313 takes pd = stateStruct.position.z, and the SRTM branch at :323 is heightAboveGndEst = MAX((terrain_srtm_alt - pd), rngOnGnd) — the same vertical position state your commit message says "carries no height information at all" when EK3_SRCn_POSZ is None.

Measured, bit 5 set, EK3_SRC1_POSZ = None, terrain enabled:

selected flow scale height : 0.10 m
simulator altitude         : 603.54 - 584.00 = 19.54 m above home
EKF health / horiz_pos_rel : true throughout

0.10 m is rngOnGnd — with pd ≈ 0 and terrain at origin level, the MAX() falls through to its floor. Flow velocity scales linearly with assumed height, so the estimate is out by ~195×, and because the flow observation is an angular rate the innovation is zero by construction at that fixed point, so flowTestRatio and the flowFusionTimeout backstop cannot see it. The filter reports healthy while navigating on a velocity two orders of magnitude wrong.

The two new legs make the inconsistency explicit: arducopter.py:2271 asserts that with EK3_SRC1_POSZ=0 and no terrain the flag must be invalid; :2276 asserts that with the same EK3_SRC1_POSZ=0 and terrain enabled it must be valid. Same helper, so the only difference is terrain availability. Applying activeHgtSource != SourceZ::NONE to the bit-5-only forwarding, or isolating that leg with a real height source, resolves both.

A second one, pre-existing but promoted by this change

The SRTM AGL adds where it should subtract. terrain_srtm_alt is "terrain altitude above the EKF origin" (AP_NavEKF3_core.h:1347), written as alt_amsl_m - origin.alt*0.01 — positive up. pd is NED, positive down. So terrain_srtm_alt - pd sums them:

                                 as written   truth   error
terrain  0 m up, vehicle 20 m up     20 m      20 m     +0
terrain 10 m up, vehicle 20 m up     30 m      10 m    +20
terrain 10 m up, vehicle 50 m up     60 m      40 m    +20

The error is 2 × (terrain above origin), zero only when the terrain sits at origin altitude — the flat SITL case, which is why the leg passes. Contrast the correct form one line up at :317: terrainState - pd works because terrainState is itself NED-down (:130). Not introduced here, but this delta makes it bit 5's automatic first choice, so "terrain database altitude is preferred" is currently a stronger claim than the code earns.

The new tests

The terrain leg fails on a cold cache. It sets TERRAIN_ENABLE=1 but never calls install_terrain_handlers_context(), unlike the ten other copter tests that need terrain. Run standalone against an empty cache it fails with "Terrain altitude did not keep EKF relative position valid"; adding the handler makes it pass. As written it passes or fails according to what MAVProxy happens to have cached. One line.

And it does not test terrain-height use. With the cache populated, deleting the SRTM height assignment at OptFlowFusion.cpp:323 outright still passes the whole test. The two mutations that are caught — forcing flatGroundAssumed() false, and removing the bit-5 forwarding — only prove the flag reaches the status word. An assertion on scaled velocity against simulator truth, with the terrain and frozen-ground heights deliberately different, is what would close it. Relatedly, the subtest name claims more than the leg proves: with POSZ=0, flatGroundAssumed() is false, so nothing competes — it shows terrain reaches the core under bit 5, not that it is preferred over the assumption. No leg exercises both being available.

Credit where it is due

TERRAIN_ENABLE=0 in the base parameters, with the comment that terrain is preferred wherever available and is available in SITL "so it has to be off for these legs to be testing the flat-ground path at all", is a test-validity problem you found yourself — the earlier legs had been passing partly on terrain data rather than on the behaviour they name. That is the kind of thing that usually only surfaces when someone changes the surrounding behaviour. The stale "three legs" commit message is also fixed and now matches the code.

Still open from before

status.flags.dead_reckoning at AP_NavEKF3_Control.cpp:813 still uses bare gndOffsetValid rather than the optflow_gnd_offset computed two lines above. The @Description still says "a terrain offset the range finder measured" when gndHgtValidTime_ms is stamped at both OptFlowFusion.cpp:175 and :258. The option is still silently inert indoors with EK3_SRCn_POSZ = RANGEFINDER — the new forwarding helps outdoors, where lostRngHgt moves the source to BARO, but indoors nothing latches and the description does not say so. And "only safe over flat ground" is still not the first thing the bit-5 text says; it moved to third, with the new terrain sentence inserted ahead of it. Worth scoping the "Requires a terrain offset…" sentence to the fallback too, since with coverage none of those requirements apply.

CI: the single failure is cubeorangeplus-quadplane, which is a job in test_renode.yml and fails on test_driver_inventory.py::test_live_catalog_is_consistent — 3 failed, 172 passed, nothing EKF-related. Master fails identically at 371990d846: #34297 renamed AP_RANGEFINDER_BENEWAKE_TFS20L_ENABLED and #33975 added Tools/renode/driver_catalog.py naming the old symbol at :927. One line there fixes it for everyone. All seven sitltest-copter-tests* jobs are still pending here, so EK3_OptflowAssumeFlatGnd has not been exercised in CI at this head. One correction to my own earlier note: I recorded this PR as having gone pending → autotest (sitltest-quadplane) failing at the previous head; no such job appears in this head's rollup at all.

@tridge

tridge commented Sep 6, 2026 •

Copy link
Copy Markdown
Contributor

Deprecated — see below for the updated review.

Previous review (2026-09-06)

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

Re-reviewed at head f266fd0fd9. My earlier comment above is superseded.

Follow-up report: https://uav.tridgell.net/DevCallReviews/followups/2026_09_06_0937/devcall_pr_reviews.html#pr33585

REQUEST CHANGES — unchanged; one of four findings is fixed

The cold-cache fix is right and properly done, and CI is now fully green with the copter buckets finally exercised. But the delta is test-only, and I verified the three EKF3 sources are byte-identical to the previous head — AP_NavEKF3_Control.cpp (38,762 B), AP_NavEKF3.cpp (103,195 B) and AP_NavEKF3_OptFlowFusion.cpp (42,192 B) all compare equal. So the blocker could not have moved, and it has not.

Previous round — triage

Finding Status
Blocker — bit 5 reaches the SRTM path without flatGroundAssumed()'s authorisation STILL OPEN
SRTM AGL adds where it should subtract (OptFlowFusion.cpp:323) STILL OPEN
Test A — terrain leg fails on a cold cache RESOLVED
Test B — the leg does not test terrain-height use STILL OPEN
dead_reckoning uses bare gndOffsetValid (Control.cpp:813) STILL OPEN
@Description wording; RANGEFINDER inert indoors; bit-5 text order STILL OPEN
CI: cubeorangeplus-quadplane (was pre-existing master breakage) RESOLVED upstream

Test A is properly fixed. arducopter.py:2174 now calls install_terrain_handlers_context() at the top of the test, matching the helper's docstring — 8 TERRAIN_REQUEST events served in a local run.

The blocker, re-measured this round rather than carried over. Built at f266fd0fd9, instrumented FuseOptFlow to emit the selected heightAboveGndEst with its inputs, and read SIM_STATE.alt at the exact point the test asserts the flag must be valid:

at-assert SIM alt 604.19  agl 20.26
OFDBG H=0.10  srtm=-2.32  pd=+2.76  gndOffsetValid=0  terrain_srtm_alt_valid=1

0.10 m is rngOnGnd — the assumed flow scale height is out by ~200× against a true 20.26 m AGL, with horiz_pos_rel reported valid throughout. Because the flow observation is an angular rate the innovation is zero by construction, so flowTestRatio and the flowFusionTimeout backstop cannot see it.

And serving terrain to that leg is what makes this uncomfortable. Before this delta the leg could not run on a cold cache; now it runs, and the configuration it certifies as correct is precisely the one measured above. The PR has gone from having no test of that path to having a green one.

Test B — still open, proven again by mutation. Deleting the SRTM height assignment at OptFlowFusion.cpp:323 outright and rebuilding still passes the entire test, both reviewers independently. The reason is structural: terrain_srtm_alt_valid is assigned at :321, outside the if at :322, and every assertion is on EKF_POS_HORIZ_REL.

The second commit is correct — its comment is not

Removing wait_terrain_offset_stale() is right, and I proved the leg still discriminates: mutating OptFlowFusion.cpp:45 from if (!inFlight) to if (false) fails with exactly "Flat-ground assumption carried over from a previous flight". Your primary reason holds.

But the supporting clause at arducopter.py:2301-2306 does not. It says an OutOfRangeLow reading is fused by the ground-clearance substitution, holding gndHgtValidTime_ms fresh. In AP_NavEKF3_Measurements.cpp the status test at :45 is followed by else { continue; } at :52-54, which ends the loop iteration — the substitution at :96-99 is reachable only when the status is Good but the three stored samples are not all fresh. A rangefinder that is not Good never reaches it. My two passes disagreed on this and I resolved it against the source; flagging it because that comment is what a future reader will trust.

Minor: the header comment at :2167-2169 says every leg checks the flag both set and clear. True of three legs; the final carry-over leg's positive check at :2298 is on the first flight, before land_and_disarm().

Out of scope here

My independent pass flagged the bit-4 velD fallback at AP_NavEKF3_PosVelFusion.cpp:760 for keying off GPS message freshness rather than the receiver's have_vz capability. I am not raising it against this PR — that block arrives in be64a046fa, one of the three commits your description assigns to #33478. Noting it only so it is not lost.

What would clear the blocker

Authorise the SRTM contribution at AP_NavEKF3_Control.cpp:790 with the same !hgtTimeout && activeHgtSource != SourceZ::NONE test flatGroundAssumed() applies (or keep the forwarding gate bit-2-only and have bit 5 carry bit 2's own checks); fix the sign at OptFlowFusion.cpp:323 to MAX((-pd) - terrain_srtm_alt, rngOnGnd); and give the test one assertion against SIM_STATE truth with terrain height and the frozen ground height deliberately different, so that deleting :323 fails it.

CI: 102 pass / 0 fail / 0 pending — all seven sitltest-copter-tests* buckets green (all were pending last round), and the new test's job log confirms all five subtests ran.

@andyp1per

Copy link
Copy Markdown
Contributor Author

The SRTM sign fix on this branch (535cfca48f) is now its own PR against master: #34360.

It is master's defect rather than this PR's - terrain_srtm_alt is measured up from the EKF origin while pd is measured down, so FuseOptFlow() differences the two branches of one variable in opposite conventions - but it only ever got fixed here, and anyone setting EK3_OPTIONS bit 2 on master today is affected. Splitting it out also shortens this PR by a commit and lets the fix land without waiting on the option-bit discussion.

It is also now measured, which it was not when it was written here. Off the Kalaupapa cliffs, holding 60 m above an origin the ground falls 160 m below, GPS navigating so both builds fly the same trajectory: the flow innovation consistency ratio saturates its logged 255 ceiling with the old expression against a peak of 0 with the corrected one. Flow at that geometry is rejected outright. That contradicts the note in my own analysis that the effect was not exposed by any log field - no field carries the scale height, but XKF5.NI carries the ratio it drives, so it is testable after all, and #34360 carries an autotest that fails on master.

Suggest rebasing this branch onto #34360 and dropping 535cfca48f here.

@andyp1per
andyp1per force-pushed the pr-optflow-flat-ground branch from f266fd0 to a4d8966 Compare September 10, 2026 09:52
@tridge

tridge commented Sep 10, 2026 •

Copy link
Copy Markdown
Contributor

Deprecated — see below for the updated review.

Previous review (2026-09-10)

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

Re-reviewed at head a4d8966c85. My earlier comment above is superseded.

Follow-up report: https://uav.tridgell.net/DevCallReviews/followups/2026_09_10_2038/devcall_pr_reviews.html#pr33585

REQUEST CHANGES — but the blocker and the sign bug are both genuinely fixed

Taking the previous round's two headline items first, because that is what you pushed for:

  • The blocker is fixed. terrainAltUsable() (AP_NavEKF3_Control.cpp:771) now makes the bit-5 path carry flowScaleHgtUsable(), and — more to the point — the fix has a test that actually discriminates. I reverted Control.cpp:804 to bare terrain_srtm_alt_valid, rebuilt, and EK3_OptflowAssumeFlatGnd fails with "Terrain altitude authorised flow nav with no height source". That is the first time a leg in this test has failed on a mutation of the code it covers, and it is the right one to have got there.
  • The sign fix is correct. Derived it independently rather than taking the comment's word: AP_AHRS.cpp:2006 passes alt_amsl - origin.alt, so terrain_srtm_alt is terrain-above-origin positive up, pd is positive down, and AGL is (-pd) - terrain_srtm_alt. That is what OptFlowFusion.cpp:330 now says.

assert_refused() requiring EKF_VELOCITY_HORIZ set and EKF_CONST_POS_MODE clear is a real strengthening over what I asked for, and the terrain_srtm_alt_ms != 0 guard at :327 is a genuine bug fix in its own right — without it terrain_srtm_alt_valid came up true for the first five seconds after boot with the altitude still zero.

Three things remain.

1. BUG — the new gate is not applied at the one site that consumes the height

terrainAltUsable() is called from exactly two places: AP_NavEKF3_Control.cpp:804 (the status flag) and AP_NavEKF3_Outputs.cpp:98 (the height limit). The site that actually uses the SRTM height — AP_NavEKF3_OptFlowFusion.cpp:329 — is still gated on !gndOffsetValid && terrain_srtm_alt_valid alone, with no option bit and no health check.

So with bit 5 set and a height source that has timed out or is NONE: flowScaleHgtUsable() is false, terrainAltUsable() refuses, the status flag correctly clears — and the filter keeps fusing flow scaled by (-pd) - terrain_srtm_alt, off the very pd the gate just declared unusable. FuseOptFlow() is reached from SelectFlowFusion() at OptFlowFusion.cpp:82-88 on flowDataToFuse && tiltOK alone and never consults the filter status. With EK3_SRC1_POSZ=0 the constant-zero fusion drives pd toward zero, so the scale height collapses onto the rngOnGnd clamp — instrumenting that branch measured 0.05 m.

This is milder than the original blocker, since the flag is now honest and the vehicle should failsafe rather than navigate on it. But the states are being corrupted while it does, and those are what the filter recovers onto if the height source returns. Bit 5 is what makes it reachable: AP_NavEKF3.cpp:1790-1793 now forwards terrain data to the cores for bit 5 as well as bit 2.

Fix: gate :329 on terrainAltUsable() instead of bare terrain_srtm_alt_valid.

2. BUG — the datum-reset carry covers terrainState but not the SRTM path

terrainState += posResetD at AP_NavEKF3_PosVelFusion.cpp:281 is correct and does exactly what the commit message claims: with pd' = pd + Δ, the rangefinder-derived AGL terrainState - pd is preserved.

The SRTM-derived AGL is not, and nothing adjusts it. terrain_srtm_alt is assigned only in writeTerrainData() and is never touched by a reset, so (-pd') - terrain_srtm_alt = [(-pd) - terrain_srtm_alt] - Δ — the flow scale height steps by the whole reset. And because the error is in pd rather than in terrain_srtm_alt, the next terrain write does not heal it: AP_AHRS.cpp:2006 recomputes alt_amsl - origin.alt, which the reset did not change. The step is permanent.

That is precisely the failure your own commit message describes seeing in flight — "height above ground stepped from 0.41 to 4.00 m in one sample, the flow innovations saturated and that core never recovered" — fixed for one of the two height sources this PR can be flying on. Reproduced in SITL with flow and terrain enabled: a −5.98 m reset held the rangefinder AGL at 3.09 m while SRTM scaling used 8.97 m, against near-zero true vertical movement.

I'd rather flag this than prescribe a patch: mechanically offsetting terrain_srtm_alt by -posResetD would preserve the AGL but makes the variable stop meaning "terrain height above origin", and it has to survive every subsequent terrain write. The real constraint is that the SRTM path is only sound while -pd is an unbiased height above origin, and a reset onto a drifted source is what breaks that. Your call which way it goes — but the asymmetry shouldn't stay silent.

3. STILL OPEN (re-raised) — the sign fix you just landed has no regression test

This is the previous round's "Test B", and it survived the rewrite. I built SITL at this head and ran EK3_OptflowAssumeFlatGnd four times, including a control mutation that must fail:

Mutation Result
baseline, unmodified PASS
delete the SRTM height assignment at OptFlowFusion.cpp:330, keep the freshness calc at :327-328 PASS
revert the sign fix to the pre-fix MAX((terrain_srtm_alt - pd), rngOnGnd) PASS
(control) revert terrainAltUsable() to bare terrain_srtm_alt_valid at Control.cpp:804 FAIL

The control failing is what makes the other two mean something. The gap is narrow and specific: every assertion in the terrain legs is on EKF_POS_HORIZ_REL, which is driven by terrainAltUsable() and hence by terrain_srtm_alt_valid — assigned at :327, outside the if at :329. The height computed at :330 reaches nothing the test observes. So the bug you fixed this round would regress undetected.

What would close it: one assertion against SIM_STATE truth on a leg where terrain height and the frozen ground height are deliberately different — e.g. fly a known horizontal velocity and compare the EKF's reported velocity against simulator truth, since that is what the flow scale height actually controls. The bar is just that deleting :330 must fail it.

Minor

  • One predicate, four spellings. Control.cpp:804 has the full form; Control.cpp:827 (dead_reckoning) still has bare gndOffsetValid and neither new path; Measurements.cpp:661 gained the flat-ground term but not the terrain one; OptFlowFusion.cpp:329 is finding 1. Control.cpp:827 is && doingWindRelNav so it is fly-forward-only in practice, but you added flowScaleHgtUsable()/terrainAltUsable() to centralise this — routing all four through them would make finding 1 unable to recur.
  • The PR description no longer matches the test. Classification & Testing still says "terrain altitude alone holds relative position valid without bit 2 being set" — that leg is now inverted. It also still says "Every leg checks the flag set as well as clear" (the test's own header comment now says "except the last pair") and "Each fails if the code it covers is removed" (the table above disproves that). Worth a pass before merge; it is what a reviewer reads first.

Checked and correct

The !inFlight → onGround change at OptFlowFusion.cpp:48 is right and your stated reason holds — detectFlight()'s fly-forward branch only sets inFlight via highGndSpd off gpsDataNew.vel, so on a GPS-denied fly-forward vehicle inFlight never latches. onGround is !motorsArmed on both branches, which also matches "since the vehicle was last disarmed" more exactly than !inFlight did. The reordering at PosVelFusion.cpp:1574-1578 is sound, skipping the carry when the rangefinder is being taken up is correct (hgtMea is referenced through terrainState at :1512, so shifting it would double-count), the other two ResetPositionD() call sites are right to carry it, and ResetHeight() is unaffected since it reassigns terrainState itself at :308-314. The replacement comment about onGround being the armed flag inverted is accurate — the previous round's OutOfRangeLow claim is correctly gone.

Out of scope

My second pass again flagged PosVelFusion.cpp:773 for keying the velD fallback off GPS message freshness rather than the receiver's have_vz. git log -L puts that block in be64a046fa, one of the three commits your description assigns to #33478 — not raised against this PR, noted so it isn't lost when that one is reviewed.

CI: 46 pass / 0 fail / 55 pending at this head — snapshotted once, still running. Note gh pr checks renders cancelled jobs as fail, so superseded runs will look like failures until the current run settles.

@andyp1per
andyp1per force-pushed the pr-optflow-flat-ground branch from a4d8966 to 4fd131e Compare September 10, 2026 13:11
@andyp1per

Copy link
Copy Markdown
Contributor Author

Rebased onto #34360 and force-pushed, head 4fd131e6bf.

Three things changed and none of them touch this PR's own design.

The sign fix is gone from here. It is #34360 against master, since the defect is master's rather than this PR's - terrain_srtm_alt is measured up from the EKF origin while pd is measured down. Until this rebase both branches carried it and git merge-tree confirmed they would conflict.

The terrain_srtm_alt_ms != 0 guard has moved there too. It was a hunk inside the bit-5 commit here, and it is a second master defect in the same expression: the timestamp is zero until the first terrain write, so the age test passes against an altitude the core has never received for the first five seconds of uptime. It belongs with the sign fix rather than being hostage to the option-bit discussion, so #34360 now carries both and this commit is one hunk smaller.

Two autotests needed their takeoff() calls translated. The rebase moved the base forward 1348 upstream commits, and takeoff() was renamed in that span: alt_min/max_err became altitude_min/altitude_max, and the semantics differ - the old body did wait_altitude(alt_min-1, alt_min+max_err) where master does wait_altitude(altitude_min-1, altitude_max). So max_err was a tolerance and altitude_max is a ceiling, and the five call sites became altitude_max = alt_min + max_err. This branch would have hit that at merge whether or not #34360 existed.

All three autotests pass on the rebased branch: EK3_OptflowAssumeFlatGnd, EK3_AglKfVelForVelD, EK3_TerrainStateFollowsDatumReset. Only one conflict arose in the rebase itself, in the SRTM block, resolved in favour of #34360's version.

This PR now needs #34360 to merge first.

getHeightControlLimit() skipped the terrain state with the range finder
as the height source, as the terrain state stayed where it was set at
start. Subtract it for every height source. On its own this moves the
limit only by the terrain state with range finder height, the ground
clearance at start, about 0.1 m (derived from the source, not
measured).

It comes first so the series bisects: the next commit carries the
terrain state across a vertical datum reset, after which a fall back
from the range finder to baro and back leaves the vertical position in
the baro's datum with the terrain state moved with it, and the limit
taken without the terrain state would be in the wrong datum. In SITL,
a fall back across an 8 m baro step then stopped a full stick climb at
7.6 m against a 13 m limit, and with the other sign it climbs past it.
The terrain state is a D coordinate in the same datum as position.z, so
a reset that moves the datum has to move it too, or the height above
ground steps by the whole reset. On master that happens at a height
timeout reset after a rejected baro glitch (in SITL a 30 m glitch
stepped the height above ground by 30 m), when range finder height falls
back onto a baro that has stepped away from it (6 m in SITL), and at an
external nav start. It applies to any vehicle that fuses a range finder
into the terrain state, with or without a flow sensor fitted, in builds
with optical flow support, which the terrain estimator needs; without it
only leaving range finder height is carried.

A height source change moves the datum by definition, so the terrain
state follows it whenever the ground has been measured this flight,
except where the range finder is becoming the height source, whose
measurement is referenced through the terrain state. A height timeout
either moves the datum or corrects drift in position.z, which cannot be
told apart: a terrain state still being fused drifted with position.z
and follows, while one frozen above the range did not, and is left
alone, or the remembered ground would move by the drift - in a SITL
baro outage, 2.2 m of height above ground error, against 51.8 m if the
frozen terrain state is carried. One fused within the last 5 s still
follows: it misses at most 5 s of drift, where not carrying it would
step the height above ground by the whole datum move, 30 m in a SITL
glitch reset 1-2 s after the range was lost.

The other ResetPositionD() callers now carry it too, a GPS receiver
change with GPS height and an external nav position reset with
external nav height, as both move the datum.

With the range finder as the height source, a fall back to baro and back
keeps the baro's datum for the rest of the flight: position.z steps once
and the height above ground stays right. Master steps position.z back on
return, and has the height above ground wrong by the difference while on
baro. What reads the altitude above home or origin carries the
difference for the rest of the flight, which is the part of the baro
step the baro offset filter had not absorbed when the range was lost:
the fence's ALT_MAX in those frames, AUTO and GUIDED relative-altitude
targets, RTL_ALT and the altitude a GCS shows. The flow height limit
read it too, and the previous commit takes it from the terrain state.
Derived from the source, not measured.

The height source change is recorded after the reset so the reset can
see the source being left.
An external nav start resets the height from setAidingMode(), which runs
before selectHeightForFusion() in the same step, so after a source set
change the reset could still see the old height source. With the range
finder as the old source the terrain state carry read it as the source
being taken up and skipped, and the height above ground stepped by the
whole datum move: in SITL, 2 of 10 starts from a flow and range finder
source set, 12.6 to 0.1 m and 16.2 to 33.6 m. Which start takes that
path depends on the scheduling phase.

Naming external nav as the height source first makes the reset a source
change, which the carry follows. In flight, with external nav velocity
in use and GPS vertical velocity not, ResetHeight() now takes vertical
velocity from external nav on every start, as it already did when
selectHeightForFusion() ran first.
EK3_TerrainStateFollowsDatumReset and EK3_TerrainStateFollowsHeightReset
force a fall back from the range finder onto a displaced baro and a
rejected 30 m baro glitch held past the height timeout, and fail if
height above ground moves more than 0.5 m across it; on master the
height timeout case moves it 30 m and the fall back 6 m. A second leg
loses the range 1-2 s before the timeout reset, measured from the last
Good range reading, and the ground must still follow; with a 500 ms
window it moves 30 m.

EK3_TerrainStateFollowsSourceChangeAboveRange measures the ground, climbs
above the range until it is no longer valid, then changes height source
to GPS after a 10 m GPS altitude step: height above ground must not
move. On master, or with the measured-this-flight term removed from the
carry, it moves 10 m.

EK3_TerrainStateHeldOnDriftReset drifts the height with no baro above
the range and checks the reset when the baro returns leaves the
remembered ground alone: 2.2 m of error at worst, against 51.8 m if the
frozen terrain state is carried. It passes on master by design, as the
guard against carrying too much.

A second leg of EK3_TerrainStateFollowsDatumReset guards the range
finder exclusion. With a 20 m range finder pointed away, the baro
drifts 5 m low while it is the height source, and the range finder is
then given back: at the reset that puts the height back on the range,
height above ground must be within 1 m of truth. It was 0.50 m off,
against 4.87 m with the terrain state carried into the range finder,
an error range height fusion then removes within about half a second.
It passes on master by design.
EK3_FlowHeightLimitAfterRangeFallback flies on optical flow with the
range finder as the height source and a 13 m flow height limit, points
the range finder away for 3 s with the baro displaced by 8 m, gives it
back, and climbs at full stick: the true height must stop within 1.5 m
of 13 m. Without taking the limit from the terrain state it stops at
7.6 m; with it, 13.1 m.

It also requires the terrain state to step by 3 m or more at the fall
back (5.6 m), or the climb would stop at the limit with or without the
fix: without the baro step the climb still stopped at 13.1 m, and the
test now fails there instead.
writeTerrainData() logged RTER only when the height changed, so Replay
saw no new terrain data while the height stayed the same, where the
vehicle saw it arrive at 10 Hz. EKF3 judges terrain data stale by when
it last arrived, so over level terrain the replayed filter dropped it
after 5 s and the original did not. Log every write.
Matches wheel_encoder_enabled(): true if any source set uses optical
flow for horizontal velocity, not only the active one. Named for that,
as dal.opticalflow_enabled() already means the sensor is enabled.
Above the range finder's range the terrain offset goes stale and
optical flow relative position was dropped, so a vehicle navigating on
flow alone hit the EKF failsafe there although flow was still fused.

Keep it valid instead. Where terrain data reaches the EKF and optical
flow is configured, the ground height follows the terrain database,
anchored to the height the range finder last measured, so only the
database's shape is used and errors in the origin altitude or the
database's absolute height cancel. Where there is no terrain data, or it
stops, the ground is assumed to stay at its last measured or anchored
height. Neither engages without optical flow.

The fallback needs optical flow as a velocity source, a usable height
source, and a ground height measured this flight, and engages only
once the range data has gone. It applies above 0.7 x the range finder
maximum less 1 m, the height the existing optical flow limit uses,
with 1 m of hysteresis, and only if the last good range reading was at
least 0.7 x the maximum: a climb out of range passes through such
readings, a range finder that fails or loses its return low down does
not. Range finder status cannot tell the two apart, since most lidar
drivers report a lost return as out of range high at any height.
Under a 2.9 m maximum the limit is at its 1 m floor and nothing tells
them apart, so there is no fallback. Without terrain data the
assumption also ends 10 x the height above ground from where the
ground was last known, as error over sloping ground grows with
distance.

The fallback ends once valid range measurements have arrived without a
break for 2 s, timed on the samples since a break is only seen 500 ms
after it, so a later loss has to pass those checks afresh. A shorter
return, from a roof or tree flown over, keeps it engaged, and the
terrain estimate saved when the return began is put back once it ends,
so the fallback does not carry on at the obstacle's height. A return
that lasts is fused at once, and one that breaks off with the vehicle
under the height limit over the ground held is a descent and is kept.
Losing flow as a velocity source or a usable flow height part way
through a return keeps the saved estimate until the same 2 s of range
samples expire it, so the obstacle's height is not left as the ground. A
disarm, which clears gndOffsetMeasured, drops it, and it is never
restored after one, so a vehicle that lands on the obstacle and disarms
does not get the ground below it back.

The dead reckoning status follows relative position, so a flow
vehicle with drag fusion does not report dead reckoning above the
range while the fallback holds. EK3_OPTIONS bit 2 keeps its behaviour:
the database height used directly, including with no height source,
and the height limit lifted while terrain data is available.

A vertical datum reset moves the anchor and the saved terrain estimate
as ArduPilot#35014 moves the terrain state, so the ground held above
the range keeps its height across one.

Flow velocity above the range is out by the terrain height error over
the height: SRTM's relative error of about 6 m is 30% of 20 m. Nothing
can be done about that in flight, so warn once while disarmed when
optical flow is in any source set and the downward range finder's
maximum is under 20 m, checking every 5 s until a late range finder
such as DroneCAN has appeared.
EK3_OptflowAboveRangefinder checks EKF_POS_HORIZ_REL, the flag the
failsafe reads, across a climb out of the range finder's range; a range
finder lost after a climb out, then a descent below the height limit; a
4 m range finder failing at 2.5 m; a 2 m range finder, whose limit is at
its floor, climbed out of; no height source; bit 2 on terrain data,
which alone must not lift the flow height limit; a second flight that
never measured the ground; flat ground trusted only within 10 x the
height above ground, including after 300 m on GPS, which moves the
origin; LOITER surviving above the range with an origin; a range finder
lost in range after a climb out and back; and a brief range return at
height, close enough to put the vehicle under the height limit, which
must be fused, last under 2 s and leave the height above ground back at
the height flown, with and without the AGL KF for flow scaling, again
with a return of 1.7-1.95 s, which an end timed on the clock rather than
on the range samples would keep as the ground, and with flow dropped as
a velocity source part way through, the log showing the drop inside a
return already fused. Each return needs three range samples, and its
window is compared in integer microseconds so a 1.7 s window cannot fail
its own lower bound. It also waits for the startup warning for its 8 m
range finder. Negative legs check the filter is healthy and aiding.

EK3_OptflowAnchoredTerrain flies up rising terrain with the origin set
30 m low and checks height above ground stays within 3 m of truth, from
TERR.TerrH, which SITL's terrain shares.

Each fails with its fix removed: no anchoring 8.0 m off, no last-good
reading check held on the failed short range finder, no floor held above
the 2 m range finder, no distance bound held for the whole flight, no
origin shift held 300 m away; no height limit held below it with the
range finder lost after a climb out, an end timed on the clock kept a
1.7 s return as the ground; and the flow drop case fails with the
snapshot dropped.

The brief returns are timed by parameter sets sent without waiting for
their acknowledgement, so a return's length varies with when they land:
the 1.6 s case measured 1.5 and 2.1 s in 3 of 21 runs. The length, and
that the return was fused, are checked before the flight is judged, and
a case that misses them is flown again, a brief return up to five times
and the carry-over takeoff up to three. The 1.7-1.95 s window keeps
about 0.1 s from both the 2 s that ends the fallback and the 1.5 s at
which an end timed on the clock would, as the EKF times a return about
0.1 s shorter than the log. The case is held for 1.65 s to land inside
it; one that misses on every attempt still fails. The 4.8-5.5 m reading
after a climb out and back and the 5.8-7.9 m reading on the open loop
climb are flown again the same way when the descent or climb settles
outside the band. The parameters a return changes are restored with an
acknowledged set after each flight.
EK3_OptflowAnchoredTerrain read the anchor only through XKF5.HAGL, which
need not be the height flow is scaled by. Flow scaled by the wrong
height reads speed in proportion, so once the ground has risen 5 m the
true speed must also be within 10% of the EKF's. Anchored, 1.011; with
the anchor removed, 0.829, which the height check also catches today.
Above the range finder's reach the anchored terrain height was written
into the terrain state each step, so the choice of the height flow is
scaled by was made outside FuseOptFlow(), which already makes it for
EK3_OPTIONS bit 2 and the AGL KF. Make it there instead, beside bit 2,
and leave the terrain state to the terrain estimator and the resets
while the anchor is in use.

The flow speed limit and nav gain scaler, the flow height limit, the
flat-ground checks and XKF5's HAGL and TOfs read the anchored height
through flowTerrainState(), so none of them changes and TOfs - HAGL is
still the vertical position. The snapshot taken when a range return
begins saves the anchored ground, so the check for a descent at the end
of a short return and a return that is undone use it as before. Where
the anchor ends without the ground being measured - EK3_OPTIONS bit 2
set in flight, a disarm, optical flow removed from the source sets, or
the range finder becoming the height source - the last anchored ground
is copied into the terrain state once, so the ground the limits, the
flat-ground checks and XKF5 read, and flow where no other height scales
it, falls back to it as it did when the anchor wrote the terrain state.
TOfs is saturated as HAGL is, since the anchored ground can lie further
from the origin than a measured one.

When the range finder becomes the height source the anchor is ended in
selectHeightForFusion(), before the range height is formed, so the
height is reset against the anchored ground rather than the ground held
from before it: in SITL, coming back into range after a traverse over
ground about 10 m higher, the height above ground stepped -8.8 m and
relative position was lost when the hand-over was left to
SelectFlowFusion() a step later; -0.3 m with it.

What still differs, derived from the source: while the anchor is in use
the terrain state is the ground held from before it. The range finder
height switch reads it but cannot act above the range; with
EK3_FLOW_USE 2 the terrain estimator's flow fusion starts from it
rather than from the anchored ground; and the clamp that keeps the
terrain state below the vehicle no longer applies to the anchored
ground once terrain data has stopped.

Replay of a flight with the anchor in use for 125 s (EK3_OPTIONS 90):
every EKF record, XKF5 included, is byte-identical to the anchor writing
the terrain state.
Above the range finder's range, a return lasting 2 s ends the
flat-ground fallback and its surface becomes the ground, while a shorter
one is undone 500 ms after it stops. Both were timed on every valid
range reading, including ones the terrain innovation gate rejects. An
obstacle fused at once and followed by readings of a lower surface that
the gate rejects against it therefore counted as 2 s of ground: height
above ground stayed at the obstacle, and with that reading short of
the fallback's threshold relative position was lost once the terrain
offset timed out.

Only readings fused into the terrain estimator now count, so readings
the gate rejects are a break: the obstacle is undone 500 ms after its
last fused reading and the readings that follow are judged as a return
of their own. With the range finder as the height source nothing is
fused into the terrain state, so every valid reading still counts
there.

SITL, RNGFND1_MAX 8, flow navigation at 21 m, a 1.76 m return then
7.3 m readings for 3.6-3.8 s: before, the gate rejected every 7.3 m
reading, height above ground stayed 1.8 m and relative position was
lost 5.2 s after the obstacle's last reading. After, the obstacle was
undone 0.6 s after its last reading, and the 7.3 m readings were then
fused and, lasting over 2 s, became the ground: height above ground
7.3 m, relative position held.
EK3_OptflowAboveRangefinder flies the flat-ground fallback at about
21 m with an 8 m range finder, then reads 1.76 m for 0.2 s, an obstacle
fused at once, and 7.3 m for 3.5 s, which the terrain innovation gate
rejects against it. The 7.3 m readings are over the 5.6 m the fallback
needs to re-engage and the obstacle is not. The leg requires the
obstacle to be held against the 7.3 m readings for under 1 s, height
above ground well clear of the obstacle afterwards, and relative
position held past the terrain offset timeout. Once the obstacle is
undone the 7.3 m readings last long enough to be taken as the ground,
so the leg does not assert which surface is kept.

The parameter sets that time the readings are not acknowledged, so
their lengths come from the log. An obstacle read for 2 s is the
ground by design, 7.3 m readings under 2.5 s end the return within 2 s
whatever counted, and one 5 s after the obstacle's last fused reading
resets the terrain state to it whatever was held, so a flight with the
obstacle over 1.5 s or the 7.3 m readings outside 2.5-4.5 s proves
nothing and is flown again, up to three times.

Without the previous commit it fails with the obstacle held against
the 7.3 m readings for 3.7 s (height above ground 1.8 m at 21.0 m up);
with it, 0.2-0.4 s and 7.3 m. With the 7.3 m readings run on for 10 s
and the length check removed, the terrain reset leaves 7.3 m either way,
and the hold check still fails it (4.9 s).
After its traverse up 8 m of rising ground, EK3_OptflowAnchoredTerrain
sets EK3_OPTIONS bit 2 in flight, which ends the anchor with no range
return, holds for 5 s and checks XKF5's height above ground against the
truth from then on. With the last anchored ground held the worst error
is 1.4 m; with the ground measured before the climb it is 7.9 m, the
rise, and the leg fails at 3 m.

Over the traverse XKF5 TOfs less HAGL has to follow XKF1 PD, as it does
with the anchored ground logged as TOfs: a spread of 0.03 m, against
8.27 m with TOfs logging the terrain state held from before the climb.
EK3_OptflowAnchorToRangeHeight engages the anchor above an 8 m range
finder under baro height, traverses ground that rises about 10 m,
switches to a source set with range finder height and descends into
range: the height above ground must stay within 2 m of SIM truth for
15 s. Without handing the anchored ground over before the range height
is formed it steps -8.8 m and relative position is lost; with it the
worst is -0.3 m.
With EK3_SRCn_POSZ set to the range finder, the range finder takes the
height back on its first fresh reading and the height is reset to it,
so a return from something passed over above the range moves the
height onto that object. While the flat-ground fallback is engaged the
height now stays on the source it fell back to. The fallback ends once
the ground has been measured again for 2 s, and the range finder takes
the height then. The terrain estimator measuring the ground in those
2 s also ends a terrain anchor first, so under flow navigation the
anchor hand-over in selectHeightForFusion() is no longer reached; it
still is where flow is not the velocity source.

On its own this matters only where the fallback had already engaged
after the ground was measured under baro height; the next commit lets
it engage under range finder height. Changes the compiler output.
With EK3_SRCn_POSZ set to the range finder the terrain estimator is
skipped, so nothing recorded the ground as measured and the fallback
above the range never engaged: relative position was lost as soon as
the range finder went out of range. A step that takes the range height
now measures the ground instead, latching it as measured in flight and
recording where and the reading the climb-out check reads. Only a step
whose range height passed its innovation check counts.

gndHgtValidTime_ms is untouched, so gndOffsetValid, EKF_POS_VERT_AGL
and getHAGL() read as before, and the fallback engages in the step the
height falls back to baro. Limited to the range finder as the
configured height source, so EK3_RNG_USE_HGT configurations are
unchanged, and the terrain anchor is still never taken from range
height. Changes the compiler output.
EK3_OptflowAboveRangefinderRangeHeight, with EK3_SRC1_POSZ 2 and an 8 m
range finder. Without the previous two commits relative position is
lost above the range (EKF failsafe at about 8 m). A 1 s return reading
2.5-3.5 m at 20 m leaves the EKF height within 0.04 m of SIM truth
through it, read from XKF1; without the hold the range finder takes the
height on the return and moves it -18.7 m. Back in range at 5 m the
range finder takes the height again, shown by a 5 m baro drift leaving
the vehicle in place, and a range finder lost there is refused. With
the fallback never ending the vehicle followed the baro to 6.9 m, and
without the last good reading check relative position was held.
Under flow navigation the flat-ground hold keeps the range finder off
the height until the terrain estimator has measured the ground again,
which ends the anchor first, so EK3_OptflowAnchorToRangeHeight no
longer reached the hand-over in selectHeightForFusion(): without it the
test still passed, at -0.3 m. A second flight switches to a source set
without flow before descending into range, where nothing holds the
range finder off. Without the hand-over the EKF height steps -8.7 m,
the ground's rise over the traverse, and the vehicle flies into the
ground; with it the height moves 0.5 m and the vehicle holds 3.5 m.
Both flights now also check that the ground rose (8 m), that the EKF
height did not step, and that the range finder took the height (a 5 m
baro drift left the vehicle in place).
The EKF status flag read was written out four times across
EK3_OptflowAboveRangefinder, EK3_OptflowAboveRangefinderRangeHeight and
EK3_OptflowAnchoredTerrain, the terrain tile wait three times, and the
range finder band check and the rising-ground traverse twice each. Each
is now one method; no test changes what it does.
The warning that flow above the range finder may drift went out once,
5 s into boot, so a GCS that connected later never saw it. It now goes
out once each time the motors arm, a motor test included, when someone
is looking, or once a range finder that appears late is there.

Its 20 m threshold was where a 6 m SRTM error, a figure without a good
source, is 30% of the height. It is now 11 m: in flight logs the
database's error changed by 3.4 m (90th percentile) between points
50-100 m apart, which is 30% of 11 m, so a range finder that reaches
11 m no longer warns; 30% remains a judgement of where the drift
matters. Under 2.86 m there is no flat-ground fallback and relative
position is dropped above the range rather than left to drift, so it
does not warn there either, unless EK3_OPTIONS bit 2 uses the database
directly. That 2.86 m test is now shared with the fallback as
rngMaxAllowsFlatGround().
EK3_OptflowAboveRangefinder waited for the warning at boot. It now
checks the warning has not gone out once the vehicle is ready to arm
and does after arming with the 8 m range finder, goes out again on a
second arming without a reboot, and does not go out with the 2 m range
finder, which has no fallback, or with an 11 m one. Decided while
disarmed it fails before arming; once per boot, on the second arming;
without the 2.86 m test, on the 2 m range finder; and with a 12 m
threshold, on the 11 m one.
With EK3_RNG_USE_HGT set, the switch to range finder height was taken
while the flat-ground fallback held the ground above the range, as the
configured range finder height was before. In a takeoff or landing,
where Copter marks the terrain stable, a return from something passed
over that the terrain estimator took as the ground under the switch
height moved the height source onto that object. The switch now waits
for the fallback to end, after 2 s of fused range, as the configured
range finder height does. Changes the compiler output.
EK3_OptflowAboveRangefinderRangeHeight flies a 1 s return reading 1.9-
2.5 m in LAND above an 8 m range finder, with baro height and
EK3_RNG_USE_HGT 70. The switch does not step the height, as the terrain
estimator resets its ground onto the object first, so the leg checks
the terrain estimator keeps running to the end of the return: its range
innovation, which stands still while the range finder is the height
source, took 5-6 values over the return's last 0.6 s in three runs,
and 1 without the previous commit. The brief return the test already
flew becomes a helper both use.
Three legs of EK3_OptflowAboveRangefinder and
EK3_OptflowAboveRangefinderRangeHeight fly a descent or climb again
when it settles outside the band they need, but a wait_altitude() that
timed out short of the band left the retries instead. It now counts as
a missed attempt too.
EK3_OptflowAboveRangefinder's short range finder leg needs the takeoff
to settle with the range finder reading 2.0 to 2.75 m, and checked that
once, so a takeoff settling just outside it failed the test (1 run in
11 in a review's runs). It is flown again, up to three times, as the
other legs that need a band already are.
EK3_OptflowAboveRangefinderRangeHeight tells whether EK3_RNG_USE_HGT's
switch took the height during a return by counting the distinct
terrain estimator range innovations over the return's last 0.6 s. Once
the range finder has the height the innovation stands still at one
value; while the terrain estimator still fuses the return it moves, but
it also pauses for up to 0.4 s (three runs here), and one of those runs
left 2 values in the window, which the limit of 3 failed. It now fails
on fewer than 2, a whole window standing still; with the switch the
innovation stands still for 1.3 s and the leg fails. A switch in about
the return's last 0.5 s goes unseen, against about 0.4 s before: no
window on this innovation can tell a later one from its own pauses.
@andyp1per
andyp1per force-pushed the pr-optflow-flat-ground branch from 2a676c1 to a0b981f Compare November 14, 2026 15:24
@andyp1per

Copy link
Copy Markdown
Contributor Author

Both done. f43e842819: the short range finder leg flies the takeoff again, up to three times, when the reading settles outside 2.0-2.75 m.

a0b981f17e: the switch check now fails on fewer than 2 values. Here the innovation paused for up to 0.4 s with no switch, and one of three runs left only 2 values in the window, which the limit of 3 would have failed; with the hold reverted it stands still for the last 1.3 s of the return and the leg fails. No window on that innovation can tell a switch in the return's last 0.4 s or so from a pause, at either limit, so the comment now says it sees a switch made before about the last 0.5 s.

@AP-Review

Copy link
Copy Markdown

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

Reviewed at head a0b981f17e.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/33585/20.html#pr33585

The two test commits at a0b981f check out, and nothing is left to fix. Both minor items from the last review are fixed: the short range finder takeoff is flown again when it misses its band (arducopter.py:2840), and the switch check now fails only on fewer than 2 innovation values (arducopter.py:3214). The guard-removed mutant still fails that check every time.

Not fully reviewed: CI at this head (still queued when checked), and hardware or real flights with the anchor, the flat-ground fallback and the EK3_RNG_USE_HGT guard.

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.

6 participants