First iteration of PID controller.
Added default Filter1 constructor. Fixed Vote test.
This commit is contained in:
Jérôme
parent
a532068fbc
commit
cfc7631d16
4 changed files with 275 additions and 52 deletions
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
{
|
||||
|
||||
|
|
@ -35,7 +42,7 @@ Real Sat1(Real x, Real a, Real b);
|
|||
/// Limits the rate of the signal x(t) : [dx/dt](t) to the interval [dy_dt_min, dy_dt_max].
|
||||
/// Use like a recursive filter.
|
||||
///
|
||||
/// \variable x_n : Input value at step n
|
||||
/// \variable x_n : Input value at step n
|
||||
/// \variable y_n_1 : Output value at step n-1
|
||||
/// \variable dy_dt_min : Min derivative
|
||||
/// \variable dy_dt_max : Max derivative
|
||||
|
|
@ -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
|
||||
|
|
|
|||
155
main.cpp
155
main.cpp
|
|
@ -10,54 +10,121 @@ 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)
|
||||
{// step response of simple 1st order filter
|
||||
double tend = 10.,
|
||||
dt = 0.1,
|
||||
t, y = 0.;
|
||||
FlyByWire::Filter1 filt1(1, 2, 3, 4, dt, y);
|
||||
|
||||
for(int i = 0 ; i <= (tend/dt) ; i++)
|
||||
{
|
||||
t = i*dt;
|
||||
cout << t << " " << y << "\n";
|
||||
y = filt1.Filter(1.);
|
||||
}
|
||||
}
|
||||
|
||||
// if(0)
|
||||
{// 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.,
|
||||
dt = 0.025,
|
||||
omega = 2*M_PI*10,
|
||||
Q = 20.,
|
||||
t, y = 0., y2;
|
||||
FlyByWire::Filter2 filt1 = FlyByWire::Filter2::Bandpass(omega, Q, dt);
|
||||
|
||||
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 1;
|
||||
}
|
||||
|
||||
out << "a = [";
|
||||
for(int i = 0 ; i <= (tend/dt) ; i++)
|
||||
{
|
||||
t = i*dt;
|
||||
y = RAND_XORWOW_A_B(state, -1., 1.);
|
||||
y2 = filt1.Filter(y);
|
||||
out << t << " " << y << " " << y2 << "\n";
|
||||
}
|
||||
out << "];\nplot(a(:,1), a(:,2), a(:,1), a(:,3), 'linewidth', 2.), grid on, legend('original', 'filtered')";
|
||||
}
|
||||
// test_Filter1_step_response();
|
||||
// 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.;
|
||||
FlyByWire::Filter1 filt1(1, 2, 3, 4, dt, y);
|
||||
|
||||
for(int i = 0 ; i <= (tend/dt) ; i++)
|
||||
{
|
||||
t = i*dt;
|
||||
cout << t << " " << y << "\n";
|
||||
y = filt1.Filter(1.);
|
||||
}
|
||||
}
|
||||
|
||||
void test_Filter2_random_noise()
|
||||
{
|
||||
// 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.,
|
||||
dt = 0.025,
|
||||
omega = 2*M_PI*10,
|
||||
Q = 20.,
|
||||
t, y = 0., y2;
|
||||
FlyByWire::Filter2 filt1 = FlyByWire::Filter2::Bandpass(omega, Q, dt);
|
||||
|
||||
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(int i = 0 ; i <= (tend/dt) ; i++)
|
||||
{
|
||||
t = i*dt;
|
||||
y = RAND_XORWOW_A_B(state, -1., 1.);
|
||||
y2 = filt1.Filter(y);
|
||||
out << t << " " << y << " " << y2 << "\n";
|
||||
}
|
||||
out << "];\nplot(a(:,1), a(:,2), a(:,1), a(:,3), 'linewidth', 2.), grid on, legend('original', 'filtered')";
|
||||
}
|
||||
|
||||
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";
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
REQUIRE(FlyByWire::Vote(a,c,b) == a);
|
||||
REQUIRE(FlyByWire::Vote(b,a,c) == a);
|
||||
REQUIRE(FlyByWire::Vote(c,b,a) == a);
|
||||
REQUIRE(FlyByWire::Vote(c,a,b) == a);
|
||||
CHECK(FlyByWire::Vote(a,b,c) == a);
|
||||
CHECK(FlyByWire::Vote(a,c,b) == a);
|
||||
CHECK(FlyByWire::Vote(b,a,c) == a);
|
||||
CHECK(FlyByWire::Vote(b,c,a) == a);
|
||||
CHECK(FlyByWire::Vote(c,a,b) == a);
|
||||
CHECK(FlyByWire::Vote(c,b,a) == a);
|
||||
}
|
||||
|
||||
SECTION( "Deadzone" ) {
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue