zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Compass: add soft-iron corrections

master
Jonathan Challinger 10 years ago committed by Andrew Tridgell
parent
ee1209c03f
commit
f20ef69777
3 changed files with 136 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 14
      libraries/AP_Compass/AP_Compass_Backend.cpp
  2. 118
      libraries/AP_Compass/Compass.cpp
  3. 4
      libraries/AP_Compass/Compass.h

14
libraries/AP_Compass/AP_Compass_Backend.cpp
Unescape View File

@ -53,7 +53,13 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i) @@ -53,7 +53,13 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t 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 &diagonals = state.diagonals.get();
const Vector3f &offdiagonals = state.offdiagonals.get();
const Vector3f &mot = state.motor_compensation.get();
/*
@ -67,6 +73,14 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i) @@ -67,6 +73,14 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
} else {
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)

118
libraries/AP_Compass/Compass.cpp
Unescape View File

@ -265,6 +265,106 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = { @@ -265,6 +265,106 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
AP_GROUPINFO("EXTERN3",23, Compass, _state[2].external, 0),
#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
};
@ -405,6 +505,24 @@ Compass::set_and_save_offsets(uint8_t i, const Vector3f &offsets) @@ -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
Compass::save_offsets(uint8_t i)
{

4
libraries/AP_Compass/Compass.h
Unescape View File

@ -88,6 +88,8 @@ public: @@ -88,6 +88,8 @@ public:
/// @param offsets Offsets to the raw mag_ values.
///
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
///
@ -315,6 +317,8 @@ private: @@ -315,6 +317,8 @@ private:
bool healthy;
AP_Int8 orientation;
AP_Vector3f offset;
AP_Vector3f diagonals;
AP_Vector3f offdiagonals;
#if COMPASS_MAX_INSTANCES > 1
// device id detected at init.

Write Preview
Loading…
Cancel
Save