Repository navigation
Conversation
072a072 to
b4e5e1c
Compare
|
Note Superseded. This review was for head b4e5e1cba9. The current review is at 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 says1. 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 Two features, one titleThe body describes only bit 5, but the five commits split cleanly into two independent features:
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
But index 12 is contested. #32471, also open, carries the comment One more interactionA cross-check pass noted that bit 3 ( Credit where it's due: the autotest asserts on |
|
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 POSZ = NONE. Worse than the flag suggests: the flow scale height is 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 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:
The AGL KF freezes because with no rangefinder its velocity is washed to zero, then hands back to Two features. They are two PRs - #33478 is open and carries the first three commits (bit 4, Index 12. A collision between two of my own branches rather than anything released - index 12 has never been used in EKF3's 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.) |
|
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. |
b4e5e1c to
18a2bbe
Compare
|
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 One consequence worth stating plainly. EKF3 cannot tell a landing from a disarm on a copter - 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 The autotest could have passed for free. 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 The covariance question that came up alongside this is now measured rather than argued, on #33478's side. (Investigated and written by Claude, reviewed by me before posting.) |
|
Note Superseded. This review was for head 18a2bbe7b4. The current review is at 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 CHANGESEvery 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: Previous round1. The latch is now flight-scoped ( 2. 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. 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 Credit where it is due on the autotest: New1. 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 — 2. 3. The option is silently inert with 4. "a terrain offset the range finder measured" is imprecise. 5. Quantified, because it supports your framing. The flow observation is an angular rate 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 For the record I also confirmed the trade is not circular: 6. Two smaller ones. #34292 ( 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: Your 08:16 self-correction identified the 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 |
18a2bbe to
62a3fbe
Compare
|
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": The "set bit 2 as well" line in my description was the tell. 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 (Investigated and written by Claude, reviewed by me before posting.) |
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 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 ( What moves the verdict is that widening the gate lets bit 5 reach the SRTM path with none of the authorisation The blocker
const bool optflow_gnd_offset = gndOffsetValid || terrain_srtm_alt_valid || flatGndAssumed;so The Measured, bit 5 set, 0.10 m is The two new legs make the inconsistency explicit: A second one, pre-existing but promoted by this changeThe SRTM AGL adds where it should subtract. The error is The new testsThe terrain leg fails on a cold cache. It sets And it does not test terrain-height use. With the cache populated, deleting the SRTM height assignment at Credit where it is due
Still open from before
CI: the single failure is |
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 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 fixedThe 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 — Previous round — triage
Test A is properly fixed. The blocker, re-measured this round rather than carried over. Built at 0.10 m is 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 The second commit is correct — its comment is notRemoving But the supporting clause at Minor: the header comment at Out of scope hereMy independent pass flagged the bit-4 velD fallback at What would clear the blockerAuthorise the SRTM contribution at CI: 102 pass / 0 fail / 0 pending — all seven |
|
The SRTM sign fix on this branch ( It is master's defect rather than this PR's - 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 Suggest rebasing this branch onto #34360 and dropping |
f266fd0 to
a4d8966
Compare
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 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 fixedTaking the previous round's two headline items first, because that is what you pushed for:
Three things remain. 1. BUG — the new gate is not applied at the one site that consumes the height
So with bit 5 set and a height source that has timed out or is 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: Fix: gate 2. BUG — the datum-reset carry covers
|
| 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:804has the full form;Control.cpp:827(dead_reckoning) still has baregndOffsetValidand neither new path;Measurements.cpp:661gained the flat-ground term but not the terrain one;OptFlowFusion.cpp:329is finding 1.Control.cpp:827is&& doingWindRelNavso it is fly-forward-only in practice, but you addedflowScaleHgtUsable()/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.
a4d8966 to
4fd131e
Compare
|
Rebased onto #34360 and force-pushed, head 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 - The Two autotests needed their All three autotests pass on the rebased branch: 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.
2a676c1 to
a0b981f
Compare
|
Both done.
|
|
Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting. Reviewed at head 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. |
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)
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.
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.