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 class FGXMLAutoInput {
  53 private:
  54      SGPropertyNode_ptr property; // The name of the property containing the value
  55      double             value;    // The value as a constant or initializer for the property
  56      double             offset;   // A fixed offset
  57      double             scale;    // A constant scaling factor
  58
  59 public:
  60     FGXMLAutoInput() :
  61       property(NULL),
  62       value(0.0),
  63       offset(0.0),
  64       scale(1.0) {}
  65
  66     void   parse( SGPropertyNode_ptr, double value = 0.0, double offset = 0.0, double scale = 1.0 );
  67     inline double getValue() {
  68       if( property != NULL ) value = property->getDoubleValue();
  69       return value * scale + offset;
  70     }
  71 };
  72
  73 /**
  74  * Base class for other autopilot components
  75  */
  76
  77 class FGXMLAutoComponent : public SGReferenced {
  78
  79 private:
  80     bool clamp;
  81     vector <SGPropertyNode_ptr> output_list;
  82
  83     SGSharedPtr<const SGCondition> _condition;
  84     SGPropertyNode_ptr enable_prop;
  85     string * enable_value;
  86
  87     SGPropertyNode_ptr passive_mode;
  88     bool honor_passive;
  89
  90     string name;
  91 protected:
  92
  93     FGXMLAutoInput valueInput;
  94     FGXMLAutoInput referenceInput;
  95     FGXMLAutoInput uminInput;
  96     FGXMLAutoInput umaxInput;
  97     // debug flag
  98     bool debug;
  99     bool enabled;
 100
 101 public:
 102
 103     FGXMLAutoComponent( SGPropertyNode *node);
 104     virtual ~FGXMLAutoComponent();
 105
 106     virtual void update (double dt)=0;
 107
 108     inline const string& get_name() { return name; }
 109
 110     inline double Clamp( double value ) {
 111         if( clamp ) {
 112             double d = umaxInput.getValue();
 113             if( value > d ) value = d;
 114             d = uminInput.getValue();
 115             if( value < d ) value = d;
 116         }
 117         return value;
 118     }
 119
 120     inline void setOutputValue( double value ) {
 121         // passive_ignore == true means that we go through all the
 122         // motions, but drive the outputs.  This is analogous to
 123         // running the autopilot with the "servos" off.  This is
 124         // helpful for things like flight directors which position
 125         // their vbars from the autopilot computations.
 126         if ( honor_passive && passive_mode->getBoolValue() ) return;
 127         for ( unsigned i = 0; i < output_list.size(); ++i ) {
 128             output_list[i]->setDoubleValue( Clamp(value) );
 129         }
 130     }
 131
 132     inline double getOutputValue() {
 133       return output_list.size() == 0 ? 0.0 : Clamp(output_list[0]->getDoubleValue());
 134     }
 135
 136     /*
 137        Returns true if the enable-condition is true.
 138
 139        If a <condition> is defined, this condition is evaluated,
 140        <prop> and <value> tags are ignored.
 141
 142        If a <prop> is defined and no <value> is defined, the property
 143        named in the <prop></prop> tags is evaluated as boolean.
 144
 145        If a <prop> is defined a a <value> is defined, the property named
 146        in <prop></prop> is compared (as a string) to the value defined in
 147        <value></value>
 148
 149        Returns true, if neither <condition> nor <prop> exists
 150
 151        Examples:
 152        Using a <condition> tag
 153        <enable>
 154          <condition>
 155            <!-- any legal condition goes here and is evaluated -->
 156          </condition>
 157          <prop>This is ignored</prop>
 158          <value>This is also ignored</value>
 159        </enable>
 160
 161        Using a single boolean property
 162        <enable>
 163          <prop>/some/property/that/is/evaluated/as/boolean</prop>
 164        </enable>
 165
 166        Using <prop> == <value>
 167        This is the old style behaviour
 168        <enable>
 169          <prop>/only/true/if/this/equals/true</prop>
 170          <value>true<value>
 171        </enable>
 172     */
 173     bool isPropertyEnabled();
 174 };
 175
 176
 177 /**
 178  * Roy Ovesen's PID controller
 179  */
 180
 181 class FGPIDController : public FGXMLAutoComponent {
 182
 183 private:
 184
 185
 186     // Configuration values
 187     FGXMLAutoInput Kp;          // proportional gain
 188     FGXMLAutoInput Ti;          // Integrator time (sec)
 189     FGXMLAutoInput Td;          // Derivator time (sec)
 190
 191     double alpha;               // low pass filter weighing factor (usually 0.1)
 192     double beta;                // process value weighing factor for
 193                                 // calculating proportional error
 194                                 // (usually 1.0)
 195     double gamma;               // process value weighing factor for
 196                                 // calculating derivative error
 197                                 // (usually 0.0)
 198
 199     // Previous state tracking values
 200     double ep_n_1;              // ep[n-1]  (prop error)
 201     double edf_n_1;             // edf[n-1] (derivative error)
 202     double edf_n_2;             // edf[n-2] (derivative error)
 203     double u_n_1;               // u[n-1]   (output)
 204     double desiredTs;            // desired sampling interval (sec)
 205     double elapsedTime;          // elapsed time (sec)
 206
 207
 208
 209 public:
 210
 211     FGPIDController( SGPropertyNode *node );
 212     FGPIDController( SGPropertyNode *node, bool old );
 213     ~FGPIDController() {}
 214
 215     void update_old( double dt );
 216     void update( double dt );
 217 };
 218
 219
 220 /**
 221  * A simplistic P [ + I ] PID controller
 222  */
 223
 224 class FGPISimpleController : public FGXMLAutoComponent {
 225
 226 private:
 227
 228     // proportional component data
 229     FGXMLAutoInput Kp;
 230
 231     // integral component data
 232     FGXMLAutoInput Ki;
 233     double int_sum;
 234
 235
 236 public:
 237
 238     FGPISimpleController( SGPropertyNode *node );
 239     ~FGPISimpleController() {}
 240
 241     void update( double dt );
 242 };
 243
 244
 245 /**
 246  * Predictor - calculates value in x seconds future.
 247  */
 248
 249 class FGPredictor : public FGXMLAutoComponent {
 250
 251 private:
 252
 253     // proportional component data
 254     double last_value;
 255     double average;
 256     double seconds;
 257     double filter_gain;
 258
 259     // Input values
 260     double ivalue;                 // input value
 261
 262 public:
 263
 264     FGPredictor( SGPropertyNode *node );
 265     ~FGPredictor() {}
 266
 267     void update( double dt );
 268 };
 269
 270
 271 /**
 272  * FGDigitalFilter - a selection of digital filters
 273  *
 274  * Exponential filter
 275  * Double exponential filter
 276  * Moving average filter
 277  * Noise spike filter
 278  *
 279  * All these filters are low-pass filters.
 280  *
 281  */
 282
 283 class FGDigitalFilter : public FGXMLAutoComponent
 284 {
 285 private:
 286     FGXMLAutoInput samplesInput; // Number of input samples to average
 287     FGXMLAutoInput rateOfChangeInput;  // The maximum allowable rate of change [1/s]
 288     FGXMLAutoInput gainInput;     //
 289     FGXMLAutoInput TfInput;            // Filter time [s]
 290
 291     deque <double> output;
 292     deque <double> input;
 293     enum filterTypes { exponential, doubleExponential, movingAverage,
 294                        noiseSpike, gain, reciprocal };
 295     filterTypes filterType;
 296
 297 public:
 298     FGDigitalFilter(SGPropertyNode *node);
 299     ~FGDigitalFilter() {}
 300
 301     void update(double dt);
 302 };
 303
 304 /**
 305  * Model an autopilot system.
 306  *
 307  */
 308
 309 class FGXMLAutopilot : public SGSubsystem
 310 {
 311
 312 public:
 313
 314     FGXMLAutopilot();
 315     ~FGXMLAutopilot();
 316
 317     void init();
 318     void reinit();
 319     void bind();
 320     void unbind();
 321     void update( double dt );
 322
 323     bool build();
 324
 325 protected:
 326
 327     typedef vector<SGSharedPtr<FGXMLAutoComponent> > comp_list;
 328
 329 private:
 330
 331     bool serviceable;
 332     SGPropertyNode_ptr config_props;
 333     comp_list components;
 334 };
 335
 336
 337 #endif // _XMLAUTO_HXX