Commit Graph

687 Commits

Author SHA1 Message Date
bresch 9fc331b7ea ekf: requires kinematically consistent range finder data to continue terrain aiding 2022-04-13 11:41:48 +02:00
bresch 78211f9dbb ekf: improve rng consistency check
To pass from invalid to valid:
- time hysteresis
- some vertical velocity
- test ratio < 1
- low-passed signed test ratio < 1

To pass from valid to invalid:
- low-passed signed test ratio >= 1
2022-04-13 11:41:48 +02:00
bresch b1ea2e4e15 ekf: use same gate for innov and innov sequence monitoring 2022-04-13 11:41:48 +02:00
bresch f96287b80a ekf: access member variable without getter 2022-04-13 11:41:48 +02:00
bresch 904bf8ef9f ekf: add range finder kinematic consistency check
At each new valid range measurement, the time derivative of the distance
to the ground is computed and compared with the estimated velocity.
The average of a normalized innovation squared statistic check is used
to detect a bias in the derivative of distance measurement,
indicating that the distance measurements are kinematically inconsistent
with the filter.
2022-04-13 11:41:48 +02:00
bresch 064518f57a ekf: extract range finder noise computation 2022-04-13 11:41:48 +02:00
Daniel Agar 8166a500ac Update world_magnetic_model to latest Mon Apr 11 11:14:11 UTC 2022 (#19475) 2022-04-12 10:11:51 -04:00
Daniel Agar bb2ea574aa ekf2: properly reset IMU biases on calibration change (non-multi-EKF)
- this was working in the multi-EKF case using vehicle_imu, but missing
in sensor_combined
2022-04-11 12:23:55 -04:00
Daniel Agar 04f37222f8 ekf2: fix IMU missed perf count when not using multi-EKF 2022-04-10 11:07:33 -04:00
Daniel Agar 0c31f63896 sensors: add baro calibration and cleanup
- sensor_baro.msg use SI (pressure in Pascals)
 - update all barometer drivers to publish directly and remove PX4Barometer helper
 - introduce baro cal (offset) mainly as a mechanism to adjust
relative priority
 - commander: add simple baro cal that sets baro offsets to align with
GPS altitude (if available)
 - create new sensors_status.msg to generalize sensor reporting
2022-03-26 16:08:41 -04:00
Daniel Agar f4c3084c26 init adjustments to ensure used topics are advertised early (primarily for logging)
- multi-EKF create each instance as soon as IMU & mag are advertised (before device id populated)
2022-03-23 16:51:16 -04:00
Daniel Agar 7ef38112d2 ekf2: return saved mag bias variance when not in 3d magnetometer fusion
- the estimated mag bias was requiring > 30 seconds of continuous 3d
mag fusion to be reported stable (and saved back to mag cal), this
restores the original intent requiring 30 seconds of accumulated valid
3d mag fusion
2022-03-14 21:03:41 -04:00
Daniel Agar 5717434e93 ekf2: initialize GPS drift metrics to NAN rather than 0 2022-03-14 10:09:24 -04:00
Daniel Agar a430f0ccae ekf2: add simple zero velocity update when vehicle at rest (#19149)
- further decreases initial tilt alignment time (now down to 2.5 seconds if still) and improves initial bias estimates
2022-03-12 12:56:31 -05:00
wangwwno1 cd5a1e510a ekf2: typo Fix: pub.advertised() -> pub.advertise() (#19302) 2022-03-09 08:39:08 +01:00
Daniel Agar b7e0f17c6a ekf2: minor position/velocity reset cleanup
- try to use avoid resetVelocity() call where possible
 - reset timeouts centrally
2022-03-08 11:16:35 -05:00
Daniel Agar c10ea97967 ekf2: fusion helpers return success/fail and set pos/vel update timestamps centrally (if healthy) 2022-03-08 11:16:35 -05:00
Daniel Agar 6224e11463 Update world_magnetic_model to latest Sat Feb 26 16:47:08 UTC 2022 (#19262)
* Update world_magnetic_model to latest Sat Feb 26 16:47:08 UTC 2022
* [AUTO COMMIT] update change indication

Co-authored-by: PX4 BuildBot <bot@px4.io>
2022-02-26 15:39:37 -05:00
Daniel Agar 11f617ca9b ekf2: default to in air and not at rest
- this is a more conservative default if a vehicle isn't set (no land detector running)
 - handled horizontal preflight failures in commander when disarmed
rather than overloading xy_valid and v_xy_valid flags
 - ekf2 no longer depend on arming or standby status
2022-02-22 16:59:10 -05:00
Daniel Agar d6d529539d ekf2: consolidate and simplify gnd effect logic 2022-02-22 16:59:10 -05:00
Daniel Agar 1d7791dad6 commander: monitor GPS validity and EKF2 dead reckoning
- ekf2: expose dead reckoning as control status flags
 - commander:
    - add GPS validity check
    - in AUTO MISSION if dependent on GPS then a loss of GPS will
2022-02-22 09:46:21 -05:00
bresch 0e0e0d8be7 [AUTO COMMIT] update change indication 2022-02-22 11:49:25 +01:00
bresch 4f0a959244 gps_hgt: add and improve unit tests 2022-02-22 11:49:25 +01:00
bresch edabfd2f0e ekf_hgt: call specific height reset function instead of generic one
- Also use the delayed current data instead of newest that might not be
available (gps buffer is sometimes empty if the dt between samples is
larger than the delayed horizon).

- Separate "baro fault" from "baro intermittent": intermittent is a
  temporary failure and should prevent from switching to baro right now,
  but "fault" means that it should never be used anymore

- In case of height timeout, check for metrics but not for consistency
  as the filter is likely to have diverged already.
2022-02-22 11:49:25 +01:00
bresch cba73585e1 use recorded last sensor timestamp for intermittent check 2022-02-22 11:49:25 +01:00
Daniel Agar f431b233f3 ekf2: preflight checker fix flow y innovation filter 2022-02-22 09:39:22 +01:00
Daniel Agar 591b7b6934 ekf2: add new estimator_gps_status.msg
- includes the estimator status check fail bits broken out as descriptive booleans
 - absorbs ekf_gps_drift.msg
2022-02-21 08:58:59 -05:00
Daniel Agar f8d7574d3c ekf2: simplify global position publication validity check 2022-02-21 08:57:33 -05:00
Jukka Laitinen 9449ed6e66 Add support for protected build in drivers, systemcmds and modules Kconfig
Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
2022-02-18 07:58:58 +01:00
Daniel Agar 5370733d62 ekf2: publish flow vel only if compensated flow is available
- fix a few publication timestamp_samples
2022-02-14 10:55:10 -05:00
Daniel Agar b157afde6a ekf2: prefer airspeed_validated over raw airspeed if available (#19159)
* ekf2: prefer airspeed_validated over raw airspeed if available

Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
2022-02-14 15:37:29 +01:00
bresch 493e35b72e ekf_terr: reset rng fault when on ground 2022-02-11 10:57:45 -05:00
bresch 502ec7ef46 ekf_terr: fix unit test
the flags were valid ony because they were based on timeouts and when
starting the unit test, it takes a couple of seconds to actually timeout
2022-02-11 10:57:45 -05:00
bresch 2fb7b35a8b ekf2_terr: refactor terrain estimator - flow aiding 2022-02-11 10:57:45 -05:00
bresch 33fd1849e0 ekf2_terr: refactor terrain estimator - rng aiding 2022-02-11 10:57:45 -05:00
bresch 5818974f0f ekf: add range finder "faulty" status
When delclared faulty, the range finder cannot be used anymore
2022-02-11 10:57:45 -05:00
Daniel Agar 893cdf8f38 ekf2: test external vision adjustments after lowering fake position fusion variance 2022-02-04 21:48:32 -05:00
Daniel Agar 6b1750d8be ekf2: lower fake position observation variance when at rest 2022-02-04 21:48:32 -05:00
bresch 917910f3e2 ekf: pass a float by copy instead of a constant ref 2022-02-02 17:43:25 -05:00
bresch 3077f27821 ekf: use float instead of Vector2f for innov gate in pos/vel fusion
We always use the same innovation gate for X and Y anyway
2022-02-02 17:43:25 -05:00
Daniel Agar 5fb0084524 ekf2: remove unused shared fields for last velocity observation and variance 2022-02-02 17:43:25 -05:00
Daniel Agar 8a0581516c ekf2: don't store _auxvel_sample_delayed 2022-02-02 17:43:25 -05:00
Daniel Agar 452b5e94b4 ekf2: don't store _drag_sample_delayed 2022-02-02 17:43:25 -05:00
Daniel Agar 88ffc177f7 ekf2: utils add getEulerYaw() that uses the best rotation sequence 2022-02-02 08:54:03 -05:00
Daniel Agar c3e0b93fc8 ekf2: remove unnecessary inlines 2022-02-02 08:50:49 -05:00
Daniel Agar f3e2a197ad move vehicle at rest detection ekf2 -> land_detector
- move vehicle at reset detection ekf2 -> land_detector
 - ekf_unit: reduce init period
   - Fake fusion is when at rest is quite strong and makes the variance reduce rapidly. Reduce the amount of time we wait before checking if the variances are still large enough.
 - ekf_unit: reduce minimum vel/pos variance required after init
   - Fake pos fusion has a low observation noise, making the vel/pos variances reduce quickly.

Co-authored-by:: bresch <brescianimathieu@gmail.com>
2022-02-01 17:50:19 -05:00
Daniel Agar 999737ddd5 ekf2: refactor mag control
- 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
2022-01-31 11:47:10 -05:00
Daniel Agar 90358f078f add odometry reset counter (ekf2, mavlink, etc) 2022-01-31 09:19:36 -05:00
Daniel Agar 49d9d1c987 ekf2: only set fault status bad_acc_clipping if clipping frequently 2022-01-27 09:59:50 -05:00
Daniel Agar f499749c23 ekf2: mag field strength check respect requirements
- requires valid mag data (_mag_data_ready)
 - respect mag_fusion_type (including MAG_FUSE_TYPE_INDOOR)
2022-01-27 09:05:13 -05:00