Update GPS offset when setting BARO_FIELD_ELV

[Maarrk]

Summary

Allows BARO_FIELD_ELV parameter to override EKF3 height. Currently it is apparently ignored with no mechanism to manually correct takeoff height in case of GNSS altitude error

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 below (e.g. SITL)
• Tested on hardware
• Logs attached
• Logs available on request

The error condition this PR helps with can be simulated by setting SIM_GPS_ALT_OFS to e.g. 50 in SITL

Description

Problem

If GPS has altitude error on startup, the autopilot will consistently use this incorrect altitude. Example:

• GPS altitude error +50 m (value repeatedly observed in practice! tends to stay constant during flight)
• Relative altitude 0m (because we didn't move since startup)
• Absolute altitude 170 m
• Terrain elevation (and correct absolute altitude) 120 m

When only using relative altitudes, there are no apparent issues. But if a mission or command contains absolute altitude, this will be wrong, potentially causing flight into terrain.

Solution

Allow the operator to specify a known airfield elevation, just like in manned aviation.
This is already implemented with BARO_FIELD_ELV parameter, but it had no effect on this problem. Changing it would correctly notify altitude reset all the way to EKF, but GNSS altitude was used anyway.

I decided to expose read-only field_elevation, otherwise I would have to weave the altitude through many function calls on parameter update, and change much more code.

---

Once we agree if this is a good approach, I will update the parameter description and look through the Wiki for places where this behaviour should be mentioned.

Contribution on behalf of @FlyfocusUAV

[andyp1per]

Having submitted a bunch of similar PR's your best approach is to construct a test that demonstrates the problem you are trying to solve and then show that your fix resolves the problem.

[Maarrk]

Thanks for the suggestion @andyp1per, I added an autotest that works on this PR but fails on master.

For (my own) reference, it can be run with:

./Tools/autotest/autotest.py build.Plane test.Plane.GroundAltitudeWithGPSError

[Maarrk]

I believe all the technical suggestions have been addressed, there's one matter of taste left:

I think that renaming to FIELD_ELEVATION would be a good solution, to make it clear what it does and that will affect more subsystems. On one hand this is more invasive change, however I feel this is in the spirit of parameter renames in ArduPlane 4.5 that made them more general and less cryptic:

AIRSPEED_CRUISE replaces TRIM_ARSPD_CM`
AIRSPEED_MIN replaces ARSPD_FBW_MIN
AIRSPEED_MAX replaces ARSPD_FBW_MAX

I feel like this is would be just a short RFC on the Dev Call. Is pinging @rmackay9 the right way to propose that?

[rmackay9]

Hi @Maarrk,

I've added the "DevCallTopic" so it will be discussed at the next weekly dev call.

[andyp1per]

Wait, I am literally trying to make the origin truly immutable in another PR - this feels like a retrograde step. #32768 I think we truly do need the origin to be fixed. As soon as we start making changes all kinds of assumptions fall apart. I think @peterbarker was in agreement with this maybe?

[Maarrk]

@andyp1per thanks for speaking up, I'm happy to coordinate with you.

This PR is part of a wider effort at @FlyfocusUAV where we find ourselves improving ways to tell ArduPilot "the GNSS is wrong, you're actually here, trust me". I am planning to package this forum thread into a PR very soon, also seems related.

[andyp1per]

Hi @Maarrk you are therefore likely to need Paul's change in this area: #31738

[IamPete1]

I think the interaction with baro here is confusing. Really you just want to set the EKF origin altitude, but use the current origin lat/lon. Could this be done via the existing set origin code?

Why does setting the baro field elevation not work correctly already if baro is the primary altitude source?

[rmackay9]

I agree with AndyP that once the origin is set, we shouldn't move it. That assumption is used within Copter's navigation code and we would end up with bad behaviour especially during transitions between waypoints.

[Maarrk]

Now with better understanding of the issue, I agree that I shouldn't move the EKF origin.

What I'm really after is described in the autotest, and it's passing now. I changed the conditionals to directly modify the EKF GPS reference height, and I'm getting the behaviour that I was after.

---

I see that there are a few competing options for manually correcting the altitude (baro field elevation, set origin, AHRS_ORIGIN_ALT). Another valid option would be to remove BARO_FIELD_ELV entirely, since it seems to be broken anyway, and only support a smaller set of ways to do the correction.

[rmackay9]

We discussed this on the dev call and at least some of us thought that it would be better to update the EKF's barometer offset. I've created an issue here with some ideas: #33029

[Maarrk]

Thanks @rmackay9 for discussing this topic. I saw the issue, and it should help with another problem that we had but it's not exactly what this PR is about. It's really unfortunate I didn't manage to join the call (should've waited at the desk not the soft couch).

The issue #33029 talks about updating baro from GPS. What I need to do in this PR is somehow correct the GPS altitude error. So far we used to fly relative altitudes and pretend it's alright, but we really need the AMSL altitude to be correct. This is how airspace is partitioned and we can't just be off by 150 feet because we got an unlucky boot sequence.

What option do we have to have the user tell the autopilot "this is the correct altitude even though the GPS says otherwise"? The autotest explains all that I need to do:

self.set_parameter("SIM_GPS1_ALT_OFS", 50) # In practice we can get a wrong reading like this

self.assert_altitude(self.sitl_start_location().alt + 50) # Now the autopilot believes the GPS

self.set_parameter("BARO_FIELD_ELV", self.sitl_start_location().alt) # User *somehow* inputs known altitude

self.assert_altitude(self.sitl_start_location().alt) # This should fix the altitude

Preferably a parameter and BARO_FIELD_ELV used to be how we did it, but I'm happy with any possible way.
EXTERNAL_POSITION_ESTIMATE won't work when we have GPS. EKF origin shouldn't be moved.

[Maarrk]

I looked at all the linked PRs and issues and made some adjustments:

Re: @rishabsingh3003, @IamPete1 I updated the code to only consider the user altitude if barometer is the primary vertical position source for this core.

Re: @andyp1per @rmackay9 I agree that the EKF origin shouldn't be moved. I don't have a ready solution on how to prevent it here, but my changes don't affect this behaviour now.

Regarding the issue #33029 - it looks good but it's the opposite of what I need to do - with our hardware and conditions, it's the GPS that has the altitude error. This tends to stay constant throughout the flight, so offsetting the reference height based on user input seems to me to be the perfect way to resolve it.

[Maarrk]

Current CI failures seem unrelated to this PR, I don't have authorization to run only the failing ones

[IamPete1]

Current CI failures seem unrelated to this PR, I don't have authorization to run only the failing ones

I have restarted them.

I'm still not clear on why setting baro as the primary altitude source in the EKF and then setting the feild elevation does not work as expected.

[Maarrk]

I'm still not clear on why setting baro as the primary altitude source in the EKF and then setting the feild elevation does not work as expected.

My autotest fails on current master, things attempted:

• Setting baro as altitude source
• Using EKF2
• Using DCM

I am figuring out how to move my test out of source control to git bisect what happened, I'll report results when I manage to make it work.

[IamPete1]

Seems like this is related #33431