- avoid unnecessary filter reset on parameter update
- additional minor optimizations (precomputing dt inverse, etc)
- moving filter reset check and dynamic notch filter update checks out of the update loops
- this were necessary previously when the scale factor wasn't applied prior to filtering the otherwise raw data
Having a 2nd order low-pass filter in the derivative loop reduces
stability at low cutoff values as too much phase is lost through
the filter. Using a 1st order filter avoids this issue because its
maximum phase loss is 90 degrees instead of 180 degrees for a 2nd order
lpf.
- initial frequency peak tracking SNR increased from 10->15 db
- after initial detection the threshold decreases to SNR 5db
- gyro_fft large method refactored into smaller pieces
- sensors/vehicle_angular_velocity: dynamic notch FFT make sample rate
check a percentage and relax lower bound safety threshold
- apply sensor scaling immediately to keep things simple (FIFO vs regular)
- inline filter helpers (minor performance improvement)
- dynamic notch filtering
- reorder by axis (applied per axis)
- don't remove notch filters immediately if ESC or FFT data times out
- constrain notch filter frequency and bandwidth to safe range (minimum bandwidth for flaot precision, Nyquist, etc)
- add safe constraint on dt
- merge NotchFilterArray into regular NotchFilter (apply vs applyArray)
- only use direct form 1 to prevent reset confusion
- safe default field initialization
- update VehicleAngularVelocity usage
For the sake of efficiency (at 8 kHz) all filtering is performed on the raw data before the calibration and rotation is applied to only the final output. As a result we have to be a bit more careful when switching between sensors or in cases where the gyro scale factor changes (eg icm42688p 20 bit data rescaled to fit in int16 output).
- rotates accel & gyro FIFO data before publication both to simplify downstream usage (including log review) and fix other issues
- to best handle int16_t data rotations are now either performed with swaps if possible, otherwise promoted to float, rotated using the full rotation matrix, then rounded back to int16_t
- fix sensors/vehicle_angular_velocity filter reset both with proper rotation and new calibration uncorrect helper
- in FIFO case filtering is done before calibration is applied, but we need to handle a possible reset from a completely different sensor (vehicle body angular velocity -> sensor frame uncorrected data)
- parameter updates can be quite expensive because they trigger nearly all modules to reload all of their parameters immediately
- limit modules from updating faster than once per second
- ekf2 can now run in multi-instance mode (currently up to 9 instances)
- in multi mode all estimates are published to alternate topics (eg estimator_attitude instead of vehicle_attitude)
- new ekf2 selector runs in multi-instance mode to monitor and compare all instances, selecting a primary (eg N x estimator_attitude => vehicle_attitude)
- sensors module accel & gyro inconsistency checks are now relative to the mean of all instances, rather than the current primary (when active ekf2 selector is responsible for choosing primary accel & gyro)
- existing consumers of estimator_status must check estimator_selector_status to select current primary instance status
- ekf2 single instance mode is still fully supported and the default
Co-authored-by: Paul Riseborough <gncsolns@gmail.com>
- remove all remaining IOCTLs for accel and gyro and handle all calibration entirely in sensors module with parameters
- sensor_accel and sensor_gyro are now always raw sensor data
- calibration procedures no longer need to first clear existing values before starting
- temperature calibration (TC) remove all scale (SCL) parameters
- gyro and baro scale are completely unused
- regular accel calibration scale can be used (CAL_ACC*_xSCALE) instead of TC scale
Greatly improves the noise on the d-term on a Kopis 2. Apparently the order
of filtering vs differentiating matters.
Also disables the DGYRO filter by default, as the D-term now already has
a default filter of 30Hz applied.
- IMU integration move from drivers (PX4Accelerometer/PX4Gyroscope) to sensors/vehicle_imu
- sensors: voted_sensors_update now consumes vehicle_imu
- delete sensor_accel_integrated, sensor_gyro_integrated
- merge sensor_accel_status/sensor_gyro_status into vehicle_imu_status
- sensors status output minor improvements (ordering, whitespace, show selected sensor device id and instance)
Daniel Agar