diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index 25cab75724..c14a9ff1b2 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -322,8 +322,13 @@ bool AP_Compass_HMC5843::read() if (product_id == AP_COMPASS_TYPE_HMC5883L) { rot_mag.rotate(ROTATION_YAW_90); } + + // add components orientation rot_mag.rotate(_orientation); + // add in board orientation + rot_mag.rotate(_board_orientation); + rot_mag += _offset.get(); mag_x = rot_mag.x; mag_y = rot_mag.y; diff --git a/libraries/AP_Compass/AP_Compass_PX4.cpp b/libraries/AP_Compass/AP_Compass_PX4.cpp index 476957ac03..f4022b50cb 100644 --- a/libraries/AP_Compass/AP_Compass_PX4.cpp +++ b/libraries/AP_Compass/AP_Compass_PX4.cpp @@ -64,7 +64,7 @@ bool AP_Compass_PX4::read(void) _sum /= _count; _sum *= 1000; _sum.rotate(_orientation); - + _sum.rotate(_board_orientation); _sum += _offset.get(); mag_x = _sum.x; diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index 8195a9caea..6b35cc0e55 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -123,6 +123,11 @@ public: static const struct AP_Param::GroupInfo var_info[]; + // set overall board orientation + void set_board_orientation(enum Rotation orientation) { + _board_orientation = orientation; + } + protected: enum Rotation _orientation; AP_Vector3f _offset; @@ -137,5 +142,8 @@ protected: static const uint8_t _mag_history_size = 20; uint8_t _mag_history_index; Vector3i _mag_history[_mag_history_size]; + + // board orientation from AHRS + enum Rotation _board_orientation; }; #endif