first commit

2022-10-20 20:29:11 +08:00
commit 4d531f8044
3238 changed files with 1387862 additions and 0 deletions
@@ -0,0 +1,93 @@
+// pidcontroller.hxx - implementation of PID controller
+//
+// Written by Torsten Dreyer
+// Based heavily on work created by Curtis Olson, started January 2004.
+//
+// Copyright (C) 2004  Curtis L. Olson  - http://www.flightgear.org/~curt
+// Copyright (C) 2010  Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// 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, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+//
+#ifndef __PIDCONTROLLER_HXX
+#define __PIDCONTROLLER_HXX 1
+
+#ifdef HAVE_CONFIG_H
+#  include <config.h>
+#endif
+
+#include "analogcomponent.hxx"
+
+#include <simgear/props/props.hxx>
+#include <simgear/structure/subsystem_mgr.hxx>
+
+namespace FGXMLAutopilot {
+
+/**
+ * Roy Ovesen's PID controller
+ */
+class PIDController : public AnalogComponent
+{
+private:
+    // Configuration values
+    InputValueList Kp;          // proportional gain
+    InputValueList Ti;          // Integrator time (sec)
+    InputValueList Td;          // Derivator time (sec)
+
+    double alpha;               // low pass filter weighing factor (usually 0.1)
+    double beta;                // process value weighing factor for
+                                // calculating proportional error
+                                // (usually 1.0)
+    double gamma;               // process value weighing factor for
+                                // calculating derivative error
+                                // (usually 0.0)
+
+    // Previous state tracking values
+    double ep_n_1;              // ep[n-1]  (prop error)
+    double edf_n_1;             // edf[n-1] (derivative error)
+    double edf_n_2;             // edf[n-2] (derivative error)
+    double u_n_1;               // u[n-1]   (output)
+    double desiredTs;           // desired sampling interval (sec)
+    double elapsedTime;         // elapsed time (sec)
+
+    /* If startup_current is false (the default), we initialise internal state
+    variables to zero.
+
+    Otherwise we initialise internal variables to the current values, which can
+    reduce initial transient behaviour. */
+    bool    startup_current;
+    
+    /* For the first startup_its iterations we don't modify our output. Default
+    is zero. */
+    unsigned    startup_its;
+
+    unsigned iteration;
+
+protected:
+    virtual bool configure( SGPropertyNode& cfg_node,
+                            const std::string& cfg_name,
+                            SGPropertyNode& prop_root );
+
+public:
+    PIDController();
+    ~PIDController() {}
+
+    // Subsystem identification.
+    static const char* staticSubsystemClassId() { return "pid-controller"; }
+
+    void update( bool firstTime, double dt ) override;
+};
+
+}
+#endif // __PIDCONTROLLER_HXX