Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: allow diagonals and off-diagonals to be calculated on rot change

Browse Source 
this re-runs the fit on change in orientation
This commit is contained in:
Andrew Tridgell 2018-07-17 12:57:23 +10:00
parent b4c7d1925e
commit a5749c1869
1 changed files with 26 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_Compass/CompassCalibrator.cpp
View File

@ -816,23 +816,33 @@ bool CompassCalibrator::calculate_orientation(void)
_fitness += 1000;
return false;
}
if (_orientation != besti) {
// correct the offsets for the new orientation
Vector3f rot_offsets = _params.offset;
rot_offsets.rotate_inverse(_orientation);
rot_offsets.rotate(besti);
_params.offset = rot_offsets;
Vector3f &diagonals = _params.diag;
Vector3f &offdiagonals = _params.offdiag;
// we should calculated the corrected diagonals and off-diagonals here, but
// for now just avoid eliptical correction if we're changing the orientation
diagonals = Vector3f(1,1,1);
offdiagonals.zero();
_orientation = besti;
if (_orientation == besti) {
// no orientation change
return true;
}
return true;
// correct the offsets for the new orientation
Vector3f rot_offsets = _params.offset;
rot_offsets.rotate_inverse(_orientation);
rot_offsets.rotate(besti);
_params.offset = rot_offsets;
// rotate the samples for the new orientation
for (uint32_t i=0; i<_samples_collected; i++) {
Vector3f s = _sample_buffer[i].get();
s.rotate_inverse(_orientation);
s.rotate(besti);
_sample_buffer[i].set(s);
}
_orientation = besti;
// re-run the fit to get the diagonals and off-diagonals for the
// new orientation
initialize_fit();
run_sphere_fit();
run_ellipsoid_fit();
return fit_acceptable();
}