diff --git a/msg/vehicle_imu_status.msg b/msg/vehicle_imu_status.msg index 411f619f8a..3f8d29c350 100644 --- a/msg/vehicle_imu_status.msg +++ b/msg/vehicle_imu_status.msg @@ -1,9 +1,9 @@ -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) -uint32 accel_device_id # unique device ID for the sensor that does not change between power cycles -uint32 gyro_device_id # unique device ID for the sensor that does not change between power cycles +uint32 accel_device_id # unique device ID for the sensor that does not change between power cycles +uint32 gyro_device_id # unique device ID for the sensor that does not change between power cycles -uint32[3] accel_clipping # clipping per axis +uint32[3] accel_clipping # total clipping per axis uint32 accel_error_count uint32 gyro_error_count @@ -12,5 +12,11 @@ uint16 accel_rate_hz uint16 gyro_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 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[3] mean_accel # average accelerometer readings since last publication +float32[3] mean_gyro # average gyroscope readings 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 847ac21876..366250053e 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -237,7 +237,12 @@ void VehicleIMU::Run() _status.gyro_error_count = gyro.error_count; } - _gyro_integrator.put(gyro.timestamp_sample, Vector3f{gyro.x, gyro.y, gyro.z}); + const Vector3f gyro_raw{gyro.x, gyro.y, gyro.z}; + _gyro_sum += gyro_raw; + _gyro_temperature += gyro.temperature; + _gyro_sum_count++; + + _gyro_integrator.put(gyro.timestamp_sample, gyro_raw); _last_timestamp_sample_gyro = gyro.timestamp_sample; if (!sensor_data_gap && _intervals_configured && _gyro_integrator.integral_ready()) { @@ -275,7 +280,12 @@ void VehicleIMU::Run() _status.accel_error_count = accel.error_count; } - _accel_integrator.put(accel.timestamp_sample, Vector3f{accel.x, accel.y, accel.z}); + const Vector3f accel_raw{accel.x, accel.y, accel.z}; + _accel_sum += accel_raw; + _accel_temperature += accel.temperature; + _accel_sum_count++; + + _accel_integrator.put(accel.timestamp_sample, accel_raw); _last_timestamp_sample_accel = accel.timestamp_sample; if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) { @@ -364,6 +374,23 @@ void VehicleIMU::Run() if (publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms)) { _status.accel_device_id = _accel_calibration.device_id(); _status.gyro_device_id = _gyro_calibration.device_id(); + + // mean accel + const Vector3f accel_mean{_accel_calibration.Correct(_accel_sum / _accel_sum_count)}; + accel_mean.copyTo(_status.mean_accel); + _status.temperature_accel = _accel_temperature / _accel_sum_count; + _accel_sum.zero(); + _accel_temperature = 0; + _accel_sum_count = 0; + + // mean gyro + const Vector3f gyro_mean{_gyro_calibration.Correct(_gyro_sum / _gyro_sum_count)}; + gyro_mean.copyTo(_status.mean_gyro); + _status.temperature_gyro = _gyro_temperature / _gyro_sum_count; + _gyro_sum.zero(); + _gyro_temperature = 0; + _gyro_sum_count = 0; + _status.timestamp = hrt_absolute_time(); _vehicle_imu_status_pub.publish(_status); } diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index 9d88a94fe2..ff615be350 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -107,6 +107,13 @@ private: unsigned _gyro_last_generation{0}; unsigned _consecutive_data_gap{0}; + matrix::Vector3f _accel_sum{}; + matrix::Vector3f _gyro_sum{}; + int _accel_sum_count{0}; + int _gyro_sum_count{0}; + 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