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

Compass: add soft-iron corrections

Browse Source
This commit is contained in:
Jonathan Challinger 2015-03-18 16:18:47 -07:00 committed by Andrew Tridgell
parent ee1209c03f
commit f20ef69777
3 changed files with 136 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -53,7 +53,13 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
{ {
Compass::mag_state &state = _compass._state[i]; Compass::mag_state &state = _compass._state[i];
if (state.diagonals.get().is_zero()) {
state.diagonals.set(Vector3f(1.0f,1.0f,1.0f));
}
const Vector3f &offsets = state.offset.get(); const Vector3f &offsets = state.offset.get();
const Vector3f &diagonals = state.diagonals.get();
const Vector3f &offdiagonals = state.offdiagonals.get();
const Vector3f &mot = state.motor_compensation.get(); const Vector3f &mot = state.motor_compensation.get();
/* /*
@ -67,6 +73,14 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
} else { } else {
state.motor_offset.zero(); state.motor_offset.zero();
} }
Matrix3f mat(
diagonals.x, offdiagonals.x, offdiagonals.y,
offdiagonals.x, diagonals.y, offdiagonals.z,
offdiagonals.y, offdiagonals.z, diagonals.z
);
mag = mat * mag;
} }
void AP_Compass_Backend::publish_unfiltered_field(const Vector3f &mag, uint32_t time_us, uint8_t instance) void AP_Compass_Backend::publish_unfiltered_field(const Vector3f &mag, uint32_t time_us, uint8_t instance)

118
libraries/AP_Compass/Compass.cpp
View File

@ -265,6 +265,106 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
AP_GROUPINFO("EXTERN3",23, Compass, _state[2].external, 0), AP_GROUPINFO("EXTERN3",23, Compass, _state[2].external, 0),
#endif #endif
// @Param: DIA_X
// @DisplayName: Compass soft-iron diagonal X component
// @Description: DIA_X in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
// @User: Advanced
// @Param: DIA_Y
// @DisplayName: Compass soft-iron diagonal Y component
// @Description: DIA_Y in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
// @User: Advanced
// @Param: DIA_Z
// @DisplayName: Compass soft-iron diagonal Z component
// @Description: DIA_Z in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
// @User: Advanced
AP_GROUPINFO("DIA", 24, Compass, _state[0].diagonals, 0),
// @Param: ODI_X
// @DisplayName: Compass soft-iron off-diagonal X component
// @Description: ODI_X in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
// @User: Advanced
// @Param: ODI_Y
// @DisplayName: Compass soft-iron off-diagonal Y component
// @Description: ODI_Y in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
// @User: Advanced
// @Param: ODI_Z
// @DisplayName: Compass soft-iron off-diagonal Z component
// @Description: ODI_Z in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
// @User: Advanced
AP_GROUPINFO("ODI", 25, Compass, _state[0].offdiagonals, 0),
#if COMPASS_MAX_INSTANCES > 1
// @Param: DIA2_X
// @DisplayName: Compass2 soft-iron diagonal X component
// @Description: DIA_X in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
// @User: Advanced
// @Param: DIA2_Y
// @DisplayName: Compass2 soft-iron diagonal Y component
// @Description: DIA_Y in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
// @User: Advanced
// @Param: DIA2_Z
// @DisplayName: Compass2 soft-iron diagonal Z component
// @Description: DIA_Z in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
// @User: Advanced
AP_GROUPINFO("DIA2", 26, Compass, _state[1].diagonals, 0),
// @Param: ODI2_X
// @DisplayName: Compass2 soft-iron off-diagonal X component
// @Description: ODI_X in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
// @User: Advanced
// @Param: ODI2_Y
// @DisplayName: Compass2 soft-iron off-diagonal Y component
// @Description: ODI_Y in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
// @User: Advanced
// @Param: ODI2_Z
// @DisplayName: Compass2 soft-iron off-diagonal Z component
// @Description: ODI_Z in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
// @User: Advanced
AP_GROUPINFO("ODI2", 27, Compass, _state[1].offdiagonals, 0),
#endif
#if COMPASS_MAX_INSTANCES > 2
// @Param: DIA3_X
// @DisplayName: Compass3 soft-iron diagonal X component
// @Description: DIA_X in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
// @User: Advanced
// @Param: DIA3_Y
// @DisplayName: Compass3 soft-iron diagonal Y component
// @Description: DIA_Y in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
// @User: Advanced
// @Param: DIA3_Z
// @DisplayName: Compass3 soft-iron diagonal Z component
// @Description: DIA_Z in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
// @User: Advanced
AP_GROUPINFO("DIA3", 28, Compass, _state[2].diagonals, 0),
// @Param: ODI3_X
// @DisplayName: Compass3 soft-iron off-diagonal X component
// @Description: ODI_X in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
// @User: Advanced
// @Param: ODI3_Y
// @DisplayName: Compass3 soft-iron off-diagonal Y component
// @Description: ODI_Y in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
// @User: Advanced
// @Param: ODI3_Z
// @DisplayName: Compass3 soft-iron off-diagonal Z component
// @Description: ODI_Z in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
// @User: Advanced
AP_GROUPINFO("ODI3", 29, Compass, _state[2].offdiagonals, 0),
#endif
AP_GROUPEND AP_GROUPEND
}; };
@ -405,6 +505,24 @@ Compass::set_and_save_offsets(uint8_t i, const Vector3f &offsets)
} }
} }
void
Compass::set_and_save_diagonals(uint8_t i, const Vector3f &diagonals)
{
// sanity check compass instance provided
if (i < COMPASS_MAX_INSTANCES) {
_state[i].diagonals.set_and_save(diagonals);
}
}
void
Compass::set_and_save_offdiagonals(uint8_t i, const Vector3f &offdiagonals)
{
// sanity check compass instance provided
if (i < COMPASS_MAX_INSTANCES) {
_state[i].offdiagonals.set_and_save(offdiagonals);
}
}
void void
Compass::save_offsets(uint8_t i) Compass::save_offsets(uint8_t i)
{ {

4
libraries/AP_Compass/Compass.h
View File

@ -88,6 +88,8 @@ public:
/// @param offsets Offsets to the raw mag_ values. /// @param offsets Offsets to the raw mag_ values.
/// ///
void set_and_save_offsets(uint8_t i, const Vector3f &offsets); void set_and_save_offsets(uint8_t i, const Vector3f &offsets);
void set_and_save_diagonals(uint8_t i, const Vector3f &diagonals);
void set_and_save_offdiagonals(uint8_t i, const Vector3f &diagonals);
/// Saves the current offset x/y/z values for one or all compasses /// Saves the current offset x/y/z values for one or all compasses
/// ///
@ -315,6 +317,8 @@ private:
bool healthy; bool healthy;
AP_Int8 orientation; AP_Int8 orientation;
AP_Vector3f offset; AP_Vector3f offset;
AP_Vector3f diagonals;
AP_Vector3f offdiagonals;
#if COMPASS_MAX_INSTANCES > 1 #if COMPASS_MAX_INSTANCES > 1
// device id detected at init. // device id detected at init.