Browse Source

mc_att_control: move the board_rotation computation to the parameters_update

It's not necessary to do this computation in each loop iteration.
sbg
Beat Küng 8 years ago committed by Matthias Grob
parent
commit
2cc18d2d52
  1. 21
      src/modules/mc_att_control/mc_att_control_main.cpp

21
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[1], &(_params.board_offset[1]));
param_get(_params_handles.board_offset[2], &(_params.board_offset[2])); 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; return OK;
} }
@ -979,16 +990,6 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
_rates_int.zero(); _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 // get the raw gyro data and correct for thermal errors
math::Vector<3> rates; math::Vector<3> rates;

Loading…
Cancel
Save