From c17a9e8003b2ba4be4a8943f2fd7aad2c6265994 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 6 Jan 2022 14:43:20 -0500 Subject: [PATCH] sensors/vehicle_imu: refactor SensorCalibrationUpdate() to separate accel/gyro cal saving --- .../sensors/vehicle_imu/VehicleIMU.cpp | 121 ++++++++++-------- .../sensors/vehicle_imu/VehicleIMU.hpp | 5 +- 2 files changed, 71 insertions(+), 55 deletions(-) diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index e39e9669f7..4fd36644ea 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -103,8 +103,10 @@ void VehicleIMU::Stop() Deinit(); } -void VehicleIMU::ParametersUpdate(bool force) +bool VehicleIMU::ParametersUpdate(bool force) { + bool updated = false; + // Check if parameters have changed if (_parameter_update_sub.updated() || force) { // clear update @@ -115,6 +117,8 @@ void VehicleIMU::ParametersUpdate(bool force) updateParams(); + updated = true; + const auto accel_calibration_count = _accel_calibration.calibration_count(); const auto gyro_calibration_count = _gyro_calibration.calibration_count(); _accel_calibration.ParametersUpdate(); @@ -123,6 +127,7 @@ void VehicleIMU::ParametersUpdate(bool force) if (accel_calibration_count != _accel_calibration.calibration_count()) { // if calibration changed reset any existing learned calibration _accel_cal_available = false; + _in_flight_calibration_check_timestamp_last = hrt_absolute_time(); for (auto &learned_cal : _accel_learned_calibration) { learned_cal = {}; @@ -132,6 +137,7 @@ void VehicleIMU::ParametersUpdate(bool force) if (gyro_calibration_count != _gyro_calibration.calibration_count()) { // if calibration changed reset any existing learned calibration _gyro_cal_available = false; + _in_flight_calibration_check_timestamp_last = hrt_absolute_time(); for (auto &learned_cal : _gyro_learned_calibration) { learned_cal = {}; @@ -155,13 +161,15 @@ void VehicleIMU::ParametersUpdate(bool force) _update_integrator_config = true; } } + + return updated; } void VehicleIMU::Run() { const hrt_abstime now_us = hrt_absolute_time(); - ParametersUpdate(); + const bool parameters_updated = ParametersUpdate(); if (!_accel_calibration.enabled() || !_gyro_calibration.enabled()) { _sensor_gyro_sub.unregisterCallback(); @@ -251,9 +259,17 @@ void VehicleIMU::Run() } } - if (hrt_elapsed_time(&_in_flight_calibration_check_timestamp_last) > 1_s) { - SensorCalibrationUpdate(); - _in_flight_calibration_check_timestamp_last = hrt_absolute_time(); + if (!parameters_updated) { + if (_armed) { + if (now_us > _in_flight_calibration_check_timestamp_last + 1_s) { + SensorCalibrationUpdate(); + _in_flight_calibration_check_timestamp_last = now_us; + } + + } else { + SensorCalibrationSaveAccel(); + SensorCalibrationSaveGyro(); + } } } @@ -703,45 +719,45 @@ void VehicleIMU::PrintStatus() void VehicleIMU::SensorCalibrationUpdate() { - if (_armed) { - for (int i = 0; i < _estimator_sensor_bias_subs.size(); i++) { - estimator_sensor_bias_s estimator_sensor_bias; + for (int i = 0; i < _estimator_sensor_bias_subs.size(); i++) { + estimator_sensor_bias_s estimator_sensor_bias; - if (_estimator_sensor_bias_subs[i].update(&estimator_sensor_bias) - && (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s)) { + if (_estimator_sensor_bias_subs[i].update(&estimator_sensor_bias) + && (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s)) { - // find corresponding accel bias - if (estimator_sensor_bias.accel_bias_valid && estimator_sensor_bias.accel_bias_stable - && (estimator_sensor_bias.accel_device_id != 0) - && (estimator_sensor_bias.accel_device_id == _accel_calibration.device_id())) { + // find corresponding accel bias + if (estimator_sensor_bias.accel_bias_valid && estimator_sensor_bias.accel_bias_stable + && (estimator_sensor_bias.accel_device_id != 0) + && (estimator_sensor_bias.accel_device_id == _accel_calibration.device_id())) { - const Vector3f bias{estimator_sensor_bias.accel_bias}; + const Vector3f bias{estimator_sensor_bias.accel_bias}; - _accel_learned_calibration[i].offset = _accel_calibration.BiasCorrectedSensorOffset(bias); - _accel_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.accel_bias_variance}; - _accel_learned_calibration[i].valid = true; - _accel_cal_available = true; - } + _accel_learned_calibration[i].offset = _accel_calibration.BiasCorrectedSensorOffset(bias); + _accel_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.accel_bias_variance}; + _accel_learned_calibration[i].valid = true; + _accel_cal_available = true; + } - // find corresponding gyro calibration - if (estimator_sensor_bias.gyro_bias_valid && estimator_sensor_bias.gyro_bias_stable - && (estimator_sensor_bias.gyro_device_id != 0) - && (estimator_sensor_bias.gyro_device_id == _gyro_calibration.device_id())) { + // find corresponding gyro calibration + if (estimator_sensor_bias.gyro_bias_valid && estimator_sensor_bias.gyro_bias_stable + && (estimator_sensor_bias.gyro_device_id != 0) + && (estimator_sensor_bias.gyro_device_id == _gyro_calibration.device_id())) { - const Vector3f bias{estimator_sensor_bias.gyro_bias}; + const Vector3f bias{estimator_sensor_bias.gyro_bias}; - _gyro_learned_calibration[i].offset = _gyro_calibration.BiasCorrectedSensorOffset(bias); - _gyro_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.gyro_bias_variance}; - _gyro_learned_calibration[i].valid = true; - _gyro_cal_available = true; - } + _gyro_learned_calibration[i].offset = _gyro_calibration.BiasCorrectedSensorOffset(bias); + _gyro_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.gyro_bias_variance}; + _gyro_learned_calibration[i].valid = true; + _gyro_cal_available = true; } } } +} - // not armed and accel cal available - if (!_armed && _accel_cal_available) { - const Vector3f accel_cal_orig{_accel_calibration.offset()}; +void VehicleIMU::SensorCalibrationSaveAccel() +{ + if (_accel_cal_available) { + const Vector3f cal_orig{_accel_calibration.offset()}; bool initialised = false; Vector3f offset_estimate{}; Vector3f bias_variance{}; @@ -763,14 +779,17 @@ void VehicleIMU::SensorCalibrationUpdate() bias_variance(axis_index) *= k2; } } + + // reset + _accel_learned_calibration[i] = {}; } } - if (initialised && (accel_cal_orig - offset_estimate).longerThan(0.05f)) { + if (initialised && (cal_orig - offset_estimate).longerThan(0.05f)) { if (_accel_calibration.set_offset(offset_estimate)) { - PX4_INFO("accel %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])", - _instance, _accel_calibration.device_id(), - (double)accel_cal_orig(0), (double)accel_cal_orig(1), (double)accel_cal_orig(2), + PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])", + _accel_calibration.SensorString(), _instance, _accel_calibration.device_id(), + (double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2), (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2)); if (_accel_calibration.ParametersSave()) { @@ -781,16 +800,13 @@ void VehicleIMU::SensorCalibrationUpdate() // reset _accel_cal_available = false; - - for (auto &learned_cal : _accel_learned_calibration) { - learned_cal = {}; - } } +} - - // not armed and gyro cal available - if (!_armed && _gyro_cal_available) { - const Vector3f gyro_cal_orig{_gyro_calibration.offset()}; +void VehicleIMU::SensorCalibrationSaveGyro() +{ + if (_gyro_cal_available) { + const Vector3f cal_orig{_gyro_calibration.offset()}; bool initialised = false; Vector3f offset_estimate{}; Vector3f bias_variance{}; @@ -812,14 +828,17 @@ void VehicleIMU::SensorCalibrationUpdate() bias_variance(axis_index) *= k2; } } + + // reset + _gyro_learned_calibration[i] = {}; } } - if (initialised && (gyro_cal_orig - offset_estimate).longerThan(0.01f)) { + if (initialised && (cal_orig - offset_estimate).longerThan(0.01f)) { if (_gyro_calibration.set_offset(offset_estimate)) { - PX4_INFO("gyro %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])", - _instance, _gyro_calibration.device_id(), - (double)gyro_cal_orig(0), (double)gyro_cal_orig(1), (double)gyro_cal_orig(2), + PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])", + _gyro_calibration.SensorString(), _instance, _gyro_calibration.device_id(), + (double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2), (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2)); if (_gyro_calibration.ParametersSave()) { @@ -830,10 +849,6 @@ void VehicleIMU::SensorCalibrationUpdate() // reset _gyro_cal_available = false; - - for (auto &learned_cal : _gyro_learned_calibration) { - learned_cal = {}; - } } } diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index 543b67f623..a7e390577f 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -76,7 +76,7 @@ public: void PrintStatus(); private: - void ParametersUpdate(bool force = false); + bool ParametersUpdate(bool force = false); bool Publish(); void Run() override; @@ -88,6 +88,8 @@ private: void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity); void SensorCalibrationUpdate(); + void SensorCalibrationSaveAccel(); + void SensorCalibrationSaveGyro(); uORB::PublicationMulti _vehicle_imu_pub{ORB_ID(vehicle_imu)}; uORB::PublicationMulti _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)}; @@ -173,7 +175,6 @@ private: InFlightCalibration _accel_learned_calibration[ORB_MULTI_MAX_INSTANCES] {}; InFlightCalibration _gyro_learned_calibration[ORB_MULTI_MAX_INSTANCES] {}; - perf_counter_t _accel_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": accel data gap")}; perf_counter_t _gyro_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap")};