dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #1905 from PX4/mc_att_rates_ff 

Multicopter Attitude Control: Introduce rates setpoint feedforward
This commit is contained in:
Lorenz Meier 2015-03-15 12:50:00 +01:00
parent 60d2346f7a 7236f22f12
commit 2d8908ad01
2 changed files with 45 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -166,14 +166,17 @@ private:
param_t roll_rate_p; param_t roll_rate_p;
param_t roll_rate_i; param_t roll_rate_i;
param_t roll_rate_d; param_t roll_rate_d;
param_t roll_rate_ff;
param_t pitch_p; param_t pitch_p;
param_t pitch_rate_p; param_t pitch_rate_p;
param_t pitch_rate_i; param_t pitch_rate_i;
param_t pitch_rate_d; param_t pitch_rate_d;
param_t pitch_rate_ff;
param_t yaw_p; param_t yaw_p;
param_t yaw_rate_p; param_t yaw_rate_p;
param_t yaw_rate_i; param_t yaw_rate_i;
param_t yaw_rate_d; param_t yaw_rate_d;
param_t yaw_rate_ff;
param_t yaw_ff; param_t yaw_ff;
param_t yaw_rate_max; 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_p; /**< P gain for angular rate error */
math::Vector<3> rate_i; /**< I 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_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_ff; /**< yaw control feed-forward */
float yaw_rate_max; /**< max yaw rate */ float yaw_rate_max; /**< max yaw rate */
@ -316,6 +320,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params.rate_p.zero(); _params.rate_p.zero();
_params.rate_i.zero(); _params.rate_i.zero();
_params.rate_d.zero(); _params.rate_d.zero();
_params.rate_ff.zero();
_params.yaw_ff = 0.0f; _params.yaw_ff = 0.0f;
_params.yaw_rate_max = 0.0f; _params.yaw_rate_max = 0.0f;
_params.man_roll_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_p = param_find("MC_ROLLRATE_P");
_params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I");
_params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); _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_p = param_find("MC_PITCH_P");
_params_handles.pitch_rate_p = param_find("MC_PITCHRATE_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_i = param_find("MC_PITCHRATE_I");
_params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); _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_p = param_find("MC_YAW_P");
_params_handles.yaw_rate_p = param_find("MC_YAWRATE_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_i = param_find("MC_YAWRATE_I");
_params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); _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_ff = param_find("MC_YAW_FF");
_params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX");
_params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); _params_handles.man_roll_max = param_find("MC_MAN_R_MAX");
@ -394,6 +402,8 @@ MulticopterAttitudeControl::parameters_update()
_params.rate_i(0) = v; _params.rate_i(0) = v;
param_get(_params_handles.roll_rate_d, &v); param_get(_params_handles.roll_rate_d, &v);
_params.rate_d(0) = v; _params.rate_d(0) = v;
param_get(_params_handles.roll_rate_ff, &v);
_params.rate_ff(0) = v;
/* pitch gains */ /* pitch gains */
param_get(_params_handles.pitch_p, &v); param_get(_params_handles.pitch_p, &v);
@ -404,6 +414,8 @@ MulticopterAttitudeControl::parameters_update()
_params.rate_i(1) = v; _params.rate_i(1) = v;
param_get(_params_handles.pitch_rate_d, &v); param_get(_params_handles.pitch_rate_d, &v);
_params.rate_d(1) = v; _params.rate_d(1) = v;
param_get(_params_handles.pitch_rate_ff, &v);
_params.rate_ff(1) = v;
/* yaw gains */ /* yaw gains */
param_get(_params_handles.yaw_p, &v); param_get(_params_handles.yaw_p, &v);
@ -414,6 +426,8 @@ MulticopterAttitudeControl::parameters_update()
_params.rate_i(2) = v; _params.rate_i(2) = v;
param_get(_params_handles.yaw_rate_d, &v); param_get(_params_handles.yaw_rate_d, &v);
_params.rate_d(2) = 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_ff, &_params.yaw_ff);
param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
@ -653,7 +667,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
/* angular rates error */ /* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates; 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; _rates_prev = rates;
/* update integral only if not saturated on low limit */ /* update integral only if not saturated on low limit */

30
src/modules/mc_att_control/mc_att_control_params.c
View File

@ -82,6 +82,16 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f);
*/ */
PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); 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 * Pitch P gain
* *
@ -123,6 +133,16 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f);
*/ */
PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); 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 * Yaw P gain
* *
@ -164,6 +184,16 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
*/ */
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 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 * Yaw feed forward
* *