Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: add support for custom board orientations

Browse Source
This commit is contained in:
Jacob Walser 2018-03-08 21:26:18 -05:00 committed by Francisco Ferreira
parent f97ac4af30
commit 6699c59ad3
3 changed files with 14 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_Compass/AP_Compass.cpp
View File

@ -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;
}

4
libraries/AP_Compass/AP_Compass.h
View File

@ -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;

6
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -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());