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 /*
  28 Torsten Dreyer:
  29 I'd like to deprecate the so called autopilot helper function
  30 (which is now part of the AutopilotGroup::update() method).
  31 Every property calculated within this helper can be calculated
  32 using filters defined in an external autopilot definition file.
  33 The complete set of calculations may be extracted into a separate
  34 configuration file. The current implementation is able to hande
  35 multiple config files and autopilots. The helper doubles code
  36 and writes properties used only by a few aircraft.
  37 */
  38 // FIXME: this should go into config.h and/or configure
  39 // or removed along with the "helper" one day.
  40 #define XMLAUTO_USEHELPER
  41
  42 #include <simgear/compiler.h>
  43
  44 #include <string>
  45 #include <vector>
  46 #include <deque>
  47
  48 #include <simgear/props/props.hxx>
  49 #include <simgear/structure/subsystem_mgr.hxx>
  50
  51 template<typename T>
  52 class SGExpression;
  53
  54 typedef SGExpression<double> SGExpressiond;
  55 class SGCondition;
  56
  57 typedef SGSharedPtr<class FGXMLAutoInput> FGXMLAutoInput_ptr;
  58 typedef SGSharedPtr<class FGPeriodicalValue> FGPeriodicalValue_ptr;
  59
  60 class FGPeriodicalValue : public SGReferenced {
  61 private:
  62      FGXMLAutoInput_ptr minPeriod; // The minimum value of the period
  63      FGXMLAutoInput_ptr maxPeriod; // The maximum value of the period
  64 public:
  65      FGPeriodicalValue( SGPropertyNode_ptr node );
  66      double normalize( double value );
  67 };
  68
  69 class FGXMLAutoInput : public SGReferenced {
  70 private:
  71      double             value;    // The value as a constant or initializer for the property
  72      bool               abs;      // return absolute value
  73      SGPropertyNode_ptr property; // The name of the property containing the value
  74      FGXMLAutoInput_ptr offset;   // A fixed offset, defaults to zero
  75      FGXMLAutoInput_ptr scale;    // A constant scaling factor defaults to one
  76      FGXMLAutoInput_ptr min;      // A minimum clip defaults to no clipping
  77      FGXMLAutoInput_ptr max;      // A maximum clip defaults to no clipping
  78      FGPeriodicalValue_ptr  periodical; //
  79      SGSharedPtr<const SGCondition> _condition;
  80      SGSharedPtr<SGExpressiond> _expression;  ///< expression to generate the value
  81
  82 public:
  83     FGXMLAutoInput( SGPropertyNode_ptr node = NULL, double value = 0.0, double offset = 0.0, double scale = 1.0 );
  84
  85     void parse( SGPropertyNode_ptr, double value = 0.0, double offset = 0.0, double scale = 1.0 );
  86
  87     /* get the value of this input, apply scale and offset and clipping */
  88     double get_value();
  89
  90     /* set the input value after applying offset and scale */
  91     void set_value( double value );
  92
  93     inline double get_scale() {
  94       return scale == NULL ? 1.0 : scale->get_value();
  95     }
  96
  97     inline double get_offset() {
  98       return offset == NULL ? 0.0 : offset->get_value();
  99     }
 100
 101     inline bool is_enabled() {
 102       return _condition == NULL ? true : _condition->test();
 103     }
 104
 105 };
 106
 107 class FGXMLAutoInputList : public std::vector<FGXMLAutoInput_ptr> {
 108   public:
 109     FGXMLAutoInput_ptr get_active() {
 110       for (iterator it = begin(); it != end(); ++it) {
 111         if( (*it)->is_enabled() )
 112           return *it;
 113       }
 114       return NULL;
 115     }
 116
 117     double get_value( double def = 0.0 ) {
 118       FGXMLAutoInput_ptr input = get_active();
 119       return input == NULL ? def : input->get_value();
 120     }
 121
 122 };
 123
 124 /**
 125  * Base class for other autopilot components
 126  */
 127
 128 class FGXMLAutoComponent : public SGReferenced {
 129
 130 private:
 131     simgear::PropertyList output_list;
 132
 133     SGSharedPtr<const SGCondition> _condition;
 134     SGPropertyNode_ptr enable_prop;
 135     std::string * enable_value;
 136
 137     SGPropertyNode_ptr passive_mode;
 138     bool honor_passive;
 139
 140     std::string name;
 141
 142     /* Feed back output property to input property if
 143        this filter is disabled. This is for multi-stage
 144        filter where one filter sits behind a pid-controller
 145        to provide changes of the overall output to the pid-
 146        controller.
 147        feedback is disabled by default.
 148      */
 149     bool feedback_if_disabled;
 150     void do_feedback_if_disabled();
 151
 152 protected:
 153     FGXMLAutoComponent();
 154
 155     /*
 156      * Parse a component specification read from a property-list.
 157      * Calls the hook methods below to allow derived classes to
 158      * specialise parsing bevaiour.
 159      */
 160     void parseNode(SGPropertyNode* aNode);
 161
 162     /**
 163      * Helper to parse the config section
 164      */
 165     void parseConfig(SGPropertyNode* aConfig);
 166
 167     /*
 168      * Over-rideable hook method to allow derived classes to refine top-level
 169      * node parsing. Return true if the node was handled, false otherwise.
 170      */
 171     virtual bool parseNodeHook(const std::string& aName, SGPropertyNode* aNode);
 172
 173     /**
 174      * Over-rideable hook method to allow derived classes to refine config
 175      * node parsing. Return true if the node was handled, false otherwise.
 176      */
 177     virtual bool parseConfigHook(const std::string& aName, SGPropertyNode* aNode);
 178
 179     FGXMLAutoInputList valueInput;
 180     FGXMLAutoInputList referenceInput;
 181     FGXMLAutoInputList uminInput;
 182     FGXMLAutoInputList umaxInput;
 183     FGPeriodicalValue_ptr periodical;
 184     // debug flag
 185     bool debug;
 186     bool enabled;
 187
 188
 189     inline void do_feedback() {
 190         if( feedback_if_disabled ) do_feedback_if_disabled();
 191     }
 192
 193 public:
 194
 195     virtual ~FGXMLAutoComponent();
 196
 197     virtual void update (double dt)=0;
 198
 199     inline const std::string& get_name() { return name; }
 200
 201     double clamp( double value );
 202
 203     inline void set_output_value( double value ) {
 204         // passive_ignore == true means that we go through all the
 205         // motions, but drive the outputs.  This is analogous to
 206         // running the autopilot with the "servos" off.  This is
 207         // helpful for things like flight directors which position
 208         // their vbars from the autopilot computations.
 209         if ( honor_passive && passive_mode->getBoolValue() ) return;
 210         for( simgear::PropertyList::iterator it = output_list.begin();
 211              it != output_list.end(); ++it)
 212           (*it)->setDoubleValue( clamp( value ) );
 213     }
 214
 215     inline void set_output_value( bool value ) {
 216         // passive_ignore == true means that we go through all the
 217         // motions, but drive the outputs.  This is analogous to
 218         // running the autopilot with the "servos" off.  This is
 219         // helpful for things like flight directors which position
 220         // their vbars from the autopilot computations.
 221         if ( honor_passive && passive_mode->getBoolValue() ) return;
 222         for( simgear::PropertyList::iterator it = output_list.begin();
 223              it != output_list.end(); ++it)
 224           (*it)->setBoolValue( value ); // don't use clamp here, bool is clamped anyway
 225     }
 226
 227     inline double get_output_value() {
 228       return output_list.size() == 0 ? 0.0 : clamp(output_list[0]->getDoubleValue());
 229     }
 230
 231     inline bool get_bool_output_value() {
 232       return output_list.size() == 0 ? false : output_list[0]->getBoolValue();
 233     }
 234
 235     /*
 236        Returns true if the enable-condition is true.
 237
 238        If a <condition> is defined, this condition is evaluated,
 239        <prop> and <value> tags are ignored.
 240
 241        If a <prop> is defined and no <value> is defined, the property
 242        named in the <prop></prop> tags is evaluated as boolean.
 243
 244        If a <prop> is defined a a <value> is defined, the property named
 245        in <prop></prop> is compared (as a string) to the value defined in
 246        <value></value>
 247
 248        Returns true, if neither <condition> nor <prop> exists
 249
 250        Examples:
 251        Using a <condition> tag
 252        <enable>
 253          <condition>
 254            <!-- any legal condition goes here and is evaluated -->
 255          </condition>
 256          <prop>This is ignored</prop>
 257          <value>This is also ignored</value>
 258        </enable>
 259
 260        Using a single boolean property
 261        <enable>
 262          <prop>/some/property/that/is/evaluated/as/boolean</prop>
 263        </enable>
 264
 265        Using <prop> == <value>
 266        This is the old style behaviour
 267        <enable>
 268          <prop>/only/true/if/this/equals/true</prop>
 269          <value>true<value>
 270        </enable>
 271     */
 272     bool isPropertyEnabled();
 273 };
 274
 275 typedef SGSharedPtr<FGXMLAutoComponent> FGXMLAutoComponent_ptr;
 276
 277
 278 /**
 279  * Roy Ovesen's PID controller
 280  */
 281
 282 class FGPIDController : public FGXMLAutoComponent {
 283
 284 private:
 285
 286
 287     // Configuration values
 288     FGXMLAutoInputList Kp;          // proportional gain
 289     FGXMLAutoInputList Ti;          // Integrator time (sec)
 290     FGXMLAutoInputList Td;          // Derivator time (sec)
 291
 292     double alpha;               // low pass filter weighing factor (usually 0.1)
 293     double beta;                // process value weighing factor for
 294                                 // calculating proportional error
 295                                 // (usually 1.0)
 296     double gamma;               // process value weighing factor for
 297                                 // calculating derivative error
 298                                 // (usually 0.0)
 299
 300     // Previous state tracking values
 301     double ep_n_1;              // ep[n-1]  (prop error)
 302     double edf_n_1;             // edf[n-1] (derivative error)
 303     double edf_n_2;             // edf[n-2] (derivative error)
 304     double u_n_1;               // u[n-1]   (output)
 305     double desiredTs;            // desired sampling interval (sec)
 306     double elapsedTime;          // elapsed time (sec)
 307
 308
 309 protected:
 310   bool parseConfigHook(const std::string& aName, SGPropertyNode* aNode);
 311
 312 public:
 313
 314     FGPIDController( SGPropertyNode *node );
 315     FGPIDController( SGPropertyNode *node, bool old );
 316     ~FGPIDController() {}
 317
 318     void update( double dt );
 319 };
 320
 321
 322 /**
 323  * A simplistic P [ + I ] PID controller
 324  */
 325
 326 class FGPISimpleController : public FGXMLAutoComponent {
 327
 328 private:
 329
 330     // proportional component data
 331     FGXMLAutoInputList Kp;
 332
 333     // integral component data
 334     FGXMLAutoInputList Ki;
 335     double int_sum;
 336
 337 protected:
 338   bool parseConfigHook(const std::string& aName, SGPropertyNode* aNode);
 339
 340 public:
 341
 342     FGPISimpleController( SGPropertyNode *node );
 343     ~FGPISimpleController() {}
 344
 345     void update( double dt );
 346 };
 347
 348
 349 /**
 350  * Predictor - calculates value in x seconds future.
 351  */
 352
 353 class FGPredictor : public FGXMLAutoComponent {
 354
 355 private:
 356     double last_value;
 357     double average;
 358     FGXMLAutoInputList seconds;
 359     FGXMLAutoInputList filter_gain;
 360
 361 protected:
 362   bool parseNodeHook(const std::string& aName, SGPropertyNode* aNode);
 363
 364 public:
 365     FGPredictor( SGPropertyNode *node );
 366     ~FGPredictor() {}
 367
 368     void update( double dt );
 369 };
 370
 371
 372 /**
 373  * FGDigitalFilter - a selection of digital filters
 374  *
 375  * Exponential filter
 376  * Double exponential filter
 377  * Moving average filter
 378  * Noise spike filter
 379  *
 380  * All these filters are low-pass filters.
 381  *
 382  */
 383
 384 class FGDigitalFilter : public FGXMLAutoComponent
 385 {
 386 private:
 387     FGXMLAutoInputList samplesInput; // Number of input samples to average
 388     FGXMLAutoInputList rateOfChangeInput;  // The maximum allowable rate of change [1/s]
 389     FGXMLAutoInputList gainInput;     //
 390     FGXMLAutoInputList TfInput;            // Filter time [s]
 391
 392     std::deque <double> output;
 393     std::deque <double> input;
 394     enum FilterTypes { exponential, doubleExponential, movingAverage,
 395                        noiseSpike, gain, reciprocal, differential, none };
 396     FilterTypes filterType;
 397
 398 protected:
 399   bool parseNodeHook(const std::string& aName, SGPropertyNode* aNode);
 400
 401 public:
 402     FGDigitalFilter(SGPropertyNode *node);
 403     ~FGDigitalFilter() {}
 404
 405     void update(double dt);
 406 };
 407
 408 class FGXMLAutoLogic : public FGXMLAutoComponent
 409 {
 410 private:
 411     SGSharedPtr<SGCondition> input;
 412     bool inverted;
 413
 414 protected:
 415     bool parseNodeHook(const std::string& aName, SGPropertyNode* aNode);
 416
 417 public:
 418     FGXMLAutoLogic(SGPropertyNode * node );
 419     ~FGXMLAutoLogic() {}
 420
 421     void update(double dt);
 422 };
 423
 424 class FGXMLAutoFlipFlop : public FGXMLAutoComponent
 425 {
 426 private:
 427 protected:
 428     SGSharedPtr<SGCondition> sInput;
 429     SGSharedPtr<SGCondition> rInput;
 430     SGSharedPtr<SGCondition> clockInput;
 431     SGSharedPtr<SGCondition> jInput;
 432     SGSharedPtr<SGCondition> kInput;
 433     SGSharedPtr<SGCondition> dInput;
 434     bool inverted;
 435     FGXMLAutoFlipFlop( SGPropertyNode * node );
 436     bool parseNodeHook(const std::string& aName, SGPropertyNode* aNode);
 437
 438     void update( double dt );
 439
 440 public:
 441     ~FGXMLAutoFlipFlop() {};
 442     virtual bool getState( bool & result ) = 0;
 443 };
 444
 445 /**
 446  * Model an autopilot system.
 447  *
 448  */
 449
 450 class FGXMLAutopilotGroup : public SGSubsystemGroup
 451 {
 452 public:
 453     FGXMLAutopilotGroup();
 454     void init();
 455     void reinit();
 456     void update( double dt );
 457 private:
 458     std::vector<std::string> _autopilotNames;
 459
 460 #ifdef XMLAUTO_USEHELPER
 461     double average;
 462     double v_last;
 463     double last_static_pressure;
 464
 465     SGPropertyNode_ptr vel;
 466     SGPropertyNode_ptr lookahead5;
 467     SGPropertyNode_ptr lookahead10;
 468     SGPropertyNode_ptr bug;
 469     SGPropertyNode_ptr mag_hdg;
 470     SGPropertyNode_ptr bug_error;
 471     SGPropertyNode_ptr fdm_bug_error;
 472     SGPropertyNode_ptr target_true;
 473     SGPropertyNode_ptr true_hdg;
 474     SGPropertyNode_ptr true_error;
 475     SGPropertyNode_ptr target_nav1;
 476     SGPropertyNode_ptr true_nav1;
 477     SGPropertyNode_ptr true_track_nav1;
 478     SGPropertyNode_ptr nav1_course_error;
 479     SGPropertyNode_ptr nav1_selected_course;
 480     SGPropertyNode_ptr vs_fps;
 481     SGPropertyNode_ptr vs_fpm;
 482     SGPropertyNode_ptr static_pressure;
 483     SGPropertyNode_ptr pressure_rate;
 484     SGPropertyNode_ptr track;
 485 #endif
 486 };
 487
 488 class FGXMLAutopilot : public SGSubsystem
 489 {
 490
 491 public:
 492
 493     FGXMLAutopilot();
 494     ~FGXMLAutopilot();
 495
 496     void init();
 497     void reinit();
 498     void bind();
 499     void unbind();
 500     void update( double dt );
 501
 502
 503     bool build( SGPropertyNode_ptr );
 504 protected:
 505     typedef std::vector<FGXMLAutoComponent_ptr> comp_list;
 506
 507 private:
 508     bool serviceable;
 509     comp_list components;
 510
 511 };
 512
 513
 514 #endif // _XMLAUTO_HXX