src/Autopilot/pidcontroller.hxx

   1 // pidcontroller.hxx - implementation of PID controller
   2 //
   3 // Written by Torsten Dreyer
   4 // Based heavily on work created by Curtis Olson, started January 2004.
   5 //
   6 // Copyright (C) 2004  Curtis L. Olson  - http://www.flightgear.org/~curt
   7 // Copyright (C) 2010  Torsten Dreyer - Torsten (at) t3r (dot) de
   8 //
   9 // This program is free software; you can redistribute it and/or
  10 // modify it under the terms of the GNU General Public License as
  11 // published by the Free Software Foundation; either version 2 of the
  12 // License, or (at your option) any later version.
  13 //
  14 // This program is distributed in the hope that it will be useful, but
  15 // WITHOUT ANY WARRANTY; without even the implied warranty of
  16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  17 // General Public License for more details.
  18 //
  19 // You should have received a copy of the GNU General Public License
  20 // along with this program; if not, write to the Free Software
  21 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  22 //
  23 #ifndef __PIDCONTROLLER_HXX
  24 #define __PIDCONTROLLER_HXX 1
  25
  26 #ifdef HAVE_CONFIG_H
  27 #  include <config.h>
  28 #endif
  29
  30 #include "analogcomponent.hxx"
  31
  32 #include <simgear/props/props.hxx>
  33 #include <simgear/structure/subsystem_mgr.hxx>
  34
  35 namespace FGXMLAutopilot {
  36
  37 /**
  38  * Roy Ovesen's PID controller
  39  */
  40 class PIDController : public AnalogComponent {
  41
  42 private:
  43     // Configuration values
  44     InputValueList Kp;          // proportional gain
  45     InputValueList Ti;          // Integrator time (sec)
  46     InputValueList Td;          // Derivator time (sec)
  47
  48     double alpha;               // low pass filter weighing factor (usually 0.1)
  49     double beta;                // process value weighing factor for
  50                                 // calculating proportional error
  51                                 // (usually 1.0)
  52     double gamma;               // process value weighing factor for
  53                                 // calculating derivative error
  54                                 // (usually 0.0)
  55
  56     // Previous state tracking values
  57     double ep_n_1;              // ep[n-1]  (prop error)
  58     double edf_n_1;             // edf[n-1] (derivative error)
  59     double edf_n_2;             // edf[n-2] (derivative error)
  60     double u_n_1;               // u[n-1]   (output)
  61     double desiredTs;            // desired sampling interval (sec)
  62     double elapsedTime;          // elapsed time (sec)
  63
  64 protected:
  65     virtual bool configure( SGPropertyNode& cfg_node,
  66                             const std::string& cfg_name,
  67                             SGPropertyNode& prop_root );
  68 public:
  69     PIDController();
  70     ~PIDController() {}
  71
  72     void update( bool firstTime, double dt );
  73 };
  74
  75 }
  76 #endif // __PIDCONTROLLER_HXX