391 lines
15 KiB
C++
391 lines
15 KiB
C++
#ifndef DEF_FlyByWire
|
|
#define DEF_FlyByWire
|
|
|
|
#include <cmath>
|
|
#include <complex>
|
|
|
|
#define HUGE_VALUE_REAL Real(1e20)
|
|
|
|
// =============================================================================
|
|
// 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
|
|
{
|
|
|
|
typedef double Real;
|
|
typedef std::complex<Real> Complex;
|
|
|
|
/// Computes the heading error using the shortest route from hdg to tgt, both expressed in radians.
|
|
Real Hdg_err(Real tgt, Real hdg);
|
|
|
|
/// Computes the heading error using the shortest route from hdg to tgt, both expressed in degrees.
|
|
Real Hdg_err_deg(Real tgt, Real hdg);
|
|
|
|
/// Computes the median of the triplet (a, b, c). Useful to reject an invalid measurement from a triplet of sensors, or to mix 3 control laws in a continuous fashion (C0 continuity).
|
|
/// Internally, it is implemented as a sorting network. The middle value is the median of the triplet.
|
|
Real Vote(Real a, Real b, Real c);
|
|
|
|
/// Copies the input as long as it's not within +/- a from the origin. The output is then 0.
|
|
Real Deadzone(Real x, Real a);
|
|
|
|
/// Saturation function. Ensures that a <= x <= b.
|
|
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 y_n_1 : Output value at step n-1
|
|
/// \variable dy_dt_min : Min derivative
|
|
/// \variable dy_dt_max : Max derivative
|
|
/// \variable dt : Time step
|
|
/// \return y[n] rate-limited.
|
|
Real Ratelim(Real x_n, Real y_n_1, Real dy_dt_min, Real dy_dt_max, Real dt);
|
|
|
|
/// Rate limiter implemented as a self-contained recursive filter.
|
|
class RateLimiter
|
|
{
|
|
public:
|
|
/// Creates the RateLimiter object with the specified limits, and initial value. By default, y0 = 0.
|
|
RateLimiter(Real dy_dt_min, Real dy_dt_max, Real dt, Real y_ = Real(0.));
|
|
|
|
/// Applies the rate limiter filter to the input x[n] (parameter x_n) and returns the result y[n].
|
|
Real Filter(Real x_n);
|
|
|
|
public:
|
|
Real dy_dt_min; //!< Min rate.
|
|
Real dy_dt_max; //!< Max rate.
|
|
Real dt; //!< Time step.
|
|
Real y_n_1; //!< Previous filter value.
|
|
};
|
|
|
|
/// Heaviside function. 1 when x >= 0, 0 otherwise.
|
|
Real Heaviside(Real x);
|
|
|
|
// =============================================================================
|
|
// IRR Filters from the continuous Laplace transfer function.
|
|
// =============================================================================
|
|
|
|
/// 1st order integrator implemented using the trapezoïdal method.
|
|
/// G(s) = 1/s
|
|
/// The integrator can also be limited using the lower_bound and upper_bound parameters.
|
|
/// By default, the integrator is not bounded.
|
|
///
|
|
/// The class is to be used like so :
|
|
///
|
|
/// flt = Integrator1(Ts)
|
|
///
|
|
/// while control/filtering loop:
|
|
///
|
|
/// y_n = flt.Filter(x_n)
|
|
///
|
|
class Integrator1
|
|
{
|
|
public:
|
|
/// Creates an integrator object. By default, y0 = 0, and the limits are +/- 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.
|
|
void SetState(Real y_ = Real(0.), Real x_ = Real(0.));
|
|
|
|
/// Computes the next output value of the filter y[n] from the next input value x[n].
|
|
Real Filter(Real x_n);
|
|
|
|
public:
|
|
Real Ts;//!< Time step.
|
|
Real lower_bound;//!< Lower bound.
|
|
Real upper_bound;//!< Upper bound.
|
|
Real x_n_1;//!< x[n-1].
|
|
Real y_n_1;//!< y[n-1].
|
|
};
|
|
|
|
/// First order discretetized filter. The coefficients are automatically computed from the continuous-time transfer function :
|
|
///
|
|
/// Y b1 s + b0
|
|
/// G(s) = --- = ---------
|
|
/// X a1 s + a0
|
|
///
|
|
/// The Laplace transfer function is first discretized using the Tustin method (sampling period Ts).
|
|
///
|
|
/// Y -2 b1 + b0 Ts + (2 b1 + b0 Ts) z
|
|
/// G(z) = --- = --------------------------------
|
|
/// X -2 a1 + a0 Ts + (2 a1 + a0 Ts) z
|
|
///
|
|
/// The coefficients of the recurrence equation are then computed :
|
|
///
|
|
/// 2 b1 + b0 Ts
|
|
/// C(x[n]) = ------------
|
|
/// 2 a1 + a0 Ts
|
|
///
|
|
///
|
|
/// -2 b1 + b0 Ts
|
|
/// C(x[n-1]) = ------------
|
|
/// 2 a1 + a0 Ts
|
|
///
|
|
///
|
|
/// 2 a1 - a0 Ts
|
|
/// C(y[n-1]) = ------------
|
|
/// 2 a1 + a0 Ts
|
|
///
|
|
/// The class is to be used like so :
|
|
///
|
|
/// flt = filter1(b1, b0, a1, a0, Ts)
|
|
///
|
|
///
|
|
/// while control/filtering loop:
|
|
///
|
|
/// y_n = flt.Filter(x_n)
|
|
///
|
|
///
|
|
/// The necessary previous values y[n-1] and x[n-1] are automatically stored within the object.
|
|
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.));
|
|
|
|
/// Computes the filter coefficients from the continuous TF's coefficients.
|
|
void ComputeCoeffsFromContinuousTF(Real b1, Real b0, Real a1, Real a0, Real Ts);
|
|
|
|
/// Sets the state of the filter so that it can be initialized at any value.
|
|
/// By default, the states are initialized to 0.
|
|
void SetState(Real y_ = Real(0.), Real x_ = Real(0.));
|
|
|
|
/// Computes the next output value of the filter y[n] from the next input value x[n].
|
|
Real Filter(Real x_n);
|
|
|
|
/// First order discretized filter that computes the derivative of the input signal.
|
|
/// tau controls how fast the filter approximates the derivative of the input.
|
|
/// A good default value is 0.01.
|
|
///
|
|
/// s
|
|
/// G(s) = ---------
|
|
/// tau*s + 1
|
|
///
|
|
static Filter1 FilteredDerivative(Real Ts, Real tau = 0.01);
|
|
|
|
public:
|
|
Real cx_n; //!< C(x[n])
|
|
Real cx_n_1; //!< C(x[n-1])
|
|
Real cy_n_1; //!< C(y[n-1])
|
|
Real x_n_1; //!< x[n-1]
|
|
Real y_n_1; //!< y[n-1]
|
|
};
|
|
|
|
/// Second order discretetized filter. The coefficients are automatically computed from the continuous-time transfer function :
|
|
///
|
|
/// Y b2 s^2 + b1 s + b0
|
|
/// G(s) = --- = ------------------
|
|
/// X a2 s^2 + a1 s + a0
|
|
///
|
|
/// The Laplace transfer function is first discretized using the Tustin method (sampling period Ts).
|
|
///
|
|
/// Y 4 b2 - 2 b1 Ts + b0 Ts^2 + (-8 b2 + 2 b0 Ts^2) z + (4 b2 + 2 b1 Ts + b0 Ts^2) z^2
|
|
/// G(z) = --- = ---------------------------------------------------------------------------------
|
|
/// X 4 a2 - 2 a1 Ts + a0 Ts^2 + (-8 a2 + 2 a0 Ts^2) z + (4 a2 + 2 a1 Ts + a0 Ts^2) z^2
|
|
///
|
|
/// The coefficients of the recurrence equation are then computed :
|
|
///
|
|
/// 4 b2 + 2 b1 Ts + b0 Ts^2
|
|
/// C(x[n]) = ------------------------
|
|
/// 4 a2 + 2 a1 Ts + a0 Ts^2
|
|
///
|
|
///
|
|
/// -8 b2 + 2 b0 Ts^2
|
|
/// C(x[n-1]) = ------------------------
|
|
/// 4 a2 + 2 a1 Ts + a0 Ts^2
|
|
///
|
|
///
|
|
/// 4 b2 - 2 b1 Ts + b0 Ts^2
|
|
/// C(x[n-2]) = ------------------------
|
|
/// 4 a2 + 2 a1 Ts + a0 Ts^2
|
|
///
|
|
///
|
|
/// -8 a2 + 2 a0 Ts^2
|
|
/// C(y[n-1]) = ------------------------
|
|
/// 4 a2 + 2 a1 Ts + a0 Ts^2
|
|
///
|
|
///
|
|
/// 4 a2 - 2 a1 Ts + a0 Ts^2
|
|
/// C(y[n-2]) = ------------------------
|
|
/// 4 a2 + 2 a1 Ts + a0 Ts^2
|
|
///
|
|
/// The class is to be used like so :
|
|
///
|
|
/// flt = filter2(b2, b1, b0, a2, a1, a0, Ts)
|
|
///
|
|
/// while control/filtering loop:
|
|
///
|
|
/// 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:
|
|
/// Creates a Filter2 object from the continuous TF's coefficients.
|
|
Filter2(Real b2, Real b1, Real b0, Real a2, Real a1, Real a0, Real Ts, Real y0 = Real(0.));
|
|
|
|
/// Computes the filter coefficients from the continuous TF's coefficients.
|
|
void ComputeCoeffsFromContinuousTF(Real b2, Real b1, Real b0, Real a2, Real a1, Real a0, Real Ts);
|
|
|
|
/// Sets the state of the filter so that it can be initialized at any value.
|
|
/// By default, the states are initialized to 0.
|
|
void SetState(Real y_ = Real(0.), Real x_ = Real(0.));
|
|
|
|
/// Computes the next output value of the filter y[n] from the next input value x[n].
|
|
Real Filter(Real x_n);
|
|
|
|
/// Creates the transfer function from its poles, zeros, and a gain.
|
|
///
|
|
/// a (s - z1) (s - z2)
|
|
/// G(s) = -------------------
|
|
/// (s - p1) (s - p2)
|
|
///
|
|
static Filter2 FromPolesAndZeros(Real a, Complex p1, Complex p2, Complex z1, Complex z2, Real Ts);
|
|
|
|
/// Creates the transfer function from its poles, and the static gain a.
|
|
/// The poles can be real, or complex conjugate.
|
|
///
|
|
/// a p1 p2
|
|
/// G(s) = -----------------
|
|
/// (s - p1) (s - p2)
|
|
///
|
|
static Filter2 FromPoles(Real a, Complex p1, Complex p2, Real Ts);
|
|
|
|
/// Second order discretetized low-pass biquadratic filter.
|
|
///
|
|
/// The filter is parametrized using the cutoff frequency omega in rad/s, and the quality factor Q.
|
|
/// For no resonnance peak, use Q < 1/2
|
|
///
|
|
/// w^2
|
|
/// G(s) = -----------------
|
|
/// s^2 + w/Q*s + w^2
|
|
///
|
|
static Filter2 Lowpass(Real omega, Real Q, Real Ts);
|
|
|
|
/// Second order discretetized high-pass biquadratic filter.
|
|
/// The filter is parametrized using the cutoff frequency omega in rad/s, and the quality factor Q.
|
|
/// For no resonnance peak, use Q < 1/2
|
|
///
|
|
/// s^2
|
|
/// G(s) = -----------------
|
|
/// s^2 + w/Q*s + w^2
|
|
///
|
|
static Filter2 Highpass(Real omega, Real Q, Real Ts);
|
|
|
|
/// Second order discretetized band-pass biquadratic filter.
|
|
/// The filter is parametrized using the cutoff frequency omega in rad/s, and the quality factor Q.
|
|
/// For no resonnance peak, use Q < 1/2
|
|
///
|
|
/// w/Q*s
|
|
/// G(s) = -----------------
|
|
/// s^2 + w/Q*s + w^2
|
|
///
|
|
static Filter2 Bandpass(Real omega, Real Q, Real Ts);
|
|
|
|
/// Second order discretetized band-stop biquadratic filter.
|
|
/// The filter is parametrized using the cutoff frequency omega in rad/s, and the quality factor Q.
|
|
/// For no resonnance peak, use Q < 1/2
|
|
///
|
|
/// s^2 + w^2
|
|
/// G(s) = -----------------
|
|
/// s^2 + w/Q*s + w^2
|
|
///
|
|
static Filter2 Bandstop(Real omega, Real Q, Real Ts);
|
|
|
|
public:
|
|
Real cx_n; //!< C(x[n])
|
|
Real cx_n_1; //!< C(x[n-1])
|
|
Real cx_n_2; //!< C(x[n-2])
|
|
Real cy_n_1; //!< C(y[n-1])
|
|
Real cy_n_2; //!< C(y[n-2])
|
|
Real x_n_1; //!< x[n-1]
|
|
Real x_n_2; //!< y[n-2]
|
|
Real y_n_1; //!< x[n-1]
|
|
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
|