jerome/FlyByWireCpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Fixed PID.

Browse source
This commit is contained in:
Jérôme 2019-11-30 20:21:17 +01:00
parent cfc7631d16
commit 9c67a2ed8a
2 changed files with 54 additions and 11 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
FlyByWire.cpp
View file

@ -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);

57
main.cpp
View file

@ -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";
}