A specialised takeoff check is now always performed when we receive new flow data as the default behaviour is to try and use flow data whenever it is received, rather than limit its use to a use to a flow-only mode of operation that had to be selected via user parameter.
Enables simultaneous use of GPS and optical flow data with automatic fallback to relative position mode if GPS is lost and automatic switch-up to absolute position status if GPS gained/re-gained.
Revert "AP_NavEKF2: Fix bug in published yaw reset value found during code review"
commit 175faf1e41.
Revert "AP_NavEKF2: use a struct for all yaw step class variables"
commit 77fad065d1.
Partially revert "AP_NavEKF2: Handle yaw jumps due to core switches"
commit 885bfd1b4e.
The EK2_RNG_USE_HGT parameter sets the height (expressed as a percentage of the maximum range of the range finder as set by the RNGFND_MAX_CM parameter) below which the range finder will be used as the primary height source when the vehicle is moving slowly.
When using a height reference other than GPS, the height datum can drift due to air pressure changes if using baro, or due to terrain height changes if using range finder as the primary height source. To ensure that a consistent height datum is available when switching between altitude sources, the WGS-84 height estimate of the EKF's local positi norigin is updated using a
single state Bayes estimator,
If rngfinder or gps height data is lost whilst being used, there will be a fall-back to baro data.
When switching over to a back up magnetometer, ensure that the earth field estimate are reset. other wise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected.
This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset.
If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding.
This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed.
An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS.
Switching in and out of aiding modes was being performed in more than one place and was using two variables.
The reversion out of GPS mode due to prolonged loss of GPS was not working.
This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function.
The filter status logic calculations were being repeated every time the get function was called.
The logic is now updated once per filter update step and a separate get function added
If we start GPS aiding before the gyro bias variances have reduced, glitches on the GPS can cause attitude disturbances that degrade flight accuracy during early flight.
Co-variances were being re-zeroed after being set. This meant that the initial declination learning was sensitive to measurement errors which could result in poor initial yaw accuracy.
Fixes bugs that prevented planes being able to reset yaw to GPS to recovery from takeoff with a bad magnetoemter.
1) If the velocity innovation check had not failed by the time the in-air transition occurred, then the yaw reset would not be performed
2) The velocity states were not being reset
3) The non fly-forward vehicle (copter) reset could occur first and effectively lock out the fly-forward vehicle (plane) yaw check.
Remember the mag bias and earth field states learned during flight when the vehicle lands.
This improves performance for vehicles that do multiple flight on one power cycle
Provide consistent overshoot of 5% across a wider range of time constants and prevent selection of larger time constants causing 'ringing' in the position and velocity outputs.
Mathieu OTHACEHE
priseborough
Jonathan Challinger
Francisco Ferreira
Randy Mackay
Andrew Tridgell