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