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 #include <Main/fg_props.hxx>
  49
  50
  51 /**
  52  * Base class for other autopilot components
  53  */
  54
  55 class FGXMLAutoComponent {
  56
  57 protected:
  58
  59     string name;
  60
  61     SGPropertyNode_ptr enable_prop;
  62     SGPropertyNode_ptr passive_mode;
  63     string enable_value;
  64     bool honor_passive;
  65     bool enabled;
  66
  67     SGPropertyNode_ptr input_prop;
  68     SGPropertyNode_ptr r_n_prop;
  69     double r_n_value;
  70     vector <SGPropertyNode_ptr> output_list;
  71
  72 public:
  73
  74     FGXMLAutoComponent() :
  75       enable_prop( NULL ),
  76       passive_mode( fgGetNode("/autopilot/settings/passive-mode", true) ),
  77       enable_value( "" ),
  78       honor_passive( false ),
  79       enabled( false ),
  80       input_prop( NULL ),
  81       r_n_prop( NULL ),
  82       r_n_value( 0.0 )
  83     { }
  84
  85     virtual ~FGXMLAutoComponent() {}
  86
  87     virtual void update (double dt)=0;
  88
  89     inline const string& get_name() { return name; }
  90 };
  91
  92
  93 /**
  94  * Roy Ovesen's PID controller
  95  */
  96
  97 class FGPIDController : public FGXMLAutoComponent {
  98
  99 private:
 100
 101     // debug flag
 102     bool debug;
 103
 104     // Input values
 105     double y_n;                 // measured process value
 106     double r_n;                 // reference (set point) value
 107     double y_scale;             // scale process input from property system
 108     double r_scale;             // scale reference input from property system
 109     double y_offset;
 110     double r_offset;
 111
 112     // Configuration values
 113     double Kp;                  // proportional gain
 114
 115     double alpha;               // low pass filter weighing factor (usually 0.1)
 116     double beta;                // process value weighing factor for
 117                                 // calculating proportional error
 118                                 // (usually 1.0)
 119     double gamma;               // process value weighing factor for
 120                                 // calculating derivative error
 121                                 // (usually 0.0)
 122
 123     double Ti;                  // Integrator time (sec)
 124     double Td;                  // Derivator time (sec)
 125
 126     double u_min;               // Minimum output clamp
 127     double u_max;               // Maximum output clamp
 128
 129     // Previous state tracking values
 130     double ep_n_1;              // ep[n-1]  (prop error)
 131     double edf_n_1;             // edf[n-1] (derivative error)
 132     double edf_n_2;             // edf[n-2] (derivative error)
 133     double u_n_1;               // u[n-1]   (output)
 134     double desiredTs;            // desired sampling interval (sec)
 135     double elapsedTime;          // elapsed time (sec)
 136
 137
 138
 139 public:
 140
 141     FGPIDController( SGPropertyNode *node );
 142     FGPIDController( SGPropertyNode *node, bool old );
 143     ~FGPIDController() {}
 144
 145     void update_old( double dt );
 146     void update( double dt );
 147 };
 148
 149
 150 /**
 151  * A simplistic P [ + I ] PID controller
 152  */
 153
 154 class FGPISimpleController : public FGXMLAutoComponent {
 155
 156 private:
 157
 158     // proportional component data
 159     bool proportional;
 160     double Kp;
 161     SGPropertyNode_ptr offset_prop;
 162     double offset_value;
 163
 164     // integral component data
 165     bool integral;
 166     double Ki;
 167     double int_sum;
 168
 169     // post functions for output
 170     bool clamp;
 171
 172     // debug flag
 173     bool debug;
 174
 175     // Input values
 176     double y_n;                 // measured process value
 177     double r_n;                 // reference (set point) value
 178     double y_scale;             // scale process input from property system
 179     double r_scale;             // scale reference input from property system
 180
 181     double u_min;               // Minimum output clamp
 182     double u_max;               // Maximum output clamp
 183
 184
 185 public:
 186
 187     FGPISimpleController( SGPropertyNode *node );
 188     ~FGPISimpleController() {}
 189
 190     void update( double dt );
 191 };
 192
 193
 194 /**
 195  * Predictor - calculates value in x seconds future.
 196  */
 197
 198 class FGPredictor : public FGXMLAutoComponent {
 199
 200 private:
 201
 202     // proportional component data
 203     double last_value;
 204     double average;
 205     double seconds;
 206     double filter_gain;
 207
 208     // debug flag
 209     bool debug;
 210
 211     // Input values
 212     double ivalue;                 // input value
 213
 214 public:
 215
 216     FGPredictor( SGPropertyNode *node );
 217     ~FGPredictor() {}
 218
 219     void update( double dt );
 220 };
 221
 222
 223 /**
 224  * FGDigitalFilter - a selection of digital filters
 225  *
 226  * Exponential filter
 227  * Double exponential filter
 228  * Moving average filter
 229  * Noise spike filter
 230  *
 231  * All these filters are low-pass filters.
 232  *
 233  */
 234
 235 class FGDigitalFilter : public FGXMLAutoComponent
 236 {
 237 private:
 238     double Tf;            // Filter time [s]
 239     unsigned int samples; // Number of input samples to average
 240     double rateOfChange;  // The maximum allowable rate of change [1/s]
 241     deque <double> output;
 242     deque <double> input;
 243     enum filterTypes { exponential, doubleExponential, movingAverage, noiseSpike };
 244     filterTypes filterType;
 245
 246     bool debug;
 247
 248 public:
 249     FGDigitalFilter(SGPropertyNode *node);
 250     ~FGDigitalFilter() {}
 251
 252     void update(double dt);
 253 };
 254
 255 /**
 256  * Model an autopilot system.
 257  *
 258  */
 259
 260 class FGXMLAutopilot : public SGSubsystem
 261 {
 262
 263 public:
 264
 265     FGXMLAutopilot();
 266     ~FGXMLAutopilot();
 267
 268     void init();
 269     void reinit();
 270     void bind();
 271     void unbind();
 272     void update( double dt );
 273
 274     bool build();
 275
 276 protected:
 277
 278     typedef vector<FGXMLAutoComponent *> comp_list;
 279
 280 private:
 281
 282     bool serviceable;
 283     SGPropertyNode_ptr config_props;
 284     comp_list components;
 285 };
 286
 287
 288 #endif // _XMLAUTO_HXX