Prolonged yaw rotations with gyro scale factor errors can cause yaw errors and gyro bias estimation errors to build up to a point where EKF health checks fail.
This patch introduces the following protections:
1) The assumed yaw gyro error is scaled using a filtered yaw rate and an assumed 3% scale factor error (MPU6000 data sheet)
2) When the filtered yaw rate magnitude is greater than 1 rad/sec, the Z gyro bias process noise is zeroed and the state variance set to zero to inhibit modification of the bias state
3) When the filtered yaw rate magnitude is greater than 1 rad/sec, the magnetometer quaternion corrections are scaled by a factor of four to maintain tighter alignment with the compass
Increases magnetometer weighting on yaw corrections when there there is no other aiding to constrain yaw drift.
Prevents switch to GPS if magnetometer data is failing innovation checks which indicates a bad yaw angle
The interface definition has been modified so that it returns true for a position obtained usin geither the normal inertial navigation calculation, or a raw GPS measurement.
This enables this function to be used to set a home position before flight.
If a calculated position is not available, the function will return a value based on raw GPS or last calculation if available, but the status will be set to false to indicate that it cannot be used for control.
The inconsistent baro data during ground effect takeoff combined with the larger variances in the Z accel bias state early in flight can cause unwanted changes in bias estimate and therefore changes in height estimation error.
This patch turns of the process noise and state updates for the Z accel bias state when takeoff in ground effect is expected.
Thsi fixes a potential bug where the vehicle could land at a lower location without disarming and re-enter ground effect takeoff mode wiht a baro height floor above the current altitude, causing unpredictable height gain
Ground effect baro errors can cause a spike in height innovation on disarming if ground effect compensation was used during the landing. This causes a transient AHRS fault message if this innovation is outside the pre-arm check limits.
Resetting the vertical position state to the measured height after disarming prevents this.
This estimate is used to offset baro data if we need to switch across from range finder data due to sensor failure. The previous filter coefficients gave a 0.5 seconds time constant on the offset which was too susceptible to baro noise.
Range finder and optical flow data can drop-out and be reliable very close to ground. these patches enable the takeoff to be more relaibly detected and constrain optical flow navigation drift in the first part of takeoff.
Paul Riseborough
Tom Pittenger
Jonathan Challinger
Andrew Tridgell
Randy Mackay
Arthur Benemann