fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 00:50:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_att_control: move the board_rotation computation to the parameters_update

Browse Source 
It's not necessary to do this computation in each loop iteration.
This commit is contained in:
Beat Küng
2017-09-21 13:17:49 +02:00
committed by Matthias Grob
parent 27f020af05
commit 2cc18d2d52
1 changed files with 11 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+11 -10
View File
@@ -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;