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