From d2f1349d1a52c9116fa92ff0f45d4c34e6c82d67 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 12 Apr 2022 14:30:28 -0400 Subject: [PATCH] sensors/vehicle_imu: replace coning metric with actual integrator coning correction (averaged) - this saves a relatively expensive higih rate cross product and gives better visibility into what's actually happening internally --- msg/vehicle_imu_status.msg | 6 ++-- src/modules/mavlink/streams/VIBRATION.hpp | 2 +- .../sensors/vehicle_imu/Integrator.hpp | 2 ++ .../sensors/vehicle_imu/VehicleIMU.cpp | 28 ++++++++++++------- .../sensors/vehicle_imu/VehicleIMU.hpp | 8 ++++-- 5 files changed, 30 insertions(+), 16 deletions(-) diff --git a/msg/vehicle_imu_status.msg b/msg/vehicle_imu_status.msg index 9e4c4c24f6..26db7487a7 100644 --- a/msg/vehicle_imu_status.msg +++ b/msg/vehicle_imu_status.msg @@ -14,9 +14,9 @@ float32 gyro_rate_hz float32 accel_raw_rate_hz # full raw sensor sample rate (Hz) float32 gyro_raw_rate_hz # full raw sensor sample rate (Hz) -float32 accel_vibration_metric # high frequency vibration level in the IMU delta velocity data (m/s) -float32 gyro_vibration_metric # high frequency vibration level in the IMU delta velocity data (m/s) -float32 gyro_coning_vibration # Level of coning vibration in the IMU delta angles (rad^2) +float32 accel_vibration_metric # high frequency vibration level in the accelerometer data (m/s/s) +float32 gyro_vibration_metric # high frequency vibration level in the gyro data (rad/s) +float32 delta_angle_coning_metric # average IMU delta angle coning correction (rad^2) float32[3] mean_accel # average accelerometer readings since last publication float32[3] mean_gyro # average gyroscope readings since last publication diff --git a/src/modules/mavlink/streams/VIBRATION.hpp b/src/modules/mavlink/streams/VIBRATION.hpp index 0ab42f8232..661c9528ac 100644 --- a/src/modules/mavlink/streams/VIBRATION.hpp +++ b/src/modules/mavlink/streams/VIBRATION.hpp @@ -84,7 +84,7 @@ private: if (x.copy(&status)) { if (status.accel_device_id == sensor_selection.accel_device_id) { - msg.vibration_x = status.gyro_coning_vibration; + msg.vibration_x = status.delta_angle_coning_metric; msg.vibration_y = status.gyro_vibration_metric; msg.vibration_z = status.accel_vibration_metric; break; diff --git a/src/modules/sensors/vehicle_imu/Integrator.hpp b/src/modules/sensors/vehicle_imu/Integrator.hpp index 646e305f41..0a670f74f4 100644 --- a/src/modules/sensors/vehicle_imu/Integrator.hpp +++ b/src/modules/sensors/vehicle_imu/Integrator.hpp @@ -188,6 +188,8 @@ public: _last_alpha.zero(); } + const matrix::Vector3f &accumulated_coning_corrections() const { return _beta; } + /* Reset integrator and return current integral & integration time * * @param integral_dt Get the dt in us of the current integration. diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 99e3e75a49..12d1f1a637 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -547,6 +547,8 @@ bool VehicleIMU::Publish() Vector3f delta_angle; Vector3f delta_velocity; + const Vector3f accumulated_coning_corrections = _gyro_integrator.accumulated_coning_corrections(); + if (_accel_integrator.reset(delta_velocity, imu.delta_velocity_dt) && _gyro_integrator.reset(delta_angle, imu.delta_angle_dt)) { @@ -554,17 +556,22 @@ bool VehicleIMU::Publish() // delta angle: apply offsets, scale, and board rotation _gyro_calibration.SensorCorrectionsUpdate(); - const float gyro_dt_inv = 1.e6f / imu.delta_angle_dt; - const Vector3f angular_velocity{_gyro_calibration.Correct(delta_angle * gyro_dt_inv)}; + const float gyro_dt_s = 1.e-6f * imu.delta_angle_dt; + const Vector3f angular_velocity{_gyro_calibration.Correct(delta_angle / gyro_dt_s)}; UpdateGyroVibrationMetrics(angular_velocity); - const Vector3f delta_angle_corrected{angular_velocity / gyro_dt_inv}; + const Vector3f delta_angle_corrected{angular_velocity * gyro_dt_s}; + + // accumulate delta angle coning corrections + _coning_norm_accum += accumulated_coning_corrections.norm() * gyro_dt_s; + _coning_norm_accum_total_time_s += gyro_dt_s; + // delta velocity: apply offsets, scale, and board rotation _accel_calibration.SensorCorrectionsUpdate(); - const float accel_dt_inv = 1.e6f / imu.delta_velocity_dt; - const Vector3f acceleration{_accel_calibration.Correct(delta_velocity * accel_dt_inv)}; + const float accel_dt_s = 1.e-6f * imu.delta_velocity_dt; + const Vector3f acceleration{_accel_calibration.Correct(delta_velocity / accel_dt_s)}; UpdateAccelVibrationMetrics(acceleration); - const Vector3f delta_velocity_corrected{acceleration / accel_dt_inv}; + const Vector3f delta_velocity_corrected{acceleration * accel_dt_s}; // vehicle_imu_status // publish before vehicle_imu so that error counts are available synchronously if needed @@ -591,6 +598,11 @@ bool VehicleIMU::Publish() Vector3f(_gyro_calibration.rotation() * _raw_gyro_mean.variance()).copyTo(_status.var_gyro); _raw_gyro_mean.reset(); + // Gyro delta angle coning metric = length of coning corrections averaged since last status publication + _status.delta_angle_coning_metric = _coning_norm_accum / _coning_norm_accum_total_time_s; + _coning_norm_accum = 0; + _coning_norm_accum_total_time_s = 0; + // gyro temperature _status.temperature_gyro = _gyro_temperature_sum / _gyro_temperature_sum_count; _gyro_temperature_sum = NAN; @@ -692,10 +704,6 @@ void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &angular_velocity) _status.gyro_vibration_metric = 0.99f * _status.gyro_vibration_metric + 0.01f * Vector3f(angular_velocity - _angular_velocity_prev).norm(); - // Gyro delta angle coning metric = filtered length of (angular_velocity x angular_velocity_prev) - const Vector3f coning_metric{angular_velocity % _angular_velocity_prev}; - _status.gyro_coning_vibration = 0.99f * _status.gyro_coning_vibration + 0.01f * coning_metric.norm(); - _angular_velocity_prev = angular_velocity; } diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index 40b94fcb54..2caf491b21 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -84,8 +84,9 @@ private: bool UpdateGyro(); void UpdateIntegratorConfiguration(); - void UpdateAccelVibrationMetrics(const matrix::Vector3f &acceleration); - void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity); + + inline void UpdateAccelVibrationMetrics(const matrix::Vector3f &acceleration); + inline void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity); void SensorCalibrationUpdate(); void SensorCalibrationSaveAccel(); @@ -144,6 +145,9 @@ private: vehicle_imu_status_s _status{}; + float _coning_norm_accum{0}; + float _coning_norm_accum_total_time_s{0}; + uint8_t _delta_velocity_clipping{0}; hrt_abstime _last_clipping_notify_time{0};