Skip to content

AP_OpticalFlow/AP_NavEKF3: add FLOW_ALT_MIN to stop fusing below the sensor's minimum focus alt - #35077

Merged
peterbarker merged 6 commits into
ArduPilot:masterfrom
rmackay9:andyp1per-flow-hgt-min-simple
Nov 15, 2026
Merged

peterbarker merged 6 commits into
ArduPilot:masterfrom
rmackay9:andyp1per-flow-hgt-min-simple

Conversation

@rmackay9

@rmackay9 rmackay9 commented Nov 9, 2026 •

Copy link
Copy Markdown
Contributor

Summary

This is a cut-down and modified version of PR #34292. @andyp1per is kept as the author with Claude and I as the co-authors.

This adds a new FLOW_ALT_MIN parameter which allows the user to specify the alt-above-terrain below which an optical flow sensor is out-of-focus and thus cannot be fused into the EKF. Only EKF3 does not fuse flow when the range to the ground is below it, so unfocused readings no longer dead reckon a phantom horizontal velocity during landing. Default 0 (disabled).

Description

  • AP_OpticalFlow: gets a new FLOW_ALT_MIN parameter, constrained to 0 to 5 m, sent to the EKF with each flow sample.
  • AP_DAL: altMin is appended to ROFH. log_ROFH is not packed, so 3 "unused" uint8 fields are added before the new field instead. This ensures that rawFlowQuality keeps its offset and Replay zeroes the part of a record an older log lacks, so older logs replay with the new "alt min" check disabled.
  • AP_NavEKF3: in FuseOptFlow(), once the range is known, really_fuse is set false when the vehicle's alt-above-terrain is below altMin. The innovations are still calculated and reported. readyToUseOptFlow() is enhanced to return false if the vehicle's alt-above-terrain was below the limit during the last call to FuseOptFlow.
  • AP_NavEKF2, AP_AHRS: pass the value through. EKF2 does not use it but must write the same ROFH as EKF3.

Behaviour to be aware of:

  • The check is only run once takeoff is detected, so flow is still fused as zero on the ground before takeoff.
  • If the FLOW_HGT_OVR is set (e.g. on Rover) the alt check is not run (i.e. FLOW_ALT_MIN is ignored)
  • If the vehicle is below FLOW_ALT_MIN for more than 5 s the EKF loses flow aiding which is the correct behaviour (in my opinion). This means users with only optical flow enabled (e.g. no GPS for position or velocity) will need to arm and takeoff quickly to pass between the landed state and minimum alt within 5seconds. This could be tricky but I think it is the correct behaviour.
  • The gate uses the same "range" (aka height, aka alt-above-terrain) as the flow fusion, so it is only as good as that altitude estimate.
  • If optical flow is used for terrain alt estimation (e.g. EK3_FLOW_USE=2,) this new parameter has no effect

Parameter name question

Earlier versions of this PR named the parameter FLOW_ALT_MIN but with agreement from @andyp1per and @stephendade I've renamed it to FLOW_ALT_MIN. I will raise a follow-up PR that renames FLOW_HGT_OVR (only used on Rover) to FLOW_ALT_OVR.

Screen shots from testing below show the old parameter name but there has been no change in functionality so they are still valid.

Testing

A new autotest, OpticalFlowFocusAlt, has been added.

I have tested this fairly well including:

  • Fly in Loiter with only optical flow enabled and FLOW_ALT_MIN = 0 (the default). There was no change in behaviour from master. The normal drift vs the GPS position appeared, etc
  • Takeoff in Loiter using only optical flow. This worked as long as the takeoffs were not too slow. With a large FLOW_ALT_MIN = 5 (m) the pilot's throttle needed to be raised to above 75% (e.g. 1750) or else "EKF3: IMUx stopped aiding" would appear and an EKF failsafe would switch to pilot-controlled-LAND. The vehicle would land safely and disarm though
image
  • Landing in Loiter always triggered the EKF failsafe causing the vehicle to switch to LAND mode and disarm the vehicle. This was slightly messy but not dangerous because the vehicle was normally on the ground already. The underlying issue is the EKF does not recognise the vehicle has landed. It's messy but mostly not dangerous so I think we should address this in a follow-up PR
image - Flow based alt-above-terrain (aka "hagl", EK3_FLOW_USE=2) worked as it does in master

flow-based-hagl-works

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

  • Checked by a human programmer
  • Non-functional change
  • No-binary change
  • Infrastructure change (e.g. unit tests, helper scripts)
  • Automated test(s) verify changes (e.g. unit test, autotest)
  • Tested manually, description above (e.g. SITL)
  • Tested on hardware
  • Logs attached
  • Logs available on request

AI-assisted: this branch was reworked from #34292 with Claude Code, with the code reviewed and tested by the submitter.

@AP-Review

AP-Review commented Nov 10, 2026 •

Copy link
Copy Markdown

Deprecated — see below for the updated review.

Previous review (2026-11-10)

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

Reviewed at head c8f32f757a.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/35077/1.html#pr35077

The focus-height gate and its test look right for EKF3 navigation fusion; there are two nonblocking points.

Should fix

  • AP_NavEKF3_OptFlowFusion.cpp:354: with EK3_FLOW_USE=2 (Terrain, the Plane default), EstimateTerrainOffset runs before this gate and still fuses below-focus flow into the terrain state, which goes against the new FLOW_HGT_MIN description. Apply the limit to the terrain flow update too (keeping rangefinder updates), or document the exception.

Minor

  • arducopter.py:4970: the test still passes with the readyToUseOptFlow() check removed, even though the EKF goes back into relative aiding during the hover. Fail the first subtest if 'started relative aiding' arrives while the vehicle is below FLOW_HGT_MIN.

@rmackay9
rmackay9 force-pushed the andyp1per-flow-hgt-min-simple branch from c8f32f7 to 8ac2b84 Compare November 10, 2026 01:27
@rmackay9

Copy link
Copy Markdown
Contributor Author

Thanks for the review!

I've rebased this on master and my AI and I have attempted to address the AP_Review comments.

When EK3_FLOW_USE=2 (Terrain, the Plane default) we should not be using the new FLOW_HGT_MIN parameter so we've added some comments to that effect. The reason is that when EK3_FLOW_USE = 2 the optical flow sensor is being used to estimate the height above terrain. The output is a height so it would be "drinking our own bathwater" (as PaulR says) if we then used that height as a condition to turn on/off fusing the flow sensor data.

My AI and I have hopefully resolved the autotest issue as well. Txs again!

@github-actions

github-actions Bot commented Nov 10, 2026 •

Copy link
Copy Markdown

Global Size Summary (Total Flash delta in bytes)

Built from 9c25e44 rebased onto 23cd8d4

Board Periph tracker blimp bootloader copter heli iofw plane rover sub
DrotekP3Pro _ _ _ * _ _ _ _ _ _
Durandal _ +68 +68 _ +124 +164 _ +132 +176 +156
Hitec-Airspeed * _ _ _ _ _ _ _ _ _
KakuteF7 _ +20 +20 _ +20 +20 _ +20 +20 +20
MambaH743v4 _ +88 +88 _ +168 +176 _ +168 +240 +172
MatekF405-CAN _ _ _ * _ _ _ _ _ _
MatekF405 _ +20 +20 _ +20 +20 _ +20 +20 +20
MatekH743-bdshot _ +68 +68 _ +160 +152 _ +156 +180 +156
Pixhawk1-1M _ +76 +76 _ +156 +168 _ +152 +140 +84
disco _ +86 +104 _ +200 +192 _ +142 +236 +152
f103-GPS * _ _ _ _ _ _ _ _ _

@AP-Review

AP-Review commented Nov 10, 2026 •

Copy link
Copy Markdown

Deprecated — see below for the updated review.

Previous review (2026-11-10)

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

Reviewed at head 8ac2b8431e.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/35077/2.html#pr35077

No outstanding issues at 8ac2b84. Both earlier points are fixed: FLOW_HGT_MIN now documents the EK3_FLOW_USE=2 exception, which matches the code. The focus-height test now fails in SITL if relative aiding restarts below the threshold, which it does when the readyToUseOptFlow() check is removed.

CI for this head was still running at review time, and hardware and terrain-mode flight behaviour were not tested.

@rmackay9
rmackay9 force-pushed the andyp1per-flow-hgt-min-simple branch from 8ac2b84 to 11c3e8c Compare November 10, 2026 05:40
@rmackay9

Copy link
Copy Markdown
Contributor Author

Thanks for the review. My AI and I have made one small change to the autotest which was to remove the "SIM_TERRAIN = 0" parameter setting because it was not necessary.

I've also updated the description with information on the human testing performed in SITL

@AP-Review

AP-Review commented Nov 10, 2026 •

Copy link
Copy Markdown

Deprecated — see below for the updated review.

Previous review (2026-11-10)

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

Reviewed at head 11c3e8ca09.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/35077/3.html#pr35077

No outstanding issues at 11c3e8c; CI for this head was still running at review time with no failures so far. Since 8ac2b84 the only change removes SIM_TERRAIN=0 from the focus-height test, and it still passes in SITL with and without the terrain tile, and still fails when the readyToUseOptFlow() check is removed, so that setting was indeed not needed.

Hardware, focus-limited sensors and EK3_FLOW_USE=2 flight were not tested by this review.

@andyp1per

Copy link
Copy Markdown
Contributor

The landing failsafe reproduces in SITL on this branch, and fusing zero flow on the ground instead of discarding it fixes it in about 50 lines. Flow-only copter, analog range finder, FLOW_HGT_MIN 0.3, two or three runs per cell, all alike:

landing, then left armed on the ground this PR with the change
LOITER, with an origin "stopped aiding", then "EKF Failsafe: changed to Land Mode" 5.8 s after touchdown aiding kept until DISARM_DELAY, 12.6-12.8 s
LOITER, FLOW_HGT_MIN 0 (control) aiding kept, 12.6 s aiding kept, 11.8 s
ALT_HOLD, 2 rad/s flow offset on the ground, RNGFND1_MIN 0.2 and 0 "stopped aiding", position drift 0.10-0.13 m aiding kept, drift 0.01-0.02 m

The change zeroes the sample in FuseOptFlow() rather than clearing really_fuse while the range finder reads the vehicle as on the ground: within 5 cm of RNGFNDx_GNDCLR, or out of range low. That test is a static RangeFinder::reading_on_ground(), with on_ground() on the backend and on the DAL backend so Replay agrees. Fusing the measured flow there instead drifts 2.4-2.6 m in the same test, so the zero is what keeps a bad sensor out. OpticalFlowFocusHeight still passes with it. Branch, with the test as OpticalFlowFocusHeightLanding (it fails on this PR as it stands): andyp1per/ardupilot@11c3e8c...flow-hgt-min-on-ground

Other signals for "landed" were tried on #34292 and dropped: onGroundNotMoving reads moving whenever the motors spin (15-22 times its gyro threshold armed on the ground, in flight logs), height alone fuses "not moving" in a hover below RNGFND_MIN, and the vehicle's land and flying flags are vehicle-specific and read wrong in known cases.

One doc point: the EKF never reads the height below MAX(RNGFNDx_GNDCLR, 0.05), so a FLOW_HGT_MIN at or below that has little or no effect. Worth a sentence in the description.

@andyp1per andyp1per left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

Without the landing fix we would be merging something that essentially doesn't work - I don't think we should do that, I think we should include the landing fix as well.

@andyp1per

Copy link
Copy Markdown
Contributor

One lower-priority point from #34292's testing: once the range finder stops reporting, the gate's height is the EKF's dead reckoning with no time bound, and over ground that falls away it reads low at real height. #34292's carried range is the same dead reckoning, and in SITL it held flow off for up to 31 s at 13 m real height after 1 km over ground falling 13 m, above an 8 m range finder, which is why that version trusts a stale height for 5 s only. On master getHeightControlLimit() keeps flow-only flight within the range finder's reach, so this needs the range finder to drop out in flight, or flow alongside GPS, where it only costs the flow.

andyp1per and others added 6 commits November 11, 2026 15:26
The height at which a flow sensor can no longer focus is a property of
the sensor, so it lives here beside the mounting position and is sent to
the EKF with each sample. Defaults to 0, disabled.

Co-authored-by: Randy Mackay <rmackay9@yahoo.com>
Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
Append heightMin, the height below which the optical flow sensor cannot
focus, to the end of the ROFH replay record so it travels with each
sample.

log_ROFH is not packed, so a float placed after the uint8_t quality
would be aligned to 4 bytes and leave 3 bytes of padding that the format
string cannot describe. Declare the padding as three explicit unused
fields so the struct and the format agree. rawFlowQuality stays at its
existing offset, and Replay zeroes the part of a record a log does not
hold, so a log recorded before this change replays with heightMin 0, the
feature disabled.

Based on the AP_DAL work in ArduPilot#34292 by Andy Piper, which carried the
value in a separate ROFM message.

Co-authored-by: Randy Mackay <rmackay9@yahoo.com>
Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
EKF2 does not use the height but, like EKF3, writes the shared ROFH
replay record. Both must write the same value or the record changes on
every sample and is logged twice.

Co-authored-by: Randy Mackay <rmackay9@yahoo.com>
Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
An optical flow sensor cannot focus close to the ground and what it
returns there is not motion. Fusing it dead reckons a phantom horizontal
velocity.

In FuseOptFlow(), once the range from the sensor to the ground is known,
whichever height source gave it, skip the sample if it is below the
sensor's minimum focus altitude. The check is only made once takeoff has
been detected, so on the ground before takeoff the flow is still fused
as zero and relative aiding is not lost.

Skipping the sample leaves the filter with nothing to fuse, but the
sensor keeps reporting, so readyToUseOptFlow() would restart relative
aiding on the next sample after each flow timeout. Make it return false
while the flow is being skipped.

Based on ArduPilot#34292 by Andy Piper, without the carried range height and
ground-rest logic.

Co-authored-by: Randy Mackay <rmackay9@yahoo.com>
Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
Co-authored-by: Randy Mackay <rmackay9@yahoo.com>
Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
Co-authored-by: Randy Mackay <rmackay9@yahoo.com>
Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
@rmackay9
rmackay9 force-pushed the andyp1per-flow-hgt-min-simple branch from 11c3e8c to 9c25e44 Compare November 11, 2026 06:26
@rmackay9 rmackay9 changed the title AP_OpticalFlow/AP_NavEKF3: add FLOW_HGT_MIN to stop fusing below the sensor's minimum focus height AP_OpticalFlow/AP_NavEKF3: add FLOW_ALT_MIN to stop fusing below the sensor's minimum focus alt-above-terrain Nov 11, 2026
@rmackay9

Copy link
Copy Markdown
Contributor Author

After discussing with @andyp1per and @stephendade I've renamed the parameter and variables to use "alt" instead of "height". This includes naming the new parameter FLOW_ALT_MIN instead of FLOW_HGT_MIN.

@rmackay9 rmackay9 changed the title AP_OpticalFlow/AP_NavEKF3: add FLOW_ALT_MIN to stop fusing below the sensor's minimum focus alt-above-terrain AP_OpticalFlow/AP_NavEKF3: add FLOW_ALT_MIN to stop fusing below the sensor's minimum focus alt Nov 11, 2026
@rmackay9

Copy link
Copy Markdown
Contributor Author

We discussed on the EU Dev Call and @peterbarker was happy with this and we agree that if the AI accepted it and @andyp1per approved it that we would merge it. Thanks!

@AP-Review

Copy link
Copy Markdown

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

Reviewed at head 9c25e44f45.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/35077/4.html#pr35077

The new focus-altitude gate works in flight, but with FLOW_ALT_MIN set a flow-only Copter loses aiding and gets an EKF failsafe after every landing where it stays armed. Since 11c3e8c the branch has been rebased and FLOW_HGT_MIN renamed to FLOW_ALT_MIN with no logic change, and the earlier test points still hold.

Should fix

  • AP_NavEKF3_OptFlowFusion.cpp:354: after touchdown the vehicle is still armed, so takeOffDetected stays set and the on-ground range (about RNGFNDx_GNDCLR) is below FLOW_ALT_MIN. Every flow sample is discarded, and about 5 s later the EKF stops aiding and the failsafe switches to LAND. In SITL with andyp1per's landing test it disarmed about 5 s after touchdown with FLOW_ALT_MIN 0.3, while FLOW_ALT_MIN 0 aided normally. The PR description leaves this for a follow-up, but the FLOW_ALT_MIN description doesn't mention it. Take andyp1per's on-ground fix (not tested here), or at least say in the parameter description that landing with flow-only navigation triggers an EKF failsafe.

Not tested here: hardware or real focus-limited sensors, EK3_FLOW_USE=2 flight, or the range-finder dropout case. CI for this head was still running with no failures so far.

@rmackay9

Copy link
Copy Markdown
Contributor Author

@AP-Review, thanks for the review. I'm surprised that previously you'd accepted this but then changed your reply to comment even though no functional changes have been made.

In any case, the EKF failsafe triggering during Loiter is a known issue. This makes landing in Loiter slightly messy but it is safe. We plan to resolve this issue in a follow-up PR which is taking shape here: #35105

@andyp1per andyp1per left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

Thanks Randy!

@peterbarker
peterbarker merged commit 53aa380 into ArduPilot:master Nov 15, 2026
112 checks passed
@rmackay9
rmackay9 deleted the andyp1per-flow-hgt-min-simple branch November 16, 2026 23:14
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.

5 participants