FlyByWireCpp/FlyByWire.hpp

#ifndef DEF_FlyByWire
#define DEF_FlyByWire

#include <cmath>
#include <complex>

#define HUGE_VALUE_REAL Real(1e20)

// =============================================================================
// General fly-by-wire helper functions
// =============================================================================

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 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 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]
};

}

#endif