AP_GPS: Support integrated headings from SBF

[WickedShell]

This is an alternate to #18615. (Basically it implements most of my PR requests on that PR). This also takes the step to actually configure GNSS Attitude as enabled via the config strings, as that was being ignored or manually done before. This may introduce a breakage if any older receivers (AsteRx-m is the minimum target here) don't support the command, which will have to be tested. I haven't had a chance to test it any of this PR yet though so that needs doing. (Otherwise I will probably get back to it in early December if no one else has beaten me to it).

Needed Tests:

• Validate that normal SBF functionality isn't broken.
• Test that all expected units can handle sga, None
• Test that heading works
• Verify the yaw accuracy information is correct (I've seen early test data from a different incarnation of adding this, that was producing wildly inaccurate yaw accuracy).
• 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).

[liujinjun666]

Dear,
We are currently testing the attitude functions based on Septentrio mosaic-H module.
The contents of the test include the following:
1.      Standalone 3D GPS
2.      RTK FIXED
3.      GPS for Yam
4.      Redundancy test: switching with the second GPS (Ublox M8N).
Currently the test confirms full functionalities with navigation and yaw, the drone has normal PVT and attitude.
We still have the following issues:
1.      When the mosaic-H (GPS1) is not connected to the COM3 of flight controller, Mission Planner will show “No PVT”, while normally it should show “No GPS”
2.      During flight tests, mosaic-H (GPS1) will have sudden data lost, in Mission Planner the PVT mode is changed to “No PVT”, in the message box there is buadrate detection, then Mission Planner recovers RTK FIX. The occurrence of this issue is random and it is unknown what circumstance will trigger it.
3.      Ardupilot does unnecessary configurations to the mosaic-H (GPS1), e.g. data logging.
4. log https://septentrio-my.sharepoint.com/:u:/p/jiakang_xu/Ef8WUOhgPLBPjbJRJcPIPCMBMklUen0B273kKX61vOe7hA?e=wAQ5bV

[WickedShell]

Pushed an update to this that I've ground tested here. Fixed the following issues:

• It wouldn't ever detect the GPS on startup
• Fixed not setting the GPS yaw configured flag, which meant we would parse the yaw, but not use it or report it over MAVLink

Outside ground tests, dropouts are from me intentionally shading one of the antennas with my hand, the change in heading is a real change from me rotating the table the antennas were on, seperation between antennas was only about 0.6 m, or a lot less then it should have been:

Still pending:

• Use the GPS antenna offsets to set the offsets in the actual GPS driver (pending confirmation that this is the preferred solution)
• Flight testing
• Rebase to fix conflicts, but this should now be actually flyable.

[WickedShell]

Also need to apply the attitude offset, to get around the assumption that the antennas are north/south aligned.

[liujinjun666]

Thank you very much, just finished the Chinese New Year holiday, did not check the update in time, sorry, I will test the update as soon as possible

[SeppeG]

Hi @WickedShell
I am trying out your code but upon building I had a few build errors:

In file included from ../../libraries/AP_GPS/AP_GPS_SITL.cpp:16:
../../libraries/AP_GPS/AP_GPS_SITL.h:22:5: error: "HAL_SIM_GPS_ENABLED" is not defined, evaluates to 0 [-Werror=undef]
   22 | #if HAL_SIM_GPS_ENABLED
      |     ^~~~~~~~~~~~~~~~~~~
compilation terminated due to -Wfatal-errors.
cc1plus: all warnings being treated as errors

../../libraries/AP_GPS/AP_GPS_MAV.cpp: In member function 'virtual void AP_GPS_MAV::handle_msg(const mavlink_message_t&)':
../../libraries/AP_GPS/AP_GPS_MAV.cpp:124:23: error: 'struct AP_GPS::GPS_State' has no member named 'last_corrected_gps_time_us'
  124 |                 state.last_corrected_gps_time_us = (corrected_ms * 1000ULL);
      |                       ^~~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated due to -Wfatal-errors.

../../libraries/AP_GPS/GPS_Backend.cpp: In member function 'void AP_GPS_Backend::set_uart_timestamp(uint16_t)':
../../libraries/AP_GPS/GPS_Backend.cpp:230:15: error: 'struct AP_GPS::GPS_State' has no member named 'last_corrected_gps_time_us'
  230 |         state.last_corrected_gps_time_us = port->receive_time_constraint_us(nbytes);
      |               ^~~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated due to -Wfatal-errors.

../../libraries/AP_GPS/AP_GPS_UAVCAN.cpp: In member function 'void AP_GPS_UAVCAN::handle_fix2_msg(const Fix2Cb&)':
../../libraries/AP_GPS/AP_GPS_UAVCAN.cpp:591:23: error: 'struct AP_GPS::GPS_State' has no member named 'last_corrected_gps_time_us'
  591 |         interim_state.last_corrected_gps_time_us = jitter_correction.correct_offboard_timestamp_usec(cb.msg->timestamp.usec, (cb.msg->getUtcTimestamp().toUSec() + NATIVE_TIME_OFFSET));
      |                       ^~~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated due to -Wfatal-errors.

../../libraries/AP_GPS/AP_GPS.cpp: In member function 'uint32_t AP_GPS::get_itow(uint8_t) const':
../../libraries/AP_GPS/AP_GPS.cpp:1982:31: error: 'class AP_GPS_Backend' has no member named 'get_last_itow'; did you mean 'get_last_itow_ms'?
 1982 |     return drivers[instance]->get_last_itow();
      |                               ^~~~~~~~~~~~~
      |                               get_last_itow_ms
compilation terminated due to -Wfatal-errors.

Is this a known issue? Or am I doing something wrong? (your fork does work, so there might be a compatibility issue)

[tatsuy]

@WickedShell Hello,
I'm waiting for this to be merged into master.
Is there anything I can do to help with the merge?

I tested it after rebased this to master.
heading from sbf works.
https://github.com/tatsuy/ardupilot/tree/sbf-heading

[tridge]

@WickedShell I have rebased this PR
@tatsuy if you can get us some SBF logs with --enable-gps-logging then I can test it

[tridge]

@WickedShell I've rebased this and using the logs from @tatsuy here:
https://drive.google.com/file/d/14L-KkthvZKJGR8w8q1o3W_vioBopRaWp/view?usp=share_link
to test in SITL with GPS replay.
The BaseVectorGeod messages in those logs appear to be the delta from the RKT base station to the rover, not from one antenna to the other, so the yaw values that come out are wrong.
for example:

[tatsuy]

As a supplement, I attach the settings on the SBF side.

[WickedShell]

The BaseVectorGeod messages in those logs appear to be the delta from the RKT base station to the rover, not from one antenna to the other, so the yaw values that come out are wrong.

This is actually exactly expected. BaseVectorGeod is used for handling using 2 different units with one feeding corrections to the other. It's how all the heading information for the moving platform was derived in this.

If you are using an actual dual antenna unit that provides AttEuler we don't want to process the BaseVectorGeod any further than filling in the the RTK info, which is what should happen if the SBF_UseBaseForYaw isn't set. What were your GPS_DRV_OPTIONS set to when you were testing?

[tatsuy]

The GPS_DRV_OPTIONS is set to 0.
This is an actual dual antenna (GPS_TYPE=26) unit, so AttEuler is used for yaw.

If we set GPS_TYPE_SBF_DUAL_ANTENNA, should we remove BaseVectorGeod in this case?

[WickedShell]

No BaseVectorGeod is still useful, because my understanding is you can do RTK corrections into the unit while also getting local yaw.

If it was parsing from AttEuler, then what's the issue with BaseVectorGeod?

[tatsuy]

Now that you've explained its role and functionality, I see that there may not be any issue with it when parsing from AttEuler.

I believe this PR is working as expected.
If there is anything else I can do or any additional information you need to help facilitate the merge.

[Huibean]

Any Update for this PR?

[Luc-Gousset]

I am currently an intern at Septentrio. We have tested this pull request and believe it is working accordingly.

About the @WickedShell needed test:

• About the sga, None command, all the receivers that support moving base support this command. This includes all the following receivers (the most common receivers used in UAV's): AsteRx-m2, AsteRx-m2a, AsteRx-m3 Pro, AsteRx-m3 Pro+, mosaic-x5 and mosaic-h. However, receivers that do not support Dual Antenna will not accept the `sga, MultiAntenna' command (e.g. AsteRx-m2, AsteRx-m3 Pro and mosaic-x5). Note that this is not a problem as the command is only sent if the user selects the SBF_DUAL_ANTENNA settings.

• About the antenna location, to have better heading performance, we recommend setting the position of the antenna manually. This can be either done using the setAntennaLocation and optionally the setAttitudeOffset commands or by the user using the web interface of the receiver to configure these settings. In the future the Ardupilot code could be improved to integrate these parameters however the current pull request already works perfectly fine if the user configures these parameters in the receiver (using the web user interface or RxControl)

Are there any things else we can do or information we can provide to facilitate the merge of this pull request ?

[rmackay9]

Hi @WickedShell, my main sponsor EAMS is interested in getting this merged. I know the PR has been languishing a bit but I could help push to get it merged.

Maybe you could rebase it? Alternatively I could although I don't immediately have the hardware (but I could get it).

[Ryanf55]

Hi @WickedShell, my main sponsor EAMS is interested in getting this merged. I know the PR has been languishing a bit but I could help push to get it merged.

Maybe you could rebase it? Alternatively I could although I don't immediately have the hardware (but I could get it).

I met one of the Septentrio folks at ROSCon, Gustavo. He sent me a very nice email. They have a fork of ArduPilot they are maintaining with this merged for their customers, but would like these changes merged upstream.

Gustavo is happy to supply anyone in the dev team hardware as-needed for this PR.

[rmackay9]

I've got the hardware now and I've checked that it can provide position. It is not providing the attitude yet though for some reason (perhaps it needs a better view of the sky). What we need next is the datasheet detailing the message formats.

[rmackay9]

I'd love to hear if anybody knows which message from this GPS includes the antennas relative positions.

[Huibean]

I'd love to hear if anybody knows which message from this GPS includes the antennas relative positions.

AuxAntPositions (5942) ?

[rmackay9]

I've created a new PR which is just a rebased version of this PR #25601.

If I can get the other one merged then we can close this one.

[rmackay9]

This follow-up PR has been merged so I will close this one. #25601