mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 17:53:34 +08:00
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:
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user