88f8da27ef
save learned mag bias per sensor (Multi-EKF support)
...
- handle saving the mag bias per sensor (across all estimator instances using that mag) in sensors/vehicle_magnetometer
- this is now saving back to the actual mag calibration CAL_MAGn_OFF{X,Y,Z}
- ekf2 reset mag mag bias on any magnetometer or calibration change
- use Kalman filter scheme to update stored mag bias parameters using all available bias estimates for that sensor
Co-authored-by: Paul Riseborough <gncsolns@gmail.com >
2021-01-07 09:54:13 -05:00
64ed96bd95
ekf2: Correct airspeed used by EKF for calibration errors
2021-01-05 19:22:53 -05:00
98cff94702
px4_work_queue: directly support SITL lockstep
...
- the purpose is to ensure that every WorkItem (and WorkItems scheduled
by WorkItems) is allowed to run to completion every step
- per workqueue register a lockstep component whenever a work item is
added (if not already registered)
- once the work queue is empty unregister component
2021-01-04 09:38:05 +01:00
4f62355dec
msg: new estimator_status_flags message for more accessible ekf2 status logging
...
- log all estimator (ekf2) flags as separate booleans in a new dedicated low rate message (only publishes at 1 Hz or immediately on any change)
- this is a bit verbose, but it avoids the duplicate bit definitions we currently have across PX4 msgs, ecl analysis script, flight review, and many other custom tools and it's much easier for casual log review in FlightPlot, PlotJuggler, csv, etc
- for compatibility I've left estimator_status filter_fault_flags, innovation_check_flags, and solution_status_flags in place, but they can gradually be removed as tooling is updated
Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com >
2020-12-29 11:27:21 -05:00
b23d8244f0
ekf2: fix dist bottom constraint
2020-12-16 09:33:38 -05:00
d44e537084
ekf2: update to new ecl to fix fault status getter
...
- estimator_status filter_fault_flags was broken because the union within ecl/EKF has exceeded 16 bits
2020-12-10 12:45:41 -05:00
82746af07a
ekf2: use new IMU and mag bias and variance getters in ecl/EKF
...
- reset IMU bias on calibration change (only works with multi-ekf)
- standardize PX4_INFO/PX4_WARN/PX4_ERR output for multi-ekf with instance number
- https://github.com/PX4/PX4-ECL/pull/924
2020-12-08 16:06:18 -05:00
59564af860
wind_estimate message: add identifier for source
...
Signed-off-by: Silvan Fuhrer <silvan@auterion.com >
2020-12-08 11:43:27 -05:00
681590c510
ekf2: multi-ekf force initial selection (and sensor_selection publication)
...
- depending on the particular sensor configuration and startup timing we were getting an initial sensor_selection publication without valid accel & gyro device ids (only applies to Multi-EKF)
- fix faulty accel and gyro device ids
- initialize unused test ratio fields to NAN for logging clarity
2020-12-07 21:37:24 -05:00
01620420fa
ekf2: update to latest ecl with RingBuffer minor improvements
...
- includes https://github.com/PX4/PX4-ECL/pull/938
2020-12-03 21:17:22 -05:00
3e4c38f6e8
ekf2: Fix bug causing excessive lane selection switching
2020-11-25 10:00:07 -05:00
5d7ea62190
estimator_innovations: remove unimplemented fake vel & pos fields
...
- easy to readd if and when they're needed
2020-11-11 20:17:52 -05:00
880dc6accd
ekf2: compile at MAX_CUSTOM_OPT_LEVEL
2020-11-11 20:17:52 -05:00
1e03285405
ekf2: avoid storing in_ground_effect state
2020-11-11 20:17:52 -05:00
26de630dc5
ekf2: remove landed flag and use control_status directly
2020-11-11 20:17:52 -05:00
89ab6a5dbf
ekf2: preflight checks only reset on STANDBY change
...
- avoid storing unnecessary state and call setVehicleCanObserveHeadingInFlight() directly
2020-11-11 20:17:52 -05:00
b9fff2c221
ekf2: sensor_selection only updated in non-multi mode
...
- publish attitude as soon as possible
2020-11-11 20:17:52 -05:00
7264577eec
ekf2: split perf counters (full update or not) and reorganize Run() slightly
2020-11-11 20:17:52 -05:00
a68fe4dbd7
ekf2: move parameter update to beginning of cycle
2020-11-11 20:17:52 -05:00
5ae7c80a18
ekf2: move rangeSample update to UpdateRangeSample()
2020-11-11 20:17:52 -05:00
991015c5c6
ekf2: move magSample update to UpdateMagSample()
2020-11-11 20:17:52 -05:00
68f49619b7
ekf2: move gps_message update to UpdateGpsSample()
2020-11-11 20:17:52 -05:00
0db706011a
ekf2: move FlowSample update to UpdateFlowSample()
2020-11-11 20:17:52 -05:00
8ee197acad
ekf2: move vehicle_odometry_aligned publication to PublishOdometryAligned()
2020-11-11 20:17:52 -05:00
b29f3c7ce5
ekf2: move ExtVisionSample update to UpdateExtVisionSample()
2020-11-11 20:17:52 -05:00
0f56793116
ekf2: move BaroSample update to UpdateBaroSample()
2020-11-11 20:17:52 -05:00
6b30f01daf
ekf2: move AuxVelSample update to UpdateAuxVelSample()
2020-11-11 20:17:52 -05:00
651e292d42
ekf2: move AirspeedSample update to UpdateAirspeedSample()
2020-11-11 20:17:52 -05:00
f4f3ba47a2
ekf2: move mag calibration to UpdateMagCalibration()
2020-11-11 20:17:52 -05:00
96178a6fb0
ekf2: publish yaw estimator status last
2020-11-11 20:17:52 -05:00
05de61ff15
ekf2: move estimator innovations publication to method
2020-11-11 20:17:52 -05:00
387186c11d
ekf2: move estimator innovation test ratios publication to method
2020-11-11 20:17:52 -05:00
3b9965ff8d
ekf2: move estimator innovation variances publication to method
2020-11-11 20:17:52 -05:00
94415a45fa
ekf2: move estimator status publication to method
2020-11-11 20:17:52 -05:00
03388f4656
ekf2: move estimator states publication to method
2020-11-11 20:17:52 -05:00
98334d1325
ekf2: move local position publication to method
2020-11-11 20:17:52 -05:00
c09a5e0d9f
ekf2: move global position publication to method
2020-11-11 20:17:52 -05:00
8616345346
ekf2: move estimator_sensor_bias publication to method
...
- only publish if there's a change in bias
- publish current estimated mag bias, including saved mag bias
2020-11-11 20:17:52 -05:00
e397cbf8bb
ekf2: move ekf_gps_drift publication to method
2020-11-11 20:17:52 -05:00
0d657c74bf
ekf2: rename publish helpers mixed case (google style guide)
2020-11-11 20:17:52 -05:00
4578b8cdcc
Update submodule ecl to latest Mon Nov 2 09:57:32 EST 2020
...
- ecl in PX4/Firmware (f87b19f31b9918f73d336d10e68d39c659037582): https://github/commit/48a8992caf7a95e09a5e17235133894c072b96bd
- ecl current upstream: https://github/commit/a21092804a784d012939f96cc633e7da5f5e8cde
- Changes: https://github/compare/48a8992caf7a95e09a5e17235133894c072b96bd...a21092804a784d012939f96cc633e7da5f5e8cde
a210928
2020-11-02 12:44:14 -05:00
a1ef4d1469
ekf2: only advertise immediately in multi-EKF mode
2020-11-01 15:48:35 -05:00
ecb462f325
ekf2: EKF2.cpp using matrix Eulerf, Quatf, Vector3f
2020-10-29 10:22:57 -04:00
d1af095c0b
ekf2: don't store vehicle_status_s
2020-10-29 10:22:57 -04:00
d27573b797
ekf2: don't store vehicle_land_detected_s
2020-10-29 10:22:57 -04:00
7757aeda94
ekf2: don't store sensor_selection_s copy
2020-10-29 10:22:57 -04:00
db5235b074
ekf2: avoid storing message copies
2020-10-29 10:22:57 -04:00
3f9f2c6fdf
ekf2: multi ekf supports up to 4 IMUs
2020-10-28 20:50:58 -04:00
2c874f1cd9
ekf2 selector fix SITL lockstep
...
- ekf2 selector don't register lockstep until first attitude publication
- sensors module register lockstep component
- enable multi-EKF2 in SITL
2020-10-28 09:42:41 -04:00
df2f26ebdf
rename vehicle_visual_odometry_aligned -> estimator_visual_odometry_aligned
...
- saves a small amount of work for the ekf2 selector in multi-EKF mode (visual_odometry_aligned now ignored)
- helps to distinguish the origin/purpose from vehicle_odometry and vehicle_visual_odometry
2020-10-27 12:33:39 -04:00
Daniel Agar