From f2ae8ae81466b54cc725c07aa8467991bf74c541 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 22 Jul 2021 09:44:21 -0400 Subject: [PATCH] sensors/vehicle_imu: vibration metrics use acceleration and angular velocity - this avoids any jitter in the integration timespan from impacting the vibration metrics - vehicle_imu_status vibration metrics are not consumed by anything (yet), so changing the scaling of the metric shouldn't matter --- .../sensors/vehicle_imu/VehicleIMU.cpp | 35 ++++++++++--------- .../sensors/vehicle_imu/VehicleIMU.hpp | 8 ++--- 2 files changed, 22 insertions(+), 21 deletions(-) diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index fb31014f92..cad55e2c15 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -423,15 +423,16 @@ 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 delta_angle_corrected{_gyro_calibration.Correct(delta_angle * gyro_dt_inv) / gyro_dt_inv}; + const Vector3f angular_velocity{_gyro_calibration.Correct(delta_angle * gyro_dt_inv)}; + UpdateGyroVibrationMetrics(angular_velocity); + const Vector3f delta_angle_corrected{angular_velocity / gyro_dt_inv}; // delta velocity: apply offsets, scale, and board rotation _accel_calibration.SensorCorrectionsUpdate(); const float accel_dt_inv = 1.e6f / imu.delta_velocity_dt; - Vector3f delta_velocity_corrected{_accel_calibration.Correct(delta_velocity * accel_dt_inv) / accel_dt_inv}; - - UpdateAccelVibrationMetrics(delta_velocity_corrected); - UpdateGyroVibrationMetrics(delta_angle_corrected); + const Vector3f acceleration{_accel_calibration.Correct(delta_velocity * accel_dt_inv)}; + UpdateAccelVibrationMetrics(acceleration); + const Vector3f delta_velocity_corrected{acceleration / accel_dt_inv}; // vehicle_imu_status // publish before vehicle_imu so that error counts are available synchronously if needed @@ -535,26 +536,26 @@ void VehicleIMU::UpdateIntegratorConfiguration() } } -void VehicleIMU::UpdateAccelVibrationMetrics(const Vector3f &delta_velocity) +void VehicleIMU::UpdateAccelVibrationMetrics(const Vector3f &acceleration) { - // Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity) - const Vector3f delta_velocity_diff = delta_velocity - _delta_velocity_prev; - _status.accel_vibration_metric = 0.99f * _status.accel_vibration_metric + 0.01f * delta_velocity_diff.norm(); + // Accel high frequency vibe = filtered length of (acceleration - acceleration_prev) + _status.accel_vibration_metric = 0.99f * _status.accel_vibration_metric + + 0.01f * Vector3f(acceleration - _acceleration_prev).norm(); - _delta_velocity_prev = delta_velocity; + _acceleration_prev = acceleration; } -void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &delta_angle) +void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &angular_velocity) { - // Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle) - const Vector3f delta_angle_diff = delta_angle - _delta_angle_prev; - _status.gyro_vibration_metric = 0.99f * _status.gyro_vibration_metric + 0.01f * delta_angle_diff.norm(); + // Gyro high frequency vibe = filtered length of (angular_velocity - angular_velocity_prev) + _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 (delta_angle x prev_delta_angle) - const Vector3f coning_metric = delta_angle % _delta_angle_prev; + // 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(); - _delta_angle_prev = delta_angle; + _angular_velocity_prev = angular_velocity; } void VehicleIMU::PrintStatus() diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index 4ea00d3454..c12cd1508d 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -81,8 +81,8 @@ private: bool UpdateGyro(); void UpdateIntegratorConfiguration(); - void UpdateAccelVibrationMetrics(const matrix::Vector3f &delta_velocity); - void UpdateGyroVibrationMetrics(const matrix::Vector3f &delta_angle); + void UpdateAccelVibrationMetrics(const matrix::Vector3f &acceleration); + void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity); uORB::PublicationMulti _vehicle_imu_pub{ORB_ID(vehicle_imu)}; uORB::PublicationMulti _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)}; @@ -125,8 +125,8 @@ private: float _accel_temperature{0}; float _gyro_temperature{0}; - matrix::Vector3f _delta_angle_prev{0.f, 0.f, 0.f}; // delta angle from the previous IMU measurement - matrix::Vector3f _delta_velocity_prev{0.f, 0.f, 0.f}; // delta velocity from the previous IMU measurement + matrix::Vector3f _acceleration_prev{}; // acceleration from the previous IMU measurement for vibration metrics + matrix::Vector3f _angular_velocity_prev{}; // angular velocity from the previous IMU measurement for vibration metrics vehicle_imu_status_s _status{};