133 lines
2.7 KiB
C++
133 lines
2.7 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;
|
|
}
|
|
|
|
}// namespace FlyByWire
|