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.

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;

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] */
 

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);