From 6ce38b829659bec7d9d74b3d69b516423bd5e9cc Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 18 Sep 2022 11:25:24 -0400 Subject: [PATCH] vehicle_imu: only reset raw accel/gyro Welford mean periodically - vehicle_imu_status can publish immediately on any measured sample rate change or sensor error increment, but the windowed mean/variance shouldn't necessarily reset that often --- src/modules/sensors/vehicle_imu/VehicleIMU.cpp | 18 +++++++++++++----- src/modules/sensors/vehicle_imu/VehicleIMU.hpp | 2 ++ 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index ab639bde06..2b26223435 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -574,15 +574,18 @@ bool VehicleIMU::Publish() // vehicle_imu_status // publish before vehicle_imu so that error counts are available synchronously if needed - if (_raw_accel_mean.valid() && _raw_gyro_mean.valid() - && (_publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms))) { + const bool imu_status_publishing_interval_exceeded = hrt_elapsed_time(&_status.timestamp) >= + kIMUStatusPublishingInterval; + if (_raw_accel_mean.valid() && _raw_gyro_mean.valid() + && (_publish_status || imu_status_publishing_interval_exceeded)) { + + // Accel _status.accel_device_id = _accel_calibration.device_id(); // accel mean and variance Vector3f(_accel_calibration.rotation() * _raw_accel_mean.mean()).copyTo(_status.mean_accel); Vector3f(_accel_calibration.rotation() * _raw_accel_mean.variance()).copyTo(_status.var_accel); - _raw_accel_mean.reset(); // accel temperature _status.temperature_accel = _accel_temperature_sum / _accel_temperature_sum_count; @@ -590,12 +593,12 @@ bool VehicleIMU::Publish() _accel_temperature_sum_count = 0; + // Gyro _status.gyro_device_id = _gyro_calibration.device_id(); // gyro mean and variance Vector3f(_gyro_calibration.rotation() * _raw_gyro_mean.mean()).copyTo(_status.mean_gyro); 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; @@ -607,11 +610,16 @@ bool VehicleIMU::Publish() _gyro_temperature_sum = NAN; _gyro_temperature_sum_count = 0; - + // publish _status.timestamp = hrt_absolute_time(); _vehicle_imu_status_pub.publish(_status); _publish_status = false; + + if (imu_status_publishing_interval_exceeded) { + _raw_accel_mean.reset(); + _raw_gyro_mean.reset(); + } } // publish vehicle_imu diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index 896a0a385f..e2a4d18dac 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -95,6 +95,8 @@ private: uORB::PublicationMulti _vehicle_imu_pub{ORB_ID(vehicle_imu)}; uORB::PublicationMulti _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)}; + static constexpr hrt_abstime kIMUStatusPublishingInterval{100_ms}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; // Used to check, save and use learned magnetometer biases