fw attitude controllers: restructuring in order to allow acro flight mode

- split rate control into body rate control and euler rate control
- this allows standard attitude control but also direct rate control
of all three axes
- allow activation/deactivation of turn compensation

Signed-off-by: Roman <bapstroman@gmail.com>

attitude_fw/ecl_controller.h

@@ -56,9 +56,9 @@ struct ECL_ControlData {
 	float roll;
 	float pitch;
 	float yaw;
-	float roll_rate;
-	float pitch_rate;
-	float yaw_rate;
+	float body_x_rate;
+	float body_y_rate;
+	float body_z_rate;
 	float speed_body_u;
 	float speed_body_v;
 	float speed_body_w;
@@ -78,6 +78,7 @@ struct ECL_ControlData {
 	float groundspeed;
 	float groundspeed_scaler;
 	bool lock_integrator;
+	bool do_turn_compensation;
 };
 
 class __EXPORT ECL_Controller
@@ -87,6 +88,7 @@ public:
 	~ECL_Controller() = default;
 
 	virtual float control_attitude(const struct ECL_ControlData &ctl_data) = 0;
+	virtual float control_euler_rate(const struct ECL_ControlData &ctl_data) = 0;
 	virtual float control_bodyrate(const struct ECL_ControlData &ctl_data) = 0;
 
 	/* Setters */
@@ -96,6 +98,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;};
 
 	/* Getters */
 	float get_rate_error();

attitude_fw/ecl_pitch_controller.cpp

@@ -91,8 +91,8 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
 	/* Do not calculate control signal with bad inputs */
 	if (!(PX4_ISFINITE(ctl_data.roll) &&
 	      PX4_ISFINITE(ctl_data.pitch) &&
-	      PX4_ISFINITE(ctl_data.pitch_rate) &&
-	      PX4_ISFINITE(ctl_data.yaw_rate) &&
+	      PX4_ISFINITE(ctl_data.body_y_rate) &&
+	      PX4_ISFINITE(ctl_data.body_z_rate) &&
 	      PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
 	      PX4_ISFINITE(ctl_data.airspeed_min) &&
 	      PX4_ISFINITE(ctl_data.airspeed_max) &&
@@ -112,10 +112,6 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
 		lock_integrator = true;
 	}
 
-	/* Transform setpoint to body angular rates (jacobian) */
-	_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
-			     cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
-
 	/* apply turning offset to desired bodyrate setpoint*/
 	/* flying inverted (wings upside down)*/
 	bool inverted = false;
@@ -144,22 +140,24 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
 	/* input conditioning */
 	float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max);
 
-	/* Calculate desired body fixed y-axis angular rate needed to compensate for roll angle.
-	   For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175
-	   Availible on google books 8/11/2015:
-	   https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/
-	float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) *
-					      tanf(constrained_roll) * sinf(constrained_roll)));
+	if (ctl_data.do_turn_compensation) {
+		/* Calculate desired body fixed y-axis angular rate needed to compensate for roll angle.
+		   For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175
+		   Availible on google books 8/11/2015:
+		   https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/
+		float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) *
+						tanf(constrained_roll) * sinf(constrained_roll)));
 
-	if (inverted) {
-		body_fixed_turn_offset = -body_fixed_turn_offset;
+		if (inverted) {
+			body_fixed_turn_offset = -body_fixed_turn_offset;
+		}
+
+		/* Finally add the turn offset to your bodyrate setpoint*/
+		_bodyrate_setpoint += body_fixed_turn_offset;
 	}
 
-	/* Finally add the turn offset to your bodyrate setpoint*/
-	_bodyrate_setpoint += body_fixed_turn_offset;
 
-
-	_rate_error = _bodyrate_setpoint - ctl_data.pitch_rate;
+	_rate_error = _bodyrate_setpoint - ctl_data.body_y_rate;
 
 	if (!lock_integrator && _k_i > 0.0f) {
 
@@ -188,7 +186,15 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
 	_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
 		       _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
 		       + integrator_constrained;  //scaler is proportional to 1/airspeed
-//	warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
-//	warnx("roll: _last_output %.4f", (double)_last_output);
+
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }
+
+float ECL_PitchController::control_euler_rate(const struct ECL_ControlData &ctl_data)
+{
+	/* Transform setpoint to body angular rates (jacobian) */
+	_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
+			     cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
+
+	return control_bodyrate(ctl_data);
+}

attitude_fw/ecl_pitch_controller.h

@@ -62,6 +62,7 @@ public:
 	~ECL_PitchController() = default;
 
 	float control_attitude(const struct ECL_ControlData &ctl_data);
+	float control_euler_rate(const struct ECL_ControlData &ctl_data);
 	float control_bodyrate(const struct ECL_ControlData &ctl_data);
 
 	/* Additional Setters */

attitude_fw/ecl_roll_controller.cpp

@@ -79,8 +79,8 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
 {
 	/* Do not calculate control signal with bad inputs */
 	if (!(PX4_ISFINITE(ctl_data.pitch) &&
-	      PX4_ISFINITE(ctl_data.roll_rate) &&
-	      PX4_ISFINITE(ctl_data.yaw_rate) &&
+	      PX4_ISFINITE(ctl_data.body_x_rate) &&
+	      PX4_ISFINITE(ctl_data.body_z_rate) &&
 	      PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
 	      PX4_ISFINITE(ctl_data.airspeed_min) &&
 	      PX4_ISFINITE(ctl_data.airspeed_max) &&
@@ -100,11 +100,8 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
 		lock_integrator = true;
 	}
 
-	/* Transform setpoint to body angular rates (jacobian) */
-	_bodyrate_setpoint = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
-
 	/* Calculate body angular rate error */
-	_rate_error = _bodyrate_setpoint - ctl_data.roll_rate; //body angular rate error
+	_rate_error = _bodyrate_setpoint - ctl_data.body_x_rate; //body angular rate error
 
 	if (!lock_integrator && _k_i > 0.0f) {
 
@@ -127,8 +124,7 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
 
 	/* integrator limit */
 	//xxx: until start detection is available: integral part in control signal is limited here
-	float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max);
-	//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
+	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
 
 	/* Apply PI rate controller and store non-limited output */
 	_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
@@ -138,3 +134,12 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
+float ECL_RollController::control_euler_rate(const struct ECL_ControlData &ctl_data)
+{
+	/* Transform setpoint to body angular rates (jacobian) */
+	_bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
+
+	return control_bodyrate(ctl_data);
+
+}
+

attitude_fw/ecl_roll_controller.h

@@ -62,6 +62,7 @@ public:
 	~ECL_RollController() = default;
 
 	float control_attitude(const struct ECL_ControlData &ctl_data);
+	float control_euler_rate(const struct ECL_ControlData &ctl_data);
 	float control_bodyrate(const struct ECL_ControlData &ctl_data);
 };
 

attitude_fw/ecl_wheel_controller.cpp

@@ -54,12 +54,12 @@ ECL_WheelController::ECL_WheelController() :
 
 float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data)
 {
-	/* Do not calculate control signal with bad inputs */
-	if (!(PX4_ISFINITE(ctl_data.yaw_rate) &&
-	      PX4_ISFINITE(ctl_data.groundspeed) &&
-	      PX4_ISFINITE(ctl_data.groundspeed_scaler))) {
-		return math::constrain(_last_output, -1.0f, 1.0f);
-	}
+    /* Do not calculate control signal with bad inputs */
+    if (!(PX4_ISFINITE(ctl_data.body_z_rate) &&
+          PX4_ISFINITE(ctl_data.groundspeed) &&
+          PX4_ISFINITE(ctl_data.groundspeed_scaler))) {
+        return math::constrain(_last_output, -1.0f, 1.0f);
+    }
 
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
@@ -76,8 +76,8 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da
 	/* input conditioning */
 	float min_speed = 1.0f;
 
-	/* Calculate body angular rate error */
-	_rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error
+    /* Calculate body angular rate error */
+    _rate_error = _rate_setpoint - ctl_data.body_z_rate; //body angular rate error
 
 	if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) {
 

attitude_fw/ecl_wheel_controller.h

@@ -61,8 +61,11 @@ public:
 	ECL_WheelController();
 	~ECL_WheelController() = default;
 
-	float control_attitude(const struct ECL_ControlData &ctl_data);
-	float control_bodyrate(const struct ECL_ControlData &ctl_data);
+    float control_attitude(const struct ECL_ControlData &ctl_data);
+
+    float control_bodyrate(const struct ECL_ControlData &ctl_data);
+
+    float control_euler_rate(const struct ECL_ControlData &ctl_data) {return 0;};
 };
 
 #endif // ECL_HEADING_CONTROLLER_H

attitude_fw/ecl_yaw_controller.cpp

@@ -76,25 +76,6 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data
 	return _rate_setpoint;
 }
 
-float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data)
-{
-	switch (_coordinated_method) {
-	case COORD_METHOD_OPEN:
-	case COORD_METHOD_CLOSEACC:
-		return control_bodyrate_impl(ctl_data);
-
-	default:
-		static hrt_abstime last_print = 0;
-
-		if (ecl_elapsed_time(&last_print) > 5e6) {
-			warnx("invalid param setting FW_YCO_METHOD");
-			last_print = ecl_absolute_time();
-		}
-	}
-
-	return math::constrain(_last_output, -1.0f, 1.0f);
-}
-
 float ECL_YawController::control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
@@ -108,7 +89,6 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
 		return _rate_setpoint;
 	}
 
-//	static int counter = 0;
 	/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
 	_rate_setpoint = 0.0f;
 
@@ -122,25 +102,17 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
 					  9.81f * sinf(ctl_data.roll) * cosf(ctl_data.pitch) +
 					  ctl_data.speed_body_u * ctl_data.pitch_rate_setpoint * sinf(ctl_data.roll)) /
 					 denumerator;
-
-//			warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint);
 		}
-
-//		if(counter % 20 == 0) {
-//			warnx("denumerator: %.4f, speed_body_u: %.4f, speed_body_w: %.4f, cosf(roll): %.4f, cosf(pitch): %.4f, sinf(pitch): %.4f", (double)denumerator, (double)speed_body_u, (double)speed_body_w, (double)cosf(roll), (double)cosf(pitch), (double)sinf(pitch));
-//		}
 	}
 
-	/* limit the rate */ //XXX: move to body angluar rates
 
+
+	/* 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;
 	}
 
-
-//	counter++;
-
 	if (!PX4_ISFINITE(_rate_setpoint)) {
 		warnx("yaw rate sepoint not finite");
 		_rate_setpoint = 0.0f;
@@ -149,11 +121,11 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
 	return _rate_setpoint;
 }
 
-float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl_data)
+float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.pitch_rate) &&
-	      PX4_ISFINITE(ctl_data.yaw_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) &&
+	if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.body_y_rate) &&
+	      PX4_ISFINITE(ctl_data.body_z_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) &&
 	      PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) &&
 	      PX4_ISFINITE(ctl_data.scaler))) {
 		return math::constrain(_last_output, -1.0f, 1.0f);
@@ -182,11 +154,6 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl
 		airspeed = ctl_data.airspeed_min;
 	}
 
-
-	/* Transform setpoint to body angular rates (jacobian) */
-	_bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
-			     cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint;
-
 	/* Close the acceleration loop if _coordinated_method wants this: change body_rate setpoint */
 	if (_coordinated_method == COORD_METHOD_CLOSEACC) {
 		//XXX: filtering of acceleration?
@@ -194,7 +161,7 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl
 	}
 
 	/* Calculate body angular rate error */
-	_rate_error = _bodyrate_setpoint - ctl_data.yaw_rate; //body angular rate error
+	_rate_error = _bodyrate_setpoint - ctl_data.body_z_rate; //body angular rate error
 
 	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
 
@@ -222,7 +189,6 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl
 	/* Apply PI rate controller and store non-limited output */
 	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler *
 		       ctl_data.scaler;  //scaler is proportional to 1/airspeed
-	//warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
 
 
 	return math::constrain(_last_output, -1.0f, 1.0f);
@@ -233,3 +199,13 @@ float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_Co
 	/* dont set a rate setpoint */
 	return 0.0f;
 }
+
+float ECL_YawController::control_euler_rate(const struct ECL_ControlData &ctl_data)
+{
+	/* Transform setpoint to body angular rates (jacobian) */
+	_bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
+			     cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint;
+
+	return control_bodyrate(ctl_data);
+
+}

attitude_fw/ecl_yaw_controller.h

@@ -61,6 +61,7 @@ public:
 	~ECL_YawController() = default;
 
 	float control_attitude(const struct ECL_ControlData &ctl_data);
+	float control_euler_rate(const struct ECL_ControlData &ctl_data);
 	float control_bodyrate(const struct ECL_ControlData &ctl_data);
 
 	/* Additional setters */
@@ -85,8 +86,6 @@ protected:
 
 	int32_t _coordinated_method;
 
-	float control_bodyrate_impl(const struct ECL_ControlData &ctl_data);
-
 	float control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data);
 
 	float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data);