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

Compass: split compass learning out to its own C++ file

This commit is contained in:
Andrew Tridgell 2014-02-16 13:21:06 +11:00
parent 24e1070eb2
commit 9bb8f73d56
4 changed files with 107 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

101
libraries/AP_Compass/Compass.cpp
View File

@ -218,104 +218,3 @@ Compass::calculate_heading(const Matrix3f &dcm_matrix) const
}
/*
* this offset nulling 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::null_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;
// 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;
}
// set the new offsets
_offset[k].set(_offset[k].get() - diff);
}
}

4
libraries/AP_Compass/Compass.h
View File

@ -91,7 +91,7 @@ public:
/// Saves the current compass offset x/y/z values.
///
/// This should be invoked periodically to save the offset values maintained by
/// ::null_offsets.
/// ::learn_offsets.
///
void save_offsets();
@ -135,7 +135,7 @@ public:
/// Perform automatic offset updates
///
void null_offsets(void);
void learn_offsets(void);
/// return true if the compass should be used for yaw calculations
bool use_for_yaw(void) const {

104
libraries/AP_Compass/Compass_learn.cpp Normal file
View File

@ -0,0 +1,104 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Compass.h"
/*
* 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;
// 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;
}
// set the new offsets
_offset[k].set(_offset[k].get() - diff);
}
}

2
libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde
View File

@ -81,7 +81,7 @@ void loop()
// use roll = 0, pitch = 0 for this example
dcm_matrix.from_euler(0, 0, 0);
heading = compass.calculate_heading(dcm_matrix);
compass.null_offsets();
compass.learn_offsets();
// capture min
const Vector3f &mag = compass.get_field();