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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, 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)=0;
  82
  83     inline const 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     double y_offset;
 104     double r_offset;
 105
 106     // Configuration values
 107     double Kp;                  // proportional gain
 108
 109     double alpha;               // low pass filter weighing factor (usually 0.1)
 110     double beta;                // process value weighing factor for
 111                                 // calculating proportional error
 112                                 // (usually 1.0)
 113     double gamma;               // process value weighing factor for
 114                                 // calculating derivative error
 115                                 // (usually 0.0)
 116
 117     double Ti;                  // Integrator time (sec)
 118     double Td;                  // Derivator time (sec)
 119
 120     double u_min;               // Minimum output clamp
 121     double u_max;               // Maximum output clamp
 122
 123     // Previous state tracking values
 124     double ep_n_1;              // ep[n-1]  (prop error)
 125     double edf_n_1;             // edf[n-1] (derivative error)
 126     double edf_n_2;             // edf[n-2] (derivative error)
 127     double u_n_1;               // u[n-1]   (output)
 128     double desiredTs;            // desired sampling interval (sec)
 129     double elapsedTime;          // elapsed time (sec)
 130
 131
 132
 133 public:
 134
 135     FGPIDController( SGPropertyNode *node );
 136     FGPIDController( SGPropertyNode *node, bool old );
 137     ~FGPIDController() {}
 138
 139     void update_old( double dt );
 140     void update( double dt );
 141 };
 142
 143
 144 /**
 145  * A simplistic P [ + I ] PID controller
 146  */
 147
 148 class FGPISimpleController : public FGXMLAutoComponent {
 149
 150 private:
 151
 152     // proportional component data
 153     bool proportional;
 154     double Kp;
 155     SGPropertyNode *offset_prop;
 156     double offset_value;
 157
 158     // integral component data
 159     bool integral;
 160     double Ki;
 161     double int_sum;
 162
 163     // post functions for output
 164     bool clamp;
 165
 166     // debug flag
 167     bool debug;
 168
 169     // Input values
 170     double y_n;                 // measured process value
 171     double r_n;                 // reference (set point) value
 172     double y_scale;             // scale process input from property system
 173     double r_scale;             // scale reference input from property system
 174
 175     double u_min;               // Minimum output clamp
 176     double u_max;               // Maximum output clamp
 177
 178
 179 public:
 180
 181     FGPISimpleController( SGPropertyNode *node );
 182     ~FGPISimpleController() {}
 183
 184     void update( double dt );
 185 };
 186
 187
 188 /**
 189  * Predictor - calculates value in x seconds future.
 190  */
 191
 192 class FGPredictor : public FGXMLAutoComponent {
 193
 194 private:
 195
 196     // proportional component data
 197     double last_value;
 198     double average;
 199     double seconds;
 200     double filter_gain;
 201
 202     // debug flag
 203     bool debug;
 204
 205     // Input values
 206     double ivalue;                 // input value
 207
 208 public:
 209
 210     FGPredictor( SGPropertyNode *node );
 211     ~FGPredictor() {}
 212
 213     void update( double dt );
 214 };
 215
 216
 217 /**
 218  * FGDigitalFilter - a selection of digital filters
 219  *
 220  * Exponential filter
 221  * Double exponential filter
 222  * Moving average filter
 223  * Noise spike filter
 224  *
 225  * All these filters are low-pass filters.
 226  *
 227  */
 228
 229 class FGDigitalFilter : public FGXMLAutoComponent
 230 {
 231 private:
 232     double Tf;            // Filter time [s]
 233     unsigned int samples; // Number of input samples to average
 234     double rateOfChange;  // The maximum allowable rate of change [1/s]
 235     deque <double> output;
 236     deque <double> input;
 237     enum filterTypes { exponential, doubleExponential, movingAverage, noiseSpike };
 238     filterTypes filterType;
 239
 240     bool debug;
 241
 242 public:
 243     FGDigitalFilter(SGPropertyNode *node);
 244     ~FGDigitalFilter() {}
 245
 246     void update(double dt);
 247 };
 248
 249 /**
 250  * Model an autopilot system.
 251  *
 252  */
 253
 254 class FGXMLAutopilot : public SGSubsystem
 255 {
 256
 257 public:
 258
 259     FGXMLAutopilot();
 260     ~FGXMLAutopilot();
 261
 262     void init();
 263     void reinit();
 264     void bind();
 265     void unbind();
 266     void update( double dt );
 267
 268     bool build();
 269
 270 protected:
 271
 272     typedef vector<FGXMLAutoComponent *> comp_list;
 273
 274 private:
 275
 276     bool serviceable;
 277     SGPropertyNode *config_props;
 278     comp_list components;
 279 };
 280
 281
 282 #endif // _XMLAUTO_HXX