From 9c67a2ed8a9176668a44095be16be62b23ed3637 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?J=C3=A9r=C3=B4me?= <jerome.tabeaud@gmail.com>
Date: Sat, 30 Nov 2019 20:21:17 +0100
Subject: [PATCH] Fixed PID.

---
 FlyByWire.cpp |  8 ++++++--
 main.cpp      | 57 +++++++++++++++++++++++++++++++++++++++++++--------
 2 files changed, 54 insertions(+), 11 deletions(-)

diff --git a/FlyByWire.cpp b/FlyByWire.cpp
index 9ecb44e..1f1766c 100644
--- a/FlyByWire.cpp
+++ b/FlyByWire.cpp
@@ -255,13 +255,17 @@ PID::PID(Real Kp_, Real Ki_, Real Kd_, Real Ts_, Real output_lower_bound_, Real
         auto_anti_windup(auto_anti_windup_),
         freeze_integration(false)
 {
-    integrator = Integrator1(Ts, integrator_lower_bound_, integrator_upper_bound_);
+    integrator = Integrator1(Ts, Real(0.), 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 inte;
+    if(ComputeFreezeIntegrator())
+        inte = integrator.Filter(Real(0.));
+    else
+        inte = integrator.Filter(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);
diff --git a/main.cpp b/main.cpp
index 70f2923..41c7e84 100644
--- a/main.cpp
+++ b/main.cpp
@@ -76,10 +76,11 @@ 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;
+	const Real tf = 100., dt = 1./50.;
+	const Real g = 1*9.81, wMag = 1*1.;
+	// const Real Kp = 0., Ki = .1, Kd = 0.;
+	const Real Kp = 10, Ki = Kp*.2, Kd = Kp*0.5;
+	Real x = 5., xdot = 0., xdotdot = 0., u, w, t, xd, e;
 
 	uint32_t state[5] = {123456, 654897, 812656, 87913951, 0};// noise generator state
 
@@ -96,6 +97,13 @@ void test_PID_simple_system()
 		true,// auto_anti_windup_
 		1e-3);// anti_windup margin
 
+	// Check initialization of PID
+	PRINT_VAR(pid.integrator.Ts);
+	PRINT_VAR(pid.integrator.x_n_1);
+	PRINT_VAR(pid.integrator.y_n_1);
+	PRINT_VAR(pid.integrator.lower_bound);
+	PRINT_VAR(pid.integrator.upper_bound);
+
 	// Output to file
 	std::ofstream out("data_out.m", std::ios_base::out | std::ios_base::trunc);
 
@@ -109,11 +117,30 @@ void test_PID_simple_system()
 	for(t = 0. ; t <= tf ; t += dt)
 	{
 		// compute system target
-		xd = sin(2*M_PI*t/2);
+		xd = 1.*FlyByWire::Sat1(2. * sin(2*M_PI*t/10), -1., 1.);
 		e = xd-x;
 
+		// change pid settings on the fly
+		// force freeze integration
+		if(t >= 10. && t <= 20.)
+			pid.SetFreezeIntegration(true);
+		else
+			pid.SetFreezeIntegration(false);
+
+		// change output limits
+		if(t >= 40. && t <= 50.)
+		{
+			pid.output_lower_bound = 7.;
+			pid.output_upper_bound = 10.;
+		}
+		else
+		{
+			pid.output_lower_bound = -15.;//HUGE_VALUE_REAL
+			pid.output_upper_bound =  15.;
+		}
+
 		// compute system Input
-		// u = pid.Filter(e);
+		u = pid.Filter(e);
 
 		// simulate system
 		w = RAND_XORWOW_A_B(state, -wMag, wMag);
@@ -122,9 +149,21 @@ void test_PID_simple_system()
 		x 		+= xdot*dt;		// dirty Forward Euler
 
 		// write to file
-		out << t << " " << xd << " " << x << " " << xdot << " " << xdotdot << " " << e << " " << u << " " << w << "\n";
+		out << t << " "// 1
+			<< xd << " "// 2
+			<< x << " "// 3
+			<< xdot << " "// 4
+			<< xdotdot << " "// 5
+			<< e << " "// 6
+			<< u << " "// 7
+			<< w << " "// 8
+			<< pid.integrator.y_n_1 << " "// 9
+			<< pid.ComputeFreezeIntegrator() << " "// 10
+			<< "\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";
+	out << "];\nfigure(1), subplot(2,2,1), plot(a(:,1), a(:,2), 'linewidth', 2., a(:,1), a(:,3), 'linewidth', 2.), grid on, legend('xd', 'x')\n";
+	out << "subplot(2,2,2), plot(a(:,1), a(:,6), 'linewidth', 2., a(:,1), a(:,7), 'linewidth', 2.), grid on, legend('e', 'u')\n";
+	out << "subplot(2,2,3), plot(a(:,1), a(:,9), 'linewidth', 2.), grid on, legend('int(e)')\n";
+	out << "subplot(2,2,4), plot(a(:,1), a(:,10), 'linewidth', 2.), grid on, legend('freeze integration')\n";
 }