From 6473f1458fc22edada9883428e9c29ce8352a283 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 30 Mar 2017 09:23:15 +1100 Subject: [PATCH] commander: reset learned ekf2 mag biases when performing a mag cal The learned ekf2 mag bias values are invalidated when the sensor calibration is updated and must be reset. --- src/modules/commander/mag_calibration.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index a7c7b63cd3..3742e8fd27 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -113,6 +113,24 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub) char str[30]; + // reset the learned EKF mag in-flight bias offsets which have been learned for the previous + // sensor calibration and will be invalidated by a new sensor calibration + (void)sprintf(str, "EKF2_MAGBIAS_X"); + result = param_set(param_find(str), &mscale_null.x_offset); + if (result != PX4_OK) { + PX4_ERR("unable to reset %s", str); + } + (void)sprintf(str, "EKF2_MAGBIAS_Y"); + result = param_set(param_find(str), &mscale_null.y_offset); + if (result != PX4_OK) { + PX4_ERR("unable to reset %s", str); + } + (void)sprintf(str, "EKF2_MAGBIAS_Z"); + result = param_set(param_find(str), &mscale_null.z_offset); + if (result != PX4_OK) { + PX4_ERR("unable to reset %s", str); + } + for (size_t i = 0; i < max_mags; i++) { device_ids[i] = 0; // signals no mag }