mirror of https://github.com/ArduPilot/ardupilot
125 lines
4.5 KiB
C++
125 lines
4.5 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
#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
|
|
*
|
|
* http://gentlenav.googlecode.com/files/MagnetometerOffsetNullingRevisited.pdf
|
|
*
|
|
* The base algorithm works well, but is quite sensitive to
|
|
* noise. After long discussions with Bill, the following changes were
|
|
* made:
|
|
*
|
|
* 1) we keep a history buffer that effectively divides the mag
|
|
* vectors into a set of N streams. The algorithm is run on the
|
|
* streams separately
|
|
*
|
|
* 2) within each stream we only calculate a change when the mag
|
|
* vector has changed by a significant amount.
|
|
*
|
|
* This gives us the property that we learn quickly if there is no
|
|
* noise, but still learn correctly (and slowly) in the face of lots of
|
|
* noise.
|
|
*/
|
|
void
|
|
Compass::learn_offsets(void)
|
|
{
|
|
if (_learn == 0) {
|
|
// auto-calibration is disabled
|
|
return;
|
|
}
|
|
|
|
// this gain is set so we converge on the offsets in about 5
|
|
// minutes with a 10Hz compass
|
|
const float gain = 0.01;
|
|
const float max_change = 10.0;
|
|
const float min_diff = 50.0;
|
|
|
|
if (!_null_init_done) {
|
|
// first time through
|
|
_null_init_done = true;
|
|
for (uint8_t k=0; k<COMPASS_MAX_INSTANCES; k++) {
|
|
const Vector3f &ofs = _offset[k].get();
|
|
for (uint8_t i=0; i<_mag_history_size; i++) {
|
|
// fill the history buffer with the current mag vector,
|
|
// with the offset removed
|
|
_mag_history[k][i] = Vector3i((_field[k].x+0.5f) - ofs.x, (_field[k].y+0.5f) - ofs.y, (_field[k].z+0.5f) - ofs.z);
|
|
}
|
|
_mag_history_index[k] = 0;
|
|
}
|
|
return;
|
|
}
|
|
|
|
for (uint8_t k=0; k<COMPASS_MAX_INSTANCES; k++) {
|
|
const Vector3f &ofs = _offset[k].get();
|
|
Vector3f b1, diff;
|
|
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
|
|
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]].z);
|
|
|
|
// the history buffer doesn't have the offsets
|
|
b1 += ofs;
|
|
|
|
// get the current vector
|
|
const Vector3f &b2 = _field[k];
|
|
|
|
// calculate the delta for this sample
|
|
diff = b2 - b1;
|
|
length = diff.length();
|
|
if (length < min_diff) {
|
|
// the mag vector hasn't changed enough - we don't get
|
|
// enough information from this vector to use it.
|
|
// Note that we don't put the current vector into the mag
|
|
// history here. We want to wait for a larger rotation to
|
|
// build up before calculating an offset change, as accuracy
|
|
// of the offset change is highly dependent on the size of the
|
|
// rotation.
|
|
_mag_history_index[k] = (_mag_history_index[k] + 1) % _mag_history_size;
|
|
continue;
|
|
}
|
|
|
|
// put the vector in the history
|
|
_mag_history[k][_mag_history_index[k]] = Vector3i((_field[k].x+0.5f) - ofs.x,
|
|
(_field[k].y+0.5f) - ofs.y,
|
|
(_field[k].z+0.5f) - ofs.z);
|
|
_mag_history_index[k] = (_mag_history_index[k] + 1) % _mag_history_size;
|
|
|
|
// equation 6 of Bills paper
|
|
diff = diff * (gain * (b2.length() - b1.length()) / length);
|
|
|
|
// limit the change from any one reading. This is to prevent
|
|
// single crazy readings from throwing off the offsets for a long
|
|
// time
|
|
length = diff.length();
|
|
if (length > max_change) {
|
|
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
|
|
_offset[k].set(new_offsets);
|
|
}
|
|
}
|