From 055b5404c36ef903d86026ff60309ec401602211 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 30 May 2021 15:09:10 -0400 Subject: [PATCH] sensors/vehicle_imu: switch to WelfordMean library and other improvements - use WelfordMean library to determine sensor sample rate average - if gyro samples exceeds queue depth, instead round to nearest even integer to ease scheduling - force update if publication latency is falling behind - remove redundant sample interval perf counters - status print accel & gyro interval standard deviation --- .../sensors/vehicle_imu/VehicleIMU.cpp | 352 ++++++++++-------- .../sensors/vehicle_imu/VehicleIMU.hpp | 53 +-- 2 files changed, 221 insertions(+), 184 deletions(-) diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 3a98b6b3b1..180a581ccd 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -48,25 +48,23 @@ namespace sensors VehicleIMU::VehicleIMU(int instance, uint8_t accel_index, uint8_t gyro_index, const px4::wq_config_t &config) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, config), - _sensor_accel_sub(this, ORB_ID(sensor_accel), accel_index), + _sensor_accel_sub(ORB_ID(sensor_accel), accel_index), _sensor_gyro_sub(this, ORB_ID(sensor_gyro), gyro_index), _instance(instance) { - const float configured_interval_us = 1e6f / _param_imu_integ_rate.get(); + _imu_integration_interval_us = 1e6f / _param_imu_integ_rate.get(); - _accel_integrator.set_reset_interval(configured_interval_us); + _accel_integrator.set_reset_interval(_imu_integration_interval_us); _accel_integrator.set_reset_samples(sensor_accel_s::ORB_QUEUE_LENGTH); - _gyro_integrator.set_reset_interval(configured_interval_us); + _gyro_integrator.set_reset_interval(_imu_integration_interval_us); _gyro_integrator.set_reset_samples(sensor_gyro_s::ORB_QUEUE_LENGTH); #if defined(ENABLE_LOCKSTEP_SCHEDULER) // currently with lockstep every raw sample needs a corresponding vehicle_imu publication - _sensor_accel_sub.set_required_updates(1); _sensor_gyro_sub.set_required_updates(1); #else // schedule conservatively until the actual accel & gyro rates are known - _sensor_accel_sub.set_required_updates(sensor_accel_s::ORB_QUEUE_LENGTH / 2); _sensor_gyro_sub.set_required_updates(sensor_gyro_s::ORB_QUEUE_LENGTH / 2); #endif @@ -80,9 +78,7 @@ VehicleIMU::~VehicleIMU() Stop(); perf_free(_accel_generation_gap_perf); - perf_free(_accel_update_perf); perf_free(_gyro_generation_gap_perf); - perf_free(_gyro_update_perf); _vehicle_imu_pub.unadvertise(); _vehicle_imu_status_pub.unadvertise(); @@ -94,7 +90,6 @@ bool VehicleIMU::Start() ParametersUpdate(true); _sensor_gyro_sub.registerCallback(); - _sensor_accel_sub.registerCallback(); ScheduleNow(); return true; } @@ -102,7 +97,6 @@ bool VehicleIMU::Start() void VehicleIMU::Stop() { // clear all registered callbacks - _sensor_accel_sub.unregisterCallback(); _sensor_gyro_sub.unregisterCallback(); Deinit(); @@ -133,74 +127,21 @@ void VehicleIMU::ParametersUpdate(bool force) _param_imu_integ_rate.commit_no_notification(); } - _imu_integration_interval_us = 1000000 / imu_integration_rate_hz; + _imu_integration_interval_us = 1e6f / imu_integration_rate_hz; if (_param_imu_integ_rate.get() != imu_integ_rate_prev) { // force update - UpdateIntegratorConfiguration(); + _update_integrator_config = true; } } } -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 - uint32_t max_interval_us = 10000; // 100 Hz - uint32_t min_interval_us = 100; // 10,000 Hz - - if (intavg.update_interval > 0.f) { - // if available use previously calculated interval as bounds - max_interval_us = roundf(1.5f * intavg.update_interval); - min_interval_us = roundf(0.5f * intavg.update_interval); - } - - 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 = (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_samples = 0; - intavg.interval_count = 0; - } - - } else { - // reset - intavg.interval_sum = 0.f; - intavg.interval_samples = 0; - intavg.interval_count = 0; - } - - intavg.timestamp_sample_last = timestamp_sample; - - return updated; -} - void VehicleIMU::Run() { + const hrt_abstime now_us = hrt_absolute_time(); + // backup schedule - ScheduleDelayed(10_ms); + ScheduleDelayed(_backup_schedule_timeout_us); ParametersUpdate(); @@ -208,53 +149,123 @@ void VehicleIMU::Run() return; } - UpdateGyro(); - UpdateAccel(); + // reset data gap monitor + _data_gap = false; - if (_sensor_data_gap) { - _consecutive_data_gap++; + while (_sensor_gyro_sub.updated() || _sensor_accel_sub.updated()) { + bool updated = false; - // if there's consistently a gap in data start monitoring publication interval again - if (_consecutive_data_gap > 10) { - _intervals_configured = false; + 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))) { + + PX4_DEBUG("gyro update mean sample latency: %.6f, publish latency %.6f us", + (double)_gyro_update_latency_mean.mean()(0), + (double)_gyro_update_latency_mean.mean()(1)); + + consume_all_gyro = true; } - _sensor_data_gap = false; + // update gyro until integrator ready and not falling behind + if (!_gyro_integrator.integral_ready() || consume_all_gyro) { + if (UpdateGyro()) { + updated = true; + } + } - } else { - _consecutive_data_gap = 0; + + 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) { + if (UpdateAccel()) { + updated = true; + } + } + + // reconfigure integrators if calculated sensor intervals have changed + if (_update_integrator_config || !_intervals_configured) { + UpdateIntegratorConfiguration(); + } + + // 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())) { + continue; + } + + if (_intervals_configured) { + if (Publish()) { + // record gyro publication latency and integrated samples + if (_gyro_update_latency_mean.count() > 10000) { + // reset periodically to avoid numerical issues + _gyro_update_latency_mean.reset(); + } + + 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}); + + return; + } + } + + // finish if there are no more updates, but didn't publish + if (!updated) { + return; + } } - - // reconfigure integrators if calculated sensor intervals have changed - if (_update_integrator_config) { - UpdateIntegratorConfiguration(); - _update_integrator_config = false; - } - - Publish(); } -void VehicleIMU::UpdateAccel() +bool VehicleIMU::UpdateAccel() { - // update accel, stopping once caught up to the last gyro sample + bool updated = false; + + // integrate queued accel sensor_accel_s accel; - while (_sensor_accel_sub.update(&accel)) { - perf_count_interval(_accel_update_perf, accel.timestamp_sample); - + if (_sensor_accel_sub.update(&accel)) { if (_sensor_accel_sub.get_last_generation() != _accel_last_generation + 1) { - _sensor_data_gap = true; + _data_gap = true; perf_count(_accel_generation_gap_perf); - _accel_interval.timestamp_sample_last = 0; // invalidate any ongoing publication rate averaging + // reset average sample measurement + _accel_interval_mean.reset(); } else { // collect sample interval average for filters - 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; + if (_accel_timestamp_sample_last != 0) { + float interval_us = accel.timestamp_sample - _accel_timestamp_sample_last; + _accel_interval_mean.update(Vector2f{interval_us, interval_us / accel.samples}); + } + + if (_accel_interval_mean.valid() + && ((_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); + const float percent_changed = interval_delta_us / _accel_interval_us; + + if (!PX4_ISFINITE(_accel_interval_us) || (percent_changed > 0.001f)) { + // update integrator configuration if interval has changed by more than 10% + if (interval_delta_us > 0.1f * _accel_interval_us) { + _update_integrator_config = true; + } + + _accel_interval_us = _accel_interval_mean.mean()(0); + _accel_interval_best_variance = _accel_interval_mean.variance()(0); + + _status.accel_rate_hz = 1e6f / _accel_interval_mean.mean()(0); + _status.accel_raw_rate_hz = 1e6f / _accel_interval_mean.mean()(1); // FIFO + _publish_status = true; + } + + if (_accel_interval_mean.count() > 10000) { + // reset periodically to prevent numerical issues + _accel_interval_mean.reset(); + } } } @@ -272,13 +283,13 @@ void VehicleIMU::UpdateAccel() _accel_temperature += accel.temperature; _accel_sum_count++; - const float dt = (accel.timestamp_sample - _last_timestamp_sample_accel) * 1e-6f; - _last_timestamp_sample_accel = accel.timestamp_sample; + const float dt = (accel.timestamp_sample - _accel_timestamp_sample_last) * 1e-6f; + _accel_timestamp_sample_last = accel.timestamp_sample; _accel_integrator.put(accel_raw, dt); + updated = true; if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) { - // rotate sensor clip counts into vehicle body frame const Vector3f clipping{_accel_calibration.rotation() * Vector3f{(float)accel.clip_counter[0], (float)accel.clip_counter[1], (float)accel.clip_counter[2]}}; @@ -317,41 +328,62 @@ void VehicleIMU::UpdateAccel() } } } - - // break once caught up to gyro - if (!_sensor_data_gap && _intervals_configured - && (_last_timestamp_sample_accel >= (_last_timestamp_sample_gyro - 0.5f * _accel_interval.update_interval))) { - - break; - } } + + return updated; } -void VehicleIMU::UpdateGyro() +bool VehicleIMU::UpdateGyro() { + bool updated = false; + // integrate queued gyro sensor_gyro_s gyro; while (_sensor_gyro_sub.update(&gyro)) { - perf_count_interval(_gyro_update_perf, gyro.timestamp_sample); - if (_sensor_gyro_sub.get_last_generation() != _gyro_last_generation + 1) { - _sensor_data_gap = true; + _data_gap = true; perf_count(_gyro_generation_gap_perf); - _gyro_interval.timestamp_sample_last = 0; // invalidate any ongoing publication rate averaging + // reset average sample measurement + _gyro_interval_mean.reset(); } else { // collect sample interval average for filters - 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; + if (_gyro_timestamp_sample_last != 0) { + float interval_us = gyro.timestamp_sample - _gyro_timestamp_sample_last; + _gyro_interval_mean.update(Vector2f{interval_us, interval_us / gyro.samples}); + } + + if (_gyro_interval_mean.valid() + && ((_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); + const float percent_changed = interval_delta_us / _gyro_interval_us; + + if (!PX4_ISFINITE(_gyro_interval_us) || (percent_changed > 0.001f)) { + // update integrator configuration if interval has changed by more than 10% + if (interval_delta_us > 0.1f * _gyro_interval_us) { + _update_integrator_config = true; + } + + _gyro_interval_us = _gyro_interval_mean.mean()(0); + _gyro_interval_best_variance = _gyro_interval_mean.variance()(0); + + _status.gyro_rate_hz = 1e6f / _gyro_interval_mean.mean()(0); + _status.gyro_raw_rate_hz = 1e6f / _gyro_interval_mean.mean()(1); // FIFO + _publish_status = true; + } + + if (_gyro_interval_mean.count() > 10000) { + // reset periodically to prevent numerical issues + _gyro_interval_mean.reset(); + } } } _gyro_last_generation = _sensor_gyro_sub.get_last_generation(); + _gyro_timestamp_last = gyro.timestamp; _gyro_calibration.set_device_id(gyro.device_id); @@ -365,22 +397,20 @@ void VehicleIMU::UpdateGyro() _gyro_temperature += gyro.temperature; _gyro_sum_count++; - const float dt = (gyro.timestamp_sample - _last_timestamp_sample_gyro) * 1e-6f; - _last_timestamp_sample_gyro = gyro.timestamp_sample; + const float dt = (gyro.timestamp_sample - _gyro_timestamp_sample_last) * 1e-6f; + _gyro_timestamp_sample_last = gyro.timestamp_sample; _gyro_integrator.put(gyro_raw, dt); - - // break if interval is configured and we haven't fallen behind - if (_intervals_configured && _gyro_integrator.integral_ready() - && (hrt_elapsed_time(&gyro.timestamp) < _imu_integration_interval_us) && !_sensor_data_gap) { - - break; - } + updated = true; } + + return updated; } -void VehicleIMU::Publish() +bool VehicleIMU::Publish() { + bool updated = false; + // publish if both accel & gyro integrators are ready if (_accel_integrator.integral_ready() && _gyro_integrator.integral_ready()) { @@ -431,12 +461,14 @@ void VehicleIMU::Publish() _status.timestamp = hrt_absolute_time(); _vehicle_imu_status_pub.publish(_status); + + _publish_status = false; } // publish vehicle_imu vehicle_imu_s imu; - imu.timestamp_sample = _last_timestamp_sample_gyro; + 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); @@ -447,48 +479,60 @@ void VehicleIMU::Publish() imu.calibration_count = _accel_calibration.calibration_count() + _gyro_calibration.calibration_count(); imu.timestamp = hrt_absolute_time(); _vehicle_imu_pub.publish(imu); + + updated = true; } // reset clip counts _delta_velocity_clipping = 0; - - return; } } + + return updated; } void VehicleIMU::UpdateIntegratorConfiguration() { - if ((_accel_interval.update_interval > 0) && (_gyro_interval.update_interval > 0)) { - - const float configured_interval_us = 1e6f / _param_imu_integ_rate.get(); + if (PX4_ISFINITE(_accel_interval_us) && PX4_ISFINITE(_gyro_interval_us)) { // determine number of sensor samples that will get closest to the desired integration interval - const uint8_t accel_integral_samples = math::max(1.f, roundf(configured_interval_us / _accel_interval.update_interval)); - const uint8_t gyro_integral_samples = math::max(1.f, roundf(configured_interval_us / _gyro_interval.update_interval)); + uint8_t gyro_integral_samples = math::max(1, (int)roundf(_imu_integration_interval_us / _gyro_interval_us)); + + // if gyro samples exceeds queue depth, instead round to nearest even integer to improve scheduling options + if (gyro_integral_samples > sensor_gyro_s::ORB_QUEUE_LENGTH) { + gyro_integral_samples = math::max(1, (int)roundf(_imu_integration_interval_us / _gyro_interval_us / 2) * 2); + } + + uint32_t integration_interval_us = roundf(gyro_integral_samples * _gyro_interval_us); + + // accel follows gyro as closely as possible + uint8_t accel_integral_samples = math::max(1, (int)roundf(integration_interval_us / _accel_interval_us)); // let the gyro set the configuration and scheduling - // accel integrator will be forced to reset when gyro integrator is ready - _gyro_integrator.set_reset_samples(gyro_integral_samples); - _accel_integrator.set_reset_samples(1); - // relaxed minimum integration time required - _accel_integrator.set_reset_interval(roundf((accel_integral_samples - 0.5f) * _accel_interval.update_interval)); - _gyro_integrator.set_reset_interval(roundf((gyro_integral_samples - 0.5f) * _gyro_interval.update_interval)); + _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); // 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) { + gyro_integral_samples /= 2; + } + if (gyro_integral_samples % n == 0) { _sensor_gyro_sub.set_required_updates(n); - // run when there are enough new gyro samples, unregister accel - _sensor_accel_sub.unregisterCallback(); - - _intervals_configured = true; // stop monitoring topic publication rates + _intervals_configured = true; + _update_integrator_config = false; PX4_DEBUG("accel (%d), gyro (%d), accel samples: %d, gyro samples: %d, accel interval: %.1f, gyro interval: %.1f sub samples: %d", _accel_calibration.device_id(), _gyro_calibration.device_id(), accel_integral_samples, gyro_integral_samples, - (double)_accel_interval.update_interval, (double)_gyro_interval.update_interval, n); + (double)_accel_interval_us, (double)_gyro_interval_us, n); break; } @@ -520,20 +564,12 @@ void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &delta_angle) void VehicleIMU::PrintStatus() { - if (_accel_calibration.device_id() == _gyro_calibration.device_id()) { - PX4_INFO("%d - IMU ID: %d, accel interval: %.1f us, gyro interval: %.1f us", _instance, _accel_calibration.device_id(), - (double)_accel_interval.update_interval, (double)_gyro_interval.update_interval); - - } else { - PX4_INFO("%d - Accel ID: %d, interval: %.1f us, Gyro ID: %d, interval: %.1f us", _instance, - _accel_calibration.device_id(), - (double)_accel_interval.update_interval, _gyro_calibration.device_id(), (double)_gyro_interval.update_interval); - } + PX4_INFO("%d - Accel ID: %d, interval: %.1f us (SD %.1f us), Gyro ID: %d, 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)); perf_print_counter(_accel_generation_gap_perf); perf_print_counter(_gyro_generation_gap_perf); - perf_print_counter(_accel_update_perf); - perf_print_counter(_gyro_update_perf); _accel_calibration.PrintStatus(); _gyro_calibration.PrintStatus(); diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index f65d4babd5..4ea00d3454 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -36,6 +36,7 @@ #include "Integrator.hpp" #include +#include #include #include #include @@ -72,31 +73,23 @@ public: void PrintStatus(); private: - struct IntervalAverage { - hrt_abstime timestamp_sample_last{0}; - 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}; - }; - void ParametersUpdate(bool force = false); - void Publish(); + bool Publish(); void Run() override; - void UpdateAccel(); - void UpdateAccelVibrationMetrics(const matrix::Vector3f &delta_velocity); - void UpdateGyro(); - void UpdateGyroVibrationMetrics(const matrix::Vector3f &delta_angle); - bool UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime ×tamp_sample, uint8_t samples = 1); + + bool UpdateAccel(); + bool UpdateGyro(); + void UpdateIntegratorConfiguration(); + void UpdateAccelVibrationMetrics(const matrix::Vector3f &delta_velocity); + void UpdateGyroVibrationMetrics(const matrix::Vector3f &delta_angle); uORB::PublicationMulti _vehicle_imu_pub{ORB_ID(vehicle_imu)}; uORB::PublicationMulti _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::SubscriptionCallbackWorkItem _sensor_accel_sub; + uORB::Subscription _sensor_accel_sub; uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub; calibration::Accelerometer _accel_calibration{}; @@ -105,17 +98,25 @@ private: Integrator _accel_integrator{}; IntegratorConing _gyro_integrator{}; - hrt_abstime _last_timestamp_sample_accel{0}; - hrt_abstime _last_timestamp_sample_gyro{0}; + uint32_t _imu_integration_interval_us{5000}; - uint32_t _imu_integration_interval_us{4000}; + hrt_abstime _accel_timestamp_sample_last{0}; + hrt_abstime _gyro_timestamp_sample_last{0}; + hrt_abstime _gyro_timestamp_last{0}; - IntervalAverage _accel_interval{}; - IntervalAverage _gyro_interval{}; + math::WelfordMean _accel_interval_mean{}; + math::WelfordMean _gyro_interval_mean{}; + + math::WelfordMean _gyro_update_latency_mean{}; + + float _accel_interval_best_variance{INFINITY}; + float _gyro_interval_best_variance{INFINITY}; + + float _accel_interval_us{NAN}; + float _gyro_interval_us{NAN}; unsigned _accel_last_generation{0}; unsigned _gyro_last_generation{0}; - unsigned _consecutive_data_gap{0}; matrix::Vector3f _accel_sum{}; matrix::Vector3f _gyro_sum{}; @@ -135,16 +136,16 @@ private: uint64_t _last_clipping_notify_total_count{0}; orb_advert_t _mavlink_log_pub{nullptr}; - bool _update_integrator_config{false}; - bool _sensor_data_gap{false}; + uint32_t _backup_schedule_timeout_us{20000}; + + bool _data_gap{false}; + bool _update_integrator_config{true}; bool _intervals_configured{false}; bool _publish_status{false}; const uint8_t _instance; - perf_counter_t _accel_update_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": accel update interval")}; perf_counter_t _accel_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": accel data gap")}; - perf_counter_t _gyro_update_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": gyro update interval")}; perf_counter_t _gyro_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap")}; DEFINE_PARAMETERS(