Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: limit rotations we try to ROTATION_MAX_AUTO_ROTATION

This commit is contained in:
Andrew Tridgell 2020-01-01 09:08:31 +11:00
parent f908e56154
commit 270844745f
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Compass/CompassCalibrator.cpp
View File

@ -823,9 +823,9 @@ bool CompassCalibrator::calculate_orientation(void)
// this function is very slow // this function is very slow
EXPECT_DELAY_MS(1000); EXPECT_DELAY_MS(1000);
float variance[ROTATION_MAX] {}; float variance[ROTATION_MAX_AUTO_ROTATION+1] {};
for (enum Rotation r = ROTATION_NONE; r<ROTATION_MAX; r = (enum Rotation)(r+1)) { for (enum Rotation r = ROTATION_NONE; r <= ROTATION_MAX_AUTO_ROTATION; r = (enum Rotation)(r+1)) {
// calculate the average implied earth field across all samples // calculate the average implied earth field across all samples
Vector3f total_ef {}; Vector3f total_ef {};
for (uint32_t i=0; i<_samples_collected; i++) { for (uint32_t i=0; i<_samples_collected; i++) {
@ -846,7 +846,7 @@ bool CompassCalibrator::calculate_orientation(void)
// find the rotation with the lowest variance // find the rotation with the lowest variance
enum Rotation besti = ROTATION_NONE; enum Rotation besti = ROTATION_NONE;
float bestv = variance[0]; float bestv = variance[0];
for (enum Rotation r = ROTATION_NONE; r<ROTATION_MAX; r = (enum Rotation)(r+1)) { for (enum Rotation r = ROTATION_NONE; r <= ROTATION_MAX_AUTO_ROTATION; r = (enum Rotation)(r+1)) {
if (variance[r] < bestv) { if (variance[r] < bestv) {
bestv = variance[r]; bestv = variance[r];
besti = r; besti = r;
@ -859,7 +859,7 @@ bool CompassCalibrator::calculate_orientation(void)
float second_best = besti==ROTATION_NONE?variance[1]:variance[0]; float second_best = besti==ROTATION_NONE?variance[1]:variance[0];
enum Rotation besti2 = ROTATION_NONE; enum Rotation besti2 = ROTATION_NONE;
for (enum Rotation r = ROTATION_NONE; r<ROTATION_MAX; r = (enum Rotation)(r+1)) { for (enum Rotation r = ROTATION_NONE; r <= ROTATION_MAX_AUTO_ROTATION; r = (enum Rotation)(r+1)) {
if (!rotation_equal(besti, r)) { if (!rotation_equal(besti, r)) {
if (variance[r] < second_best) { if (variance[r] < second_best) {
second_best = variance[r]; second_best = variance[r];