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
// apply eliptical correction
Matrix3f mat(
diagonals.x, offdiagonals.x, offdiagonals.y,
offdiagonals.x, diagonals.y, offdiagonals.z,
offdiagonals.y, offdiagonals.z, diagonals.z
);
if (!diagonals.is_zero()) {
Matrix3f mat(
diagonals.x, offdiagonals.x, offdiagonals.y,
offdiagonals.x, diagonals.y, offdiagonals.z,
offdiagonals.y, offdiagonals.z, diagonals.z
);
mag = mat * mag;
mag = mat * mag;
}
#endif
#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
// when calculating new offsets
const Vector3f &diagonals = get_diagonals(instance);
const Vector3f &offdiagonals = get_offdiagonals(instance);
Matrix3f mat {
diagonals.x, offdiagonals.x, offdiagonals.y,
offdiagonals.x, diagonals.y, offdiagonals.z,
offdiagonals.y, offdiagonals.z, diagonals.z
};
if (!mat.invert()) {
return false;
}
if (!diagonals.is_zero()) {
const Vector3f &offdiagonals = get_offdiagonals(instance);
Matrix3f mat {
diagonals.x, offdiagonals.x, offdiagonals.y,
offdiagonals.x, diagonals.y, offdiagonals.z,
offdiagonals.y, offdiagonals.z, diagonals.z
};
if (!mat.invert()) {
return false;
}
// remove impact of diagonals and off-diagonals
field = mat * field;
// remove impact of diagonals and off-diagonals
field = mat * field;
}
#endif
// remove impact of offsets