First iteration of PID controller.

Added default Filter1 constructor.
Fixed Vote test.

FlyByWire.cpp

@@ -130,6 +130,12 @@ Real Integrator1::Filter(Real x_n)
     return y_n;
 }
 
+Filter1::Filter1()
+{
+    SetState(Real(0.));
+    ComputeCoeffsFromContinuousTF(Real(0.), Real(1.), Real(0.), Real(1.), Real(1.));
+}
+
 Filter1::Filter1(Real b1, Real b0, Real a1, Real a0, Real Ts, Real y0)
 {
     SetState(y0);
@@ -233,4 +239,67 @@ Filter2 Filter2::Bandstop(Real omega, Real Q, Real Ts)
     return Filter2(Real(1.), Real(0.), omegasqr, Real(1.), omega/Q, omegasqr, Ts);
 }
 
+// =============================================================================
+// PID controller
+// =============================================================================
+
+PID::PID(Real Kp_, Real Ki_, Real Kd_, Real Ts_, Real output_lower_bound_, Real output_upper_bound_, Real tau_filtered_derivative, Real integrator_lower_bound_, Real integrator_upper_bound_, bool auto_anti_windup_, Real auto_anti_windup_margin_)
+    :   Kp(Kp_),
+        Ki(Ki_),
+        Kd(Kd_),
+        Ts(Ts_),
+        output_lower_bound(output_lower_bound_),
+        output_upper_bound(output_upper_bound_),
+        auto_anti_windup_margin(auto_anti_windup_margin_),
+        u_n_1(Real(0.)),
+        auto_anti_windup(auto_anti_windup_),
+        freeze_integration(false)
+{
+    integrator = Integrator1(Ts, integrator_lower_bound_, integrator_upper_bound_);
+    differentiator = Filter1::FilteredDerivative(Ts, tau_filtered_derivative);
+}
+
+Real PID::Filter(Real e_n)
+{
+    Real inte = integrator.Filter(Real(!ComputeFreezeIntegrator())*e_n);
+    Real dere = differentiator.Filter(e_n);
+    Real u = Kp*e_n + Ki*inte + Kd*dere;
+    u = Sat1(u, output_lower_bound, output_upper_bound);
+    u_n_1 = u;
+    return u;
+}
+
+void PID::SetFreezeIntegration(bool freeze_integration_)
+{
+    freeze_integration = freeze_integration_;
+}
+
+void PID::SetFilteredDerivativeTimeConstant(Real tau_filtered_derivative)
+{
+    Filter1 differentiator2 = Filter1::FilteredDerivative(Ts, tau_filtered_derivative);// Recompute filter coefficients
+    differentiator2.x_n_1 = differentiator.x_n_1;
+    differentiator2.y_n_1 = differentiator.y_n_1;// copy current state of filter for continuity
+    differentiator = differentiator2;
+}
+
+void PID::SetIntegratorLimits(Real integrator_lower_bound, Real integrator_upper_bound)
+{
+    integrator.lower_bound = integrator_lower_bound;
+    integrator.upper_bound = integrator_upper_bound;
+}
+
+void PID::SetIntegratorValue(Real integrator_value)
+{
+    integrator.SetState(integrator_value, integrator_value);
+}
+
+bool PID::ComputeFreezeIntegrator()
+{
+    // automatic anti-windup freezes the integrator when the option is activated and the input is past either bound minus the margin
+    bool anti_windup_active = auto_anti_windup && ((u_n_1 <= output_lower_bound + auto_anti_windup_margin) || (u_n_1 >= output_upper_bound - auto_anti_windup_margin));
+
+    // Final order is computed from the user-forced inhibition and the automatic inhibition
+    return anti_windup_active || freeze_integration;
+}
+
 }// namespace FlyByWire

FlyByWire.hpp

@@ -10,6 +10,13 @@
 // General fly-by-wire helper functions
 // =============================================================================
 
+#ifndef MIN
+	#define MIN(a,b) (((a) < (b)) ? (a) : (b))
+#endif
+#ifndef MAX
+	#define MAX(a,b) (((a) > (b)) ? (a) : (b))
+#endif
+
 namespace FlyByWire
 {
 
@@ -84,7 +91,7 @@ class Integrator1
 {
     public:
         /// Creates an integrator object. By default, y0 = 0, and the limits are +/- HUGE_VALUE_REAL.
-        Integrator1(Real Ts_, Real y_ = Real(0.), Real lower_bound_ = -HUGE_VALUE_REAL, Real upper_bound_ = HUGE_VALUE_REAL);
+        Integrator1(Real Ts_ = Real(1.), Real y_ = Real(0.), Real lower_bound_ = -HUGE_VALUE_REAL, Real upper_bound_ = HUGE_VALUE_REAL);
 
         /// Sets the state of the filter so that it can be initialized at any value.
         /// By default, the states are initialized to 0.
@@ -143,6 +150,9 @@ class Integrator1
 class Filter1
 {
     public:
+		/// Creates pass-through Filter1 object.
+		Filter1();
+
         /// Creates a Filter1 object from the continuous TF's coefficients.
         Filter1(Real b1, Real b0, Real a1, Real a0, Real Ts, Real y0 = Real(0.));
 
@@ -221,7 +231,6 @@ class Filter1
 ///     y_n = flt.Filter(x_n)
 ///
 /// The necessary previous values y[n-1], y[n-2], x[n-1], and x[n-2] are automatically stored within the object.
-
 class Filter2
 {
     public:
@@ -308,6 +317,75 @@ class Filter2
         Real y_n_2;     //!< y[n-2]
 };
 
+// =============================================================================
+// PID controller
+// =============================================================================
+
+/// PID controller in its parallel form :
+/// u = Kp*e + Ki*int(e*dt, t) + Kd*de/dt
+///
+/// The derivative of the error is computed using a 1st order discretized filtered derivative filter.
+/// The integral of the error is computed using the trapezoïdal rule.
+/// The integrator can be limited using integrator_lower_bound and integrator_upper_bound.
+/// The integration can be frozen using the boolean freeze_integration (true to freeze it).
+/// The output can be limited using output_lower_bound and output_upper_bound.
+///
+/// An automatic anti-windup functionnality is provided : the user can choose a value a past which
+/// the integrator is automatically frozen in order to prevent wind-ups.
+/// For instance, if margin = 0.05 and the limits are [-1 ; 1], the integrator will be frozen
+/// in the intervals [-1 ; -0.95] and [0.95 ; 1]
+class PID
+{
+	public:
+		/// Initializes the PID controller with the right coefficients.
+		/// \param Kp_						: Proportionnal gain
+		/// \param Ki_						: Integral gain
+		/// \param Kd_						: Derivative gain
+		/// \param Ts_						: Time step
+		/// \param output_lower_bound_		: Lower bound of the controller output
+		/// \param output_upper_bound_		: Upper bound of the controller output
+		/// \param tau_filtered_derivative_	: Time constant of the filtered derivative
+		/// \param integrator_lower_bound_	: Lower bound of the integrator
+		/// \param integrator_upper_bound_	: Upper bound of the integrator
+		/// \param auto_anti_windup_		: State boolean to activate or deactivate the auto-anti-windup feature
+		/// \param auto_anti_windup_margin_	: Absolute margin for the auto-anti-windup feature
+		PID(Real Kp_, Real Ki_, Real Kd_, Real Ts_, Real output_lower_bound_ = -HUGE_VALUE_REAL, Real output_upper_bound_ = HUGE_VALUE_REAL, Real tau_filtered_derivative = Real(0.01), Real integrator_lower_bound_ = -HUGE_VALUE_REAL, Real integrator_upper_bound_ = HUGE_VALUE_REAL, bool auto_anti_windup_ = true, Real auto_anti_windup_margin_ = Real(1e-3));
+
+		/// Computes the next controller output value of the PID u[n] from the next error value e[n].
+        Real Filter(Real e_n);
+
+		/// Sets the state of the integration freezing feature. True to freeze the integration.
+		void SetFreezeIntegration(bool freeze_integration_);
+
+		/// Sets the time constant of the filtered derivative.
+		void SetFilteredDerivativeTimeConstant(Real tau_filtered_derivative);
+
+		/// Sets the integrator limits.
+		void SetIntegratorLimits(Real integrator_lower_bound, Real integrator_upper_bound);
+
+		/// Sets the current integrator value.
+		void SetIntegratorValue(Real integrator_value);
+
+		/// Computes whether the integrator should be frozen or not.
+		bool ComputeFreezeIntegrator();
+
+	public:
+		Real Kp;							//!< Proportionnal gain
+		Real Ki;							//!< Integral gain
+		Real Kd;							//!< Derivative gain
+		Real Ts;							//!< Time step
+		Real output_lower_bound;			//!< Lower bound of the controller output
+		Real output_upper_bound;			//!< Upper bound of the controller output
+		Real auto_anti_windup_margin;		//!< Margin for the auto-anti-windup feature
+		Real u_n_1;							//!< Previous controller output
+
+		bool auto_anti_windup;				//!< State boolean to activate the auto-anti-windup feature
+		bool freeze_integration;			//!< State boolean to freeze the integration of the error
+
+		Integrator1 integrator;				//!< Discrete 1st-order integrator
+		Filter1 differentiator;				//!< Discrete 1st-order filtered derivative
+};
+
 }
 
 #endif

main.cpp

@@ -10,12 +10,24 @@ using std::endl;
 using FlyByWire::Real;
 using FlyByWire::Complex;
 
+void test_Filter1_step_response();
+void test_Filter2_random_noise();
+void test_PID_simple_system();
+
 int main()
 {
 	cout.precision(16);
 
-	if(0)
+	// test_Filter1_step_response();
-	{// step response of simple 1st order filter
+	// test_Filter2_random_noise();
+	test_PID_simple_system();
+
+	return 0;
+}
+
+void test_Filter1_step_response()
+{
+	// step response of simple 1st order filter
 	double	tend = 10.,
 			dt = 0.1,
 			t, y = 0.;
@@ -29,8 +41,9 @@ int main()
 	}
 }
 
-	// if(0)
+void test_Filter2_random_noise()
-	{// Generate some uniform noise and filter it to see if the filter class works as expected
+{
+	// Generate some uniform noise and filter it to see if the filter class works as expected
 	uint32_t state[5] = {123456, 654897, 812656, 87913951, 0};
 
 	double	tend = 10.,
@@ -45,7 +58,7 @@ int main()
 	if(!out.is_open())
 	{
 		std::cerr << "Could not open the file in writing mode.\n";
-			return 1;
+		return;
 	}
 
 	out << "a = [";
@@ -59,5 +72,59 @@ int main()
 	out << "];\nplot(a(:,1), a(:,2), a(:,1), a(:,3), 'linewidth', 2.), grid on, legend('original', 'filtered')";
 }
 
-	return 0;
+void test_PID_simple_system()
+{
+	// Simple mass system with gravity g and perturbation w
+	// xdotdot = -g + u + w
+	const Real tf = 10., dt = 1./50.;
+	const Real g = 9.81, wMag = 1.;
+	const Real Kp = 1., Ki = 0.1, Kd = 0.1;
+	Real x = 0., xdot = 0., xdotdot = 0., u, w, t, xd, e;
+
+	uint32_t state[5] = {123456, 654897, 812656, 87913951, 0};// noise generator state
+
+	FlyByWire::PID pid(
+		Kp,
+		Ki,
+		Kd,
+		dt,
+		-HUGE_VALUE_REAL,//output_lower_bound_
+		HUGE_VALUE_REAL,//output_upper_bound_
+		Real(0.01),//tau_filtered_derivative
+		-HUGE_VALUE_REAL,//integrator_lower_bound_
+		HUGE_VALUE_REAL,//integrator_upper_bound_
+		true,// auto_anti_windup_
+		1e-3);// anti_windup margin
+
+	// Output to file
+	std::ofstream out("data_out.m", std::ios_base::out | std::ios_base::trunc);
+
+	if(!out.is_open())
+	{
+		std::cerr << "Could not open the file in writing mode.\n";
+		return;
+	}
+
+	out << "a = [";
+	for(t = 0. ; t <= tf ; t += dt)
+	{
+		// compute system target
+		xd = sin(2*M_PI*t/2);
+		e = xd-x;
+
+		// compute system Input
+		// u = pid.Filter(e);
+
+		// simulate system
+		w = RAND_XORWOW_A_B(state, -wMag, wMag);
+		xdotdot  = -g + u + w;
+		xdot 	+= xdotdot*dt;	// dirty Forward Euler
+		x 		+= xdot*dt;		// dirty Forward Euler
+
+		// write to file
+		out << t << " " << xd << " " << x << " " << xdot << " " << xdotdot << " " << e << " " << u << " " << w << "\n";
+	}
+
+	out << "];\nfigure(1), plot(a(:,1), a(:,2), a(:,1), a(:,3), 'linewidth', 2.), grid on, legend('xd', 'x')\n";
+	out << "figure(2), plot(a(:,1), a(:,6), a(:,1), a(:,7), 'linewidth', 2.), grid on, legend('e', 'u')\n";
 }

test/1_test.cpp

@@ -31,13 +31,22 @@ TEST_CASE( "Basic FlyByWire functions", "[BasicFBW]" )
         REQUIRE(check_almost_equal(FlyByWire::Hdg_err(4.71238898038469, 0.17453292519943295), -1.7453292519943295, tol));
     }
 
+    SECTION( "MIN/MAX macros" ) {
+        Real a = 1.5, b = -1.;
+        Real c = MIN(a,b);
+        REQUIRE(c == b);
+        c = MAX(a,b);
+        REQUIRE(c == a);
+    }
+
     SECTION( "Vote" ) {
         Real a = 1.5, b = -1., c = 3.;
-        REQUIRE(FlyByWire::Vote(a,b,c) == a);
+        CHECK(FlyByWire::Vote(a,b,c) == a);
-        REQUIRE(FlyByWire::Vote(a,c,b) == a);
+        CHECK(FlyByWire::Vote(a,c,b) == a);
-        REQUIRE(FlyByWire::Vote(b,a,c) == a);
+        CHECK(FlyByWire::Vote(b,a,c) == a);
-        REQUIRE(FlyByWire::Vote(c,b,a) == a);
+        CHECK(FlyByWire::Vote(b,c,a) == a);
-        REQUIRE(FlyByWire::Vote(c,a,b) == a);
+        CHECK(FlyByWire::Vote(c,a,b) == a);
+        CHECK(FlyByWire::Vote(c,b,a) == a);
     }
 
     SECTION( "Deadzone" ) {