From 5f3e883f2c1bb59fe5bc9c89d5662f38a1c87495 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 24 Feb 2021 08:13:53 -0500 Subject: [PATCH] sensors/vehicle_imu: vehicle_imu_status include accel/gyro full raw FIFO sample rate --- msg/sensor_accel.msg | 2 ++ msg/sensor_gyro.msg | 2 ++ msg/vehicle_imu_status.msg | 3 ++ .../accelerometer/PX4Accelerometer.cpp | 6 ++-- .../accelerometer/PX4Accelerometer.hpp | 3 +- src/lib/drivers/gyroscope/PX4Gyroscope.cpp | 5 ++-- src/lib/drivers/gyroscope/PX4Gyroscope.hpp | 2 +- .../sensors/vehicle_imu/VehicleIMU.cpp | 30 +++++++++++-------- .../sensors/vehicle_imu/VehicleIMU.hpp | 8 +++-- 9 files changed, 40 insertions(+), 21 deletions(-) diff --git a/msg/sensor_accel.msg b/msg/sensor_accel.msg index f70cffd1db..e47d813a7a 100644 --- a/msg/sensor_accel.msg +++ b/msg/sensor_accel.msg @@ -13,4 +13,6 @@ uint32 error_count uint8[3] clip_counter # clip count per axis in the sample period +uint8 samples # number of raw samples that went into this message + uint8 ORB_QUEUE_LENGTH = 8 diff --git a/msg/sensor_gyro.msg b/msg/sensor_gyro.msg index f040f02660..b1025e1d12 100644 --- a/msg/sensor_gyro.msg +++ b/msg/sensor_gyro.msg @@ -11,4 +11,6 @@ float32 temperature # temperature in degrees Celsius uint32 error_count +uint8 samples # number of raw samples that went into this message + uint8 ORB_QUEUE_LENGTH = 8 diff --git a/msg/vehicle_imu_status.msg b/msg/vehicle_imu_status.msg index e5be88448d..149e0ac950 100644 --- a/msg/vehicle_imu_status.msg +++ b/msg/vehicle_imu_status.msg @@ -11,6 +11,9 @@ uint32 gyro_error_count float32 accel_rate_hz 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) diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index f54feec673..1fc77a3e41 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -140,11 +140,12 @@ void PX4Accelerometer::updateFIFO(sensor_accel_fifo_s &sample) const float z = integral(2) / (float)N; // publish - Publish(sample.timestamp_sample, x, y, z, clip_count); + Publish(sample.timestamp_sample, x, y, z, clip_count, N); } } -void PX4Accelerometer::Publish(const hrt_abstime ×tamp_sample, float x, float y, float z, uint8_t clip_count[3]) +void PX4Accelerometer::Publish(const hrt_abstime ×tamp_sample, float x, float y, float z, uint8_t clip_count[3], + uint8_t samples) { // Apply rotation (before scaling) rotate_3f(_rotation, x, y, z); @@ -166,6 +167,7 @@ void PX4Accelerometer::Publish(const hrt_abstime ×tamp_sample, float x, flo report.clip_counter[0] = fabsf(roundf(clipping_x)); report.clip_counter[1] = fabsf(roundf(clipping_y)); report.clip_counter[2] = fabsf(roundf(clipping_z)); + report.samples = samples; report.timestamp = hrt_absolute_time(); _sensor_pub.publish(report); diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index 9fd673ae83..170cc18965 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -63,7 +63,8 @@ public: void updateFIFO(sensor_accel_fifo_s &sample); private: - void Publish(const hrt_abstime ×tamp_sample, float x, float y, float z, uint8_t clip_count[3]); + void Publish(const hrt_abstime ×tamp_sample, float x, float y, float z, uint8_t clip_count[3], + uint8_t samples = 1); void UpdateClipLimit(); uORB::PublicationMulti _sensor_pub{ORB_ID(sensor_accel)}; diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index f70f41c758..7f647b39c1 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -114,11 +114,11 @@ void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample) const float z = integral(2) / (float)N; // publish - Publish(sample.timestamp_sample, x, y, z); + Publish(sample.timestamp_sample, x, y, z, N); } } -void PX4Gyroscope::Publish(const hrt_abstime ×tamp_sample, float x, float y, float z) +void PX4Gyroscope::Publish(const hrt_abstime ×tamp_sample, float x, float y, float z, uint8_t samples) { // Apply rotation (before scaling) rotate_3f(_rotation, x, y, z); @@ -132,6 +132,7 @@ void PX4Gyroscope::Publish(const hrt_abstime ×tamp_sample, float x, float y report.x = x * _scale; report.y = y * _scale; report.z = z * _scale; + report.samples = samples; report.timestamp = hrt_absolute_time(); _sensor_pub.publish(report); diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 6c2bcaeb13..ffca99314a 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -62,7 +62,7 @@ public: void updateFIFO(sensor_gyro_fifo_s &sample); private: - void Publish(const hrt_abstime ×tamp_sample, float x, float y, float z); + void Publish(const hrt_abstime ×tamp_sample, float x, float y, float z, uint8_t samples = 1); uORB::PublicationMulti _sensor_pub{ORB_ID(sensor_gyro)}; uORB::PublicationMulti _sensor_fifo_pub{ORB_ID(sensor_gyro_fifo)}; diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 034c4d551d..9eb9ad5382 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -141,50 +141,54 @@ void VehicleIMU::ParametersUpdate(bool force) } } -bool VehicleIMU::UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime ×tamp_sample) +bool VehicleIMU::UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime ×tamp_sample, uint8_t samples) { bool updated = false; // conservative maximum time between samples to reject large gaps and reset averaging - float max_interval_us = 10000; // 100 Hz - float min_interval_us = 100; // 10,000 Hz + uint32_t max_interval_us = 10000; // 100 Hz + uint32_t min_interval_us = 100; // 10,000 Hz - if (intavg.update_interval > 0) { + if (intavg.update_interval > 0.f) { // if available use previously calculated interval as bounds - max_interval_us = 1.5f * intavg.update_interval; - min_interval_us = 0.5f * intavg.update_interval; + max_interval_us = roundf(1.5f * intavg.update_interval); + min_interval_us = roundf(0.5f * intavg.update_interval); } - const float interval_us = (timestamp_sample - intavg.timestamp_sample_last); + const uint32_t interval_us = (timestamp_sample - intavg.timestamp_sample_last); if ((intavg.timestamp_sample_last > 0) && (interval_us < max_interval_us) && (interval_us > min_interval_us)) { intavg.interval_sum += interval_us; + intavg.interval_samples += samples; intavg.interval_count++; // periodically calculate sensor update rate if (intavg.interval_count > 10000 || ((intavg.update_interval <= FLT_EPSILON) && intavg.interval_count > 100)) { - const float sample_interval_avg = intavg.interval_sum / intavg.interval_count; + const float sample_interval_avg = (float)intavg.interval_sum / (float)intavg.interval_count; if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.f)) { // update if interval has changed by more than 0.5% if ((fabsf(intavg.update_interval - sample_interval_avg) / intavg.update_interval) > 0.005f) { intavg.update_interval = sample_interval_avg; + intavg.update_interval_raw = (float)intavg.interval_sum / (float)intavg.interval_samples; updated = true; } } // reset sample interval accumulator intavg.interval_sum = 0.f; - intavg.interval_count = 0.f; + intavg.interval_samples = 0; + intavg.interval_count = 0; } } else { // reset intavg.interval_sum = 0.f; - intavg.interval_count = 0.f; + intavg.interval_samples = 0; + intavg.interval_count = 0; } intavg.timestamp_sample_last = timestamp_sample; @@ -222,10 +226,11 @@ void VehicleIMU::Run() } else { // collect sample interval average for filters - if (!_intervals_configured && UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample)) { + if (!_intervals_configured && UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample, gyro.samples)) { update_integrator_config = true; publish_status = true; _status.gyro_rate_hz = 1e6f / _gyro_interval.update_interval; + _status.gyro_raw_rate_hz = 1e6f / _gyro_interval.update_interval_raw; } } @@ -268,10 +273,11 @@ void VehicleIMU::Run() } else { // collect sample interval average for filters - if (!_intervals_configured && UpdateIntervalAverage(_accel_interval, accel.timestamp_sample)) { + if (!_intervals_configured && UpdateIntervalAverage(_accel_interval, accel.timestamp_sample, accel.samples)) { update_integrator_config = true; publish_status = true; _status.accel_rate_hz = 1e6f / _accel_interval.update_interval; + _status.accel_raw_rate_hz = 1e6f / _accel_interval.update_interval_raw; } } diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index 15d091c3e7..74ecf3e531 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -77,12 +77,14 @@ private: struct IntervalAverage { hrt_abstime timestamp_sample_last{0}; - float interval_sum{0.f}; - float interval_count{0.f}; + uint32_t interval_sum{0}; + uint32_t interval_samples{0}; + uint32_t interval_count{0}; float update_interval{0.f}; + float update_interval_raw{0.f}; }; - bool UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime ×tamp_sample); + bool UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime ×tamp_sample, uint8_t samples = 1); void UpdateIntegratorConfiguration(); void UpdateGyroVibrationMetrics(const matrix::Vector3f &delta_angle); void UpdateAccelVibrationMetrics(const matrix::Vector3f &delta_velocity);