Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 10:26:52 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

sensors/vehicle_magnetometer: more granular in flight mag cal reset on parameter changes

Browse Source 
- if mag calibration parameters change only reset any inflight mag cal
for the corresponding magnetometer
This commit is contained in:
Daniel Agar
2022-09-26 10:10:50 -04:00
parent 462d0af384
commit 7bc79b491f
1 changed files with 30 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
+30 -20
View File
@@ -140,41 +140,51 @@ bool VehicleMagnetometer::ParametersUpdate(bool force)
// update mag priority (CAL_MAGx_PRIO) // update mag priority (CAL_MAGx_PRIO)
for (int mag = 0; mag < MAX_SENSOR_COUNT; mag++) { for (int mag = 0; mag < MAX_SENSOR_COUNT; mag++) {
bool clear_online_mag_cal = false;
const auto calibration_count = _calibration[mag].calibration_count(); const auto calibration_count = _calibration[mag].calibration_count();
const int32_t priority_old = _calibration[mag].priority(); const int32_t priority_old = _calibration[mag].priority();
_calibration[mag].ParametersUpdate(); _calibration[mag].ParametersUpdate();
const int32_t priority_new = _calibration[mag].priority(); const int32_t priority_new = _calibration[mag].priority();
if (priority_old != priority_new) { if (_calibration[mag].enabled()) {
if (_priority[mag] == priority_old) { if (priority_old != priority_new) {
_priority[mag] = priority_new; if (_priority[mag] == priority_old) {
_priority[mag] = priority_new;
} else { } else {
// change relative priority to incorporate any sensor faults // change relative priority to incorporate any sensor faults
int priority_change = priority_new - priority_old; int priority_change = priority_new - priority_old;
_priority[mag] = math::constrain(_priority[mag] + priority_change, 1, 100); _priority[mag] = math::constrain(_priority[mag] + priority_change, 1, 100);
}
} }
if (calibration_count != _calibration[mag].calibration_count()) {
calibration_updated = true;
clear_online_mag_cal = true;
}
} else {
_priority[mag] = 0;
clear_online_mag_cal = true;
} }
if (calibration_count != _calibration[mag].calibration_count()) { if (clear_online_mag_cal) {
calibration_updated = true; // clear any mag bias estimate
_calibration_estimator_bias[mag].zero();
for (auto &cal : _mag_cal) {
// clear any in flight mag calibration
if (cal.device_id == _calibration[mag].device_id()) {
cal = {};
}
}
} }
} }
if (calibration_updated) { if (calibration_updated) {
// clear all
for (auto &bias : _calibration_estimator_bias) {
bias.zero();
}
for (auto &cal : _mag_cal) {
cal = {};
}
_in_flight_mag_cal_available = false;
_last_calibration_update = hrt_absolute_time(); _last_calibration_update = hrt_absolute_time();
} }
} }
return true; return true;