test_suite/unit_tests/Autopilot/: added unit test of pid-controller.

E.g.:
    test-suite -u PidControllerTests
    test-suite -u PidControllerTests::test

test_suite/unit_tests/Autopilot/CMakeLists.txt

@@ -2,11 +2,15 @@ set(TESTSUITE_SOURCES
     ${TESTSUITE_SOURCES}
     ${CMAKE_CURRENT_SOURCE_DIR}/TestSuite.cxx
     ${CMAKE_CURRENT_SOURCE_DIR}/testDigitalFilter.cxx
+    ${CMAKE_CURRENT_SOURCE_DIR}/testPidController.cxx
+    ${CMAKE_CURRENT_SOURCE_DIR}/testPidControllerData.cxx
     PARENT_SCOPE
 )
 
 set(TESTSUITE_HEADERS
     ${TESTSUITE_HEADERS}
     ${CMAKE_CURRENT_SOURCE_DIR}/testDigitalFilter.hxx
+    ${CMAKE_CURRENT_SOURCE_DIR}/testPidController.hxx
+    ${CMAKE_CURRENT_SOURCE_DIR}/testPidControllerData.hxx
     PARENT_SCOPE
 )

test_suite/unit_tests/Autopilot/TestSuite.cxx

@@ -18,7 +18,9 @@
  */
 
 #include "testDigitalFilter.hxx"
+#include "testPidController.hxx"
 
 
 // Set up the unit tests.
 CPPUNIT_TEST_SUITE_NAMED_REGISTRATION(DigitalFilterTests, "Unit tests");
+CPPUNIT_TEST_SUITE_NAMED_REGISTRATION(PidControllerTests, "Unit tests");

test_suite/unit_tests/Autopilot/testPidController.cxx

@@ -0,0 +1,110 @@
+#include "testPidController.hxx"
+#include "testPidControllerData.hxx"
+
+#include "test_suite/FGTestApi/testGlobals.hxx"
+
+
+#include <Autopilot/autopilot.hxx>
+#include <Autopilot/pidcontroller.hxx>
+#include <Main/fg_props.hxx>
+#include <Main/globals.hxx>
+
+
+#include <simgear/math/sg_random.h>
+#include <simgear/props/props_io.hxx>
+
+#include <unistd.h>
+
+// Set up function for each test.
+void PidControllerTests::setUp()
+{
+    FGTestApi::setUp::initTestGlobals("ap-pidcontroller");
+}
+
+
+// Clean up after each test.
+void PidControllerTests::tearDown()
+{
+    FGTestApi::tearDown::shutdownTestGlobals();
+}
+
+
+SGPropertyNode_ptr PidControllerTests::configFromString(const std::string& s)
+{
+    SGPropertyNode_ptr config = new SGPropertyNode;
+
+    std::istringstream iss(s);
+    readProperties(iss, config);
+    return config;
+}
+
+void PidControllerTests::test()
+{
+    sg_srandom(999);
+
+    // Define vertical-hold pid-controller (based on Harrier-GR3).
+    //
+    std::string config_text = 
+        R"(<?xml version="1.0" encoding="UTF-8"?>
+        <PropertyList>
+          <pid-controller>
+            <name>Vertical Speed Hold</name>
+            <debug>false</debug>
+            <enable>true</enable>
+            <input>
+              <prop>/velocities/vertical-speed-fps</prop>
+            </input>
+            <reference>
+                <prop>/autopilot/settings/vertical-speed-fpm</prop>
+                <scale>0.01667</scale>
+            </reference>
+            <output>
+              <prop>/controls/flight/elevator</prop>
+            </output>
+            <config>
+              <Kp>-0.025</Kp> <!-- proportional gain -->
+              <beta>1.0</beta>    <!-- input value weighing factor -->
+              <alpha>0.1</alpha>  <!-- low pass filter weighing factor -->
+              <gamma>0.0</gamma>  <!-- input value weighing factor for -->
+                                  <!-- unfiltered derivative error -->
+              <Ti>5</Ti>         <!-- integrator time -->
+              <Td>0.01</Td>       <!-- derivator time -->
+
+              <!-- Restrict elevator values to +/-0.35 at high speed, but allow full
+              +/-1 range at low speed. -->
+              <u_min>-1</u_min>
+              <u_max>1</u_max>
+
+            </config>
+          </pid-controller>
+        </PropertyList>
+        )";
+    
+    std::cout << "config_text is:\n" << config_text << "\n";
+    
+    SGPropertyNode_ptr config = configFromString(config_text);
+
+    auto ap = new FGXMLAutopilot::Autopilot(globals->get_props(), config);
+
+    globals->add_subsystem("ap", ap);
+    ap->bind();
+    ap->init();
+
+    assert(pidControllerInputs.size() == pidControllerOutputs.size());
+    
+    fgSetDouble("/controls/flight/elevator", 0);
+    
+    for (unsigned i=0; i<pidControllerInputs.size(); ++i) {
+        const PidControllerInput& input = pidControllerInputs[i];
+        const PidControllerOutput& output = pidControllerOutputs[i];
+        fgSetDouble("/velocities/vertical-speed-fps", input.vspeed_fps);
+        fgSetDouble("/autopilot/settings/vertical-speed-fpm", input.reference / 0.01667);
+        ap->update(0.00833333 /*dt*/);
+        double elevator = fgGetDouble("/controls/flight/elevator");
+        CPPUNIT_ASSERT_DOUBLES_EQUAL(output.output, elevator, 0.0001);
+        if (0) {
+            // This generates C++ text for setting the <outputs> vector above.
+            std::cout << "{ " << elevator << "},\n";
+        }
+    }
+}

test_suite/unit_tests/Autopilot/testPidController.hxx

@@ -0,0 +1,50 @@
+/*
+ * 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.
+struct PidControllerTests : public CppUnit::TestFixture
+{
+    // Set up function for each test.
+    void setUp();
+
+    // Clean up after each test.
+    void tearDown();
+
+    // The tests.
+    void test0();
+    void test1();
+    
+    private:
+    
+    // Set up the test suite.
+    CPPUNIT_TEST_SUITE(PidControllerTests);
+    CPPUNIT_TEST(test);
+    CPPUNIT_TEST_SUITE_END();
+
+    SGPropertyNode_ptr configFromString(const std::string& s);
+    void test();
+};

test_suite/unit_tests/Autopilot/testPidControllerData.hxx

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <vector>
+
+struct PidControllerInput
+{
+    double vspeed_fps;
+    double reference;
+};
+
+struct PidControllerOutput
+{
+    double output;
+};
+
+// Define sequence of input values. These are the values actually used with
+// Harrier-GR3 when startup_zeros=false, but we use the same inputs for
+// startup_zeros=true also.
+//
+extern std::vector<PidControllerInput> pidControllerInputs;
+
+// Expected output.
+//
+extern std::vector<PidControllerOutput> pidControllerOutputs;