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-02-09 09:15:16 +11:00 committed by Randy Mackay
parent 3cf65ce4ec
commit c483b26737
2 changed files with 23 additions and 18 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

@ -87,13 +87,15 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
} }
// 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;
}
#if COMPASS_MOT_ENABLED #if COMPASS_MOT_ENABLED
const Vector3f &mot = state.motor_compensation.get(); const Vector3f &mot = state.motor_compensation.get();

27
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -450,22 +450,25 @@ bool Compass::get_uncorrected_field(uint8_t instance, Vector3f &field) const
// form eliptical correction matrix and invert it. This is // form eliptical correction matrix and invert it. This is
// 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 &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;
}
// get corrected field // get corrected field
field = get_field(instance); field = get_field(instance);
// remove impact of diagonals and off-diagonals const Vector3f &diagonals = get_diagonals(instance);
field = mat * field; 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 offsets // remove impact of offsets
field -= get_offsets(instance); field -= get_offsets(instance);