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     simgear::PropertyList 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( simgear::PropertyList::iterator it = output_list.begin();
 205              it != output_list.end(); ++it)
 206           (*it)->setDoubleValue( clamp( value ) );
 207     }
 208
 209     inline void set_output_value( bool value ) {
 210         // passive_ignore == true means that we go through all the
 211         // motions, but drive the outputs.  This is analogous to
 212         // running the autopilot with the "servos" off.  This is
 213         // helpful for things like flight directors which position
 214         // their vbars from the autopilot computations.
 215         if ( honor_passive && passive_mode->getBoolValue() ) return;
 216         for( simgear::PropertyList::iterator it = output_list.begin();
 217              it != output_list.end(); ++it)
 218           (*it)->setBoolValue( value ); // don't use clamp here, bool is clamped anyway
 219     }
 220
 221     inline double get_output_value() {
 222       return output_list.size() == 0 ? 0.0 : clamp(output_list[0]->getDoubleValue());
 223     }
 224
 225     inline bool get_bool_output_value() {
 226       return output_list.size() == 0 ? false : output_list[0]->getBoolValue();
 227     }
 228
 229     /*
 230        Returns true if the enable-condition is true.
 231
 232        If a <condition> is defined, this condition is evaluated,
 233        <prop> and <value> tags are ignored.
 234
 235        If a <prop> is defined and no <value> is defined, the property
 236        named in the <prop></prop> tags is evaluated as boolean.
 237
 238        If a <prop> is defined a a <value> is defined, the property named
 239        in <prop></prop> is compared (as a string) to the value defined in
 240        <value></value>
 241
 242        Returns true, if neither <condition> nor <prop> exists
 243
 244        Examples:
 245        Using a <condition> tag
 246        <enable>
 247          <condition>
 248            <!-- any legal condition goes here and is evaluated -->
 249          </condition>
 250          <prop>This is ignored</prop>
 251          <value>This is also ignored</value>
 252        </enable>
 253
 254        Using a single boolean property
 255        <enable>
 256          <prop>/some/property/that/is/evaluated/as/boolean</prop>
 257        </enable>
 258
 259        Using <prop> == <value>
 260        This is the old style behaviour
 261        <enable>
 262          <prop>/only/true/if/this/equals/true</prop>
 263          <value>true<value>
 264        </enable>
 265     */
 266     bool isPropertyEnabled();
 267 };
 268
 269 typedef SGSharedPtr<FGXMLAutoComponent> FGXMLAutoComponent_ptr;
 270
 271
 272 /**
 273  * Roy Ovesen's PID controller
 274  */
 275
 276 class FGPIDController : public FGXMLAutoComponent {
 277
 278 private:
 279
 280
 281     // Configuration values
 282     FGXMLAutoInputList Kp;          // proportional gain
 283     FGXMLAutoInputList Ti;          // Integrator time (sec)
 284     FGXMLAutoInputList Td;          // Derivator time (sec)
 285
 286     double alpha;               // low pass filter weighing factor (usually 0.1)
 287     double beta;                // process value weighing factor for
 288                                 // calculating proportional error
 289                                 // (usually 1.0)
 290     double gamma;               // process value weighing factor for
 291                                 // calculating derivative error
 292                                 // (usually 0.0)
 293
 294     // Previous state tracking values
 295     double ep_n_1;              // ep[n-1]  (prop error)
 296     double edf_n_1;             // edf[n-1] (derivative error)
 297     double edf_n_2;             // edf[n-2] (derivative error)
 298     double u_n_1;               // u[n-1]   (output)
 299     double desiredTs;            // desired sampling interval (sec)
 300     double elapsedTime;          // elapsed time (sec)
 301
 302
 303 protected:
 304   bool parseConfigHook(const std::string& aName, SGPropertyNode* aNode);
 305
 306 public:
 307
 308     FGPIDController( SGPropertyNode *node );
 309     FGPIDController( SGPropertyNode *node, bool old );
 310     ~FGPIDController() {}
 311
 312     void update( double dt );
 313 };
 314
 315
 316 /**
 317  * A simplistic P [ + I ] PID controller
 318  */
 319
 320 class FGPISimpleController : public FGXMLAutoComponent {
 321
 322 private:
 323
 324     // proportional component data
 325     FGXMLAutoInputList Kp;
 326
 327     // integral component data
 328     FGXMLAutoInputList Ki;
 329     double int_sum;
 330
 331 protected:
 332   bool parseConfigHook(const std::string& aName, SGPropertyNode* aNode);
 333
 334 public:
 335
 336     FGPISimpleController( SGPropertyNode *node );
 337     ~FGPISimpleController() {}
 338
 339     void update( double dt );
 340 };
 341
 342
 343 /**
 344  * Predictor - calculates value in x seconds future.
 345  */
 346
 347 class FGPredictor : public FGXMLAutoComponent {
 348
 349 private:
 350     double last_value;
 351     double average;
 352     FGXMLAutoInputList seconds;
 353     FGXMLAutoInputList filter_gain;
 354
 355 protected:
 356   bool parseNodeHook(const std::string& aName, SGPropertyNode* aNode);
 357
 358 public:
 359     FGPredictor( SGPropertyNode *node );
 360     ~FGPredictor() {}
 361
 362     void update( double dt );
 363 };
 364
 365
 366 /**
 367  * FGDigitalFilter - a selection of digital filters
 368  *
 369  * Exponential filter
 370  * Double exponential filter
 371  * Moving average filter
 372  * Noise spike filter
 373  *
 374  * All these filters are low-pass filters.
 375  *
 376  */
 377
 378 class FGDigitalFilter : public FGXMLAutoComponent
 379 {
 380 private:
 381     FGXMLAutoInputList samplesInput; // Number of input samples to average
 382     FGXMLAutoInputList rateOfChangeInput;  // The maximum allowable rate of change [1/s]
 383     FGXMLAutoInputList gainInput;     //
 384     FGXMLAutoInputList TfInput;            // Filter time [s]
 385
 386     std::deque <double> output;
 387     std::deque <double> input;
 388     enum FilterTypes { exponential, doubleExponential, movingAverage,
 389                        noiseSpike, gain, reciprocal, differential, none };
 390     FilterTypes filterType;
 391
 392 protected:
 393   bool parseNodeHook(const std::string& aName, SGPropertyNode* aNode);
 394
 395 public:
 396     FGDigitalFilter(SGPropertyNode *node);
 397     ~FGDigitalFilter() {}
 398
 399     void update(double dt);
 400 };
 401
 402 class FGXMLAutoLogic : public FGXMLAutoComponent
 403 {
 404 private:
 405     SGSharedPtr<SGCondition> input;
 406     bool inverted;
 407
 408 protected:
 409     bool parseNodeHook(const std::string& aName, SGPropertyNode* aNode);
 410
 411 public:
 412     FGXMLAutoLogic(SGPropertyNode * node );
 413     ~FGXMLAutoLogic() {}
 414
 415     void update(double dt);
 416 };
 417
 418 class FGXMLAutoFlipFlop : public FGXMLAutoComponent
 419 {
 420 private:
 421 protected:
 422     SGSharedPtr<SGCondition> sInput;
 423     SGSharedPtr<SGCondition> rInput;
 424     SGSharedPtr<SGCondition> clockInput;
 425     SGSharedPtr<SGCondition> jInput;
 426     SGSharedPtr<SGCondition> kInput;
 427     SGSharedPtr<SGCondition> tInput;
 428     SGSharedPtr<SGCondition> dInput;
 429     bool inverted;
 430     FGXMLAutoFlipFlop( SGPropertyNode * node );
 431     bool parseNodeHook(const std::string& aName, SGPropertyNode* aNode);
 432
 433     void update( double dt );
 434     virtual void updateState( double dt ) = 0;
 435
 436 public:
 437     ~FGXMLAutoFlipFlop() {};
 438 };
 439
 440 /**
 441  * Model an autopilot system.
 442  *
 443  */
 444
 445 class FGXMLAutopilotGroup : public SGSubsystemGroup
 446 {
 447 public:
 448     FGXMLAutopilotGroup();
 449     void init();
 450     void reinit();
 451     void update( double dt );
 452 private:
 453     std::vector<std::string> _autopilotNames;
 454
 455 #ifdef XMLAUTO_USEHELPER
 456     double average;
 457     double v_last;
 458     double last_static_pressure;
 459
 460     SGPropertyNode_ptr vel;
 461     SGPropertyNode_ptr lookahead5;
 462     SGPropertyNode_ptr lookahead10;
 463     SGPropertyNode_ptr bug;
 464     SGPropertyNode_ptr mag_hdg;
 465     SGPropertyNode_ptr bug_error;
 466     SGPropertyNode_ptr fdm_bug_error;
 467     SGPropertyNode_ptr target_true;
 468     SGPropertyNode_ptr true_hdg;
 469     SGPropertyNode_ptr true_error;
 470     SGPropertyNode_ptr target_nav1;
 471     SGPropertyNode_ptr true_nav1;
 472     SGPropertyNode_ptr true_track_nav1;
 473     SGPropertyNode_ptr nav1_course_error;
 474     SGPropertyNode_ptr nav1_selected_course;
 475     SGPropertyNode_ptr vs_fps;
 476     SGPropertyNode_ptr vs_fpm;
 477     SGPropertyNode_ptr static_pressure;
 478     SGPropertyNode_ptr pressure_rate;
 479     SGPropertyNode_ptr track;
 480 #endif
 481 };
 482
 483 class FGXMLAutopilot : public SGSubsystem
 484 {
 485
 486 public:
 487
 488     FGXMLAutopilot();
 489     ~FGXMLAutopilot();
 490
 491     void init();
 492     void reinit();
 493     void bind();
 494     void unbind();
 495     void update( double dt );
 496
 497
 498     bool build( SGPropertyNode_ptr );
 499 protected:
 500     typedef std::vector<FGXMLAutoComponent_ptr> comp_list;
 501
 502 private:
 503     bool serviceable;
 504     comp_list components;
 505
 506 };
 507
 508
 509 #endif // _XMLAUTO_HXX