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

Compass: Add conditions to check sanity of results

Browse Source
This commit is contained in:
bugobliterator 2015-03-08 23:27:52 +05:30 committed by Andrew Tridgell
parent 54bc28c96d
commit 2ca0e80dc5
1 changed files with 12 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_Compass/CompassCalibrator.cpp
View File

@ -234,8 +234,18 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) {
}
bool CompassCalibrator::fit_acceptable() {
//TODO check all params
return _fitness <= sq(_tolerance);
if( _sphere_param.named.radius > 200 && //Earth's magnetic field strength range: 250-650mG
_sphere_param.named.radius < 700 &&
fabsf(_sphere_param.named.offset.x) < 1000 &&
fabsf(_sphere_param.named.offset.y) < 1000 &&
fabsf(_sphere_param.named.offset.z) < 1000 &&
fabsf(_ellipsoid_param.named.offdiag.x) < 1.0f && //absolute of sine/cosine output cannot be greater than 1
fabsf(_ellipsoid_param.named.offdiag.x) < 1.0f &&
fabsf(_ellipsoid_param.named.offdiag.x) < 1.0f ){
return _fitness <= sq(_tolerance);
}
return false;
}
void CompassCalibrator::thin_samples() {