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 #include <simgear/compiler.h>
  28
  29 #include <string>
  30 #include <vector>
  31 #include <deque>
  32
  33 #include <simgear/props/props.hxx>
  34 #include <simgear/structure/subsystem_mgr.hxx>
  35 #include <simgear/props/condition.hxx>
  36
  37
  38 class FGXMLAutoInput : public SGReferenced {
  39 private:
  40      double             value;    // The value as a constant or initializer for the property
  41      bool               abs;      // return absolute value
  42      SGPropertyNode_ptr property; // The name of the property containing the value
  43      SGSharedPtr<FGXMLAutoInput> offset;   // A fixed offset, defaults to zero
  44      SGSharedPtr<FGXMLAutoInput> scale;    // A constant scaling factor defaults to one
  45      SGSharedPtr<FGXMLAutoInput> min;      // A minimum clip defaults to no clipping
  46      SGSharedPtr<FGXMLAutoInput> max;      // A maximum clip defaults to no clipping
  47      SGSharedPtr<const SGCondition> _condition;
  48
  49 public:
  50     FGXMLAutoInput( SGPropertyNode_ptr node = NULL, double value = 0.0, double offset = 0.0, double scale = 1.0 );
  51
  52     void parse( SGPropertyNode_ptr, double value = 0.0, double offset = 0.0, double scale = 1.0 );
  53
  54     /* get the value of this input, apply scale and offset and clipping */
  55     double get_value();
  56
  57     /* set the input value after applying offset and scale */
  58     void set_value( double value );
  59
  60     inline double get_scale() {
  61       return scale == NULL ? 1.0 : scale->get_value();
  62     }
  63
  64     inline double get_offset() {
  65       return offset == NULL ? 0.0 : offset->get_value();
  66     }
  67
  68     inline bool is_enabled() {
  69       return _condition == NULL ? true : _condition->test();
  70     }
  71
  72 };
  73
  74 class FGXMLAutoInputList : public std::vector<SGSharedPtr<FGXMLAutoInput> > {
  75   public:
  76     FGXMLAutoInput * get_active() {
  77       for (iterator it = begin(); it != end(); ++it) {
  78         if( (*it)->is_enabled() )
  79           return *it;
  80       }
  81       return NULL;
  82     }
  83
  84     double get_value( double def = 0.0 ) {
  85       FGXMLAutoInput * input = get_active();
  86       return input == NULL ? def : input->get_value();
  87     }
  88
  89 };
  90
  91 /**
  92  * Base class for other autopilot components
  93  */
  94
  95 class FGXMLAutoComponent : public SGReferenced {
  96
  97 private:
  98     std::vector <SGPropertyNode_ptr> output_list;
  99
 100     SGSharedPtr<const SGCondition> _condition;
 101     SGPropertyNode_ptr enable_prop;
 102     std::string * enable_value;
 103
 104     SGPropertyNode_ptr passive_mode;
 105     bool honor_passive;
 106
 107     std::string name;
 108
 109     /* Feed back output property to input property if
 110        this filter is disabled. This is for multi-stage
 111        filter where one filter sits behind a pid-controller
 112        to provide changes of the overall output to the pid-
 113        controller.
 114        feedback is disabled by default.
 115      */
 116     bool feedback_if_disabled;
 117     void do_feedback_if_disabled();
 118
 119 protected:
 120     FGXMLAutoComponent();
 121
 122     /*
 123      * Parse a component specification read from a property-list.
 124      * Calls the hook methods below to allow derived classes to
 125      * specialise parsing bevaiour.
 126      */
 127     void parseNode(SGPropertyNode* aNode);
 128
 129     /**
 130      * Helper to parse the config section
 131      */
 132     void parseConfig(SGPropertyNode* aConfig);
 133
 134     /*
 135      * Over-rideable hook method to allow derived classes to refine top-level
 136      * node parsing. Return true if the node was handled, false otherwise.
 137      */
 138     virtual bool parseNodeHook(const std::string& aName, SGPropertyNode* aNode);
 139
 140     /**
 141      * Over-rideable hook method to allow derived classes to refine config
 142      * node parsing. Return true if the node was handled, false otherwise.
 143      */
 144     virtual bool parseConfigHook(const std::string& aName, SGPropertyNode* aNode);
 145
 146     FGXMLAutoInputList valueInput;
 147     FGXMLAutoInputList referenceInput;
 148     FGXMLAutoInputList uminInput;
 149     FGXMLAutoInputList umaxInput;
 150     // debug flag
 151     bool debug;
 152     bool enabled;
 153
 154
 155     inline void do_feedback() {
 156         if( feedback_if_disabled ) do_feedback_if_disabled();
 157     }
 158
 159 public:
 160
 161     virtual ~FGXMLAutoComponent();
 162
 163     virtual void update (double dt)=0;
 164
 165     inline const std::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( std::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 protected:
 257   bool parseConfigHook(const std::string& aName, SGPropertyNode* aNode);
 258
 259 public:
 260
 261     FGPIDController( SGPropertyNode *node );
 262     FGPIDController( SGPropertyNode *node, bool old );
 263     ~FGPIDController() {}
 264
 265     void update( double dt );
 266 };
 267
 268
 269 /**
 270  * A simplistic P [ + I ] PID controller
 271  */
 272
 273 class FGPISimpleController : public FGXMLAutoComponent {
 274
 275 private:
 276
 277     // proportional component data
 278     FGXMLAutoInputList Kp;
 279
 280     // integral component data
 281     FGXMLAutoInputList Ki;
 282     double int_sum;
 283
 284 protected:
 285   bool parseConfigHook(const std::string& aName, SGPropertyNode* aNode);
 286
 287 public:
 288
 289     FGPISimpleController( SGPropertyNode *node );
 290     ~FGPISimpleController() {}
 291
 292     void update( double dt );
 293 };
 294
 295
 296 /**
 297  * Predictor - calculates value in x seconds future.
 298  */
 299
 300 class FGPredictor : public FGXMLAutoComponent {
 301
 302 private:
 303     double last_value;
 304     double average;
 305     FGXMLAutoInputList seconds;
 306     FGXMLAutoInputList filter_gain;
 307
 308 protected:
 309   bool parseNodeHook(const std::string& aName, SGPropertyNode* aNode);
 310
 311 public:
 312     FGPredictor( SGPropertyNode *node );
 313     ~FGPredictor() {}
 314
 315     void update( double dt );
 316 };
 317
 318
 319 /**
 320  * FGDigitalFilter - a selection of digital filters
 321  *
 322  * Exponential filter
 323  * Double exponential filter
 324  * Moving average filter
 325  * Noise spike filter
 326  *
 327  * All these filters are low-pass filters.
 328  *
 329  */
 330
 331 class FGDigitalFilter : public FGXMLAutoComponent
 332 {
 333 private:
 334     FGXMLAutoInputList samplesInput; // Number of input samples to average
 335     FGXMLAutoInputList rateOfChangeInput;  // The maximum allowable rate of change [1/s]
 336     FGXMLAutoInputList gainInput;     //
 337     FGXMLAutoInputList TfInput;            // Filter time [s]
 338
 339     std::deque <double> output;
 340     std::deque <double> input;
 341     enum filterTypes { exponential, doubleExponential, movingAverage,
 342                        noiseSpike, gain, reciprocal, none };
 343     filterTypes filterType;
 344
 345 protected:
 346   bool parseNodeHook(const std::string& aName, SGPropertyNode* aNode);
 347
 348 public:
 349     FGDigitalFilter(SGPropertyNode *node);
 350     ~FGDigitalFilter() {}
 351
 352     void update(double dt);
 353 };
 354
 355 /**
 356  * Model an autopilot system.
 357  *
 358  */
 359
 360 class FGXMLAutopilot : public SGSubsystem
 361 {
 362
 363 public:
 364
 365     FGXMLAutopilot();
 366     ~FGXMLAutopilot();
 367
 368     void init();
 369     void reinit();
 370     void bind();
 371     void unbind();
 372     void update( double dt );
 373
 374     bool build();
 375
 376 protected:
 377
 378     typedef std::vector<SGSharedPtr<FGXMLAutoComponent> > comp_list;
 379
 380 private:
 381
 382     bool serviceable;
 383     SGPropertyNode_ptr config_props;
 384     comp_list components;
 385 };
 386
 387
 388 #endif // _XMLAUTO_HXX