fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 10:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensor_calibration: refactor and centralize calibration slot logic

Browse Source 
- centralize logic for selecting a preferred calibration slot
   - automatically use existing calibration slot if it exists, otherwise
find first available slot, with a preference for a requested index
 - existing commander calibration methods rewrite all calibration slots
to match current sensor ordering
This commit is contained in:
Daniel Agar
2022-01-12 16:16:26 -05:00
parent d1304e1ceb
commit bb1177d504
18 changed files with 202 additions and 175 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
+8 -43
View File
@@ -186,53 +186,18 @@ void VehicleMagnetometer::UpdateMagBiasEstimate()
if (_param_sens_mag_autocal.get() && !_armed && mag_bias_est.stable[mag_index]
&& (_calibration[mag_index].device_id() != 0) && !_calibration[mag_index].calibrated()) {
if (_calibration[mag_index].calibration_index() < 0) {
// find available slots
uint32_t cal_device_ids[MAX_SENSOR_COUNT] {};
bool cal_slot_match = false;
// set initial mag calibration
const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]);
for (unsigned cal_index = 0; cal_index < MAX_SENSOR_COUNT; cal_index++) {
char str[20] {};
sprintf(str, "CAL_%s%u_ID", "MAG", cal_index);
int32_t device_id_val = 0;
if (_calibration[mag_index].set_offset(offset)) {
_calibration[mag_index].set_temperature(_last_data[mag_index].temperature);
if (param_get(param_find_no_notification(str), &device_id_val) == PX4_OK) {
cal_device_ids[cal_index] = device_id_val;
// save parameters with preferred calibration slot to current sensor index
_calibration[mag_index].ParametersSave(mag_index);
if (cal_device_ids[cal_index] == _calibration[mag_index].device_id()) {
cal_slot_match = true;
_calibration[mag_index].set_calibration_index(cal_index);
}
}
}
_calibration_estimator_bias[mag_index].zero();
if (!cal_slot_match) {
// prefer slot that matches sensor instance
if (cal_device_ids[mag_index] == 0) {
_calibration[mag_index].set_calibration_index(mag_index);
} else {
for (int cal_index = 0; cal_index < MAX_SENSOR_COUNT; cal_index++) {
if (cal_device_ids[mag_index] == 0) {
_calibration[mag_index].set_calibration_index(cal_index);
break;
}
}
}
}
}
// set initial mag calibration if calibration index is set successfully
if (_calibration[mag_index].calibration_index() >= 0) {
const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]);
if (_calibration[mag_index].set_offset(offset)) {
_calibration[mag_index].set_temperature(_last_data[mag_index].temperature);
_calibration[mag_index].ParametersSave();
_calibration_estimator_bias[mag_index].zero();
parameters_notify = true;
}
parameters_notify = true;
}
}
}