Repository navigation
AP_OpticalFlow/AP_NavEKF3: add FLOW_ALT_MIN to stop fusing below the sensor's minimum focus alt - #35077
Conversation
Previous review (2026-11-10)Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting. Reviewed at head The focus-height gate and its test look right for EKF3 navigation fusion; there are two nonblocking points. Should fix
Minor
|
c8f32f7 to
8ac2b84
Compare
|
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! |
Global Size Summary (Total Flash delta in bytes)Built from 9c25e44 rebased onto 23cd8d4
|
Previous review (2026-11-10)Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting. Reviewed at head 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. |
8ac2b84 to
11c3e8c
Compare
|
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 |
Previous review (2026-11-10)Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting. Reviewed at head 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. |
|
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,
The change zeroes the sample in Other signals for "landed" were tried on #34292 and dropped: One doc point: the EKF never reads the height below |
andyp1per
left a comment
There was a problem hiding this comment.
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.
|
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 |
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>
11c3e8c to
9c25e44
Compare
|
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. |
|
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! |
|
Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting. Reviewed at head 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
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. |
|
@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 |
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_MINparameter 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 newFLOW_ALT_MINparameter, constrained to 0 to 5 m, sent to the EKF with each flow sample.AP_DAL:altMinis appended toROFH.log_ROFHis not packed, so 3 "unused" uint8 fields are added before the new field instead. This ensures thatrawFlowQualitykeeps 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: inFuseOptFlow(), once the range is known,really_fuseis set false when the vehicle's alt-above-terrain is belowaltMin. 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 sameROFHas EKF3.Behaviour to be aware of:
FLOW_HGT_OVRis set (e.g. on Rover) the alt check is not run (i.e. FLOW_ALT_MIN is ignored)FLOW_ALT_MINfor 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.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:
Classification & Testing (check all that apply and add your own)
AI-assisted: this branch was reworked from #34292 with Claude Code, with the code reviewed and tested by the submitter.