Heading data is already available in "float" mode but really inaccurate
(can be off by 45 degrees). It's then better to not use it when in
"float" mode as it will disturb the EKF2's estimate and the filter can
continue to keep an accurate heading even without direct observations.
- refactor all EKF backend output predictor pieces into new OutputPredictor class
- output states are now calculated immediately with new high rate IMU rather than after EKF update
- IMU delayed sample is passed as around as control data to avoid storing an extra copy and make the requirement clear
- move EV yaw and EV position to new state machines
- EV yaw and EV pos now configured via EKF2_EV_CTRL (migrated from EKF2_AID_MASK)
- new EV position offset estimator to enable EV position while GPS position is active (no more EV pos delta fusion)
- yaw_align now strictly means north (no more rotate external vision aid mask)
- automatic switching between EV yaw, and yaw align north based on GPS quality
- don't store unnecessary IMU copy in class, pass it through where required
- remove custom pi constants
- remove unused fields from ahrs_ekf_gsf (vel_NE, fuse_gps, accel_dt)
- explicitly initialize everything in header
- apply PX4 code style
- set GPS velocity before yaw estimator update, not after
During the whole flight, if the difference between the yaw estimate from
EKF2 and the emergency estimator is large and that the GNSS velocity
fusion is failing continuously, immediately reset to the emergency yaw
estimate.
- working towards keeping all height source (baro/ev/gnss/rng) handling as consistent as possible, possibly refactoring these out into separate classes later
- a growing number of samples come into the backend with the time
already delayed (sensor's interrupt setting timestamp sample)
- if the incoming timestamp is already delayed then the new data checks
(relative to latest IMU) can be slightly wrong
- handle almost all timestamps and checks on delayed time horizon,
except for explicit checks of new samples
- isRecent() and isTimedOut() helpers use delayed time
- add new isNewestSampleRecent() used for checking the incoming
timestamp of the incoming (adjusted) data
Also remove the legacy "range aid" than can be achieved by setting the
height reference to range finder and the range finder control parameter
to "conditional".
Conditional range aiding cal also be set when the height reference isn't
the range finder. This prevents the ratchetting effect due to switching
between references.
- remove class _mag_sample_delayed
- update mag fusion call graph to use mag sample delayed functionally
- Ekf::resetMagHeading()
- use low pass mag directly, but check if valid (magnitude)
- MAG_FUSE_TYPE_INDOOR treat like auto if heading required
The existing logic using the angle between velocity vectors failed to
determine a yaw failure in practice because the state velocity is often
too small compared to its uncertainty to be used. In all the failures
reported, the yaw emergency estimator converged properly and yaw reset
would have fixed the issue.
A much simpler check using the yaw difference between the main EKF
and the emergency estimator is now used to tell if the vel/pos update
failure is most likely caused by a wrong heading.
bresch