yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Move constraint after feed forward

Browse Source
This commit is contained in:
sander
2016-02-14 23:50:57 +01:00
committed by Roman
parent 7db35337da
commit dc7077b125
1 changed files with 7 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -41,6 +41,7 @@
* *
* @author Lorenz Meier <lorenz@px4.io> * @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton.babushkin@me.com> * @author Anton Babushkin <anton.babushkin@me.com>
* @author Sander Smeets <sander@droneslab.com>
* *
* The controller has two loops: P loop for angular error and PD loop for angular rate error. * The controller has two loops: P loop for angular error and PD loop for angular rate error.
* Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
@@ -734,7 +735,10 @@ MulticopterAttitudeControl::control_attitude(float dt)
} }
} }
/* weather-vane mode, dampen yaw rate */ /* feed forward yaw setpoint rate */
_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;
/* weather-vane mode, scale down yaw rate */
if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) { if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) {
float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale; float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max); _rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
@@ -742,8 +746,6 @@ MulticopterAttitudeControl::control_attitude(float dt)
_rates_int(2) = 0.0f; _rates_int(2) = 0.0f;
} }
/* feed forward yaw setpoint rate */
_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;
} }
/* /*