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