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

sensors: preserve all valid calibration parameters even if sensor currently missing

Browse Source 
- mark calibration slots active for first N sensors found
 - calibration procedure don't bother resetting unused slots
This commit is contained in:
Daniel Agar
2021-06-07 08:50:32 -04:00
parent 2ccd86102b
commit 4f850c7cd0
7 changed files with 125 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/accelerometer_calibration.cpp
+9 -9
View File
@@ -418,17 +418,17 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
accel_T_rotated.print();
#endif // DEBUD_BUILD
calibrations[i].PrintStatus();
}
// save all calibrations including empty slots
if (calibrations[i].ParametersSave()) {
param_save = true;
failed = false;
} else {
failed = true;
calibration_log_critical(mavlink_log_pub, "calibration save failed");
break;
if (calibrations[i].ParametersSave()) {
param_save = true;
failed = false;
} else {
failed = true;
calibration_log_critical(mavlink_log_pub, "calibration save failed");
break;
}
}
}
src/modules/commander/gyro_calibration.cpp
+10 -13
View File
@@ -260,22 +260,19 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
if (calibration.device_id() != 0) {
calibration.set_offset(worker_data.offset[uorb_index]);
calibration.set_calibration_index(uorb_index);
calibration.PrintStatus();
} else {
calibration.Reset();
}
if (calibration.ParametersSave()) {
param_save = true;
failed = false;
calibration.set_calibration_index(uorb_index);
if (calibration.ParametersSave()) {
param_save = true;
failed = false;
} else {
failed = true;
calibration_log_critical(mavlink_log_pub, "calibration save failed");
break;
} else {
failed = true;
calibration_log_critical(mavlink_log_pub, "calibration save failed");
break;
}
}
}
src/modules/commander/mag_calibration.cpp
+11 -14
View File
@@ -897,22 +897,19 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
current_cal.set_offdiagonal(offdiag[cur_mag]);
}
current_cal.set_calibration_index(cur_mag);
current_cal.PrintStatus();
} else {
current_cal.Reset();
}
if (current_cal.ParametersSave()) {
param_save = true;
failed = false;
current_cal.set_calibration_index(cur_mag);
if (current_cal.ParametersSave()) {
param_save = true;
failed = false;
} else {
failed = true;
calibration_log_critical(mavlink_log_pub, "calibration save failed");
break;
} else {
failed = true;
calibration_log_critical(mavlink_log_pub, "calibration save failed");
break;
}
}
}
@@ -1000,7 +997,7 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
sensor_mag_s mag{};
mag_sub.copy(&mag);
if (mag_sub.advertised() && (mag.timestamp != 0)) {
if (mag_sub.advertised() && (mag.timestamp != 0) && (mag.device_id != 0)) {
calibration::Magnetometer cal{mag.device_id, mag.is_external};