Commit Graph

291 Commits

Author SHA1 Message Date
Daniel Agar d5839e2dd5 optical flow sensor pipeline overhaul
- all sources of optical flow publish sensor_optical_flow
 - sensor_optical_flow is aggregated by the sensors module, aligned with integrated gyro, and published as vehicle_optical_flow

Co-authored-by: alexklimaj <alex@arkelectron.com>
2022-06-20 20:56:56 -04:00
Daniel Agar b81a5b3efa ekf2: request mag yaw reset after calibration or sensor change 2022-06-17 21:04:05 -04:00
Daniel Agar 1980b5c5e8 ekf2: setEkfGlobalOrigin() reset baro and hgt sensor offsets if necessary
- handle uninitalized _gps_alt_ref
 - add basic lat/lon/alt sanity checks
2022-06-16 00:59:54 -04:00
bresch 0d256b8ff6 ekf2 wind: use noise spectral density for process noise tuning
The noise spectral density, NSD, (square root of power spectral density) is a
continuous-time parameter that makes the tuning independent from the EKF
prediction rate.
NSD corresponds to the rate at which the state uncertainty increases
when no measurements are fused into the filter.
Given that the current prediction rate of EKF2 is 100Hz, the
same tuning is obtained by dividing the std_dev legacy parameter by 10:
nsd = sqrt(std_dev^2 / 100Hz)
2022-06-14 18:39:10 +10:00
bresch 4f1091792f ekf2 preflight: only check innovation of active height sources 2022-06-07 11:44:56 -04:00
Daniel Agar 3889b79342 ekf2: yaw estimator add yaw_composite_valid boolean 2022-06-01 13:16:06 -04:00
Daniel Agar 58ea6235fe ekf2: estimator aid source status (range height) 2022-05-25 09:25:12 -04:00
Daniel Agar 1e25aee6fa ekf2: estimator aid source status (baro height) 2022-05-25 09:25:12 -04:00
Daniel Agar 639222dd65 ekf2: estimator aid source status (GPS pos, GPS vel) 2022-05-23 16:21:49 -04:00
Daniel Agar 1ae467e9cd ekf2: estimator aid source status (starting with fake position) 2022-05-23 16:21:49 -04:00
mcsauder ea10eacb99 Replace EKF/common.h #defines with enums. 2022-05-18 09:25:19 -04:00
Daniel Agar d1d15a6f6d differential pressure remove filters from drivers and average in sensors/airspeed 2022-04-26 08:39:19 -04:00
bresch 4c03f0bc50 ekf: make range finder kin consistency gate tunable by parameter 2022-04-13 11:41:48 +02:00
bresch d903613c9c ekf: add logging for rng kinematic consistency check 2022-04-13 11:41:48 +02:00
bresch 8693ad15a7 ekf: add logging of range finder consistency check 2022-04-13 11:41:48 +02: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
wangwwno1 cd5a1e510a ekf2: typo Fix: pub.advertised() -> pub.advertise() (#19302) 2022-03-09 08:39:08 +01: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
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
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 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 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 90358f078f add odometry reset counter (ekf2, mavlink, etc) 2022-01-31 09:19:36 -05:00
Daniel Agar ae3070bbf1 ekf2: make filter update period configurable
- introduces new parameter EKF2_PREDICT_US to configure the filter
update period in microseconds
 - actual filter update period will be an integer multiple of IMU
2022-01-26 10:57:09 -05:00
Daniel Agar 92a5bbe97f ekf2: improve ring buffer sizing and dynamically allocate 2022-01-19 09:04:53 -05:00
Daniel Agar 45040be669 ekf2: accel/gyro/mag sensor cal minor cleanup
- cleanup obsolete comments
 - remove debug helpers
 - add additional variance requirements to mag bias stable (matching accel & gyro)
2022-01-10 09:55:59 -05:00
Julian Oes 952cfe2fd7 ekf2: fix uninitalized memory warning
The imu and sensor_combined data should not be used when it has not been
updated yet, otherwise this triggers a memory sanitizer warning:

Conditional jump or move depends on uninitialised value(s)
   at 0x2DA7AA: __sanitizer_cov_trace_const_cmp1 (in build/px4_sitl_default-clang/bin/px4)
   by 0x3C4E79: EKF2::Run() (src/modules/ekf2/EKF2.cpp:401)
   by 0x6EB881: px4::WorkQueue::Run() (platforms/common/px4_work_queue/WorkQueue.cpp:187)
   by 0x6ECB9D: px4::WorkQueueRunner(void*) (platforms/common/px4_work_queue/WorkQueueManager.cpp:230)
   by 0x4C07258: start_thread (in /usr/lib/libpthread-2.33.so)
   by 0x4D415E2: clone (in /usr/lib/libc-2.33.so)

Conditional jump or move depends on uninitialised value(s)
   at 0x3C4E7C: EKF2::Run() (src/modules/ekf2/EKF2.cpp:401)
   by 0x6EB881: px4::WorkQueue::Run() (platforms/common/px4_work_queue/WorkQueue.cpp:187)
   by 0x6ECB9D: px4::WorkQueueRunner(void*) (platforms/common/px4_work_queue/WorkQueueManager.cpp:230)
   by 0x4C07258: start_thread (in /usr/lib/libpthread-2.33.so)
   by 0x4D415E2: clone (in /usr/lib/libc-2.33.so)
2022-01-05 16:20:06 -05:00
Daniel Agar 170849c8f8 ekf2: expand accel bias stability criteria 2022-01-04 13:57:49 -05:00
bresch d094fbbd70 ekf2: fix delta angle coning metric 2022-01-04 09:28:48 -05:00
Daniel Agar 333edfe12f ekf2: use delayed IMU timestamp for publication's timestamp_sample 2022-01-03 09:20:44 -05:00
Thomas Debrunner 8db7a6225b geo: Moved the map_projection_* functions and state into a self-contained C++ class 2021-12-06 14:30:14 +01:00
Thomas Stastny 8defbc8829 update correct airsp scale param string in ekf2 2021-11-25 11:01:04 -05:00
Jukka Laitinen 5dbbddb13f EKF2: Use _distance_sensor_subs vector directly
Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
2021-11-24 09:11:45 +01:00
Jukka Laitinen d9498ade32 EKF2: Allocate distance sensor subscriptions as member variables
Just create the subscriptions and keep them, instead of
re-creating them continuously

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
2021-11-24 09:11:45 +01:00
bresch f751dd2242 FlightTask: set yaw_setpoint to NAN when yaw should not be controlled 2021-11-16 18:09:37 -05:00
Daniel Agar 4e8c234a55 ekf2: only save mag declination if changed 2021-11-16 12:33:48 -05:00
Beat Küng 4ba84d56c9 logger: introduce optional topics
The current practice of adding topics to the default set isn't scalable,
as it affects all setups.
By making sure topics are advertised on init, logger can just discard
topics that don't exist. This does not work for all topics, so topics are
specifically marked as optional. It can be extended to more topics later
on though.

This reduces the list of topics by ~35 on a pixracer configured as quad,
and reduces RAM usage by ~1KB.
2021-11-15 09:02:03 -05:00
Daniel Agar 68026eadeb save significant IMU bias changes learned by the EKF
* ekf2: make publishing of learned accel biases more robust
* ekf2: reset accel bias if calibration updated
* msg: add separate accel and gyro calibration counters
* ekf2: use separate accel and gyro calibration counters
* ekf2: rework logic to reset biases when calibration counters increment
* sensors: add saving of learned accel biases
* ekf2: generalized saving accel/gyro/mag in flight sensor calibration
* boards: holybro kakutef7 disable systemcmds/perf and systemcmds/top to save flash

Co-authored-by: Paul Riseborough <gncsolns@gmail.com>
2021-11-07 15:34:27 -05:00
bresch 6e8f0e92ff ekf2: refactor airspeed fusion control logic 2021-10-26 10:18:56 +02:00
Shubham Shah c92cd65831 Update EKF2.cpp 2021-10-09 19:39:43 -04:00
Daniel Agar 06c2d14903 ekf2: multi-mode allow first instance to save EKF2_MAG_DECL 2021-08-19 10:14:35 +02:00
David Sidrane e8c418caaf EKF:Use inttypes 2021-08-13 14:22:00 -04:00
Daniel Agar fb4ac0f08c ekf2: remove redundant IMU vibration metrics
- consume IMU vibration metrics from vehicle_imu_status
2021-08-08 06:26:03 -07:00