PX4 do not use RTK GPS heading (v1.12.0-beta4)

[jalvarez96]

Describe the bug

I am trying to set up and testing RTK GPS Heading with Dual u-blox F9P https://docs.px4.io/master/en/gps_compass/u-blox_f9p_heading.html with SparkFun F9P boards, PX4 v1.12.0-beta4 firmware and Pixhawk 4 board.

I have the following configuration: TXD2 of the "Moving Base" to RXD2 of the "Rover". UART1 from "Rover" connected to "GPS 1" port and UART1 from "Moving Base" connected to "TELEM/SERIAL 4" port.

I have configured the following relevant parameters: GPS_1_CONFIG=GPS 1, GPS_2_CONFIG=TELEM/SERIAL 4, SER_GPS1_BAUD=AUTO, SER_TEL4_BAUD=AUTO, GPS_UBX_MODE=1 (Heading), EKF2_AID_MASK=128 (GPS yaw fusion), EKF2_MAG_TYPE=None. I also update U-Blox F9P receivers to latest ublox firmware (1.13).

So, QGroundControl shows "GPS fixed RTK", visibility about 22 satellites, but the compass is not oriented correctly. Then, If I rotate only the Pixhawk 4 by yaw axis I can see the compass is rotating due to gyro integration (magnetometer is disabled), but if I rotate only the antennas "in baseline" by yaw axis the compass does not rotate. However, in the GPS_RAW_INT messages I can see yaw values ​​that vary over time.

Capture of the setup:

Capture from "u-center" ublox tool ((in this tool I could see that the yaw was not very stable..)):

Expected behavior I expected px4 shows yaw from RTK GPS heading.

[LorenzMeier]

@bresch You might be able to shed some light on this.

[sujiv0]

@jalvarez96 Hi I used to work with dual GPS setup but in ArduCopter. Have you checked whether the in dual GPS configuration setup alone is giving proper Yaw data with respect to Earth North? Try configuring the GPS as per https://www.u-blox.com/en/docs/UBX-19009093 and check the Yaw data is coming correctly.

[jalvarez96]

@sujiv0 Hi, I checked with "u-center" tool the proper Yaw from "Rover" with respect to Earth North, but with some glitches (maybe I need more distance between "Rover" and "Moving base" or change patch antennas to helical antennas?).

As next step I checked in QgroundControl the GPS_RAW_INT messages from "Rover" and I could see yaw values. But these values ​​are not reflected in the compass icon in the main Qgroundcontrol window, so pX4 is not taking them into account to calculate the yaw, I am right?

[sujiv0]

@jalvarez96 Hi, I used helical antenna (https://www.sparkfun.com/products/17383) with a separation of around 35cm. Can you try adding Tx, Rx and GND between UART2 of both GPS. I used to get heading data with maximum 2 degree deviation.

I'm not sure about the px4, but in Arducopter the code will accept the Yaw data only after getting the satellite status to 5 (If I remember correctly)

[bresch]

@jalvarez96 Would you be able to produce a .ulg file (logger on, do some motion, then logger off in the mavlink shell)? That would tell us if the data comes properly to the EKF or not.

@Igor-Misic You worked with the same modules recently, any comments on the setup/config?

[Igor-Misic]

@bresch setup and config look good. I presume GPS_YAW_OFFSET is at 0 since GPS1 (main) is in front of GPS2 (second).

[jalvarez96]

@sujiv0 thanks for your approach. My setup with patch antennas and separation of 35cm works very well checking the "UBX->NAV->RELPOSNED" messages in the "u-center ublox tool". In this messages I can see "Length≈35cm" and accurate Heading. So the problem I think is in PX4 configuration.

Hi @Igor-Misic ; yeah, GPS_YAW_OFFSET=0

Hi @bresch . I did two test: In the first I run logger on with south-facing setup and then I rotated it 360º on yaw axis (south -> east -> north -> west -> south). This is the log: https://review.px4.io/plot_app?log=a67dbc7f-28fd-479e-b33a-bfb418481a17 In the second test I did the same but with clockwise rotation (south -> west -> north -> east -> south). This is the log: https://review.px4.io/plot_app?log=323571d7-01a2-4171-b549-d918dc503032

So, the main problem is that the compass of QgroundControl does not show correct values, as if it did not take into account the values ​​of heading. At first the compass should be pointing south and it is not. Can you see something strange in the logs?

[bresch]

@jalvarez96 I had a quick look at the log and I found the issue. In you case, you set the aid mask to 128 (GPS yaw only) so the following check is false: const bool need_gps = (_params.fusion_mode & MASK_USE_GPS) || (_params.vdist_sensor_type == VDIST_SENSOR_GPS); https://github.com/PX4/PX4-ECL/blob/85f5a935fe6095f585bfb12b584e0813126cbc2a/EKF/estimator_interface.cpp#L187

If you change it to

const bool need_gps = (_params.fusion_mode & MASK_USE_GPS)
                      || (_params.fusion_mode & MASK_USE_GPSYAW)          //<<< this was missing
                      || (_params.vdist_sensor_type == VDIST_SENSOR_GPS);

it should work. Please let me know if this fixes your issue and do a follow-up PR to fix this.

Also, note that I recently refactored the selection logic of the GPS yaw fusion (https://github.com/PX4/PX4-ECL/pull/1006) and you can test it by using the master branch.

Edit: note that in the refactored version, there is a maximum time between two measurements of 1s specified here to use the GPS heading data (https://github.com/PX4/PX4-ECL/blob/85f5a935fe6095f585bfb12b584e0813126cbc2a/EKF/control.cpp#L747) which is apparently too restrictive (in your log, the data comes at < 1Hz) so you would need to increase the threshold. I'll see which value make sense and update the code.

[jalvarez96]

Hi @bresch , thanks for your help. I'll tell you what I was testing with v1.12.0-beta5:

I did the modification you told me in the estimator_interface.cpp file and the QGC compass still didn't get oriented. Then I have modified EKF2_AID_MASK = 128 to EKF2_AID_MASK = 129 (I added bit 0 to true ("to true to use GPS data if available")) and after restarting, the compass oriented correctly. Then I reverted the changes in the estimator_interface.cpp file (that is, without the (_params.fusion_mode & MASK_USE_GPSYAW) line)) and keeping EKF2_AID_MASK = 129 and the compass was still orienting correctly.

In summary, it seems that modifying the estimator_interface.cpp code has no effect, however with EKF2_AID_MASK = 129 it works correctly (now I have to try to fly). It really is not very clear to me the difference between EKF2_AID_MASK = 128 to EKF2_AID_MASK = 129 (I initially had EKF2_AID_MASK = 128 because it is what it says in the documentation about that setup). If you tell me to do more concrete tests I can do them without problem.

By the way, is it possible to use the magnetometer as a backup in case heading by RTK has interference, is not accurate enough or fails? For example, if I activate the magnetometer too (EKF2_MAG_TYPE=1) does the autopilot use the data from the RTK and the magnetometer at the same time (or the more precise of the two) or does it only use one of them? Thanks.

[bresch]

@jalvarez96

it seems that modifying the estimator_interface.cpp code has no effect

Ok, so it seems that the change is not sufficient. EKF2_AID_MASK = 129 means: GNSS pos/vel fusion (lon/lat/alt + NED velocity) + GNSS heading fusion and is the most common case. EKF2_AID_MASK = 128 means: GNSS heading only (no pos/vel fusion). It is quite likely that this use case was not expected when developing the code. We should update the documentation to tell the user to use 129 or we need to fix the code if there if the 128 use case exists.

By the way, is it possible to use the magnetometer as a backup in case heading by RTK has interference, is not accurate enough or fails? For example, if I activate the magnetometer too (EKF2_MAG_TYPE=1) does the autopilot use the data from the RTK and the magnetometer at the same time (or the more precise of the two) or does it only use one of them?

Yes, when EKF2_MAG_TYPE = 1, 2 or 3, and GNSS heading is selected (EKF2_AID_MASK), then the GNSS heading is used as the primary source and the magnetometer is used as a fallback and will automatically switch to it if there is an issue with the primary source.