From f0d8d53da66c219d38ad644b30ebdf8943d5d44f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 29 Jun 2021 10:43:09 -0400 Subject: [PATCH] sensors/vehicle_imu: minor IMU integration improvements - IMU integrator set max dt based on final return size (uint16) - improve integration consuming gyro as needed then integrate all available accel until caught up - increase required number of samples for sensor rate measurement (online Welford mean) --- .../sensors/vehicle_imu/Integrator.hpp | 11 +- .../sensors/vehicle_imu/VehicleIMU.cpp | 167 +++++++++--------- 2 files changed, 89 insertions(+), 89 deletions(-) diff --git a/src/modules/sensors/vehicle_imu/Integrator.hpp b/src/modules/sensors/vehicle_imu/Integrator.hpp index 38b0e8fcd1..646e305f41 100644 --- a/src/modules/sensors/vehicle_imu/Integrator.hpp +++ b/src/modules/sensors/vehicle_imu/Integrator.hpp @@ -51,6 +51,9 @@ public: Integrator() = default; ~Integrator() = default; + static constexpr float DT_MIN{FLT_MIN}; + static constexpr float DT_MAX{static_cast(UINT16_MAX) * 1e-6f}; + /** * Put an item into the integral. * @@ -60,7 +63,7 @@ public: */ inline void put(const matrix::Vector3f &val, const float dt) { - if (dt > 0.f && dt <= (_reset_interval_min * 2.f)) { + if ((dt > DT_MIN) && (_integral_dt + dt < DT_MAX)) { _alpha += integrate(val, dt); } else { @@ -103,7 +106,7 @@ public: * @param integral_dt Get the dt in us of the current integration. * @return true if integral valid */ - bool reset(matrix::Vector3f &integral, uint32_t &integral_dt) + bool reset(matrix::Vector3f &integral, uint16_t &integral_dt) { if (integral_ready()) { integral = _alpha; @@ -155,7 +158,7 @@ public: */ inline void put(const matrix::Vector3f &val, const float dt) { - if (dt > 0.f && dt <= (_reset_interval_min * 2.f)) { + if ((dt > DT_MIN) && (_integral_dt + dt < DT_MAX)) { // Use trapezoidal integration to calculate the delta integral const matrix::Vector3f delta_alpha{integrate(val, dt)}; @@ -190,7 +193,7 @@ public: * @param integral_dt Get the dt in us of the current integration. * @return true if integral valid */ - bool reset(matrix::Vector3f &integral, uint32_t &integral_dt) + bool reset(matrix::Vector3f &integral, uint16_t &integral_dt) { if (Integrator::reset(integral, integral_dt)) { // apply coning corrections diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 32a75bece7..c859455297 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -158,10 +158,10 @@ void VehicleIMU::Run() bool consume_all_gyro = !_intervals_configured || _data_gap; // monitor scheduling latency and force catch up with latest gyro if falling behind - if (_sensor_gyro_sub.updated() && _gyro_update_latency_mean.valid() - && (_gyro_update_latency_mean.mean()(1) > (1.5f * _gyro_interval_us * 1e-6f))) { + if (_sensor_gyro_sub.updated() && (_gyro_update_latency_mean.count() > 100) + && (_gyro_update_latency_mean.mean()(1) > _gyro_interval_us * 1e-6f)) { - PX4_DEBUG("gyro update mean sample latency: %.6f, publish latency %.6f us", + PX4_DEBUG("gyro update mean sample latency: %.6f, publish latency %.6f", (double)_gyro_update_latency_mean.mean()(0), (double)_gyro_update_latency_mean.mean()(1)); @@ -176,11 +176,11 @@ void VehicleIMU::Run() } - bool consume_all_accel = !_intervals_configured || _data_gap - || (_accel_timestamp_sample_last < (_gyro_timestamp_sample_last - 0.5f * _accel_interval_us)); - // update accel until integrator ready and caught up to gyro - if (!_accel_integrator.integral_ready() || consume_all_accel) { + while (_sensor_accel_sub.updated() + && (!_accel_integrator.integral_ready() || !_intervals_configured || _data_gap + || (_accel_timestamp_sample_last < (_gyro_timestamp_sample_last - 0.5f * _accel_interval_us)))) { + if (UpdateAccel()) { updated = true; } @@ -192,11 +192,12 @@ void VehicleIMU::Run() } // check for additional updates and that we're fully caught up before publishing - if ((consume_all_gyro && _sensor_gyro_sub.updated()) || (consume_all_accel && _sensor_accel_sub.updated())) { + if (consume_all_gyro && _sensor_gyro_sub.updated()) { continue; } - if (_intervals_configured) { + // publish if both accel & gyro integrators are ready + if (_intervals_configured && _accel_integrator.integral_ready() && _gyro_integrator.integral_ready()) { if (Publish()) { // record gyro publication latency and integrated samples if (_gyro_update_latency_mean.count() > 10000) { @@ -205,9 +206,8 @@ void VehicleIMU::Run() } const float time_run_s = now_us * 1e-6f; - const float time_gyro_timestamp_last_s = _gyro_timestamp_last * 1e-6f; - const float time_gyro_timestamp_sample_last_s = _gyro_timestamp_sample_last * 1e-6f; - _gyro_update_latency_mean.update(Vector2f{time_run_s - time_gyro_timestamp_sample_last_s, time_run_s - time_gyro_timestamp_last_s}); + + _gyro_update_latency_mean.update(Vector2f{time_run_s - _gyro_timestamp_sample_last * 1e-6f, time_run_s - _gyro_timestamp_last * 1e-6f}); return; } @@ -242,7 +242,7 @@ bool VehicleIMU::UpdateAccel() _accel_interval_mean.update(Vector2f{interval_us, interval_us / accel.samples}); } - if (_accel_interval_mean.valid() + if (_accel_interval_mean.valid() && (_accel_interval_mean.count() > 100 || !PX4_ISFINITE(_accel_interval_best_variance)) && ((_accel_interval_mean.variance()(0) < _accel_interval_best_variance) || (_accel_interval_mean.count() > 1000))) { // update sample rate if previously invalid or changed const float interval_delta_us = fabsf(_accel_interval_mean.mean()(0) - _accel_interval_us); @@ -340,7 +340,7 @@ bool VehicleIMU::UpdateGyro() // integrate queued gyro sensor_gyro_s gyro; - while (_sensor_gyro_sub.update(&gyro)) { + if (_sensor_gyro_sub.update(&gyro)) { if (_sensor_gyro_sub.get_last_generation() != _gyro_last_generation + 1) { _data_gap = true; perf_count(_gyro_generation_gap_perf); @@ -355,7 +355,7 @@ bool VehicleIMU::UpdateGyro() _gyro_interval_mean.update(Vector2f{interval_us, interval_us / gyro.samples}); } - if (_gyro_interval_mean.valid() + if (_gyro_interval_mean.valid() && (_gyro_interval_mean.count() > 100 || !PX4_ISFINITE(_gyro_interval_best_variance)) && ((_gyro_interval_mean.variance()(0) < _gyro_interval_best_variance) || (_gyro_interval_mean.count() > 1000))) { // update sample rate if previously invalid or changed const float interval_delta_us = fabsf(_gyro_interval_mean.mean()(0) - _gyro_interval_us); @@ -411,80 +411,73 @@ bool VehicleIMU::Publish() { bool updated = false; - // publish if both accel & gyro integrators are ready - if (_accel_integrator.integral_ready() && _gyro_integrator.integral_ready()) { - - uint32_t accel_integral_dt; - uint32_t gyro_integral_dt; - Vector3f delta_angle; - Vector3f delta_velocity; - - if (_accel_integrator.reset(delta_velocity, accel_integral_dt) - && _gyro_integrator.reset(delta_angle, gyro_integral_dt)) { - - if (_accel_calibration.enabled() && _gyro_calibration.enabled()) { - - // delta angle: apply offsets, scale, and board rotation - _gyro_calibration.SensorCorrectionsUpdate(); - const float gyro_dt_inv = 1.e6f / gyro_integral_dt; - const Vector3f delta_angle_corrected{_gyro_calibration.Correct(delta_angle * gyro_dt_inv) / gyro_dt_inv}; - - // delta velocity: apply offsets, scale, and board rotation - _accel_calibration.SensorCorrectionsUpdate(); - const float accel_dt_inv = 1.e6f / accel_integral_dt; - Vector3f delta_velocity_corrected{_accel_calibration.Correct(delta_velocity * accel_dt_inv) / accel_dt_inv}; - - UpdateAccelVibrationMetrics(delta_velocity_corrected); - UpdateGyroVibrationMetrics(delta_angle_corrected); - - // vehicle_imu_status - // publish before vehicle_imu so that error counts are available synchronously if needed - 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); - - _publish_status = false; - } + vehicle_imu_s imu; + Vector3f delta_angle; + Vector3f delta_velocity; + if (_accel_integrator.reset(delta_velocity, imu.delta_velocity_dt) + && _gyro_integrator.reset(delta_angle, imu.delta_angle_dt)) { - // publish vehicle_imu - vehicle_imu_s imu; - imu.timestamp_sample = _gyro_timestamp_sample_last; - imu.accel_device_id = _accel_calibration.device_id(); - imu.gyro_device_id = _gyro_calibration.device_id(); - delta_angle_corrected.copyTo(imu.delta_angle); - delta_velocity_corrected.copyTo(imu.delta_velocity); - imu.delta_angle_dt = gyro_integral_dt; - imu.delta_velocity_dt = accel_integral_dt; - imu.delta_velocity_clipping = _delta_velocity_clipping; - imu.calibration_count = _accel_calibration.calibration_count() + _gyro_calibration.calibration_count(); - imu.timestamp = hrt_absolute_time(); - _vehicle_imu_pub.publish(imu); + if (_accel_calibration.enabled() && _gyro_calibration.enabled()) { - updated = true; + // 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}; + + // 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); + + // vehicle_imu_status + // publish before vehicle_imu so that error counts are available synchronously if needed + if ((_accel_sum_count > 0) && (_gyro_sum_count > 0) + && (_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); + + _publish_status = false; } + // publish vehicle_imu + imu.timestamp_sample = _gyro_timestamp_sample_last; + imu.accel_device_id = _accel_calibration.device_id(); + imu.gyro_device_id = _gyro_calibration.device_id(); + delta_angle_corrected.copyTo(imu.delta_angle); + delta_velocity_corrected.copyTo(imu.delta_velocity); + imu.delta_velocity_clipping = _delta_velocity_clipping; + imu.calibration_count = _accel_calibration.calibration_count() + _gyro_calibration.calibration_count(); + imu.timestamp = hrt_absolute_time(); + _vehicle_imu_pub.publish(imu); + // reset clip counts _delta_velocity_clipping = 0; + + updated = true; } } @@ -513,11 +506,12 @@ void VehicleIMU::UpdateIntegratorConfiguration() _accel_integrator.set_reset_interval(roundf((accel_integral_samples - 0.5f) * _accel_interval_us)); _accel_integrator.set_reset_samples(accel_integral_samples); - _backup_schedule_timeout_us = sensor_accel_s::ORB_QUEUE_LENGTH * _accel_interval_us; - _gyro_integrator.set_reset_interval(roundf((gyro_integral_samples - 0.5f) * _gyro_interval_us)); _gyro_integrator.set_reset_samples(gyro_integral_samples); + _backup_schedule_timeout_us = math::min(sensor_accel_s::ORB_QUEUE_LENGTH * _accel_interval_us, + sensor_gyro_s::ORB_QUEUE_LENGTH * _gyro_interval_us); + // gyro: find largest integer multiple of gyro_integral_samples for (int n = sensor_gyro_s::ORB_QUEUE_LENGTH; n > 0; n--) { if (gyro_integral_samples > sensor_gyro_s::ORB_QUEUE_LENGTH) { @@ -565,12 +559,15 @@ void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &delta_angle) void VehicleIMU::PrintStatus() { - PX4_INFO("%" PRIu8 " - Accel ID: %" PRIu32 ", interval: %.1f us (SD %.1f us), Gyro ID: %" PRIu32 ", interval: %.1f us (SD %.1f us)", _instance, _accel_calibration.device_id(), (double)_accel_interval_us, (double)sqrtf(_accel_interval_best_variance), _gyro_calibration.device_id(), (double)_gyro_interval_us, (double)sqrtf(_gyro_interval_best_variance)); + PX4_DEBUG("gyro update mean sample latency: %.6f s, publish latency %.6f s, gyro interval %.6f s", + (double)_gyro_update_latency_mean.mean()(0), (double)_gyro_update_latency_mean.mean()(1), + (double)(_gyro_interval_us * 1e-6f)); + perf_print_counter(_accel_generation_gap_perf); perf_print_counter(_gyro_generation_gap_perf);