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

AP_Compass: added compass offsets for 2nd compass

Browse Source
This commit is contained in:
Andrew Tridgell 2013-12-09 17:33:54 +11:00
parent 02af644010
commit bde89fd4e2
5 changed files with 72 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Compass/AP_Compass_HIL.cpp
View File

@ -51,7 +51,7 @@ void AP_Compass_HIL::_setup_earth_field(void)
bool AP_Compass_HIL::read()
{
// get offsets
Vector3f ofs = _offset.get();
Vector3f ofs = _offset[0].get();
// apply motor compensation
if(_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {

2
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -341,7 +341,7 @@ bool AP_Compass_HMC5843::read()
_field[0].rotate(_board_orientation);
}
_field[0] += _offset.get();
_field[0] += _offset[0].get();
// apply motor compensation
if(_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {

2
libraries/AP_Compass/AP_Compass_PX4.cpp
View File

@ -118,7 +118,7 @@ bool AP_Compass_PX4::read(void)
_sum[i].rotate(_board_orientation);
}
_sum[i] += _offset.get();
_sum[i] += _offset[i].get();
// apply motor compensation
if (_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {

52
libraries/AP_Compass/Compass.cpp
View File

@ -22,7 +22,7 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
// @Description: Offset to be added to the compass z-axis values to compensate for metal in the frame
// @Range: -400 400
// @Increment: 1
AP_GROUPINFO("OFS", 1, Compass, _offset, 0),
AP_GROUPINFO("OFS", 1, Compass, _offset[0], 0),
// @Param: DEC
// @DisplayName: Compass declination
@ -98,6 +98,10 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
// @User: Advanced
AP_GROUPINFO("EXTERNAL", 9, Compass, _external, 0),
#if COMPASS_MAX_INSTANCES > 1
AP_GROUPINFO("OFS2", 10, Compass, _offset[1], 0),
#endif
AP_GROUPEND
};
@ -123,19 +127,15 @@ Compass::init()
void
Compass::set_offsets(const Vector3f &offsets)
{
_offset.set(offsets);
_offset[0].set(offsets);
}
void
Compass::save_offsets()
{
_offset.save();
for (uint8_t k=0; k<COMPASS_MAX_INSTANCES; k++) {
_offset[k].save();
}
const Vector3f &
Compass::get_offsets() const
{
return _offset;
}
void
@ -248,34 +248,37 @@ Compass::null_offsets(void)
const float gain = 0.01;
const float max_change = 10.0;
const float min_diff = 50.0;
Vector3f ofs;
ofs = _offset.get();
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[i] = Vector3i((_field[0].x+0.5f) - ofs.x, (_field[0].y+0.5f) - ofs.y, (_field[0].z+0.5f) - ofs.z);
_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;
}
_mag_history_index = 0;
return;
}
Vector3f b1, b2, diff;
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[_mag_history_index].x,
_mag_history[_mag_history_index].y,
_mag_history[_mag_history_index].z);
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
b2 = Vector3f(_field[0].x, _field[0].y, _field[0].z);
const Vector3f &b2 = _field[k];
// calculate the delta for this sample
diff = b2 - b1;
@ -288,13 +291,15 @@ Compass::null_offsets(void)
// 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 = (_mag_history_index + 1) % _mag_history_size;
return;
_mag_history_index[k] = (_mag_history_index[k] + 1) % _mag_history_size;
continue;
}
// put the vector in the history
_mag_history[_mag_history_index] = Vector3i((_field[0].x+0.5f) - ofs.x, (_field[0].y+0.5f) - ofs.y, (_field[0].z+0.5f) - ofs.z);
_mag_history_index = (_mag_history_index + 1) % _mag_history_size;
_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);
@ -308,5 +313,6 @@ Compass::null_offsets(void)
}
// set the new offsets
_offset.set(_offset.get() - diff);
_offset[k].set(_offset[k].get() - diff);
}
}

9
libraries/AP_Compass/Compass.h
View File

@ -110,7 +110,8 @@ public:
///
/// @returns The current compass offsets.
///
const Vector3f &get_offsets() const;
const Vector3f &get_offsets(uint8_t i) const { return _offset[i]; }
const Vector3f &get_offsets(void) const { return get_offsets(0); }
/// Sets the initial location used to get declination
///
@ -216,7 +217,7 @@ protected:
Vector3f _field[COMPASS_MAX_INSTANCES]; ///< magnetic field strength
AP_Int8 _orientation;
AP_Vector3f _offset;
AP_Vector3f _offset[COMPASS_MAX_INSTANCES];
AP_Float _declination;
AP_Int8 _use_for_yaw; ///<enable use for yaw calculation
AP_Int8 _auto_declination; ///<enable automatic declination code
@ -226,8 +227,8 @@ protected:
///< used by offset correction
static const uint8_t _mag_history_size = 20;
uint8_t _mag_history_index;
Vector3i _mag_history[_mag_history_size];
uint8_t _mag_history_index[COMPASS_MAX_INSTANCES];
Vector3i _mag_history[COMPASS_MAX_INSTANCES][_mag_history_size];
// motor compensation
AP_Int8 _motor_comp_type; // 0 = disabled, 1 = enabled for throttle, 2 = enabled for current