diff --git a/msg/vehicle_imu_status.msg b/msg/vehicle_imu_status.msg index 149e0ac950..188e264e82 100644 --- a/msg/vehicle_imu_status.msg +++ b/msg/vehicle_imu_status.msg @@ -20,6 +20,7 @@ float32 gyro_coning_vibration # Level of coning vibration in the IMU delta ang float32[3] mean_accel # average accelerometer readings since last publication float32[3] mean_gyro # average gyroscope readings since last publication +float32[3] var_accel # average accelerometer variance since last publication float32 temperature_accel float32 temperature_gyro diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 43b2b8bd5b..2cfcadec35 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -491,6 +491,7 @@ bool VehicleIMU::Publish() const float accel_dt_inv = 1.e6f / imu.delta_velocity_dt; const Vector3f acceleration{_accel_calibration.Correct(delta_velocity * accel_dt_inv)}; UpdateAccelVibrationMetrics(acceleration); + UpdateAccelSquaredErrorSum(acceleration); const Vector3f delta_velocity_corrected{acceleration / accel_dt_inv}; // vehicle_imu_status @@ -507,6 +508,12 @@ bool VehicleIMU::Publish() _status.temperature_accel = _accel_temperature / _accel_sum_count; _accel_sum.zero(); _accel_temperature = 0; + + // variance accel + const Vector3f variance_accel{_accel_squared_error_sum / _accel_sum_count}; + variance_accel.copyTo(_status.var_accel); + _accel_squared_error_sum.zero(); + _accel_sum_count = 0; // mean gyro @@ -595,6 +602,13 @@ void VehicleIMU::UpdateIntegratorConfiguration() } } +void VehicleIMU::UpdateAccelSquaredErrorSum(const Vector3f &acceleration) +{ + // Compute the squared error using the average from last publication for efficiency purposes + const Vector3f error{acceleration - Vector3f(_status.mean_accel)}; + _accel_squared_error_sum += error.emult(error); +} + void VehicleIMU::UpdateAccelVibrationMetrics(const Vector3f &acceleration) { // Accel high frequency vibe = filtered length of (acceleration - acceleration_prev) diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index 351ecc4010..1580b79931 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -83,6 +83,7 @@ private: void UpdateIntegratorConfiguration(); void UpdateAccelVibrationMetrics(const matrix::Vector3f &acceleration); void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity); + void UpdateAccelSquaredErrorSum(const matrix::Vector3f &acceleration); uORB::PublicationMulti _vehicle_imu_pub{ORB_ID(vehicle_imu)}; uORB::PublicationMulti _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)}; @@ -118,6 +119,7 @@ private: unsigned _accel_last_generation{0}; unsigned _gyro_last_generation{0}; + matrix::Vector3f _accel_squared_error_sum{}; matrix::Vector3f _accel_sum{}; matrix::Vector3f _gyro_sum{}; int _accel_sum_count{0};