sensor_calibration: refactor and centralize calibration slot logic

- 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
@@ -69,7 +69,7 @@ bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_s
calibration_valid = true;
} else {
calibration_valid = (calibration::FindCalibrationIndex("ACC", device_id) >= 0);
calibration_valid = (calibration::FindCurrentCalibrationIndex("ACC", device_id) >= 0);
}
if (!calibration_valid) {
@@ -68,7 +68,7 @@ bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &
calibration_valid = true;
} else {
calibration_valid = (calibration::FindCalibrationIndex("GYRO", device_id) >= 0);
calibration_valid = (calibration::FindCurrentCalibrationIndex("GYRO", device_id) >= 0);
}
if (!calibration_valid) {
@@ -70,7 +70,7 @@ bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_st
calibration_valid = true;
} else {
calibration_valid = (calibration::FindCalibrationIndex("MAG", device_id) >= 0);
calibration_valid = (calibration::FindCurrentCalibrationIndex("MAG", device_id) >= 0);
}
if (!calibration_valid) {
@@ -350,9 +350,6 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
} else {
calibrations[cur_accel].Reset();
}
// reset calibration index to match uORB numbering
calibrations[cur_accel].set_calibration_index(cur_accel);
}
if (active_sensors == 0) {
@@ -435,7 +432,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
calibrations[i].PrintStatus();
if (calibrations[i].ParametersSave()) {
if (calibrations[i].ParametersSave(i, true)) {
param_save = true;
failed = false;
@@ -587,9 +584,6 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
calibration::Accelerometer calibration{arp.device_id};
// reset cal index to uORB
calibration.set_calibration_index(accel_index);
if (!calibrated || (offset.norm() > CONSTANTS_ONE_G)
|| !PX4_ISFINITE(offset(0))
|| !PX4_ISFINITE(offset(1))
@@ -601,7 +595,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
calibration.set_offset(offset);
calibration.set_temperature(temperature_avg);
if (calibration.ParametersSave()) {
if (calibration.ParametersSave(accel_index)) {
calibration.PrintStatus();
param_save = true;
failed = false;
+1 -6
View File
@@ -202,9 +202,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
if (gyro_sub.advertised() && (gyro_sub.get().device_id != 0) && (gyro_sub.get().timestamp > 0)) {
worker_data.calibrations[cur_gyro].set_device_id(gyro_sub.get().device_id);
}
// reset calibration index to match uORB numbering
worker_data.calibrations[cur_gyro].set_calibration_index(cur_gyro);
}
unsigned try_count = 0;
@@ -274,11 +271,9 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
calibration.set_offset(worker_data.offset[uorb_index]);
calibration.set_temperature(worker_data.temperature[uorb_index]);
calibration.set_calibration_index(uorb_index);
calibration.PrintStatus();
if (calibration.ParametersSave()) {
if (calibration.ParametersSave(uorb_index, true)) {
param_save = true;
failed = false;
+2 -7
View File
@@ -914,11 +914,9 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
current_cal.set_temperature(worker_data.temperature[cur_mag]);
current_cal.set_calibration_index(cur_mag);
current_cal.PrintStatus();
if (current_cal.ParametersSave()) {
if (current_cal.ParametersSave(cur_mag, true)) {
param_save = true;
failed = false;
@@ -1018,16 +1016,13 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
calibration::Magnetometer cal{mag.device_id};
// force calibration index to uORB index
cal.set_calibration_index(cur_mag);
// use any existing scale and store the offset to the expected earth field
const Vector3f offset = Vector3f{mag.x, mag.y, mag.z} - (cal.scale().I() * cal.rotation().transpose() * expected_field);
cal.set_offset(offset);
cal.set_temperature(mag.temperature);
// save new calibration
if (cal.ParametersSave()) {
if (cal.ParametersSave(cur_mag)) {
cal.PrintStatus();
param_save = true;
failed = false;