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