From 498155cf676623abd955ba66bb3e90ab538383d2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 00:46:28 +0100 Subject: [PATCH] mc_att_control: yaw dead zone fixed, added MC_YAW_FF (yaw feed-forward) parameter --- .../mc_att_control/mc_att_control_main.cpp | 26 ++++++++++++++++--- .../mc_att_control/mc_att_control_params.c | 1 + 2 files changed, 24 insertions(+), 3 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 e37fe18b8f..dc3da0f467 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -156,6 +156,9 @@ private: param_t yaw_rate_p; param_t yaw_rate_i; param_t yaw_rate_d; + param_t yaw_ff; + + param_t rc_scale_yaw; } _params_handles; /**< handles for interesting parameters */ struct { @@ -163,6 +166,9 @@ private: math::Vector<3> rate_p; /**< P gain for angular rate error */ math::Vector<3> rate_i; /**< I gain for angular rate error */ math::Vector<3> rate_d; /**< D gain for angular rate error */ + float yaw_ff; /**< yaw control feed-forward */ + + float rc_scale_yaw; } _params; /** @@ -284,6 +290,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + _params_handles.yaw_ff = param_find("MC_YAW_FF"); + + _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); /* fetch initial parameter values */ parameters_update(); @@ -342,6 +351,10 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; + param_get(_params_handles.yaw_ff, &_params.yaw_ff); + + param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); + return OK; } @@ -461,9 +474,16 @@ MulticopterAttitudeControl::control_attitude(float dt) // reset_yaw_sp = true; //} } else { - if (_manual_control_sp.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual_control_sp.yaw) { + float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw; + if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp.yaw; + yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw; + if (_manual_control_sp.yaw > 0.0f) { + yaw_sp_move_rate -= YAW_DEADZONE; + } else { + yaw_sp_move_rate += YAW_DEADZONE; + } + yaw_sp_move_rate *= _params.rc_scale_yaw; _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); _v_att_sp.R_valid = false; publish_att_sp = true; @@ -592,7 +612,7 @@ MulticopterAttitudeControl::control_attitude(float dt) _rates_sp = _params.p.emult(e_R); /* feed forward yaw setpoint rate */ - _rates_sp(2) += yaw_sp_move_rate * yaw_w; + _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; } /* diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index a170365eec..0a89c3f4e5 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -51,3 +51,4 @@ PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);