Recently I’ve noticed quite a strange behaviour with a plane - it sometimes re-aligns yaw to GPS velocity even without having GPS fix available.
I’ve been testing plane behaviour in a scenario of GPS signal absence by using the GPS disable AUX function. I’m aware it only changes the GPS fix status and does not wipe rest of the data like sat count, xDOP, etc
This is Matek H743 slim with Matek M10-L4-3100 periph (RM3100 compass + ublox M10) running Plane v4.5.2
On the image below I’m temporarily re-enabling GPS so the correct position is fetched after flying GPS-less for some time and, as expected, the yaw is re-aligned to GPS velocity
Then after flying for a bit I disable GPS via AUX function and it still re-aligns yaw to GPS velocity with GPS being in no fix state for couple of mins at that point and this causes the trajectory jump
I started checking code for the GPS data (location, speed etc) usage to find where the data maybe leaking into EKF and it seems like all of it is wrapped in checks for fix present.
Then I went looking specifically for yaw reset and noticed a potential issue in EKF3 code here:
A small excerpt from the EKF3 fusion code:
// Read GPS data from the sensor
readGpsData();
readGpsYawData();
// get data that has now fallen behind the fusion time horizon
gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);
if (gpsDataToFuse) {
...
// we have GPS data to fuse and a request to align the yaw using the GPS course
if (gpsYawResetRequest) {
realignYawGPS(false);
}
...
The EKF code reads fresh GPS data in readGpsData()
and pushes it to the storedGPS
ringbuffer for fusion afterwards, but only if GPS fix was OK.
Later it recalls the delayed GPS data from ringbuffer and all accesses to gpsDataDelayed
are wrapped with a check for gpsDataToFuse
.
Except for realignYawGPS()
, which uses the gpsDataDelayed
inside to check the yaw error, but there is no check if whatever stored in gpsDataDelayed
is in fact a newly recalled data, and not an old sample still stored in that variable. This can possibly result in yaw re-alignment to some old GPS data (whenever the last sample with GPS fix ok was)
And I think this is what causes the behaviour I observe, which happened on several flights now in similar testing scenario. What do you guys think?