309 lines
8.5 KiB
C++
309 lines
8.5 KiB
C++
#include "FlyByWire.hpp"
|
|
|
|
// =============================================================================
|
|
// General fly-by-wire helper functions
|
|
// =============================================================================
|
|
|
|
namespace FlyByWire
|
|
{
|
|
|
|
Real Hdg_err(Real tgt, Real hdg)
|
|
{
|
|
Real err = tgt - hdg;
|
|
if(err > Real(M_PI))
|
|
return err - Real(M_PI*Real(2.));
|
|
else if(err < Real(-M_PI))
|
|
return err + Real(M_PI*Real(2.));
|
|
else
|
|
return err;
|
|
}
|
|
|
|
Real Hdg_err_deg(Real tgt, Real hdg)
|
|
{
|
|
Real err = tgt - hdg;
|
|
if(err > Real(180.))
|
|
return err - Real(360.);
|
|
else if(err < Real(-180.))
|
|
return err + Real(360.);
|
|
else
|
|
return err;
|
|
}
|
|
|
|
Real Vote(Real a, Real b, Real c)
|
|
{
|
|
Real tmp;
|
|
if(c < a)
|
|
{
|
|
tmp = c;
|
|
c = a;
|
|
a = tmp;
|
|
}
|
|
if(b < a)
|
|
{
|
|
tmp = b;
|
|
b = a;
|
|
a = tmp;
|
|
}
|
|
if(c < b)
|
|
{
|
|
tmp = c;
|
|
c = b;
|
|
b = tmp;
|
|
}
|
|
|
|
return b;
|
|
}
|
|
|
|
Real Deadzone(Real x, Real a)
|
|
{
|
|
if(fabs(x) >= a)
|
|
return x;
|
|
else
|
|
return Real(0.);
|
|
}
|
|
|
|
Real Sat1(Real x, Real a, Real b)
|
|
{
|
|
if(x < a)
|
|
return a;
|
|
else if(x > b)
|
|
return b;
|
|
else
|
|
return x;
|
|
}
|
|
|
|
Real Ratelim(Real x_n, Real y_n_1, Real dy_dt_min, Real dy_dt_max, Real dt)
|
|
{
|
|
Real dydt = (x_n - y_n_1)/dt; // candidate derivative
|
|
|
|
if(dydt < dy_dt_min) // minimum derivative
|
|
return y_n_1 + dy_dt_min*dt;
|
|
else if(dydt > dy_dt_max) // maximum derivative
|
|
return y_n_1 + dy_dt_max*dt;
|
|
else // derivative within bounds
|
|
return x_n;
|
|
}
|
|
|
|
RateLimiter::RateLimiter(Real dy_dt_min_, Real dy_dt_max_, Real dt_, Real y_)
|
|
: dy_dt_min(dy_dt_min_),
|
|
dy_dt_max(dy_dt_max_),
|
|
dt(dt_),
|
|
y_n_1(y_)
|
|
{}
|
|
|
|
Real RateLimiter::Filter(Real x_n)
|
|
{
|
|
Real y_n = Ratelim(x_n, y_n_1, dy_dt_min, dy_dt_max, dt);
|
|
y_n_1 = y_n;
|
|
return y_n;
|
|
}
|
|
|
|
Real Heaviside(Real x)
|
|
{
|
|
return (x >= Real(0.)) ? Real(1.) : Real(0.);
|
|
}
|
|
|
|
// =============================================================================
|
|
// IRR Filters from the continuous Laplace transfer function.
|
|
// =============================================================================
|
|
|
|
Integrator1::Integrator1(Real Ts_, Real y_, Real lower_bound_, Real upper_bound_)
|
|
: Ts(Ts_),
|
|
lower_bound(lower_bound_),
|
|
upper_bound(upper_bound_)
|
|
{
|
|
SetState(y_, Real(0.));
|
|
}
|
|
|
|
void Integrator1::SetState(Real y_, Real x_)
|
|
{
|
|
y_n_1 = y_;
|
|
x_n_1 = x_;
|
|
}
|
|
|
|
Real Integrator1::Filter(Real x_n)
|
|
{
|
|
Real y_n = y_n_1 + (x_n + x_n_1)*Ts/Real(2.);
|
|
y_n = Sat1(y_n, lower_bound, upper_bound);
|
|
x_n_1 = x_n;
|
|
y_n_1 = y_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);
|
|
ComputeCoeffsFromContinuousTF(b1, b0, a1, a0, Ts);
|
|
}
|
|
|
|
void Filter1::ComputeCoeffsFromContinuousTF(Real b1, Real b0, Real a1, Real a0, Real Ts)
|
|
{
|
|
cx_n = (2*b1 + b0*Ts)/(2*a1 + a0*Ts); // C(x[n])
|
|
cx_n_1 = (-2*b1 + b0*Ts)/(2*a1 + a0*Ts); // C(x[n-1])
|
|
cy_n_1 = (2*a1 - a0*Ts)/(2*a1 + a0*Ts); // C(y[n-1])
|
|
}
|
|
|
|
void Filter1::SetState(Real y_, Real x_)
|
|
{
|
|
y_n_1 = y_;
|
|
x_n_1 = x_;
|
|
}
|
|
|
|
Real Filter1::Filter(Real x_n)
|
|
{
|
|
Real y_n = cx_n*x_n + cx_n_1*x_n_1 + cy_n_1*y_n_1;
|
|
x_n_1 = x_n;
|
|
y_n_1 = y_n;
|
|
return y_n;
|
|
}
|
|
|
|
Filter1 Filter1::FilteredDerivative(Real Ts, Real tau)
|
|
{
|
|
return Filter1(Real(1.), Real(0.), tau, Real(1.), Ts, Real(0.));
|
|
}
|
|
|
|
Filter2::Filter2(Real b2, Real b1, Real b0, Real a2, Real a1, Real a0, Real Ts, Real y0)
|
|
{
|
|
SetState(y0);
|
|
ComputeCoeffsFromContinuousTF(b2, b1, b0, a2, a1, a0, Ts);
|
|
}
|
|
|
|
void Filter2::ComputeCoeffsFromContinuousTF(Real b2, Real b1, Real b0, Real a2, Real a1, Real a0, Real Ts)
|
|
{
|
|
Real Tssqr = Ts*Ts;
|
|
cx_n = (Real(4.)*b2 + Real(2.)*b1*Ts + b0*Tssqr)/(Real(4.)*a2 + Real(2.)*a1*Ts + a0*Tssqr); // C(x[n])
|
|
cx_n_1 = (-Real(8.)*b2 + Real(2.)*b0*Tssqr)/(Real(4.)*a2 + Real(2.)*a1*Ts + a0*Tssqr); // C(x[n-1])
|
|
cx_n_2 = (Real(4.)*b2 - Real(2.)*b1*Ts + b0*Tssqr)/(Real(4.)*a2 + Real(2.)*a1*Ts + a0*Tssqr); // C(x[n-2])
|
|
cy_n_1 = (-Real(8.)*a2 + Real(2.)*a0*Tssqr)/(Real(4.)*a2 + Real(2.)*a1*Ts + a0*Tssqr); // C(y[n-1])
|
|
cy_n_2 = (Real(4.)*a2 - Real(2.)*a1*Ts + a0*Tssqr)/(Real(4.)*a2 + Real(2.)*a1*Ts + a0*Tssqr); // C(y[n-2])
|
|
}
|
|
|
|
void Filter2::SetState(Real y_, Real x_)
|
|
{
|
|
y_n_1 = y_;
|
|
y_n_2 = y_;
|
|
x_n_1 = x_;
|
|
x_n_2 = x_;
|
|
}
|
|
|
|
Real Filter2::Filter(Real x_n)
|
|
{
|
|
Real y_n = cx_n*x_n + cx_n_1*x_n_1 + cx_n_2*x_n_2 - cy_n_1*y_n_1 - cy_n_2*y_n_2;
|
|
x_n_2 = x_n_1;
|
|
x_n_1 = x_n;
|
|
y_n_2 = y_n_1;
|
|
y_n_1 = y_n;
|
|
return y_n;
|
|
}
|
|
|
|
Filter2 Filter2::FromPolesAndZeros(Real a, Complex p1, Complex p2, Complex z1, Complex z2, Real Ts)
|
|
{
|
|
Complex b2(a), b1(a*(-z1 - z2)), b0(a*z1*z2),
|
|
a2(Real(1.)), a1(-p1 - p2), a0(p1*p2);
|
|
|
|
return Filter2(b2.real(), b1.real(), b0.real(), a2.real(), a1.real(), a0.real(), Ts);
|
|
}
|
|
|
|
Filter2 Filter2::FromPoles(Real a, Complex p1, Complex p2, Real Ts)
|
|
{
|
|
Complex b2(Real(0.)), b1(Real(0.)), b0(a*p1*p2),
|
|
a2(Real(1.)), a1(-p1 - p2), a0(p1*p2);
|
|
return Filter2(b2.real(), b1.real(), b0.real(), a2.real(), a1.real(), a0.real(), Ts);
|
|
}
|
|
|
|
Filter2 Filter2::Lowpass(Real omega, Real Q, Real Ts)
|
|
{
|
|
Real omegasqr = omega*omega;
|
|
return Filter2(Real(0.), Real(0.), omegasqr, Real(1.), omega/Q, omegasqr, Ts);
|
|
}
|
|
|
|
Filter2 Filter2::Highpass(Real omega, Real Q, Real Ts)
|
|
{
|
|
return Filter2(Real(1.), Real(0.), Real(0.), Real(1.), omega/Q, omega*omega, Ts);
|
|
}
|
|
|
|
Filter2 Filter2::Bandpass(Real omega, Real Q, Real Ts)
|
|
{
|
|
return Filter2(Real(0.), omega/Q, Real(0.), Real(1.), omega/Q, omega*omega, Ts);
|
|
}
|
|
|
|
Filter2 Filter2::Bandstop(Real omega, Real Q, Real Ts)
|
|
{
|
|
Real omegasqr = omega*omega;
|
|
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, Real(0.), integrator_lower_bound_, integrator_upper_bound_);
|
|
differentiator = Filter1::FilteredDerivative(Ts, tau_filtered_derivative);
|
|
}
|
|
|
|
Real PID::Filter(Real 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);
|
|
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
|