Add coherent noise filter to the AP

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)

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<double> _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<int>(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<LeadLagFilterImplementation>;
     componentForge["integrator"         ] = digitalFilterFactory<IntegratorFilterImplementation>;
     componentForge["damped-oscillation" ] = digitalFilterFactory<DampedOscillationFilterImplementation>;
+    componentForge["coherent-noise"] = digitalFilterFactory<CoherentNoiseFilterImplementation>;
   }
 
   const auto type = simgear::strutils::strip(cfg.getStringValue("type"));

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
+)

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 <http://www.gnu.org/licenses/>.
+ */
+
+#include "testDigitalFilter.hxx"
+
+
+// Set up the unit tests.
+CPPUNIT_TEST_SUITE_NAMED_REGISTRATION(DigitalFilterTests, "Unit tests");

test_suite/unit_tests/Autopilot/testDigitalFilter.cxx

@@ -0,0 +1,81 @@
+#include "testDigitalFilter.hxx"
+
+#include <strstream>
+
+#include "test_suite/FGTestApi/testGlobals.hxx"
+
+
+#include <Autopilot/autopilot.hxx>
+#include <Autopilot/digitalfilter.hxx>
+#include <Main/fg_props.hxx>
+#include <Main/globals.hxx>
+
+
+#include <simgear/math/sg_random.h>
+#include <simgear/props/props_io.hxx>
+
+// 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"(<?xml version="1.0" encoding="UTF-8"?>
+                                    <PropertyList>
+                                    <filter>
+                                        <input>/test/a</input>
+                                        <output>/test/b</output>
+                                        <type>coherent-noise</type>
+                                        <amplitude>3.0</amplitude>
+                                        <absolute type="bool">true</absolute>
+                                        <discrete-resolution>1024</discrete-resolution>
+                                    </filter>
+                                    </PropertyList>
+                                    )");
+
+    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);
+}

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 <http://www.gnu.org/licenses/>.
+ */
+
+
+#pragma once
+
+
+#include <cppunit/TestFixture.h>
+#include <cppunit/extensions/HelperMacros.h>
+
+#include <simgear/props/props.hxx>
+
+
+// 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();
+};

test_suite/unit_tests/CMakeLists.txt

@@ -9,6 +9,7 @@ foreach( unit_test_category
         Instrumentation
         Scripting
         AI
+        Autopilot
     )
 
     add_subdirectory(${unit_test_category})