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