AP_GPS: Support integrated headings from SBF

[rmackay9]

This is a rebased version of PR #19165 which in turn was an alternative to #18615.

This adds support to the SBF driver so that heading can be retrieved from the GPS. I've tested this on a Mosaic-H and confirmed that position and yaw work.

I have not added support for retrieving the antenna positions from the GPS although I have a development branch that retrieves these values (but does nothing with them). I think though that if we can't find time to do that additional development we should just merge this because it does improve things.

The original PR mentions these possible tests we should do although the other PR also has two people (including an intern at Septentrio) who says it is working correctly.

• Test that all expected units can handle sga, None -- confirmed by Septentrio tests
• Consider if we should be setting the antenna locations to rigid instead of allowing the unit to compute them for us in auto (easier setup, but maybe not ideal, need to ask the manufacturer) -- we now calculate the yaw within the driver.

This has been tested on real hardware and below is a comparison of compass yaw vs GPS yaw. Note that the yaws diverge around 405000 which is when I turned the autopilot+GPS upside-down.

The corresponding wiki update is here ArduPilot/ardupilot_wiki#5583

[tridge]

looks good!

[tridge]

@rmackay9 we should consider a 4.4.x backport too

[SeptentrioGNSS]

Seeing the work put into supporting dual antenna Septentrio receivers is nice, could I just get some input on the reasoning why the yaw (heading value represented by Septentrio receivers) value is calculated on the autopilot side? @rmackay9 @tridge could either of you shed some light on this for me?

In a different SBF block both yaw and pitch or yaw and roll are already available depending on the antenna installation (antennas mounted in-lin with vehicle/copter front or 90deg rotated). So a nice alternative approach (possible new Feature request), to the one currently in this merge request, would be grabbing the value from the SBF block (AttEuler) as the value is already calculated by the GNSS receiver.
Some background on the matter why this approach might benefit more people in the community is the way the yaw is calculated here. To my understanding the compass of the pixhawk is also used in the heading calculation but this proves to fail when high banking angles are in place.

The further expansion of the dual-antenna configuration for SBF would be helpful but any feedback on the reasoning for the merged code would help me better understand. But the proposed code is tested and confirmed to work so having this in the release is/would be greatly appreciated asit is currently still a card marked with pending.

[rmackay9]

Hi @SeptentrioGNSS,

Thanks for the question.

Calculating the yaw on the autopilot allows us to add extra checks that improve safety. These extra checks are in the calculate_moving_base_yaw() function and ensure that the user configuration in AP (e.g. GPS_POS1_X,Y,Z, GPS_POS2_X,Y,Z) matches the distances that we're getting from the GPS. We could have separated out these checks and then still used the yaw provided by the GPS but it's easier and more reliable (e.g. less chance of bugs) if we reuse existing functions.

Although I didn't check it very thoroughly, I didn't find that the yaw calculation in the GPS was better than what AP was doing. For example, both methods failed once the GPS was leaned over at 90degrees.

Re the use of the compass, depending upon the user setting, the EKF may fall back to using the compass if the GPS cannot provide the yaw. If the EKF falls back to using compass then it fuses a 3D vector and the calculation works regardless of the vehicle's orientation.

[SeptentrioGNSS]

Ok, thank you for the detailed information. Cannot wait for this to be merged into the copter 4.4 as I can see it is already in the main

[SeptentrioGNSS]

@tridge , I see that it is pending for the copter 4.4 could you provide me some details on the status? Is this parked, is this declined, is this waiting on confirmation, is this waiting on approval?

It would be nice to see this code in an official release (possibility for the copter 4.4.5) or is the release of 4.5 closer on the horizon (timeline on this would be helpful) to just wait as this is already in there?

[rmackay9]

Hi @SeptentrioGNSS,

We missed including this in 4.4.4 but if we ever do a 4.4.5 it will be included. We haven't decided on a timeline on 4.4.5 though, normally we wait until there are critical bugs that need fixing.

This change is included in 4.5.0 which has entered beta testing. That will likely become the stable version within 2 months although the timing also depends upon how much testing feedback we get and how quickly we resolve any issues found.

[SeptentrioGNSS]

Thank you for this feedback