Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 08:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: fix compilation when removing diagonals state

Browse Source
This commit is contained in:
Peter Barker 2022-10-26 18:14:11 +11:00 committed by Peter Barker
parent 9c726b8f68
commit aef745c693
6 changed files with 16 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Compass/AP_Compass.cpp
View File

@ -889,7 +889,7 @@ void Compass::mag_state::copy_from(const Compass::mag_state& state)
external.set_and_save_ifchanged(state.external); external.set_and_save_ifchanged(state.external);
orientation.set_and_save_ifchanged(state.orientation); orientation.set_and_save_ifchanged(state.orientation);
offset.set_and_save_ifchanged(state.offset); offset.set_and_save_ifchanged(state.offset);
#ifndef HAL_BUILD_AP_PERIPH #if AP_COMPASS_DIAGONALS_ENABLED
diagonals.set_and_save_ifchanged(state.diagonals); diagonals.set_and_save_ifchanged(state.diagonals);
offdiagonals.set_and_save_ifchanged(state.offdiagonals); offdiagonals.set_and_save_ifchanged(state.offdiagonals);
#endif #endif

6
libraries/AP_Compass/AP_Compass.h
View File

@ -216,13 +216,13 @@ public:
const Vector3f &get_offsets(uint8_t i) const { return _get_state(Priority(i)).offset; } const Vector3f &get_offsets(uint8_t i) const { return _get_state(Priority(i)).offset; }
const Vector3f &get_offsets(void) const { return get_offsets(_first_usable); } const Vector3f &get_offsets(void) const { return get_offsets(_first_usable); }
#ifndef HAL_BUILD_AP_PERIPH #if AP_COMPASS_DIAGONALS_ENABLED
const Vector3f &get_diagonals(uint8_t i) const { return _get_state(Priority(i)).diagonals; } const Vector3f &get_diagonals(uint8_t i) const { return _get_state(Priority(i)).diagonals; }
const Vector3f &get_diagonals(void) const { return get_diagonals(_first_usable); } const Vector3f &get_diagonals(void) const { return get_diagonals(_first_usable); }
const Vector3f &get_offdiagonals(uint8_t i) const { return _get_state(Priority(i)).offdiagonals; } const Vector3f &get_offdiagonals(uint8_t i) const { return _get_state(Priority(i)).offdiagonals; }
const Vector3f &get_offdiagonals(void) const { return get_offdiagonals(_first_usable); } const Vector3f &get_offdiagonals(void) const { return get_offdiagonals(_first_usable); }
#endif #endif // AP_COMPASS_DIAGONALS_ENABLED
// learn offsets accessor // learn offsets accessor
bool learn_offsets_enabled() const { return _learn == LEARN_INFLIGHT; } bool learn_offsets_enabled() const { return _learn == LEARN_INFLIGHT; }
@ -486,7 +486,7 @@ private:
Compass::Priority priority; Compass::Priority priority;
AP_Int8 orientation; AP_Int8 orientation;
AP_Vector3f offset; AP_Vector3f offset;
#ifndef HAL_BUILD_AP_PERIPH #if AP_COMPASS_DIAGONALS_ENABLED
AP_Vector3f diagonals; AP_Vector3f diagonals;
AP_Vector3f offdiagonals; AP_Vector3f offdiagonals;
#endif #endif

4
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -74,7 +74,7 @@ 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 #if AP_COMPASS_DIAGONALS_ENABLED
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 #endif
@ -88,7 +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 #if AP_COMPASS_DIAGONALS_ENABLED
// apply eliptical correction // apply eliptical correction
Matrix3f mat( Matrix3f mat(
diagonals.x, offdiagonals.x, offdiagonals.y, diagonals.x, offdiagonals.x, offdiagonals.y,

8
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -455,6 +455,10 @@ MAV_RESULT Compass::handle_mag_cal_command(const mavlink_command_long_t &packet)
*/ */
bool Compass::get_uncorrected_field(uint8_t instance, Vector3f &field) const bool Compass::get_uncorrected_field(uint8_t instance, Vector3f &field) const
{ {
// get corrected field
field = get_field(instance);
#if AP_COMPASS_DIAGONALS_ENABLED
// 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
@ -469,11 +473,9 @@ bool Compass::get_uncorrected_field(uint8_t instance, Vector3f &field) const
return false; return false;
} }
// get corrected field
field = get_field(instance);
// remove impact of diagonals and off-diagonals // remove impact of diagonals and off-diagonals
field = mat * field; field = mat * field;
#endif
// remove impact of offsets // remove impact of offsets
field -= get_offsets(instance); field -= get_offsets(instance);

2
libraries/AP_Compass/AP_Compass_config.h
View File

@ -3,5 +3,5 @@
#include <AP_HAL/AP_HAL_Boards.h> #include <AP_HAL/AP_HAL_Boards.h>
#ifndef AP_COMPASS_DIAGONALS_ENABLED #ifndef AP_COMPASS_DIAGONALS_ENABLED
#define AP_COMPASS_DIAGONALS_ENABLED (!defined(HAL_BUILD_AP_PERIPH)) #define AP_COMPASS_DIAGONALS_ENABLED 1
#endif #endif

4
libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
View File

@ -2938,6 +2938,10 @@ def add_apperiph_defaults(f):
#define HAL_MSP_ENABLED 0 #define HAL_MSP_ENABLED 0
#endif #endif
// periph does not make use of compass scaling or diagonals
#ifndef AP_COMPASS_DIAGONALS_ENABLED
#define AP_COMPASS_DIAGONALS_ENABLED 0
#endif
''') ''')
def add_bootloader_defaults(f): def add_bootloader_defaults(f):