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

AP_Compass: fixed zero compass diagonals

Browse Source 
this fixes a regression from 4.2 to 4.3.

previously we automatically set the diagnoals to 1,1,1 if they were
0,0,0. We don't do that any more. I was helping a user who had copied
an old config with 0,0,0 for diagonals and did not understand two
things:

- the compass did not work in 4.3
- large vehicle mag cal didn't work
This commit is contained in:
Andrew Tridgell 2023-01-16 17:04:15 +11:00
parent 8cabcfc750
commit 863b4bf928
2 changed files with 21 additions and 17 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

@ -90,13 +90,15 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
#if AP_COMPASS_DIAGONALS_ENABLED #if AP_COMPASS_DIAGONALS_ENABLED
// apply eliptical correction // apply eliptical correction
Matrix3f mat( if (!diagonals.is_zero()) {
diagonals.x, offdiagonals.x, offdiagonals.y, Matrix3f mat(
offdiagonals.x, diagonals.y, offdiagonals.z, diagonals.x, offdiagonals.x, offdiagonals.y,
offdiagonals.y, offdiagonals.z, diagonals.z offdiagonals.x, diagonals.y, offdiagonals.z,
); offdiagonals.y, offdiagonals.z, diagonals.z
);
mag = mat * mag; mag = mat * mag;
}
#endif #endif
#if COMPASS_MOT_ENABLED #if COMPASS_MOT_ENABLED

24
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -463,18 +463,20 @@ bool Compass::get_uncorrected_field(uint8_t instance, Vector3f &field) const
// needed to remove the effects of the eliptical correction // needed to remove the effects of the eliptical correction
// when calculating new offsets // when calculating new offsets
const Vector3f &diagonals = get_diagonals(instance); const Vector3f &diagonals = get_diagonals(instance);
const Vector3f &offdiagonals = get_offdiagonals(instance); if (!diagonals.is_zero()) {
Matrix3f mat { const Vector3f &offdiagonals = get_offdiagonals(instance);
diagonals.x, offdiagonals.x, offdiagonals.y, Matrix3f mat {
offdiagonals.x, diagonals.y, offdiagonals.z, diagonals.x, offdiagonals.x, offdiagonals.y,
offdiagonals.y, offdiagonals.z, diagonals.z offdiagonals.x, diagonals.y, offdiagonals.z,
}; offdiagonals.y, offdiagonals.z, diagonals.z
if (!mat.invert()) { };
return false; if (!mat.invert()) {
} return false;
}
// remove impact of diagonals and off-diagonals // remove impact of diagonals and off-diagonals
field = mat * field; field = mat * field;
}
#endif #endif
// remove impact of offsets // remove impact of offsets