External heading source via MAVLink

Nubbish · October 24, 2024, 5:51pm

I’m working on an autonomous boat and want to use an external heading sensor, but I’m running into issues with the autopilot actually using the heading values.

My setup is a Cube Orange+ connected to a Raspberry Pi 4 which hosts a MAVLink server and also reads data from a Garmin Steadycast heading sensor and forwards it via GPS_INPUT messages.

The Cube is mounted inside the aluminum hull of the boat and nearby the other electronics including high-power cables for the electric motors. This makes the internal compass pretty inaccurate, hence wanting to add the Garmin.

I can see with Mission Planner’s MAVLink Inspector tool that the Cube is receiving the GPS_INPUT messages and outputs GPS_RAW_INT messages mirroring the input.
However, GLOBAL_POSITION_INT messages still only reflect the Cube’s internal compass.

(I currently have the autopilot and Pi on my desk running a test script, hence the big deviation between the two readings)

Attached are the Cube’s parameters. I did everything I could think of to disable the internal compass readings and favour the heading in GPS_INPUT messages, but no luck.
2024-10-24_Cube bench test.param (15.5 KB)

Would be nice if there was a dedicated compass message from MAVLink, but I don’t have the time to develop something like that.

geofrancis · October 24, 2024, 7:49pm

have you set the EKF to use GPS for yaw ?

EK3_SRC1_YAW: Yaw Source¶

Note: This parameter is for advanced users

Yaw Source

Values
Value Meaning

0 None
1 Compass
2 GPS
3 GPS with Compass Fallback
6 ExternalNav
8 GSF

Nubbish · October 24, 2024, 11:46pm

Hi @geofrancis.

I did set the EK3 yaw source to GPS as shown in the parameters file.

geofrancis · October 24, 2024, 11:49pm

I think there is a check where it wont use GPS heading information if it doesn’t have a RTK gps fix, so try setting fix_type to 6.

Nubbish · October 25, 2024, 4:07pm

@geofrancis
Thanks for the suggestion. Just tried it this morning but it’s still tracking the Cube’s internal magnetometer.

geofrancis · October 25, 2024, 5:31pm

I just had a good look through all the documentation, Im not sure it’s actually possible to use GPS heading from a mavlink GPS. I think it should be supported, but I can’t find any documentation on that configuration.

https://ardupilot.org/copter/docs/common-gps-for-yaw.html

try and fill in the rest of the GPS parameters and sending it at 5hz.

Nubbish · October 25, 2024, 5:36pm

I don’t see why it wouldn’t be possible.
There’s a GPS_TYPE option (14) for MAVLink. I’ve also added some lat/lon coordinates to my test messages so the vehicle shows up on the map, meaning it’s reading the GPS_INPUT messages just fine.

geofrancis · October 25, 2024, 5:36pm

It might be what your sending, your message has satellites set to zero. are you getting any GPS unhealty messages?

@Yuri_Rage any ideas?

Nubbish · October 25, 2024, 5:39pm

I have also changed the satellite count to 8 to see if it made a difference. It doesn’t seem to affect anything except the GPS accuracy warning.
The autopilot hasn’t complained about the GPS messages and still properly returns GLOBAL_POSITION_INT messages with the same data as in GPS_INPUT

geofrancis · October 25, 2024, 5:40pm

there are extra checks for GPS heading, you might not be meeting all of them, one is RTK fix but there might be more.

Nubbish · October 25, 2024, 5:42pm

Good to know. Are these checks documented anywhere?

geofrancis · October 25, 2024, 5:46pm

im not sure,It looks like it is supported but I think you’re missing GPS values in your mavlink message.

github.com

ArduPilot/ardupilot/blob/9dca511e7e5c305ee4f5773fa62fe443d174542f/libraries/AP_GPS/AP_GPS.cpp#L1348


      
          }
          
          void AP_GPS::inject_data(uint8_t instance, const uint8_t *data, uint16_t len)
          {
              if (instance < GPS_MAX_RECEIVERS && drivers[instance] != nullptr) {
                  drivers[instance]->inject_data(data, len);
              }
          }
          
          /*
            get GPS yaw following mavlink GPS_RAW_INT and GPS2_RAW
            convention. We return 0 if the GPS is not configured to provide
            yaw. We return 65535 for a GPS configured to provide yaw that can't
            currently provide it. We return from 1 to 36000 for yaw otherwise
           */
          uint16_t AP_GPS::gps_yaw_cdeg(uint8_t instance) const
          {
              if (!have_gps_yaw_configured(instance)) {
                  return 0;
              }
              float yaw_deg, accuracy_deg;

Yuri_Rage · October 25, 2024, 6:06pm

There are quite a few checks and balances in the moving baseline driver. Simply sending GPS_RAW_INT yaw is almost certainly insufficient to mimic a moving baseline config, and if you want to fake data to that driver, you’ll have to enable it as a position source. I see you’re sending almost all 0s, which will cause a lot of compounding errors and issues.

I don’t really have any good ideas about using this data source other than writing a custom driver.

Nubbish · October 25, 2024, 6:09pm

Thanks for the input.
So basically, providing just a heading source is not possible over MAVLink currently?

I could potentially use another serial port just for NMEA heading data like @geofrancis suggested, but I’d rather not have to take up another port.

geofrancis · October 25, 2024, 6:10pm

use it with a GPS, pull all the data from an actual GPS receiver so the only thing you’re spoofing is yaw.you might be able to get the GPS data from the fight controller over mavlink then package it with the yaw data.

Nubbish · October 25, 2024, 6:12pm

I had that thought too, but the vehicle already has a proper moving baseline GPS setup.
The idea with the external heading sensor is to operate in environments without sufficient GPS signal for heading/intermittent signal.

geofrancis · October 25, 2024, 10:09pm

You could use MSP for a compass but there isnt much documentation on MSP protocol.

Nubbish · October 25, 2024, 11:34pm

Thanks for the suggestions @geofrancis. I’ve got some direction now at least.

I did try the GPS spoofing idea by copying the GPS_RAW messages from a connected GPS and using that data for GPS_INPUT messages plus the heading data, but still couldn’t get it to use the GPS heading. That could be do to a configuration issue, so I’ll keep looking into it.

In the longer-term I want to look into developing a custom MAVLink message & driver for this purpose, 'cause that would be a much cleaner solution. @Yuri_Rage Do you have any suggestions on where to start with a custom driver for the autopilot?

Thanks again

Nubbish · November 1, 2024, 5:26pm

@geofrancis
As an update, I kept working on the GPS_INPUT spoofing and managed to get it to work.
The key was to disable HDoP in the EK3_GPS_CHECK parameter.

To summarize, it is possible to use GPS_INPUT messages to send just an external heading source but it’s a bit hacky.
The GPS_INPUT messages require fix_type of 3 or greater, and a number of visible satellites of at least AHRS_GPS_MINSATS.
It also requires HDoP to be disabled in EK3_GPS_CHECK.

Overall not a perfect solution, but it’ll work for the time being.
I think a MAVLink compass message/driver would be a worthwhile addition for projects like mine.

Yuri_Rage · November 1, 2024, 5:31pm

How does that hack affect the position state? I’d guess it to be detrimental, but if it works…it works!