Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: use a separate slot for the custom compass rotation

This commit is contained in:
Andy Piper 2020-05-21 21:15:55 +01:00 committed by Andrew Tridgell
parent acff7daba5
commit 63b5711a4d
2 changed files with 13 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Compass/AP_Compass.h
View File

@ -224,9 +224,7 @@ public:
// set overall board orientation
void set_board_orientation(enum Rotation orientation, Matrix3f* custom_rotation = nullptr) {
_board_orientation = orientation;
if (custom_rotation) {
_custom_rotation = custom_rotation;
}
_custom_rotation = custom_rotation;
}
/// Set the motor compensation type
@ -438,9 +436,13 @@ private:
// board orientation from AHRS
enum Rotation _board_orientation = ROTATION_NONE;
// custom rotation matrix
// custom board rotation matrix
Matrix3f* _custom_rotation;
// custom external compass rotation matrix
Matrix3f* _custom_external_rotation;
// declination in radians
AP_Float _declination;

12
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -32,8 +32,8 @@ void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance)
// add user selectable orientation
#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph)
Rotation rotation = Rotation(state.orientation.get());
if (rotation == ROTATION_CUSTOM && _compass._custom_rotation) {
mag = *_compass._custom_rotation * mag;
if (rotation == ROTATION_CUSTOM && _compass._custom_external_rotation) {
mag = *_compass._custom_external_rotation * mag;
} else {
mag.rotate(rotation);
}
@ -234,9 +234,11 @@ void AP_Compass_Backend::set_rotation(uint8_t instance, enum Rotation rotation)
_compass._state[Compass::StateIndex(instance)].rotation = rotation;
#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph)
// lazily create the custom rotation matrix
if (!_compass._custom_rotation && Rotation(_compass._state[Compass::StateIndex(instance)].orientation.get()) == ROTATION_CUSTOM) {
_compass._custom_rotation = new Matrix3f();
_compass._custom_rotation->from_euler(radians(_compass._custom_roll), radians(_compass._custom_pitch), radians(_compass._custom_yaw));
if (!_compass._custom_external_rotation && Rotation(_compass._state[Compass::StateIndex(instance)].orientation.get()) == ROTATION_CUSTOM) {
_compass._custom_external_rotation = new Matrix3f();
if (_compass._custom_external_rotation) {
_compass._custom_external_rotation->from_euler(radians(_compass._custom_roll), radians(_compass._custom_pitch), radians(_compass._custom_yaw));
}
}
#endif
}