From b71f766affb83449e80628c7aef710816d042c80 Mon Sep 17 00:00:00 2001 From: Siddharth Bharat Purohit Date: Fri, 18 Nov 2016 13:01:00 +0530 Subject: [PATCH] AP_AccelCal: fix bug preventing accel cal fit to run more than one iteration The check for fitness being smaller than the last value should use the absolute value, to mean the fitness isn't changing since last iteration. It's currently always quiting the function after the first iteration. However for Gauss Newton we anyway want to run as many iterations as we can, because there are ups and downs along the iteration so we might get say best result where before and after iterations were bad. The lines above takes care that sane and the best is selected. (cherry picked from commit 451c2f146dd9c5ba0f76773e0ce39ea7227be421) --- libraries/AP_AccelCal/AccelCalibrator.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/libraries/AP_AccelCal/AccelCalibrator.cpp b/libraries/AP_AccelCal/AccelCalibrator.cpp index 5712da567a..d348a8f41e 100644 --- a/libraries/AP_AccelCal/AccelCalibrator.cpp +++ b/libraries/AP_AccelCal/AccelCalibrator.cpp @@ -334,8 +334,6 @@ void AccelCalibrator::run_fit(uint8_t max_iterations, float& fitness) uint8_t num_iterations = 0; while(num_iterations < max_iterations) { - float last_fitness = fitness; - float JTJ[ACCEL_CAL_MAX_NUM_PARAMS*ACCEL_CAL_MAX_NUM_PARAMS] {}; VectorP JTFI; @@ -379,9 +377,6 @@ void AccelCalibrator::run_fit(uint8_t max_iterations, float& fitness) } num_iterations++; - if (fitness - last_fitness < 1.0e-9f) { - break; - } } }