Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-12 01:23:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: fixed AP_Periph compass

Browse Source 
diagonals are no longer initialised to 1.0, which leaves us with zero
compass after matrix mul
This commit is contained in:
Andrew Tridgell 2022-09-27 16:25:12 +10:00
parent de3d763e36
commit af224dab1e
1 changed files with 4 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -74,8 +74,10 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
Compass::mag_state &state = _compass._state[Compass::StateIndex(i)]; Compass::mag_state &state = _compass._state[Compass::StateIndex(i)];
const Vector3f &offsets = state.offset.get(); const Vector3f &offsets = state.offset.get();
#ifndef HAL_BUILD_AP_PERIPH
const Vector3f &diagonals = state.diagonals.get(); const Vector3f &diagonals = state.diagonals.get();
const Vector3f &offdiagonals = state.offdiagonals.get(); const Vector3f &offdiagonals = state.offdiagonals.get();
#endif
// add in the basic offsets // add in the basic offsets
mag += offsets; mag += offsets;
@ -86,6 +88,7 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
mag *= state.scale_factor; mag *= state.scale_factor;
} }
#ifndef HAL_BUILD_AP_PERIPH
// apply eliptical correction // apply eliptical correction
Matrix3f mat( Matrix3f mat(
diagonals.x, offdiagonals.x, offdiagonals.y, diagonals.x, offdiagonals.x, offdiagonals.y,
@ -94,6 +97,7 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
); );
mag = mat * mag; mag = mat * mag;
#endif
#if COMPASS_MOT_ENABLED #if COMPASS_MOT_ENABLED
const Vector3f &mot = state.motor_compensation.get(); const Vector3f &mot = state.motor_compensation.get();