diff --git a/FlyByWire.cpp b/FlyByWire.cpp new file mode 100644 index 0000000..e607da3 --- /dev/null +++ b/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 diff --git a/FlyByWire.hpp b/FlyByWire.hpp new file mode 100644 index 0000000..e5e579c --- /dev/null +++ b/FlyByWire.hpp @@ -0,0 +1,101 @@ +#ifndef DEF_FlyByWire +#define DEF_FlyByWire + +#include + +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 diff --git a/Makefile b/Makefile index 844d085..62c74d2 100644 --- a/Makefile +++ b/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 = diff --git a/README.md b/README.md index 1541520..9700f1f 100644 --- a/README.md +++ b/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. diff --git a/TestClass.cpp b/TestClass.cpp deleted file mode 100644 index d8b75fb..0000000 --- a/TestClass.cpp +++ /dev/null @@ -1 +0,0 @@ -#include "TestClass.hpp" diff --git a/TestClass.hpp b/TestClass.hpp deleted file mode 100644 index 0b97a7b..0000000 --- a/TestClass.hpp +++ /dev/null @@ -1,6 +0,0 @@ -#ifndef DEF_TestClass -#define DEF_TestClass - - - -#endif diff --git a/main.cpp b/main.cpp index b378978..4699842 100644 --- a/main.cpp +++ b/main.cpp @@ -1,7 +1,7 @@ #include #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; } diff --git a/test/1_test.cpp b/test/1_test.cpp index fa45c9b..93fb71c 100644 --- a/test/1_test.cpp +++ b/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 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)); } } diff --git a/test/Makefile b/test/Makefile index 3a12f85..1c27b23 100644 --- a/test/Makefile +++ b/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) diff --git a/test_c_lib.c b/test_c_lib.c deleted file mode 100644 index dd20182..0000000 --- a/test_c_lib.c +++ /dev/null @@ -1 +0,0 @@ -#include "test_c_lib.h" diff --git a/test_c_lib.h b/test_c_lib.h deleted file mode 100644 index 3e16bab..0000000 --- a/test_c_lib.h +++ /dev/null @@ -1,6 +0,0 @@ -#ifndef DEF_test_c_lib -#define DEF_test_c_lib - - - -#endif