Browse Source

sensors/vehicle_magnetometer: respect calibration offset update success

- the magnetometer calibration offset setter returns false if the new offset change is negligible
release/1.12
Daniel Agar 4 years ago committed by GitHub
parent
commit
b9a721726d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
  1. 23
      src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

23
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@ -189,9 +189,7 @@ void VehicleMagnetometer::MagCalibrationUpdate() @@ -189,9 +189,7 @@ void VehicleMagnetometer::MagCalibrationUpdate()
for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
// apply all valid saved offsets
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
if ((_calibration[mag_index].device_id() != 0)
&& (_mag_cal[i].device_id == _calibration[mag_index].device_id())
&& _mag_cal[i].mag_offset.longerThan(0.01f)) {
if ((_calibration[mag_index].device_id() != 0) && (_mag_cal[i].device_id == _calibration[mag_index].device_id())) {
Vector3f mag_cal_offset{_calibration[mag_index].offset()};
@ -207,15 +205,18 @@ void VehicleMagnetometer::MagCalibrationUpdate() @@ -207,15 +205,18 @@ void VehicleMagnetometer::MagCalibrationUpdate()
state_variance(axis_index) = fmaxf(state_variance(axis_index) * (1.f - kalman_gain), 0.f);
}
PX4_INFO("%d (%d) est. offset %d committed: [%.2f %.2f %.2f]->[%.2f %.2f %.2f] (full [%.2f %.2f %.2f])",
mag_index, _calibration[mag_index].device_id(), i,
(double)_calibration[mag_index].offset()(0), (double)_calibration[mag_index].offset()(1),
(double)_calibration[mag_index].offset()(2),
(double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2),
(double)_mag_cal[i].mag_offset(0), (double)_mag_cal[i].mag_offset(1), (double)_mag_cal[i].mag_offset(2));
if (_calibration[mag_index].set_offset(mag_cal_offset)) {
_calibration[mag_index].set_offset(mag_cal_offset);
calibration_param_save_needed = true;
PX4_INFO("%d (%d) EST:%d offset committed: [%.2f %.2f %.2f]->[%.2f %.2f %.2f] (full [%.2f %.2f %.2f])",
mag_index, _calibration[mag_index].device_id(), i,
(double)_calibration[mag_index].offset()(0),
(double)_calibration[mag_index].offset()(1),
(double)_calibration[mag_index].offset()(2),
(double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2),
(double)_mag_cal[i].mag_offset(0), (double)_mag_cal[i].mag_offset(1), (double)_mag_cal[i].mag_offset(2));
calibration_param_save_needed = true;
}
// clear
_mag_cal[i].device_id = 0;

Loading…
Cancel
Save