attitude_fw: move angular rate limits to body angular rates (#404)

attitude_fw/ecl_controller.cpp

@@ -104,6 +104,11 @@ void ECL_Controller::set_max_rate(float max_rate)
 	_max_rate = max_rate;
 }
 
+void ECL_Controller::set_bodyrate_setpoint(float rate)
+{
+	_bodyrate_setpoint = math::constrain(rate, -_max_rate, _max_rate);
+}
+
 float ECL_Controller::get_rate_error()
 {
 	return _rate_error;

attitude_fw/ecl_controller.h

@@ -91,7 +91,7 @@ public:
 	void set_k_ff(float k_ff);
 	void set_integrator_max(float max);
 	void set_max_rate(float max_rate);
-	void set_bodyrate_setpoint(float rate) {_bodyrate_setpoint = rate;}
+	void set_bodyrate_setpoint(float rate);
 
 	/* Getters */
 	float get_rate_error();

attitude_fw/ecl_pitch_controller.cpp

@@ -72,17 +72,6 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
 	/*  Apply P controller: rate setpoint from current error and time constant */
 	_rate_setpoint =  pitch_error / _tc;
 
-	/* limit the rate */
-	if (_max_rate > 0.01f && _max_rate_neg > 0.01f) {
-		if (_rate_setpoint > 0.0f) {
-			_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
-
-		} else {
-			_rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
-		}
-
-	}
-
 	return _rate_setpoint;
 }
 
@@ -148,5 +137,7 @@ float ECL_PitchController::control_euler_rate(const struct ECL_ControlData &ctl_
 	_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
 			     cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
 
+	set_bodyrate_setpoint(_bodyrate_setpoint);
+
 	return control_bodyrate(ctl_data);
 }

attitude_fw/ecl_pitch_controller.h

@@ -51,6 +51,7 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <mathlib/mathlib.h>
 
 #include "ecl_controller.h"
 
@@ -76,6 +77,11 @@ public:
 		_max_rate_neg = max_rate_neg;
 	}
 
+	void set_bodyrate_setpoint(float rate)
+	{
+		_bodyrate_setpoint = math::constrain(rate, -_max_rate_neg, _max_rate);
+	}
+
 	void set_roll_ff(float roll_ff)
 	{
 		_roll_ff = roll_ff;

attitude_fw/ecl_roll_controller.cpp

@@ -65,13 +65,6 @@ float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_dat
 	/* Apply P controller */
 	_rate_setpoint = roll_error / _tc;
 
-	/* limit the rate */ //XXX: move to body angluar rates
-
-	if (_max_rate > 0.01f) {
-		_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
-		_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
-	}
-
 	return _rate_setpoint;
 }
 
@@ -136,7 +129,8 @@ float ECL_RollController::control_euler_rate(const struct ECL_ControlData &ctl_d
 	/* Transform setpoint to body angular rates (jacobian) */
 	_bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
 
+	set_bodyrate_setpoint(_bodyrate_setpoint);
+	
 	return control_bodyrate(ctl_data);
 
 }
-

attitude_fw/ecl_yaw_controller.cpp

@@ -118,13 +118,6 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
 				 ctl_data.airspeed_min : ctl_data.airspeed);
 	}
 
-	/* limit the rate */ //XXX: move to body angluar rates
-
-	if (_max_rate > 0.01f) {
-		_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
-		_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
-	}
-
 	if (!PX4_ISFINITE(_rate_setpoint)) {
 		warnx("yaw rate sepoint not finite");
 		_rate_setpoint = 0.0f;
@@ -215,6 +208,8 @@ float ECL_YawController::control_euler_rate(const struct ECL_ControlData &ctl_da
 	_bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
 			     cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint;
 
+	set_bodyrate_setpoint(_bodyrate_setpoint);
+
 	return control_bodyrate(ctl_data);
 
 }