From 3375ae2c11845383b4364e9aa1749aa77990fe6e Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Sun, 10 Mar 2019 22:43:52 +0100
Subject: [PATCH] mc_att_control: parameter processing refactor
- Remove unnecessary in between rate limits member vectors.
- Only switch the yaw rate limit in auto modes,
other values stay the same anyways.
- Fill gain vectors with parameters in one line.
---
.../AttitudeControl/AttitudeControl.hpp | 6 +++
src/modules/mc_att_control/mc_att_control.hpp | 2 -
.../mc_att_control/mc_att_control_main.cpp | 53 ++++++-------------
3 files changed, 21 insertions(+), 40 deletions(-)
diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp
index f681120871..aad68ecc24 100644
--- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp
+++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp
@@ -77,6 +77,12 @@ public:
*/
void setRateLimit(const matrix::Vector3f &rate_limit) { _rate_limit = rate_limit; }
+ /**
+ * Set hard limit for output rate setpoint around yaw axis
+ * @param rate_limit_yaw [rad/s] limits for rotation around yaw axis
+ */
+ void setRateLimitYaw(const float rate_limit_yaw) { _rate_limit(2) = rate_limit_yaw; }
+
private:
matrix::Vector3f _proportional_gain;
matrix::Vector3f _rate_limit;
diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp
index 8d53a55c67..4a57da9ea8 100644
--- a/src/modules/mc_att_control/mc_att_control.hpp
+++ b/src/modules/mc_att_control/mc_att_control.hpp
@@ -283,8 +283,6 @@ private:
matrix::Vector3f _rate_d; /**< D gain for angular rate error */
matrix::Vector3f _rate_ff; /**< Feedforward gain for desired rates */
- matrix::Vector3f _mc_rate_max; /**< attitude rate limits in stabilized modes */
- matrix::Vector3f _auto_rate_max; /**< attitude rate limits in auto modes */
matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */
float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */
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 6880681d84..43db579d88 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -131,50 +131,27 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
void
MulticopterAttitudeControl::parameters_updated()
{
- /* Store some of the parameters in a more convenient way & precompute often-used values */
-
+ // Store some of the parameters in a more convenient way & precompute often-used values
_attitude_control.setProportionalGain(Vector3f(_roll_p.get(), _pitch_p.get(), _yaw_p.get()));
- /* roll gains */
- _rate_p(0) = _roll_rate_p.get();
- _rate_i(0) = _roll_rate_i.get();
- _rate_int_lim(0) = _roll_rate_integ_lim.get();
- _rate_d(0) = _roll_rate_d.get();
- _rate_ff(0) = _roll_rate_ff.get();
-
- /* pitch gains */
- _rate_p(1) = _pitch_rate_p.get();
- _rate_i(1) = _pitch_rate_i.get();
- _rate_int_lim(1) = _pitch_rate_integ_lim.get();
- _rate_d(1) = _pitch_rate_d.get();
- _rate_ff(1) = _pitch_rate_ff.get();
-
- /* yaw gains */
- _rate_p(2) = _yaw_rate_p.get();
- _rate_i(2) = _yaw_rate_i.get();
- _rate_int_lim(2) = _yaw_rate_integ_lim.get();
- _rate_d(2) = _yaw_rate_d.get();
- _rate_ff(2) = _yaw_rate_ff.get();
+ // rate gains
+ _rate_p = Vector3f(_roll_rate_p.get(), _pitch_rate_p.get(), _yaw_rate_p.get());
+ _rate_i = Vector3f(_roll_rate_i.get(), _pitch_rate_i.get(), _yaw_rate_i.get());
+ _rate_int_lim = Vector3f(_roll_rate_integ_lim.get(), _pitch_rate_integ_lim.get(), _yaw_rate_integ_lim.get());
+ _rate_d = Vector3f(_roll_rate_d.get(), _pitch_rate_d.get(), _yaw_rate_d.get());
+ _rate_ff = Vector3f(_roll_rate_ff.get(), _pitch_rate_ff.get(), _yaw_rate_ff.get());
if (fabsf(_lp_filters_d.get_cutoff_freq() - _d_term_cutoff_freq.get()) > 0.01f) {
_lp_filters_d.set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get());
_lp_filters_d.reset(_rates_prev);
}
- /* angular rate limits */
- _mc_rate_max(0) = math::radians(_roll_rate_max.get());
- _mc_rate_max(1) = math::radians(_pitch_rate_max.get());
- _mc_rate_max(2) = math::radians(_yaw_rate_max.get());
-
- /* auto angular rate limits */
- _auto_rate_max(0) = math::radians(_roll_rate_max.get());
- _auto_rate_max(1) = math::radians(_pitch_rate_max.get());
- _auto_rate_max(2) = math::radians(_yaw_auto_max.get());
+ // angular rate limits
+ using math::radians;
+ _attitude_control.setRateLimit(Vector3f(radians(_roll_rate_max.get()), radians(_pitch_rate_max.get()), radians(_yaw_rate_max.get())));
- /* manual rate control acro mode rate limits and expo */
- _acro_rate_max(0) = math::radians(_acro_roll_max.get());
- _acro_rate_max(1) = math::radians(_acro_pitch_max.get());
- _acro_rate_max(2) = math::radians(_acro_yaw_max.get());
+ // manual rate control acro mode rate limits
+ _acro_rate_max = Vector3f(radians(_acro_roll_max.get()), radians(_acro_pitch_max.get()), radians(_acro_yaw_max.get()));
_man_tilt_max = math::radians(_man_tilt_max_deg.get());
@@ -541,12 +518,12 @@ MulticopterAttitudeControl::control_attitude()
// physical thrust axis is the negative of body z axis
_thrust_sp = -_v_att_sp.thrust_body[2];
+ // lower yawspeed limit in auto modes because we expect yaw steps
if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
!_v_control_mode.flag_control_manual_enabled) {
- _attitude_control.setRateLimit(_auto_rate_max);
-
+ _attitude_control.setRateLimitYaw(math::radians(_yaw_auto_max.get()));
} else {
- _attitude_control.setRateLimit(_mc_rate_max);
+ _attitude_control.setRateLimitYaw(math::radians(_yaw_rate_max.get()));
}
_rates_sp = _attitude_control.update(Quatf(_v_att.q), Quatf(_v_att_sp.q_d), _v_att_sp.yaw_sp_move_rate);
--
GitLab