Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: refactor run_fit_chunk logic

Browse Source
This commit is contained in:
Jonathan Challinger 2015-03-11 23:52:44 -07:00 committed by Andrew Tridgell
parent f2acf2cb88
commit 6d45660568
1 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_Compass/CompassCalibrator.cpp
View File

@ -88,29 +88,29 @@ void CompassCalibrator::run_fit_chunk() {
} }
if(_status == COMPASS_CAL_RUNNING_STEP_ONE) { if(_status == COMPASS_CAL_RUNNING_STEP_ONE) {
if(_fit_step < 10) { if (_fit_step >= 10) {
run_sphere_fit();
_fit_step++;
} else {
if(_fitness == _initial_fitness || isnan(_fitness)) { //if true, means that fitness is diverging instead of converging if(_fitness == _initial_fitness || isnan(_fitness)) { //if true, means that fitness is diverging instead of converging
set_status(COMPASS_CAL_FAILED); set_status(COMPASS_CAL_FAILED);
} }
set_status(COMPASS_CAL_RUNNING_STEP_TWO); set_status(COMPASS_CAL_RUNNING_STEP_TWO);
} else {
run_sphere_fit();
_fit_step++;
} }
} else if(_status == COMPASS_CAL_RUNNING_STEP_TWO) { } else if(_status == COMPASS_CAL_RUNNING_STEP_TWO) {
if(_fit_step < 15) { if (_fit_step >= 35) {
run_sphere_fit();
} else if(_fit_step < 35) {
run_ellipsoid_fit();
} else {
if(fit_acceptable()) { if(fit_acceptable()) {
set_status(COMPASS_CAL_SUCCESS); set_status(COMPASS_CAL_SUCCESS);
} else { } else {
set_status(COMPASS_CAL_FAILED); set_status(COMPASS_CAL_FAILED);
} }
} else if (_fit_step < 15) {
run_sphere_fit();
_fit_step++;
} else {
run_ellipsoid_fit();
_fit_step++;
} }
_fit_step++;
} }
} }