diff options
author | Lorenz Meier <lorenz@px4.io> | 2015-04-11 23:23:14 +0200 |
---|---|---|
committer | Lorenz Meier <lorenz@px4.io> | 2015-04-11 23:23:14 +0200 |
commit | 989aba363df8d98b1cb548a4b5dc0f915c49e771 (patch) | |
tree | 6341a56e006f592f269b49aa92a6d24ef0ea07b1 | |
parent | 7278952dc239d701954808030de1f6737bb033ea (diff) | |
parent | f1b2efeeaf1ca41fa20263af37b94485dcb9cee6 (diff) | |
download | px4-firmware-989aba363df8d98b1cb548a4b5dc0f915c49e771.tar.gz px4-firmware-989aba363df8d98b1cb548a4b5dc0f915c49e771.tar.bz2 px4-firmware-989aba363df8d98b1cb548a4b5dc0f915c49e771.zip |
Merge pull request #1928 from kd0aij/limitAutoRPrates
Limit auto roll/pitch rates
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_main.cpp | 31 | ||||
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_params.c | 24 |
2 files changed, 49 insertions, 6 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 85a0dda9e..d47731622 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -178,6 +178,8 @@ private: param_t yaw_rate_d; param_t yaw_rate_ff; param_t yaw_ff; + param_t roll_rate_max; + param_t pitch_rate_max; param_t yaw_rate_max; param_t man_roll_max; @@ -196,7 +198,11 @@ private: 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 */ + + float roll_rate_max; + float pitch_rate_max; + float yaw_rate_max; + math::Vector<3> mc_rate_max; /**< attitude rate limits in stabilized modes */ float man_roll_max; float man_pitch_max; @@ -322,10 +328,13 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.rate_d.zero(); _params.rate_ff.zero(); _params.yaw_ff = 0.0f; + _params.roll_rate_max = 0.0f; + _params.pitch_rate_max = 0.0f; _params.yaw_rate_max = 0.0f; _params.man_roll_max = 0.0f; _params.man_pitch_max = 0.0f; _params.man_yaw_max = 0.0f; + _params.mc_rate_max.zero(); _params.acro_rate_max.zero(); _rates_prev.zero(); @@ -352,6 +361,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _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.roll_rate_max = param_find("MC_ROLLRATE_MAX"); + _params_handles.pitch_rate_max = param_find("MC_PITCHRATE_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_pitch_max = param_find("MC_MAN_P_MAX"); @@ -430,10 +441,16 @@ MulticopterAttitudeControl::parameters_update() _params.rate_ff(2) = v; param_get(_params_handles.yaw_ff, &_params.yaw_ff); + + /* angular rate limits */ + param_get(_params_handles.roll_rate_max, &_params.roll_rate_max); + _params.mc_rate_max(0) = math::radians(_params.roll_rate_max); + param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max); + _params.mc_rate_max(1) = math::radians(_params.pitch_rate_max); param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); - _params.yaw_rate_max = math::radians(_params.yaw_rate_max); + _params.mc_rate_max(2) = math::radians(_params.yaw_rate_max); - /* manual control scale */ + /* manual attitude control scale */ param_get(_params_handles.man_roll_max, &_params.man_roll_max); param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); param_get(_params_handles.man_yaw_max, &_params.man_yaw_max); @@ -441,7 +458,7 @@ MulticopterAttitudeControl::parameters_update() _params.man_pitch_max = math::radians(_params.man_pitch_max); _params.man_yaw_max = math::radians(_params.man_yaw_max); - /* acro control scale */ + /* manual rate control scale and auto mode roll/pitch rate limits */ param_get(_params_handles.acro_roll_max, &v); _params.acro_rate_max(0) = math::radians(v); param_get(_params_handles.acro_pitch_max, &v); @@ -639,8 +656,10 @@ MulticopterAttitudeControl::control_attitude(float dt) /* calculate angular rates setpoint */ _rates_sp = _params.att_p.emult(e_R); - /* limit yaw rate */ - _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max); + /* limit rates */ + for (int i = 0; i < 3; i++) { + _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i)); + } /* feed forward yaw setpoint rate */ _rates_sp(2) += _v_att_sp.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 6a21f9046..3f19a51f0 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -206,6 +206,30 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f); PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); /** + * Max roll rate + * + * Limit for roll rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_MAX, 360.0f); + +/** + * Max pitch rate + * + * Limit for pitch rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 360.0f); + +/** * Max yaw rate * * Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. |