diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-31 00:46:28 +0100 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-31 00:46:28 +0100 |
commit | 498155cf676623abd955ba66bb3e90ab538383d2 (patch) | |
tree | 616ae32ba571b1e1e821f8970fc2e63b3c405245 /src | |
parent | 7274c0ce3010c6f3292081a96cbd003e2d6bcefa (diff) | |
download | px4-firmware-498155cf676623abd955ba66bb3e90ab538383d2.tar.gz px4-firmware-498155cf676623abd955ba66bb3e90ab538383d2.tar.bz2 px4-firmware-498155cf676623abd955ba66bb3e90ab538383d2.zip |
mc_att_control: yaw dead zone fixed, added MC_YAW_FF (yaw feed-forward) parameter
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_main.cpp | 26 | ||||
-rw-r--r-- | src/modules/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 e37fe18b8..dc3da0f46 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 a170365ee..0a89c3f4e 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); |