Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13: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)];
const Vector3f &offsets = state.offset.get();
#ifndef HAL_BUILD_AP_PERIPH
const Vector3f &diagonals = state.diagonals.get();
const Vector3f &offdiagonals = state.offdiagonals.get();
#endif
// add in the basic offsets
mag += offsets;
@ -86,6 +88,7 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
mag *= state.scale_factor;
}
#ifndef HAL_BUILD_AP_PERIPH
// apply eliptical correction
Matrix3f mat(
diagonals.x, offdiagonals.x, offdiagonals.y,
@ -94,6 +97,7 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
);
mag = mat * mag;
#endif
#if COMPASS_MOT_ENABLED
const Vector3f &mot = state.motor_compensation.get();