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