diff --git a/src/lib/sensor_calibration/Accelerometer.cpp b/src/lib/sensor_calibration/Accelerometer.cpp index bd8c9b96cb..47449faf09 100644 --- a/src/lib/sensor_calibration/Accelerometer.cpp +++ b/src/lib/sensor_calibration/Accelerometer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -60,10 +60,6 @@ void Accelerometer::set_device_id(uint32_t device_id, bool external) set_external(external); _device_id = device_id; - if (_device_id != 0) { - _calibration_index = FindCalibrationIndex(SensorString(), _device_id); - } - ParametersUpdate(); SensorCorrectionsUpdate(true); } @@ -166,6 +162,10 @@ void Accelerometer::set_rotation(Rotation rotation) void Accelerometer::ParametersUpdate() { + if (_device_id != 0) { + _calibration_index = FindCalibrationIndex(SensorString(), _device_id); + } + if (_calibration_index >= 0) { // CAL_ACCx_ROT diff --git a/src/lib/sensor_calibration/Accelerometer.hpp b/src/lib/sensor_calibration/Accelerometer.hpp index 98d9c99e2e..c50d7e5900 100644 --- a/src/lib/sensor_calibration/Accelerometer.hpp +++ b/src/lib/sensor_calibration/Accelerometer.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -67,7 +67,9 @@ public: void set_rotation(Rotation rotation); void set_temperature(float temperature) { _temperature = temperature; }; + bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); } uint8_t calibration_count() const { return _calibration_count; } + int8_t calibration_index() const { return _calibration_index; } uint32_t device_id() const { return _device_id; } bool enabled() const { return (_priority > 0); } bool external() const { return _external; } diff --git a/src/lib/sensor_calibration/Gyroscope.cpp b/src/lib/sensor_calibration/Gyroscope.cpp index 815e782036..ec610ef093 100644 --- a/src/lib/sensor_calibration/Gyroscope.cpp +++ b/src/lib/sensor_calibration/Gyroscope.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -60,10 +60,6 @@ void Gyroscope::set_device_id(uint32_t device_id, bool external) set_external(external); _device_id = device_id; - if (_device_id != 0) { - _calibration_index = FindCalibrationIndex(SensorString(), _device_id); - } - ParametersUpdate(); SensorCorrectionsUpdate(true); } @@ -151,6 +147,10 @@ void Gyroscope::set_rotation(Rotation rotation) void Gyroscope::ParametersUpdate() { + if (_device_id != 0) { + _calibration_index = FindCalibrationIndex(SensorString(), _device_id); + } + if (_calibration_index >= 0) { // CAL_GYROx_ROT diff --git a/src/lib/sensor_calibration/Gyroscope.hpp b/src/lib/sensor_calibration/Gyroscope.hpp index 8162a5909e..f7ea9f6038 100644 --- a/src/lib/sensor_calibration/Gyroscope.hpp +++ b/src/lib/sensor_calibration/Gyroscope.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -66,7 +66,9 @@ public: void set_rotation(Rotation rotation); void set_temperature(float temperature) { _temperature = temperature; }; + bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); } uint8_t calibration_count() const { return _calibration_count; } + int8_t calibration_index() const { return _calibration_index; } uint32_t device_id() const { return _device_id; } bool enabled() const { return (_priority > 0); } bool external() const { return _external; } diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp index 54011e2858..e4cf35686d 100644 --- a/src/lib/sensor_calibration/Magnetometer.cpp +++ b/src/lib/sensor_calibration/Magnetometer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index 663416aaff..c3412402ce 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor_calibration/Magnetometer.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 07d8d40c33..e39e9669f7 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -115,9 +115,29 @@ void VehicleIMU::ParametersUpdate(bool force) updateParams(); + const auto accel_calibration_count = _accel_calibration.calibration_count(); + const auto gyro_calibration_count = _gyro_calibration.calibration_count(); _accel_calibration.ParametersUpdate(); _gyro_calibration.ParametersUpdate(); + if (accel_calibration_count != _accel_calibration.calibration_count()) { + // if calibration changed reset any existing learned calibration + _accel_cal_available = false; + + for (auto &learned_cal : _accel_learned_calibration) { + learned_cal = {}; + } + } + + if (gyro_calibration_count != _gyro_calibration.calibration_count()) { + // if calibration changed reset any existing learned calibration + _gyro_cal_available = false; + + for (auto &learned_cal : _gyro_learned_calibration) { + learned_cal = {}; + } + } + // constrain IMU integration time 1-10 milliseconds (100-1000 Hz) int32_t imu_integration_rate_hz = constrain(_param_imu_integ_rate.get(), (int32_t)100, math::max(_param_imu_gyro_ratemax.get(), (int32_t) 1000)); @@ -687,75 +707,38 @@ void VehicleIMU::SensorCalibrationUpdate() 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)) { + 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_device_id != 0) - && (_accel_calibration.device_id() == estimator_sensor_bias.accel_device_id)) { + 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_variance{estimator_sensor_bias.accel_bias_variance}; - - const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s) && estimator_sensor_bias.accel_bias_valid - && estimator_sensor_bias.accel_bias_stable; - - if (valid) { - const Vector3f offset_old{_accel_learned_calibration[i].offset}; - - _accel_learned_calibration[i].offset = _accel_calibration.BiasCorrectedSensorOffset(bias); - _accel_learned_calibration[i].bias_variance = bias_variance; - _accel_learned_calibration[i].valid = true; - _accel_cal_available = true; - - if ((offset_old - _accel_learned_calibration[i].offset).longerThan(0.05f)) { - PX4_DEBUG("accel %d (%" PRIu32 ") new offset: [%.2f %.2f %.2f] (full bias [%.2f %.2f %.2f])", - _instance, _accel_calibration.device_id(), - (double)_accel_learned_calibration[i].offset(0), - (double)_accel_learned_calibration[i].offset(1), - (double)_accel_learned_calibration[i].offset(2), - (double)bias(0), (double)bias(1), (double)bias(2)); - } - } else { - _accel_learned_calibration[i].valid = false; - } + _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_device_id != 0) - && (_gyro_calibration.device_id() == estimator_sensor_bias.gyro_device_id)) { + 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_variance{estimator_sensor_bias.gyro_bias_variance}; - - const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s) && estimator_sensor_bias.gyro_bias_valid - && estimator_sensor_bias.gyro_bias_stable; - - if (valid) { - const Vector3f offset_old{_gyro_learned_calibration[i].offset}; - - _gyro_learned_calibration[i].offset = _gyro_calibration.BiasCorrectedSensorOffset(bias); - _gyro_learned_calibration[i].bias_variance = bias_variance; - _gyro_learned_calibration[i].valid = true; - _gyro_cal_available = true; - - if ((offset_old - _gyro_learned_calibration[i].offset).longerThan(0.01f)) { - PX4_DEBUG("gyro %d (%" PRIu32 ") new offset: [%.2f %.2f %.2f] (full bias [%.2f %.2f %.2f])", - _instance, _gyro_calibration.device_id(), - (double)_gyro_learned_calibration[i].offset(0), - (double)_gyro_learned_calibration[i].offset(1), - (double)_gyro_learned_calibration[i].offset(2), - (double)bias(0), (double)bias(1), (double)bias(2)); - } - } else { - _gyro_learned_calibration[i].valid = false; - } + _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()}; @@ -785,12 +768,14 @@ void VehicleIMU::SensorCalibrationUpdate() if (initialised && (accel_cal_orig - offset_estimate).longerThan(0.05f)) { if (_accel_calibration.set_offset(offset_estimate)) { - PX4_INFO("accel %d (%" PRIu32 ") offset committed: [%.2f %.2f %.2f]->[%.2f %.2f %.2f])", + 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), (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2)); - _accel_calibration.ParametersSave(); + if (_accel_calibration.ParametersSave()) { + param_notify_changes(); + } } } @@ -832,12 +817,14 @@ void VehicleIMU::SensorCalibrationUpdate() if (initialised && (gyro_cal_orig - offset_estimate).longerThan(0.01f)) { if (_gyro_calibration.set_offset(offset_estimate)) { - PX4_INFO("gyro %d (%" PRIu32 ") offset committed: [%.2f %.2f %.2f]->[%.2f %.2f %.2f])", + 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), (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2)); - _gyro_calibration.ParametersSave(); + if (_gyro_calibration.ParametersSave()) { + param_notify_changes(); + } } } diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index b807444994..543b67f623 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions