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