- 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)
- 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>
- vehicle_imu/vehicle_magnetometer add monotonically increasing `calibration_count` field so that downstream subscribers are aware of calibration changes
- this allows the default priority to be set differently for internal/external sensors
- accel and gyro initialize default priority like mag in preparation for fully supporting external sensors
- fixes#15652
Beat Küng