zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #1905 from PX4/mc_att_rates_ff 

Multicopter Attitude Control: Introduce rates setpoint feedforward
sbg
Lorenz Meier 10 years ago
parent
60d2346f7a 7236f22f12
commit
2d8908ad01
2 changed files with 45 additions and 1 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 16
      src/modules/mc_att_control/mc_att_control_main.cpp
  2. 30
      src/modules/mc_att_control/mc_att_control_params.c

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

@ -166,14 +166,17 @@ private: @@ -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: @@ -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() : @@ -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() : @@ -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() @@ -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() @@ -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() @@ -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) @@ -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 */

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

@ -82,6 +82,16 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); @@ -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); @@ -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); @@ -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
*

Write Preview
Loading…
Cancel
Save