dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sensors/vehicle_imu: refactor SensorCalibrationUpdate() to separate accel/gyro cal saving

This commit is contained in:
Daniel Agar 2022-01-06 14:43:20 -05:00
parent aec97e0020
commit c17a9e8003
2 changed files with 71 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

121
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
View File

@ -103,8 +103,10 @@ void VehicleIMU::Stop()
Deinit(); Deinit();
} }
void VehicleIMU::ParametersUpdate(bool force) bool VehicleIMU::ParametersUpdate(bool force)
{ {
bool updated = false;
// Check if parameters have changed // Check if parameters have changed
if (_parameter_update_sub.updated() || force) { if (_parameter_update_sub.updated() || force) {
// clear update // clear update
@ -115,6 +117,8 @@ void VehicleIMU::ParametersUpdate(bool force)
updateParams(); updateParams();
updated = true;
const auto accel_calibration_count = _accel_calibration.calibration_count(); const auto accel_calibration_count = _accel_calibration.calibration_count();
const auto gyro_calibration_count = _gyro_calibration.calibration_count(); const auto gyro_calibration_count = _gyro_calibration.calibration_count();
_accel_calibration.ParametersUpdate(); _accel_calibration.ParametersUpdate();
@ -123,6 +127,7 @@ void VehicleIMU::ParametersUpdate(bool force)
if (accel_calibration_count != _accel_calibration.calibration_count()) { if (accel_calibration_count != _accel_calibration.calibration_count()) {
// if calibration changed reset any existing learned calibration // if calibration changed reset any existing learned calibration
_accel_cal_available = false; _accel_cal_available = false;
_in_flight_calibration_check_timestamp_last = hrt_absolute_time();
for (auto &learned_cal : _accel_learned_calibration) { for (auto &learned_cal : _accel_learned_calibration) {
learned_cal = {}; learned_cal = {};
@ -132,6 +137,7 @@ void VehicleIMU::ParametersUpdate(bool force)
if (gyro_calibration_count != _gyro_calibration.calibration_count()) { if (gyro_calibration_count != _gyro_calibration.calibration_count()) {
// if calibration changed reset any existing learned calibration // if calibration changed reset any existing learned calibration
_gyro_cal_available = false; _gyro_cal_available = false;
_in_flight_calibration_check_timestamp_last = hrt_absolute_time();
for (auto &learned_cal : _gyro_learned_calibration) { for (auto &learned_cal : _gyro_learned_calibration) {
learned_cal = {}; learned_cal = {};
@ -155,13 +161,15 @@ void VehicleIMU::ParametersUpdate(bool force)
_update_integrator_config = true; _update_integrator_config = true;
} }
} }
return updated;
} }
void VehicleIMU::Run() void VehicleIMU::Run()
{ {
const hrt_abstime now_us = hrt_absolute_time(); const hrt_abstime now_us = hrt_absolute_time();
ParametersUpdate(); const bool parameters_updated = ParametersUpdate();
if (!_accel_calibration.enabled() || !_gyro_calibration.enabled()) { if (!_accel_calibration.enabled() || !_gyro_calibration.enabled()) {
_sensor_gyro_sub.unregisterCallback(); _sensor_gyro_sub.unregisterCallback();
@ -251,9 +259,17 @@ void VehicleIMU::Run()
} }
} }
if (hrt_elapsed_time(&_in_flight_calibration_check_timestamp_last) > 1_s) { if (!parameters_updated) {
SensorCalibrationUpdate(); if (_armed) {
_in_flight_calibration_check_timestamp_last = hrt_absolute_time(); 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() void VehicleIMU::SensorCalibrationUpdate()
{ {
if (_armed) { for (int i = 0; i < _estimator_sensor_bias_subs.size(); i++) {
for (int i = 0; i < _estimator_sensor_bias_subs.size(); i++) { estimator_sensor_bias_s estimator_sensor_bias;
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)) { && (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s)) {
// find corresponding accel bias // find corresponding accel bias
if (estimator_sensor_bias.accel_bias_valid && estimator_sensor_bias.accel_bias_stable 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 != 0)
&& (estimator_sensor_bias.accel_device_id == _accel_calibration.device_id())) { && (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].offset = _accel_calibration.BiasCorrectedSensorOffset(bias);
_accel_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.accel_bias_variance}; _accel_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.accel_bias_variance};
_accel_learned_calibration[i].valid = true; _accel_learned_calibration[i].valid = true;
_accel_cal_available = true; _accel_cal_available = true;
} }
// find corresponding gyro calibration // find corresponding gyro calibration
if (estimator_sensor_bias.gyro_bias_valid && estimator_sensor_bias.gyro_bias_stable 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 != 0)
&& (estimator_sensor_bias.gyro_device_id == _gyro_calibration.device_id())) { && (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].offset = _gyro_calibration.BiasCorrectedSensorOffset(bias);
_gyro_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.gyro_bias_variance}; _gyro_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.gyro_bias_variance};
_gyro_learned_calibration[i].valid = true; _gyro_learned_calibration[i].valid = true;
_gyro_cal_available = true; _gyro_cal_available = true;
}
} }
} }
} }
}
// not armed and accel cal available void VehicleIMU::SensorCalibrationSaveAccel()
if (!_armed && _accel_cal_available) { {
const Vector3f accel_cal_orig{_accel_calibration.offset()}; if (_accel_cal_available) {
const Vector3f cal_orig{_accel_calibration.offset()};
bool initialised = false; bool initialised = false;
Vector3f offset_estimate{}; Vector3f offset_estimate{};
Vector3f bias_variance{}; Vector3f bias_variance{};
@ -763,14 +779,17 @@ void VehicleIMU::SensorCalibrationUpdate()
bias_variance(axis_index) *= k2; 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)) { if (_accel_calibration.set_offset(offset_estimate)) {
PX4_INFO("accel %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])", PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])",
_instance, _accel_calibration.device_id(), _accel_calibration.SensorString(), _instance, _accel_calibration.device_id(),
(double)accel_cal_orig(0), (double)accel_cal_orig(1), (double)accel_cal_orig(2), (double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2),
(double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2)); (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2));
if (_accel_calibration.ParametersSave()) { if (_accel_calibration.ParametersSave()) {
@ -781,16 +800,13 @@ void VehicleIMU::SensorCalibrationUpdate()
// reset // reset
_accel_cal_available = false; _accel_cal_available = false;
for (auto &learned_cal : _accel_learned_calibration) {
learned_cal = {};
}
} }
}
void VehicleIMU::SensorCalibrationSaveGyro()
// not armed and gyro cal available {
if (!_armed && _gyro_cal_available) { if (_gyro_cal_available) {
const Vector3f gyro_cal_orig{_gyro_calibration.offset()}; const Vector3f cal_orig{_gyro_calibration.offset()};
bool initialised = false; bool initialised = false;
Vector3f offset_estimate{}; Vector3f offset_estimate{};
Vector3f bias_variance{}; Vector3f bias_variance{};
@ -812,14 +828,17 @@ void VehicleIMU::SensorCalibrationUpdate()
bias_variance(axis_index) *= k2; 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)) { if (_gyro_calibration.set_offset(offset_estimate)) {
PX4_INFO("gyro %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])", PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])",
_instance, _gyro_calibration.device_id(), _gyro_calibration.SensorString(), _instance, _gyro_calibration.device_id(),
(double)gyro_cal_orig(0), (double)gyro_cal_orig(1), (double)gyro_cal_orig(2), (double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2),
(double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2)); (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2));
if (_gyro_calibration.ParametersSave()) { if (_gyro_calibration.ParametersSave()) {
@ -830,10 +849,6 @@ void VehicleIMU::SensorCalibrationUpdate()
// reset // reset
_gyro_cal_available = false; _gyro_cal_available = false;
for (auto &learned_cal : _gyro_learned_calibration) {
learned_cal = {};
}
} }
} }

5
src/modules/sensors/vehicle_imu/VehicleIMU.hpp
View File

@ -76,7 +76,7 @@ public:
void PrintStatus(); void PrintStatus();
private: private:
void ParametersUpdate(bool force = false); bool ParametersUpdate(bool force = false);
bool Publish(); bool Publish();
void Run() override; void Run() override;
@ -88,6 +88,8 @@ private:
void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity); void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity);
void SensorCalibrationUpdate(); void SensorCalibrationUpdate();
void SensorCalibrationSaveAccel();
void SensorCalibrationSaveGyro();
uORB::PublicationMulti<vehicle_imu_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)}; uORB::PublicationMulti<vehicle_imu_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)};
uORB::PublicationMulti<vehicle_imu_status_s> _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)}; uORB::PublicationMulti<vehicle_imu_status_s> _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)};
@ -173,7 +175,6 @@ private:
InFlightCalibration _accel_learned_calibration[ORB_MULTI_MAX_INSTANCES] {}; InFlightCalibration _accel_learned_calibration[ORB_MULTI_MAX_INSTANCES] {};
InFlightCalibration _gyro_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 _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")}; perf_counter_t _gyro_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap")};