dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mc_att_control: fix yaw feedforwarding 

The feedforwarding was applied in the wrong frame: the term is given in
global coordinates, but was directly applied to body coordinates. This
patch adds the missing transformation from global to body frame.

In addition, it moves the feedforwarding before the rate limitation, so
that we cannot exceed the configured rates.
This commit is contained in:
Beat Küng 2017-09-21 13:21:06 +02:00 committed by Matthias Grob
parent 2cc18d2d52
commit 69b1bfca6c
1 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -932,6 +932,18 @@ MulticopterAttitudeControl::control_attitude(float dt)
/* calculate angular rates setpoint */
_rates_sp = _params.att_p.emult(e_R);
/* Feed forward the yaw setpoint rate. We need to transform the yaw from world into body frame.
* The following is a simplification of:
* R.transposed() * math::Vector<3>(0.f, 0.f, _v_att_sp.yaw_sp_move_rate * _params.yaw_ff)
*/
math::Vector<3> yaw_feedforward_rate(R(2, 0), R(2, 1), R(2, 2));
yaw_feedforward_rate *= _v_att_sp.yaw_sp_move_rate * _params.yaw_ff;
yaw_feedforward_rate(2) *= yaw_w;
_rates_sp += yaw_feedforward_rate;
/* limit rates */
for (int i = 0; i < 3; i++) {
if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
@ -943,9 +955,6 @@ MulticopterAttitudeControl::control_attitude(float dt)
}
}
/* feed forward yaw setpoint rate */
_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;
/* weather-vane mode, dampen yaw rate */
if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
_v_att_sp.disable_mc_yaw_control == true && !_v_control_mode.flag_control_manual_enabled) {