From 7b87b061daefe31cbeb827250be1b889f9723cf8 Mon Sep 17 00:00:00 2001 From: James Turner Date: Sat, 26 Dec 2020 16:47:49 +0000 Subject: [PATCH] Add coherent noise filter to the AP --- CMakeLists.txt | 2 + src/Autopilot/digitalfilter.cxx | 81 +++++++++++++++++++ .../unit_tests/Autopilot/CMakeLists.txt | 12 +++ test_suite/unit_tests/Autopilot/TestSuite.cxx | 24 ++++++ .../Autopilot/testDigitalFilter.cxx | 81 +++++++++++++++++++ .../Autopilot/testDigitalFilter.hxx | 50 ++++++++++++ test_suite/unit_tests/CMakeLists.txt | 1 + 7 files changed, 251 insertions(+) create mode 100644 test_suite/unit_tests/Autopilot/CMakeLists.txt create mode 100644 test_suite/unit_tests/Autopilot/TestSuite.cxx create mode 100644 test_suite/unit_tests/Autopilot/testDigitalFilter.cxx create mode 100644 test_suite/unit_tests/Autopilot/testDigitalFilter.hxx diff --git a/CMakeLists.txt b/CMakeLists.txt index a5965add9..d66c2aee1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -303,6 +303,8 @@ endif (USE_DBUS) ############################################################################## ## Qt5 setup setup if (ENABLE_QT) + set(CMAKE_AUTOMOC ON) + find_package(Qt5 5.9 COMPONENTS Widgets Gui Network Qml Quick Svg) if (Qt5Widgets_FOUND) set(HAVE_QT 1) diff --git a/src/Autopilot/digitalfilter.cxx b/src/Autopilot/digitalfilter.cxx index 13c703c3f..642a2bc47 100644 --- a/src/Autopilot/digitalfilter.cxx +++ b/src/Autopilot/digitalfilter.cxx @@ -201,6 +201,26 @@ public: double compute( double dt, double input ); virtual void initialize( double initvalue ); }; + +class CoherentNoiseFilterImplementation : public DigitalFilterImplementation +{ +protected: + InputValueList _amplitude; + + std::vector _discreteValues; + bool _absoluteVal = false; + size_t _numDiscreteValues = 1024; + + bool configure(SGPropertyNode& cfg_node, + const std::string& cfg_name, + SGPropertyNode& prop_root) override; + +public: + CoherentNoiseFilterImplementation(); + double compute(double dt, double input) override; + void initialize(double initvalue) override; +}; + /* --------------------------------------------------------------------------------- */ /* --------------------------------------------------------------------------------- */ @@ -703,6 +723,66 @@ bool LeadLagFilterImplementation::configure( SGPropertyNode& cfg_node, } return false; } + + +/* --------------------------------------------------------------------------------- */ + +CoherentNoiseFilterImplementation::CoherentNoiseFilterImplementation() : _amplitude(1.0), + _numDiscreteValues(1024) +{ +} + +void CoherentNoiseFilterImplementation::initialize(double initvalue) +{ + // allocate the array one bigger so we don't need to worry about + // wrapping.bound checking the discrete +1 lookup + _discreteValues.resize(_numDiscreteValues + 1); + std::generate(_discreteValues.begin(), _discreteValues.end(), []() { + return (sg_random() * 2.0) - 1.0; + }); +} + +static double lerp(double a, double b, double t) +{ + return (a * (1.0 - t)) + (b * t); +} + +double CoherentNoiseFilterImplementation::compute(double dt, double input) +{ + const double a = _amplitude.get_value(); + + const double t = input * _numDiscreteValues; + const int i = static_cast(floor(t)) % _numDiscreteValues; + const auto v0 = _discreteValues.at(i); + const auto v1 = _discreteValues.at(i + 1); + + const auto weight = t - floor(t); + const double output = lerp(v0, v1, weight); + + return _absoluteVal ? fabs(output) * a : output * a; +} + +//------------------------------------------------------------------------------ +bool CoherentNoiseFilterImplementation::configure(SGPropertyNode& cfg_node, + const std::string& cfg_name, + SGPropertyNode& prop_root) +{ + if (cfg_name == "discrete-resolution") { + _numDiscreteValues = cfg_node.getIntValue(); + return true; + } + + if (cfg_name == "amplitude") { + _amplitude.push_back(new InputValue(prop_root, cfg_node, 1.0)); + return true; + } + + if (cfg_name == "absolute") { + _absoluteVal = cfg_node.getBoolValue(); + } + return false; +} + /* -------------------------------------------------------------------------- */ /* Digital Filter Component Implementation */ /* -------------------------------------------------------------------------- */ @@ -746,6 +826,7 @@ bool DigitalFilter::configure( SGPropertyNode& prop_root, componentForge["lead-lag" ] = digitalFilterFactory; componentForge["integrator" ] = digitalFilterFactory; componentForge["damped-oscillation" ] = digitalFilterFactory; + componentForge["coherent-noise"] = digitalFilterFactory; } const auto type = simgear::strutils::strip(cfg.getStringValue("type")); diff --git a/test_suite/unit_tests/Autopilot/CMakeLists.txt b/test_suite/unit_tests/Autopilot/CMakeLists.txt new file mode 100644 index 000000000..3ec09b859 --- /dev/null +++ b/test_suite/unit_tests/Autopilot/CMakeLists.txt @@ -0,0 +1,12 @@ +set(TESTSUITE_SOURCES + ${TESTSUITE_SOURCES} + ${CMAKE_CURRENT_SOURCE_DIR}/TestSuite.cxx + ${CMAKE_CURRENT_SOURCE_DIR}/testDigitalFilter.cxx + PARENT_SCOPE +) + +set(TESTSUITE_HEADERS + ${TESTSUITE_HEADERS} + ${CMAKE_CURRENT_SOURCE_DIR}/testDigitalFilter.hxx + PARENT_SCOPE +) diff --git a/test_suite/unit_tests/Autopilot/TestSuite.cxx b/test_suite/unit_tests/Autopilot/TestSuite.cxx new file mode 100644 index 000000000..2732b0425 --- /dev/null +++ b/test_suite/unit_tests/Autopilot/TestSuite.cxx @@ -0,0 +1,24 @@ +/* + * Copyright (C) 2018 Edward d'Auvergne + * + * This file is part of the program FlightGear. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "testDigitalFilter.hxx" + + +// Set up the unit tests. +CPPUNIT_TEST_SUITE_NAMED_REGISTRATION(DigitalFilterTests, "Unit tests"); diff --git a/test_suite/unit_tests/Autopilot/testDigitalFilter.cxx b/test_suite/unit_tests/Autopilot/testDigitalFilter.cxx new file mode 100644 index 000000000..203636c79 --- /dev/null +++ b/test_suite/unit_tests/Autopilot/testDigitalFilter.cxx @@ -0,0 +1,81 @@ +#include "testDigitalFilter.hxx" + +#include + +#include "test_suite/FGTestApi/testGlobals.hxx" + + +#include +#include +#include
+#include
+ + +#include +#include + +// Set up function for each test. +void DigitalFilterTests::setUp() +{ + FGTestApi::setUp::initTestGlobals("ap-digitialfilter"); +} + + +// Clean up after each test. +void DigitalFilterTests::tearDown() +{ + FGTestApi::tearDown::shutdownTestGlobals(); +} + + +SGPropertyNode_ptr DigitalFilterTests::configFromString(const std::string& s) +{ + SGPropertyNode_ptr config = new SGPropertyNode; + + std::istringstream iss(s); + readProperties(iss, config); + return config; +} + +void DigitalFilterTests::testNoise() +{ + sg_srandom(999); + + auto config = configFromString(R"( + + + /test/a + /test/b + coherent-noise + 3.0 + true + 1024 + + + )"); + + auto ap = new FGXMLAutopilot::Autopilot(globals->get_props(), config); + + globals->add_subsystem("ap", ap); + ap->bind(); + ap->init(); + + // + // for (double x=0.0; x < 5.0; x+=0.01) { + // fgSetDouble("/test/a", x); + // ap->update(0.1); + // std::cerr << fgGetDouble("/test/b") << std::endl; + // } + + fgSetDouble("/test/a", 0.5); + ap->update(0.1); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.029, fgGetDouble("/test/b"), 0.001); + + fgSetDouble("/test/a", 0.8); + ap->update(0.1); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.808, fgGetDouble("/test/b"), 0.001); + + fgSetDouble("/test/a", 0.3); + ap->update(0.1); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.478, fgGetDouble("/test/b"), 0.001); +} diff --git a/test_suite/unit_tests/Autopilot/testDigitalFilter.hxx b/test_suite/unit_tests/Autopilot/testDigitalFilter.hxx new file mode 100644 index 000000000..da26acd88 --- /dev/null +++ b/test_suite/unit_tests/Autopilot/testDigitalFilter.hxx @@ -0,0 +1,50 @@ +/* + * Copyright (C) 2018 Edward d'Auvergne + * + * This file is part of the program FlightGear. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + + +#pragma once + + +#include +#include + +#include + + +// The system tests. +class DigitalFilterTests : public CppUnit::TestFixture +{ + // Set up the test suite. + CPPUNIT_TEST_SUITE(DigitalFilterTests); + CPPUNIT_TEST(testNoise); + CPPUNIT_TEST_SUITE_END(); + + SGPropertyNode_ptr configFromString(const std::string& s); + +public: + // Set up function for each test. + void setUp(); + + // Clean up after each test. + void tearDown(); + + + // The tests. + void testNoise(); +}; diff --git a/test_suite/unit_tests/CMakeLists.txt b/test_suite/unit_tests/CMakeLists.txt index d36b376ba..411e61ebd 100644 --- a/test_suite/unit_tests/CMakeLists.txt +++ b/test_suite/unit_tests/CMakeLists.txt @@ -9,6 +9,7 @@ foreach( unit_test_category Instrumentation Scripting AI + Autopilot ) add_subdirectory(${unit_test_category})