From f6dc9c972760d388f740d41edb3bc060af7b6e66 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 29 May 2015 11:54:38 -0700 Subject: [PATCH] multicopter manual attitude control: Leave some margin for yaw control --- src/modules/mc_att_control/mc_att_control_main.cpp | 3 ++- src/modules/mc_pos_control/mc_pos_control_main.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) 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 97d8625b02..4136107414 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -97,6 +97,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define YAW_DEADZONE 0.05f #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f +#define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f class MulticopterAttitudeControl { @@ -831,7 +832,7 @@ MulticopterAttitudeControl::task_main() if (_v_control_mode.flag_control_manual_enabled) { /* manual rates control - ACRO mode */ _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); - _thrust_sp = _manual_control_sp.z; + _thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER); /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index e5ea4d1047..7a3a5a679b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -83,6 +83,7 @@ #define TILT_COS_MAX 0.7f #define SIGMA 0.000001f #define MIN_DIST 0.01f +#define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f /** * Multicopter position control app start / stop handling function @@ -1387,7 +1388,7 @@ MulticopterPositionControl::task_main() //Control climb rate directly if no aiding altitude controller is active if(!_control_mode.flag_control_climb_rate_enabled) { - _att_sp.thrust = _manual.z; + _att_sp.thrust = math::min(_manual.z, MANUAL_THROTTLE_MAX_MULTICOPTER); } //Construct attitude setpoint rotation matrix