diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index 6fd3c86f03..b0875d3112 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/libraries/AP_Compass/AP_Compass_Backend.cpp @@ -202,3 +202,8 @@ bool AP_Compass_Backend::field_ok(const Vector3f &field) return ret; } + +enum Rotation AP_Compass_Backend::get_board_orientation(void) const +{ + return _compass._board_orientation; +} diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h index 8d5f2f6789..301f28fe15 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.h +++ b/libraries/AP_Compass/AP_Compass_Backend.h @@ -106,6 +106,9 @@ protected: // set rotation of an instance void set_rotation(uint8_t instance, enum Rotation rotation); + // get board orientation (for SITL) + enum Rotation get_board_orientation(void) const; + // access to frontend Compass &_compass; diff --git a/libraries/AP_Compass/AP_Compass_SITL.cpp b/libraries/AP_Compass/AP_Compass_SITL.cpp index 046a43c9b1..a063705173 100644 --- a/libraries/AP_Compass/AP_Compass_SITL.cpp +++ b/libraries/AP_Compass/AP_Compass_SITL.cpp @@ -114,7 +114,9 @@ void AP_Compass_SITL::_timer() if (i == 0) { // rotate the first compass, allowing for testing of external compass rotation f.rotate_inverse((enum Rotation)_sitl->mag_orient.get()); + f.rotate(get_board_orientation()); } + rotate_field(f, _compass_instance[i]); publish_raw_field(f, _compass_instance[i]); correct_field(f, _compass_instance[i]);