src/Autopilot/xmlauto.hxx

   1 // xmlauto.hxx - a more flexible, generic way to build autopilots
   2 //
   3 // Written by Curtis Olson, started January 2004.
   4 //
   5 // Copyright (C) 2004  Curtis L. Olson  - http://www.flightgear.org/~curt
   6 //
   7 // This program is free software; you can redistribute it and/or
   8 // modify it under the terms of the GNU General Public License as
   9 // published by the Free Software Foundation; either version 2 of the
  10 // License, or (at your option) any later version.
  11 //
  12 // This program is distributed in the hope that it will be useful, but
  13 // WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15 // General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU General Public License
  18 // along with this program; if not, write to the Free Software
  19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  20 //
  21 // $Id$
  22
  23
  24 #ifndef _XMLAUTO_HXX
  25 #define _XMLAUTO_HXX 1
  26
  27 #ifndef __cplusplus
  28 # error This library requires C++
  29 #endif
  30
  31 #ifdef HAVE_CONFIG_H
  32 #  include <config.h>
  33 #endif
  34
  35 #include <simgear/compiler.h>
  36
  37 #include STL_STRING
  38 #include <vector>
  39 #include <deque>
  40
  41 SG_USING_STD(string);
  42 SG_USING_STD(vector);
  43 SG_USING_STD(deque);
  44
  45 #include <simgear/props/props.hxx>
  46 #include <simgear/structure/subsystem_mgr.hxx>
  47
  48
  49 /**
  50  * Base class for other autopilot components
  51  */
  52
  53 class FGXMLAutoComponent {
  54
  55 protected:
  56
  57     string name;
  58
  59     SGPropertyNode *enable_prop;
  60     string enable_value;
  61     bool enabled;
  62
  63     SGPropertyNode *input_prop;
  64     SGPropertyNode *r_n_prop;
  65     double r_n_value;
  66     vector <SGPropertyNode *> output_list;
  67
  68 public:
  69
  70     FGXMLAutoComponent() :
  71       enable_prop( NULL ),
  72       enable_value( "" ),
  73       enabled( false ),
  74       input_prop( NULL ),
  75       r_n_prop( NULL ),
  76       r_n_value( 0.0 )
  77     { }
  78
  79     virtual ~FGXMLAutoComponent() {}
  80
  81     virtual void update (double dt) {}
  82
  83     inline string get_name() { return name; }
  84 };
  85
  86
  87 /**
  88  * Roy Ovesen's PID controller
  89  */
  90
  91 class FGPIDController : public FGXMLAutoComponent {
  92
  93 private:
  94
  95     // debug flag
  96     bool debug;
  97
  98     // Input values
  99     double y_n;                 // measured process value
 100     double r_n;                 // reference (set point) value
 101     double y_scale;             // scale process input from property system
 102     double r_scale;             // scale reference input from property system
 103
 104     // Configuration values
 105     double Kp;                  // proportional gain
 106
 107     double alpha;               // low pass filter weighing factor (usually 0.1)
 108     double beta;                // process value weighing factor for
 109                                 // calculating proportional error
 110                                 // (usually 1.0)
 111     double gamma;               // process value weighing factor for
 112                                 // calculating derivative error
 113                                 // (usually 0.0)
 114
 115     double Ti;                  // Integrator time (sec)
 116     double Td;                  // Derivator time (sec)
 117
 118     double u_min;               // Minimum output clamp
 119     double u_max;               // Maximum output clamp
 120
 121     // Previous state tracking values
 122     double ep_n_1;              // ep[n-1]  (prop error)
 123     double edf_n_1;             // edf[n-1] (derivative error)
 124     double edf_n_2;             // edf[n-2] (derivative error)
 125     double u_n_1;               // u[n-1]   (output)
 126
 127
 128
 129 public:
 130
 131     FGPIDController( SGPropertyNode *node );
 132     FGPIDController( SGPropertyNode *node, bool old );
 133     ~FGPIDController() {}
 134
 135     void update_old( double dt );
 136     void update( double dt );
 137 };
 138
 139
 140 /**
 141  * A simplistic P [ + I ] PID controller
 142  */
 143
 144 class FGPISimpleController : public FGXMLAutoComponent {
 145
 146 private:
 147
 148     // proportional component data
 149     bool proportional;
 150     double Kp;
 151     SGPropertyNode *offset_prop;
 152     double offset_value;
 153
 154     // integral component data
 155     bool integral;
 156     double Ki;
 157     double int_sum;
 158
 159     // post functions for output
 160     bool clamp;
 161
 162     // debug flag
 163     bool debug;
 164
 165     // Input values
 166     double y_n;                 // measured process value
 167     double r_n;                 // reference (set point) value
 168     double y_scale;             // scale process input from property system
 169     double r_scale;             // scale reference input from property system
 170
 171     double u_min;               // Minimum output clamp
 172     double u_max;               // Maximum output clamp
 173
 174
 175 public:
 176
 177     FGPISimpleController( SGPropertyNode *node );
 178     ~FGPISimpleController() {}
 179
 180     void update( double dt );
 181 };
 182
 183
 184 /**
 185  * Predictor - calculates value in x seconds future.
 186  */
 187
 188 class FGPredictor : public FGXMLAutoComponent {
 189
 190 private:
 191
 192     // proportional component data
 193     double last_value;
 194     double average;
 195     double seconds;
 196     double filter_gain;
 197
 198     // debug flag
 199     bool debug;
 200
 201     // Input values
 202     double ivalue;                 // input value
 203
 204 public:
 205
 206     FGPredictor( SGPropertyNode *node );
 207     ~FGPredictor() {}
 208
 209     void update( double dt );
 210 };
 211
 212
 213 /**
 214  * FGDigitalFilter - a selection of digital filters
 215  *
 216  * Exponential filter
 217  * Double exponential filter
 218  * Moving average filter
 219  * Noise spike filter
 220  *
 221  * All these filters are low-pass filters.
 222  *
 223  */
 224
 225 class FGDigitalFilter : public FGXMLAutoComponent
 226 {
 227 private:
 228     double Tf;            // Filter time [s]
 229     unsigned int samples; // Number of input samples to average
 230     double rateOfChange;  // The maximum allowable rate of change [1/s]
 231     deque <double> output;
 232     deque <double> input;
 233     string filterType;
 234
 235     bool debug;
 236
 237 public:
 238     FGDigitalFilter(SGPropertyNode *node);
 239     ~FGDigitalFilter() {}
 240
 241     void update(double dt);
 242 };
 243
 244 /**
 245  * Model an autopilot system.
 246  *
 247  */
 248
 249 class FGXMLAutopilot : public SGSubsystem
 250 {
 251
 252 public:
 253
 254     FGXMLAutopilot();
 255     ~FGXMLAutopilot();
 256
 257     void init();
 258     void reinit();
 259     void bind();
 260     void unbind();
 261     void update( double dt );
 262
 263     bool build();
 264
 265 protected:
 266
 267     typedef vector<FGXMLAutoComponent *> comp_list;
 268
 269 private:
 270
 271     bool serviceable;
 272     SGPropertyNode *config_props;
 273     comp_list components;
 274 };
 275
 276
 277 #endif // _XMLAUTO_HXX