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

sensors: minor IMU bias saving updates 

- sensors/vehicle_imu: reset learned cal on any calibration change
during parameter update
 - sensors/vehicle_imu: cleanup logic estimated bias -> calibration offset
saving
   - don't invalidate saved calibration (the point is to keep the last valid)
   - remove old debug code, etc
 - sensors/vehicle_imu: notify parameter changes if accel or gyro
calibration has changed
 - lib/sensor_calibration: add calibrated() and calibration_index()
getters, keep Accelerometer/Gyroscope/Magnetometer in sync
This commit is contained in:
Daniel Agar 2022-01-06 14:19:26 -05:00
parent 5d7ddf5734
commit aec97e0020
8 changed files with 66 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/lib/sensor_calibration/Accelerometer.cpp
View File

@ -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

4
src/lib/sensor_calibration/Accelerometer.hpp
View File

@ -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; }

10
src/lib/sensor_calibration/Gyroscope.cpp
View File

@ -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

4
src/lib/sensor_calibration/Gyroscope.hpp
View File

@ -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; }

2
src/lib/sensor_calibration/Magnetometer.cpp
View File

@ -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

2
src/lib/sensor_calibration/Magnetometer.hpp
View File

@ -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

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

@ -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();
}
}
}

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

@ -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