AP_NavEKF3: ground clearance fusion fix

[rmackay9]

This may resolve issue #30489 in which the EKF altitude estimate drifts soon after takeoff if EK3_SRC1_POSZ = 2 (Rangefinder) and the rangefinder is out-of-range-low. I say may resolve because I've been unable to reproduce the issue in SITL.

The key change in behaviour is that while the vehicle is on the ground (determined by its arming state) the rangefinder's ground clearance distance (aka RNGFND1_GNDCLEAR) value is fused instead of falling back to the barometer reading.

Looking at the existing readRangeFinder code I believe this was the original intention but there is a bug which means the code that should do this is unreachable. It is unreachable because higher up in the function, there is a check that the rangefinder status is good and if not, it skips to the next rangefinder. For what it's worth, I think the issue was introduced with this PR which did correct other similar issues #14375

@rishabsingh3003's related PR to add an arming check: #29537

[Copilot]

Pull Request Overview

This PR fixes the EKF’s ground-clearance fallback so that when the vehicle is armed but still on the ground, an out-of-range-low reading from the downward-facing rangefinder is fused instead of defaulting to the barometer.

• Adds an early update of rngOnGnd from the rangefinder’s configured ground-clearance.
• Refactors the per-sensor loop to handle OutOfRangeLow on-ground and remove the old unreachable fallback block.
• Stores the selected range_distance into the ring buffer for fusion.

Comments suppressed due to low confidence (1)

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp:53

• [nitpick] Consider adding a unit or integration test to verify that when the rangefinder status is OutOfRangeLow and onGround is true, the code properly falls back to rngOnGnd.

            } else if (onGround && sensor->status() == AP_DAL_RangeFinder::Status::OutOfRangeLow) {

[peterbarker]

Well - that sounds rather suspect.

No reason we couldn't add this into the loop below in the future.

[rmackay9]

Hi @peterbarker,

yes, indeed it is suspect which led to me adding the comment. This is existing code of course and I did look at expanding the scope of the change to use rangefinder specific ground clearance but sadly I could not get rid of the global variable completely because there are some places which use the ground clearance but have no direct connection to the rangefinder code so I decided it was better to be consistent.

[peterbarker]

I would dearly like to know where the magic 25ms comes from...

[rmackay9]

Hi @peterbarker,

thanks very much for the review!

The "25" is also existing code and as old as the EKF itself. I think it must be a hardcoded guestimate of the rangefinder's lag.

[rishabsingh3003]

I think we should have a "rangefinder delay" parameter like we do for some other sensors. Not part of this PR, of course

[rishabsingh3003]

Nice catch regarding the lines that were never being reached when all rangefinders are unhealthy.

While it is indeed a bug, I think we need to fetch the right "rngOnGnd" per rangefinder backend.

The other thing that worries me is that as soon as the vehicle arms, "onGround" will be false ( atleast for copters), and we'd immediately fall back to baro until the rangefinder is back, which makes me think; why does this feature even exist? Unless someone has a very good reason to have this, I would vote for making this code simpler and getting rid of the entire "range on ground" bit.

Honestly, I feel this entire function has several bugs. The one that has annoyed me for years is that it only checks for the first three sensor indices. So if you don't have a downward facing rangefinder as RNGFND1/2/3, EKF would never fuse it (sorry about the unrelated rant).

As another side note, I am not so sure this will fix the runaway because even with the bug present in master, the EKF should have fallen back on the baro, which is what confuses me the most about that issue...

[rishabsingh3003]

There are a few odd things about doing this:

1. rngOnGnd is being fetched by _rng->ground_clearance_orient() a few lines above. Looking at the RangeFinder library, looks like if we have multiple rangefinders in the downward direction and all rangefinders are unhealthy, it will return the RNGFNDx_GNDCLEAR of the first index in the downward direction. So this would mean the buffer "storedRngMeas" is now storing ground clearance range from a random rangefinder to a random index? Also note that the rangefinder actual position is compensated for later in the posvel fusion code.
2. If a user has multiple downward rangefinders and all of them are unhealthy, will this code now fuse the same "rngOnGnd" multiple times at different rangefinder indices? And then if those rangefinders have different RNGFND1_POS_ parameters, we'd essentially be fusing in slightly different "rngOnGnd" readings.

[rishabsingh3003]

I think we should have a "rangefinder delay" parameter like we do for some other sensors. Not part of this PR, of course

[rmackay9]

@rishabsingh3003,

Thanks very much for the review. I agree with most of your comments but I worry about scope creep and "making perfect the enemy of the good".

re "we need to fetch the right "rngOnGnd" per rangefinder backend" -- @peterbarker made a similar comment using a rngOnGnd value for each rangefinder, and I replied that it's not easy.

I did look at expanding the scope of the change to use rangefinder specific ground clearance but sadly I could not get rid of the global variable completely because there are some places which use the ground clearance but have no direct connection to the rangefinder code so I decided it was better to be consistent.

The core issue is, as you say, the altitude runaway. I think there is a chance it will fix the issue but I guess we've never managed to reproduce it in SITL? I tried but could not. Still, this PR causes some changes that might make it better:

• While trying to reproduce the issue in master, I very occasionally saw the "dead code" being run perhaps due to unlikely timing of the rangefinder becoming healthy and rangefinder and baro data arriving
• This keeps the EKF altitude estimate closer to what the rangefinder reports during takeoff because while disarmed the altitude is forced to rngOnGnd value during this period, I think the baro is constantly reset to match. After arming the baro is used but only until the rangefinder comes into range which tends to be a short period of time.

I think if we can't reproduce the problem in SITL we have little chance of fixing it. I'm of course not going to try and force this change in over the objections of others.