From d7812f83f15ea66497434e1adaad24a12747d4ff Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 20 Jan 2021 14:11:37 +0100 Subject: [PATCH] mag_cal: fix race condition causing invalid mag calibration In some cases, the fitness of the optimizer does not shrink at each iteration, causing the "decreasing fitness check" to fail. This stops the optimization and returns with sub-optimal, or unrealistic results. --- src/modules/commander/mag_calibration.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index fe79b78794..618e5641ab 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -587,8 +587,13 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma sphere_fit_success = true; } else { - // stop here if fit has previously succeeded - if (sphere_fit_success) { + // stop here if fit has previously succeeded an if it is good enough + // TODO: extract those conditions for the unit test + if (sphere_fit_success + && (i > 10) + && (fitness < 0.01f) + && (sphere_radius[cur_mag] >= 0.2f) + && (sphere_radius[cur_mag] <= 0.7f)) { break; } }