This avoids creating two pointers of different types to the same memory.
Having two pointers to the same memory can lead to the compiler
optimising code such that a write to one pointer is rearranged to be
either before or after a read from the other pointer depending on which
is deemed faster - not a good outcome.
Fix rounding error bug preventing state from updating after initial convergence.
Decouple GPS reference height from published EKf origin height.
Add bitmask parameter to control update and publishing of GPS reference height.
this changes the stragegy for load levelling between EKF cores so it
works between EK2 and EK3, and with future estimators as well.
It allows us to run EK3 and EK2 at the same time with good scheduling
performance
If the GPS receiver selection changes and we are using GPS for height, the
vertical position will be reset to the new GPS height measurement.
correct output observer history when doing a GPS height reset
1) Do not switch to a core until its states have been updated.
2) Distinguish between mandated switches required for health failure and optional switches required due to innovation checks failing.
3) Apply hyseresis to innovation check levels
This can improve position hold performance where it is not practical to have the IMU located at the centroid.
Although this enables the effect of IMU position offsets to be corrected, users will still need to be instructed to place the IMU as close to the vehicle c.g. as practical as correcting for large offsets makes the velocity estimates noisy.
Correction requires the body rates averaged across the flow sensor sampling interval. This data has been added to the sensor buffer.
The body rate data from the flow sensor driver does not contain the Z component, so an equivalent value sampled from the navigation IMU has been used instead.
The variable omegaAcrossFlowTime has been moved out of the class and into the only function that uses it.
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.
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.
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
