When the mag is pulled away before takeoff, the EKF can end up learning
a bad yaw gyro bias. This bad gyro bias makes the estimated heading spin
and even if the bias continues to be learned, it can end up being stuck
in limit cycle due to the innovation changing its sign every time the
heading estimate overshoots the measurement.
To prevent this, the Z gyro bias variance is reset when the mag heading
fusion test ratio fails on ground.
We do not want to rely on a potentially badly initialized heading as it
could lead to a flyaway directly after takeoff
Also remove the resetMagHeading() call from the GPS fusion control logic
as this is properly handled in mag_control.
the starting condition needs more than 1 sample to determine if the data
comes regularly
the timeout on "no data" is defined by the fusion reset timeout
parameter (7 seconds)
A value of 1m/s was too conservative and the EKF takes too much time to
trigger a yaw reset. The value can be safely reduced because the
threshold is computed using the accuracy of the measurement and estimate
before being limited by this value.
In the normal and lowlight mode for the PAW3902/3 sensor, the ODR is 66Hz, while in super lowlight mode it is 50Hz.
The IMU ODR vs flow ODR mismatch threshold was too conservative for the 66Hz ODR, since https://github.com/PX4/PX4-ECL/pull/928. The EKF would keep waiting for the data to line up and never fuse measurements.
Fine tilt alignment is accomplished by fusing fake position data after
a coarse tilt leveling. The observation noise can be reduced if the
vehicle is at rest to speed up the process.
While a yaw test ration > 1 rejects the mag data in air, the fusion is
forced with a limited innovation on ground to fix gyro bias and local
magnetic effects problems pre-takeoff. However, the same logic is
applied after landing and immediately after the land detector detects
landing. In some cases, this can disturb the other EKF states which can
lead to a state change of the land detector.
To avoid such a loop, the start of this forced yaw fusion is delayed by
5 seconds after landing.
Paul Riseborough