From 6699c59ad383261d17ebbe84693de13d9c37e071 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Thu, 8 Mar 2018 21:26:18 -0500 Subject: [PATCH] AP_Compass: add support for custom board orientations --- libraries/AP_Compass/AP_Compass.cpp | 7 ++++++- libraries/AP_Compass/AP_Compass.h | 4 +++- libraries/AP_Compass/AP_Compass_Backend.cpp | 6 +++++- 3 files changed, 14 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 41af7371e0..90446408c6 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -460,6 +460,7 @@ Compass::Compass(void) : _backend_count(0), _compass_count(0), _board_orientation(ROTATION_NONE), + _custom_rotation(nullptr), _null_init_done(false), _hil_mode(false) { @@ -1210,7 +1211,11 @@ void Compass::setHIL(uint8_t instance, float roll, float pitch, float yaw) if (!_state[0].external) { // and add in AHRS_ORIENTATION setting if not an external compass - _hil.field[instance].rotate(_board_orientation); + if (_board_orientation == ROTATION_CUSTOM && _custom_rotation) { + _hil.field[instance] = *_custom_rotation * _hil.field[instance]; + } else { + _hil.field[instance].rotate(_board_orientation); + } } _hil.healthy[instance] = true; } diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index bc6a4e1eb9..aa7a23538f 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -205,8 +205,9 @@ public: float get_declination() const; // set overall board orientation - void set_board_orientation(enum Rotation orientation) { + void set_board_orientation(enum Rotation orientation, Matrix3f* custom_rotation = nullptr) { _board_orientation = orientation; + _custom_rotation = custom_rotation; } /// Set the motor compensation type @@ -390,6 +391,7 @@ private: // board orientation from AHRS enum Rotation _board_orientation; + Matrix3f* _custom_rotation; // primary instance AP_Int8 _primary; diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index 388c5472e4..d35630d344 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/libraries/AP_Compass/AP_Compass_Backend.cpp @@ -20,7 +20,11 @@ void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance) if (!state.external) { // and add in AHRS_ORIENTATION setting if not an external compass - mag.rotate(_compass._board_orientation); + if (_compass._board_orientation == ROTATION_CUSTOM && _compass._custom_rotation) { + mag = *_compass._custom_rotation * mag; + } else { + mag.rotate(_compass._board_orientation); + } } else { // add user selectable orientation mag.rotate((enum Rotation)state.orientation.get());