Browse Source

sensors/vehicle_imu: fix SENS_IMU_AUTOCAL saving logic

- on cycles that don't check for updated calibraton (estimated bias) we
check if there's anything valid to save (when disarmed)
master
Daniel Agar 3 years ago
parent
commit
2eec7842ae
  1. 12
      src/modules/sensors/vehicle_imu/VehicleIMU.cpp

12
src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@ -260,13 +260,13 @@ void VehicleIMU::Run()
} }
if (_param_sens_imu_autocal.get() && !parameters_updated) { if (_param_sens_imu_autocal.get() && !parameters_updated) {
if (_armed || !_accel_calibration.calibrated() || !_gyro_calibration.calibrated()) { if ((_armed || !_accel_calibration.calibrated() || !_gyro_calibration.calibrated())
if (now_us > _in_flight_calibration_check_timestamp_last + 1_s) { && (now_us > _in_flight_calibration_check_timestamp_last + 1_s)) {
SensorCalibrationUpdate(); SensorCalibrationUpdate();
_in_flight_calibration_check_timestamp_last = now_us; _in_flight_calibration_check_timestamp_last = now_us;
}
} else { } else if (!_armed) {
SensorCalibrationSaveAccel(); SensorCalibrationSaveAccel();
SensorCalibrationSaveGyro(); SensorCalibrationSaveGyro();
} }
@ -785,7 +785,7 @@ void VehicleIMU::SensorCalibrationSaveAccel()
} }
} }
if (initialised && (cal_orig - offset_estimate).longerThan(0.05f)) { if (initialised && ((cal_orig - offset_estimate).longerThan(0.05f) || !_accel_calibration.calibrated())) {
if (_accel_calibration.set_offset(offset_estimate)) { if (_accel_calibration.set_offset(offset_estimate)) {
PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])", PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])",
_accel_calibration.SensorString(), _instance, _accel_calibration.device_id(), _accel_calibration.SensorString(), _instance, _accel_calibration.device_id(),
@ -873,7 +873,7 @@ void VehicleIMU::SensorCalibrationSaveGyro()
} }
} }
if (initialised && (cal_orig - offset_estimate).longerThan(0.01f)) { if (initialised && ((cal_orig - offset_estimate).longerThan(0.01f) || !_gyro_calibration.calibrated())) {
if (_gyro_calibration.set_offset(offset_estimate)) { if (_gyro_calibration.set_offset(offset_estimate)) {
PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])", PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])",
_gyro_calibration.SensorString(), _instance, _gyro_calibration.device_id(), _gyro_calibration.SensorString(), _instance, _gyro_calibration.device_id(),

Loading…
Cancel
Save