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