From 7236f22f12dc5609f2775b846d12a62175c11d80 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 10 Mar 2015 21:37:26 +0100 Subject: [PATCH] added feed-forward for rates --- .../mc_att_control/mc_att_control_main.cpp | 16 +++++++++- .../mc_att_control/mc_att_control_params.c | 30 +++++++++++++++++++ 2 files changed, 45 insertions(+), 1 deletion(-) 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 401cf05f17..85a0dda9e3 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -166,14 +166,17 @@ private: param_t roll_rate_p; param_t roll_rate_i; param_t roll_rate_d; + param_t roll_rate_ff; param_t pitch_p; param_t pitch_rate_p; param_t pitch_rate_i; param_t pitch_rate_d; + param_t pitch_rate_ff; param_t yaw_p; param_t yaw_rate_p; param_t yaw_rate_i; param_t yaw_rate_d; + param_t yaw_rate_ff; param_t yaw_ff; param_t yaw_rate_max; @@ -191,6 +194,7 @@ 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 */ + math::Vector<3> rate_ff; /**< Feedforward gain for desired rates */ float yaw_ff; /**< yaw control feed-forward */ float yaw_rate_max; /**< max yaw rate */ @@ -316,6 +320,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_d.zero(); + _params.rate_ff.zero(); _params.yaw_ff = 0.0f; _params.yaw_rate_max = 0.0f; _params.man_roll_max = 0.0f; @@ -335,14 +340,17 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); + _params_handles.roll_rate_ff = param_find("MC_ROLLRATE_FF"); _params_handles.pitch_p = param_find("MC_PITCH_P"); _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); + _params_handles.pitch_rate_ff = param_find("MC_PITCHRATE_FF"); _params_handles.yaw_p = param_find("MC_YAW_P"); _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_rate_ff = param_find("MC_YAWRATE_FF"); _params_handles.yaw_ff = param_find("MC_YAW_FF"); _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); @@ -394,6 +402,8 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(0) = v; param_get(_params_handles.roll_rate_d, &v); _params.rate_d(0) = v; + param_get(_params_handles.roll_rate_ff, &v); + _params.rate_ff(0) = v; /* pitch gains */ param_get(_params_handles.pitch_p, &v); @@ -404,6 +414,8 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(1) = v; param_get(_params_handles.pitch_rate_d, &v); _params.rate_d(1) = v; + param_get(_params_handles.pitch_rate_ff, &v); + _params.rate_ff(1) = v; /* yaw gains */ param_get(_params_handles.yaw_p, &v); @@ -414,6 +426,8 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(2) = v; param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; + param_get(_params_handles.yaw_rate_ff, &v); + _params.rate_ff(2) = v; param_get(_params_handles.yaw_ff, &_params.yaw_ff); param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); @@ -653,7 +667,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; - _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp); _rates_prev = rates; /* update integral only if not saturated on low limit */ 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 302551959b..6a21f9046f 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -82,6 +82,16 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); */ PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); +/** + * Roll rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f); + /** * Pitch P gain * @@ -123,6 +133,16 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); */ PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); +/** + * Pitch rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f); + /** * Yaw P gain * @@ -164,6 +184,16 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); */ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); +/** + * Yaw rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f); + /** * Yaw feed forward *