FW attitude control scaling fixes and cleanup (#15256)

* FW attitude control scaling fixes and cleanup

This commit aligns the scaling better with the derivations in
https://dev.px4.io/master/en/flight_stack/controller_diagrams.html#airspeed-scaling

Integrator terms now scale with IAS^2 (all three axes)

To match the roll and pitch controllers:
- Yaw integrator scale is now applied during accumulation, not to
  integral value (so now FW_YR_IMAX is respected more intuitively)
- Yaw FF term now scale with IAS instead of IAS^2

Also made a number of small changes to make the three files
  (roll, pitch, yaw) 3-way diffable to be clearer about what the
  differences among them are.

* Remove unused yaw coordination method

- "Coordination method" open vs. closed code removed, since closed
  is never used and not actually implemented.
- No change to behavior

* Remove PX4_WARN messages

Co-authored-by: george <george@campsix.com>

src/modules/fw_att_control/ecl_pitch_controller.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2013-2020 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,7 +39,6 @@
  */
 
 #include "ecl_pitch_controller.h"
-#include <math.h>
 #include <float.h>
 #include <lib/ecl/geo/geo.h>
 #include <mathlib/mathlib.h>
@@ -53,7 +52,6 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
 	      PX4_ISFINITE(ctl_data.pitch) &&
 	      PX4_ISFINITE(ctl_data.airspeed))) {
 
-		PX4_WARN("not controlling pitch");
 		return _rate_setpoint;
 	}
 
@@ -93,11 +91,13 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
 		lock_integrator = true;
 	}
 
+	/* Calculate body angular rate error */
 	_rate_error = _bodyrate_setpoint - ctl_data.body_y_rate;
 
 	if (!lock_integrator && _k_i > 0.0f) {
 
-		float id = _rate_error * dt * ctl_data.scaler;
+		/* Integral term scales with 1/IAS^2 */
+		float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler;
 
 		/*
 		 * anti-windup: do not allow integrator to increase if actuator is at limit
@@ -116,9 +116,10 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
 	}
 
 	/* Apply PI rate controller and store non-limited output */
+	/* FF terms scales with 1/TAS and P,I with 1/IAS^2 */
 	_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
 		       _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
-		       + _integrator;  //scaler is proportional to 1/airspeed
+		       + _integrator;
 
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }

src/modules/fw_att_control/ecl_pitch_controller.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2013-2020 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

src/modules/fw_att_control/ecl_roll_controller.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2016 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2013-2020 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -46,14 +46,16 @@
 float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(PX4_ISFINITE(ctl_data.roll_setpoint) && PX4_ISFINITE(ctl_data.roll))) {
+	if (!(PX4_ISFINITE(ctl_data.roll_setpoint) &&
+	      PX4_ISFINITE(ctl_data.roll))) {
+
 		return _rate_setpoint;
 	}
 
-	/* Calculate error */
+	/* Calculate the error */
 	float roll_error = ctl_data.roll_setpoint - ctl_data.roll;
 
-	/* Apply P controller */
+	/*  Apply P controller: rate setpoint from current error and time constant */
 	_rate_setpoint = roll_error / _tc;
 
 	return _rate_setpoint;
@@ -86,15 +88,16 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
 	}
 
 	/* Calculate body angular rate error */
-	_rate_error = _bodyrate_setpoint - ctl_data.body_x_rate; //body angular rate error
+	_rate_error = _bodyrate_setpoint - ctl_data.body_x_rate;
 
 	if (!lock_integrator && _k_i > 0.0f) {
 
-		float id = _rate_error * dt * ctl_data.scaler;
+		/* Integral term scales with 1/IAS^2 */
+		float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler;
 
 		/*
-		* anti-windup: do not allow integrator to increase if actuator is at limit
-		*/
+		 * anti-windup: do not allow integrator to increase if actuator is at limit
+		 */
 		if (_last_output < -1.0f) {
 			/* only allow motion to center: increase value */
 			id = math::max(id, 0.0f);
@@ -109,9 +112,10 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
 	}
 
 	/* Apply PI rate controller and store non-limited output */
+	/* FF terms scales with 1/TAS and P,I with 1/IAS^2 */
 	_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
 		       _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
-		       + _integrator;  //scaler is proportional to 1/airspeed
+		       + _integrator;
 
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }
@@ -124,5 +128,4 @@ float ECL_RollController::control_euler_rate(const struct ECL_ControlData &ctl_d
 	set_bodyrate_setpoint(_bodyrate_setpoint);
 
 	return control_bodyrate(ctl_data);
-
 }

src/modules/fw_att_control/ecl_roll_controller.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2013-2020 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

src/modules/fw_att_control/ecl_yaw_controller.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2016 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2013-2020 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -45,27 +45,7 @@
 
 float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data)
 {
-	switch (_coordinated_method) {
-	case COORD_METHOD_OPEN:
-		return control_attitude_impl_openloop(ctl_data);
 
-	case COORD_METHOD_CLOSEACC:
-		return control_attitude_impl_accclosedloop(ctl_data);
-
-	default:
-		static hrt_abstime last_print = 0;
-
-		if (hrt_elapsed_time(&last_print) > 5e6) {
-			PX4_WARN("invalid param setting FW_YCO_METHOD");
-			last_print = hrt_absolute_time();
-		}
-	}
-
-	return _rate_setpoint;
-}
-
-float ECL_YawController::control_attitude_impl_openloop(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) &&
@@ -118,9 +98,13 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
 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.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) &&
+	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);
@@ -138,29 +122,13 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
 		lock_integrator = true;
 	}
 
-	/* input conditioning */
-	float airspeed = ctl_data.airspeed;
-
-	if (!PX4_ISFINITE(airspeed)) {
-		/* airspeed is NaN, +- INF or not available, pick center of band */
-		airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
-
-	} else if (airspeed < ctl_data.airspeed_min) {
-		airspeed = ctl_data.airspeed_min;
-	}
-
-	/* Close the acceleration loop if _coordinated_method wants this: change body_rate setpoint */
-	if (_coordinated_method == COORD_METHOD_CLOSEACC) {
-		// XXX lateral acceleration needs to go into integrator with a gain
-		//_bodyrate_setpoint -= (ctl_data.acc_body_y / (airspeed * cosf(ctl_data.pitch)));
-	}
-
 	/* Calculate body angular rate error */
-	_rate_error = _bodyrate_setpoint - ctl_data.body_z_rate; // body angular rate error
+	_rate_error = _bodyrate_setpoint - ctl_data.body_z_rate;
 
-	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
+	if (!lock_integrator && _k_i > 0.0f) {
 
-		float id = _rate_error * dt;
+		/* Integral term scales with 1/IAS^2 */
+		float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler;
 
 		/*
 		 * anti-windup: do not allow integrator to increase if actuator is at limit
@@ -179,21 +147,14 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
 	}
 
 	/* Apply PI rate controller and store non-limited output */
-	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + _integrator) * ctl_data.scaler *
-		       ctl_data.scaler;  //scaler is proportional to 1/airspeed
-
+	/* FF terms scales with 1/TAS and P,I with 1/IAS^2 */
+	_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
+		       _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+		       + _integrator;
 
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
-float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data)
-{
-	(void)ctl_data; // unused
-
-	/* 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) */
@@ -203,5 +164,4 @@ float ECL_YawController::control_euler_rate(const struct ECL_ControlData &ctl_da
 	set_bodyrate_setpoint(_bodyrate_setpoint);
 
 	return control_bodyrate(ctl_data);
-
 }

src/modules/fw_att_control/ecl_yaw_controller.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2016 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2013-2020 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -45,6 +45,7 @@
  *   which in turn is based on initial work of
  *   Jonathan Challinger, 2012.
  */
+
 #ifndef ECL_YAW_CONTROLLER_H
 #define ECL_YAW_CONTROLLER_H
 
@@ -61,32 +62,9 @@ public:
 	float control_euler_rate(const struct ECL_ControlData &ctl_data) override;
 	float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
 
-	/* Additional setters */
-	void set_coordinated_min_speed(float coordinated_min_speed)
-	{
-		_coordinated_min_speed = coordinated_min_speed;
-	}
-
-	void set_coordinated_method(int32_t coordinated_method)
-	{
-		_coordinated_method = coordinated_method;
-	}
-
-	enum {
-		COORD_METHOD_OPEN = 0,
-		COORD_METHOD_CLOSEACC = 1
-	};
-
 protected:
-	float _coordinated_min_speed{1.0f};
 	float _max_rate{0.0f};
 
-	int32_t _coordinated_method{COORD_METHOD_OPEN};
-
-	float control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data);
-
-	float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data);
-
 };
 
 #endif // ECL_YAW_CONTROLLER_H