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

AP_Compass: constrain compass offsets to +/- 2000

Browse Source 
this is prompted by a user log showing NaN compass offsets, resulting
in a crash. The patch ensures we never end up with NaN offsets, and
also constrains the offsets to a reasonable limit
This commit is contained in:
Andrew Tridgell 2014-02-16 13:33:41 +11:00
parent aeb7578e5d
commit 189d635493
1 changed files with 21 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_Compass/Compass_learn.cpp
View File

@ -1,6 +1,9 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Compass.h" #include "Compass.h"
// don't allow any axis of the offset to go above 2000
#define COMPASS_OFS_LIMIT 2000
/* /*
* this offset learning algorithm is inspired by this paper from Bill Premerlani * this offset learning algorithm is inspired by this paper from Bill Premerlani
* *
@ -55,6 +58,11 @@ Compass::learn_offsets(void)
Vector3f b1, diff; Vector3f b1, diff;
float length; float length;
if (ofs.is_nan()) {
// offsets are bad possibly due to a past bug - zero them
_offset[k].set(Vector3f());
}
// get a past element // get a past element
b1 = Vector3f(_mag_history[k][_mag_history_index[k]].x, b1 = Vector3f(_mag_history[k][_mag_history_index[k]].x,
_mag_history[k][_mag_history_index[k]].y, _mag_history[k][_mag_history_index[k]].y,
@ -98,7 +106,19 @@ Compass::learn_offsets(void)
diff *= max_change / length; diff *= max_change / length;
} }
Vector3f new_offsets = _offset[k].get() - diff;
if (new_offsets.is_nan()) {
// don't apply bad offsets
continue;
}
// constrain offsets
new_offsets.x = constrain_float(new_offsets.x, -COMPASS_OFS_LIMIT, COMPASS_OFS_LIMIT);
new_offsets.y = constrain_float(new_offsets.y, -COMPASS_OFS_LIMIT, COMPASS_OFS_LIMIT);
new_offsets.z = constrain_float(new_offsets.z, -COMPASS_OFS_LIMIT, COMPASS_OFS_LIMIT);
// set the new offsets // set the new offsets
_offset[k].set(_offset[k].get() - diff); _offset[k].set(new_offsets);
} }
} }