Required to prevent acquisition of GPS mid flight causing unwanted change in position and velocity
A distinction has been mad between the arm and disarm transition and the decision to use position hold mode (formerly static mode)
When regaining GPS after a timeout, an offset is applied when fusing GPS velocity so that GPS velocity and position data as fused by the EKF is kinematically consistent.
This velocity offset is also accounted for when fusing air data so that wind estimates are not corrupted when the GPS position offset is being pulle back to zero.
The intended behaviour is that the EKF position will be pulled back to the GPS position at a rate of 5m/s for planes and 1 m/s for copters. This avoids large deviations in trajectory when GPS is regained.
1) Un-used public methods to report height and position drifting have been removed
2) A time-out has been added to the airspeed innovation consistency check so that if we are relying on airspeed to constrain velocity drift, a filter divergence or other fault that causes the airspeed to be continually rejected will trigger a change in health status.
3) A timeout of velocity, position or height measurements does not cause a filter fault to be reported. Timeouts can be due to sensor errors and do not necessarily indicate that the filter has failed.
4) Time-outs of various measurements are used to present a consolidated bitmask which inidicates which parts of the solution can be used, eg attitude, height, velocity, relative position, absolute position, etc.
This provides the calling vehicle software the abiity to request the EKF to not use GPS.
An integer is returned that indicates the type of operation available:
0 = request rejected (request will only be accepted if the EKF is in static mode, eg pre-armed)
1 = request accepted, attitude, vertical velocity and position estimates available
2 = request accepted, attitude, height rate, height, horizontal velocity and relative position estimates available
Re-initialisation of the magnetic field states and yaw angle is now only performed a maximum of two times after start-up.
Once when coming out of static modefor the first time (first arm event)
Again (for copter only) when the altitude gain above the arming altitude exceeds 1.5m
this prevents magnetic interference present at arming (eg arming on a metal roof)from corrupting the magnetic field states enough to cause bad heading errors and toilet bowling on copter
(Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector.
Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight.
For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
GPS measurement variance is doubled if only 5 satellites, and quadrupled if 4 or less.
The GPS glitch rejection thresholds remain the same
This will reduce the impact of GPS glitches on attitude.
This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
If the vehicle can fly without a compass (a fly forward vehicle)
then if the compass times out (large errors for more than 10 seconds,
then it will be declared permanently failed and will not be
used until the filter is reset
Divergence is now detected by looking for very large changes in the gyro
bias. This will cause the filter to be reset and declared unhealthy for
10 seconds.
Don't reset filterDiverged status immediately during reset
Set filterDiverged true if covariance blows up
Add fault status reporting
priseborough
Jonathan Challinger
Andrew Tridgell
Emile Castelnuovo