A few things are implemented.

FlyByWire.cpp

@@ -0,0 +1,133 @@
+#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

FlyByWire.hpp

@@ -0,0 +1,101 @@
+#ifndef DEF_FlyByWire
+#define DEF_FlyByWire
+
+#include <cmath>
+
+typedef double Real;
+
+#define HUGE_VALUE_REAL Real(1e20)
+
+// =============================================================================
+// General fly-by-wire helper functions
+// =============================================================================
+
+namespace FlyByWire
+{
+
+/// 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;//< Time step.
+        Real upper_bound;//< Time step.
+        Real x_n_1;//< x[n-1].
+        Real y_n_1;//< y[n-1].
+};
+
+}
+
+#endif

Makefile

@@ -3,7 +3,7 @@ C = clang
 COMMON_FLAGS = -Wall -MMD
 C_FLAGS = $(COMMON_FLAGS)
 CC = clang++
-CC_FLAGS = $(COMMON_FLAGS) -std=c++17 # -funsafe-math-optimizations
+CC_FLAGS = $(COMMON_FLAGS) -std=c++17 -ffast-math # -funsafe-math-optimizations
 LD_FLAGS =
 INCLUDES =
 

README.md

@@ -1,3 +1,4 @@
-CppQuickStart
-=============
-"Empty" project for a quicker start writing something in c++.
+Fly By Wire
+===========
+C++ classes to make a fly-by-wire system. 1st and 2nd order IIR Filters,
+PID controllers with saturations, rate limiters, etc.

TestClass.cpp

@@ -1 +0,0 @@
-#include "TestClass.hpp"

TestClass.hpp

@@ -1,6 +0,0 @@
-#ifndef DEF_TestClass
-#define DEF_TestClass
-
-
-
-#endif

main.cpp

@@ -1,7 +1,7 @@
 #include <iostream>
 
 #include "utils.hpp"
-#include "TestClass.hpp"
+#include "FlyByWire.hpp"
 
 using std::cout;
 using std::endl;
@@ -9,6 +9,6 @@ using std::endl;
 int main()
 {
 
-	
+
 	return 0;
 }

test/1_test.cpp

@@ -3,32 +3,110 @@
 
 #include "../utils.hpp"
 #include "utils_test.hpp"
+#include "../FlyByWire.hpp"
 
 #define print(x) PRINT_VAR(x);
 #define printvec(x) PRINT_VEC(x);
 #define printstr(x) PRINT_STR(x);
 
-TEST_CASE( "Test case 1", "[test1]" )
+const Real deg = M_PI/180.;
+const Real tol = 1e-12;
+
+TEST_CASE( "Basic FlyByWire functions", "[BasicFBW]" )
 {
     std::cout.precision(16);
 
-    SECTION( "Check almost equal" ) {
-        CHECK(check_almost_equal(1.00, 1.01, 0.1));
-        CHECK(check_almost_equal(1.00, 3.01, 0.01));
-        CHECK(check_almost_equal(1.00, 1.01, 0.001));
-        REQUIRE(true);
+    SECTION( "Heading error computation" ) {
+        REQUIRE(check_almost_equal(FlyByWire::Hdg_err_deg(180, 45), 135., tol));
+        REQUIRE(check_almost_equal(FlyByWire::Hdg_err_deg(45, 180), -135., tol));
+        REQUIRE(check_almost_equal(FlyByWire::Hdg_err_deg(10, 270), 100., tol));
+        REQUIRE(check_almost_equal(FlyByWire::Hdg_err_deg(270, 10), -100., tol));
+
+        REQUIRE(check_almost_equal(FlyByWire::Hdg_err(M_PI, 0.7853981633974483), 2.356194490192345, tol));
+        REQUIRE(check_almost_equal(FlyByWire::Hdg_err(0.7853981633974483, 3.141592653589793), -2.356194490192345, tol));
+        REQUIRE(check_almost_equal(FlyByWire::Hdg_err(0.17453292519943295, 4.71238898038469), 1.7453292519943295, tol));
+        REQUIRE(check_almost_equal(FlyByWire::Hdg_err(4.71238898038469, 0.17453292519943295), -1.7453292519943295, tol));
     }
 
-    SECTION( "Check almost equal on vectors" ) {
-        unsigned int N = 5;
-        std::vector<double> v1(N), v2(N);
-        for (size_t i = 0; i < N; i++) {
-            v1[i] = double(i);
-            v2[i] = v1[i] + 0.0001;
+    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);
+    }
+
+    SECTION( "Deadzone" ) {
+        REQUIRE(FlyByWire::Deadzone(1.5, 0.1) == 1.5);
+        REQUIRE(FlyByWire::Deadzone(-1.5, 0.1) == -1.5);
+        REQUIRE(FlyByWire::Deadzone(0.1, 0.1) == 0.1);
+        REQUIRE(FlyByWire::Deadzone(-0.1, 0.1) == -0.1);
+        REQUIRE(FlyByWire::Deadzone(0.05, 0.1) == 0.);
+    }
+
+    SECTION( "Sat1" ) {
+        REQUIRE(FlyByWire::Sat1(1.5, -0.2, 0.1) == 0.1);
+        REQUIRE(FlyByWire::Sat1(-1.5, -0.2, 0.1) == -0.2);
+        REQUIRE(FlyByWire::Sat1(0.1, -0.2, 0.1) == 0.1);
+        REQUIRE(FlyByWire::Sat1(-0.2, -0.2, 0.1) == -0.2);
+        REQUIRE(FlyByWire::Sat1(0.05, -0.2, 0.1) == 0.05);
+        REQUIRE(FlyByWire::Sat1(-0.05, -0.2, 0.1) == -0.05);
+    }
+
+    SECTION( "Ratelim" ) {
+        Real    y_n_1 = 10.,
+                dy_dt_min = -1.5,
+                dy_dt_max = 2.5,
+                dt = 0.1;
+        // REQUIRE(Ratelim(Real x_n, Real y_n_1, Real dy_dt_min, Real dy_dt_max, Real dt) == );
+        REQUIRE(FlyByWire::Ratelim(y_n_1+dy_dt_max/2.*dt, y_n_1, dy_dt_min, dy_dt_max, dt) == y_n_1+dy_dt_max/2.*dt);// Within bounds positive
+        REQUIRE(FlyByWire::Ratelim(y_n_1+dy_dt_min/2.*dt, y_n_1, dy_dt_min, dy_dt_max, dt) == y_n_1+dy_dt_min/2.*dt);// Within bounds negative
+        REQUIRE(FlyByWire::Ratelim(y_n_1+dy_dt_max*2.*dt, y_n_1, dy_dt_min, dy_dt_max, dt) == y_n_1+dy_dt_max*dt);// Outside bounds positive
+        REQUIRE(FlyByWire::Ratelim(y_n_1+dy_dt_min*2.*dt, y_n_1, dy_dt_min, dy_dt_max, dt) == y_n_1+dy_dt_min*dt);// Outside bounds negative
+    }
+
+    // REQUIRE(true);
+    // CHECK(check_almost_equalV(v1, v2, 0.001));
+}
+
+TEST_CASE( "Integrator1", "[Integrator1]" )
+{
+    std::cout.precision(16);
+
+    SECTION( "Integral of a known function" )
+    {
+        Real dt = 0.01, t = 0., v_int;
+        FlyByWire::Integrator1 int1 = FlyByWire::Integrator1(dt, 0.);
+
+        for(int i = 0 ; i <= (int)(1./dt) ; i++)
+        {
+            t = i*dt;
+            v_int = int1.Filter(t*t);
         }
 
-        CHECK(check_almost_equalV(v1, v2, 0.001));
-        CHECK(check_almost_equalV(v1, v2, 0.0001));
-        CHECK(check_almost_equalV(v1, v2, 0.00001));
+        REQUIRE(check_almost_equal(v_int, 1./3., 1e-3));
+    }
+
+    SECTION( "Upper limit of integrator" )
+    {
+        Real dt = 0.1, lower_bound = -5., upper_bound = 2.5, v_int;
+        FlyByWire::Integrator1 int1 = FlyByWire::Integrator1(dt, 0., lower_bound, upper_bound);
+
+        for(int i = 0 ; i <= (int)(1./dt) ; i++)
+            v_int = int1.Filter(1000.);
+
+        REQUIRE(check_almost_equal(v_int, upper_bound, tol));
+    }
+
+    SECTION( "Lower limit of integrator" )
+    {
+        Real dt = 0.1, lower_bound = -5., upper_bound = 2.5, v_int;
+        FlyByWire::Integrator1 int1 = FlyByWire::Integrator1(dt, 0., lower_bound, upper_bound);
+
+        for(int i = 0 ; i <= (int)(1./dt) ; i++)
+            v_int = int1.Filter(-1000.);
+
+        REQUIRE(check_almost_equal(v_int, lower_bound, tol));
     }
 }

test/Makefile

@@ -12,7 +12,9 @@ EXEC = run
 CSOURCES = $(wildcard *.c)
 COBJECTS = $(CSOURCES:.c=.o)
 SOURCES = $(wildcard *.cpp)
+SOURCES += ../FlyByWire.cpp
 OBJECTS = $(SOURCES:.cpp=.o)
+# OBJECTS += ../FlyByWire.o
 
 # Main target
 $(EXEC): $(COBJECTS) $(OBJECTS)

test_c_lib.c

@@ -1 +0,0 @@
-#include "test_c_lib.h"

test_c_lib.h

@@ -1,6 +0,0 @@
-#ifndef DEF_test_c_lib
-#define DEF_test_c_lib
-
-
-
-#endif