diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 8528d5f57c..264dc4bf88 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -664,6 +664,17 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.board_offset[1], &(_params.board_offset[1])); param_get(_params_handles.board_offset[2], &(_params.board_offset[2])); + + /* get transformation matrix from sensor/board to body frame */ + get_rot_matrix((enum Rotation)_params.board_rotation, &_board_rotation); + + /* fine tune the rotation */ + math::Matrix<3, 3> board_rotation_offset; + board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _params.board_offset[0], + M_DEG_TO_RAD_F * _params.board_offset[1], + M_DEG_TO_RAD_F * _params.board_offset[2]); + _board_rotation = board_rotation_offset * _board_rotation; + return OK; } @@ -979,16 +990,6 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) _rates_int.zero(); } - /* get transformation matrix from sensor/board to body frame */ - get_rot_matrix((enum Rotation)_params.board_rotation, &_board_rotation); - - /* fine tune the rotation */ - math::Matrix<3, 3> board_rotation_offset; - board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _params.board_offset[0], - M_DEG_TO_RAD_F * _params.board_offset[1], - M_DEG_TO_RAD_F * _params.board_offset[2]); - _board_rotation = board_rotation_offset * _board_rotation; - // get the raw gyro data and correct for thermal errors math::Vector<3> rates;