src/Autopilot/xmlauto.cxx

   1 // xmlauto.cxx - 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 #ifdef HAVE_CONFIG_H
  24 #  include <config.h>
  25 #endif
  26
  27 #include <iostream>
  28
  29 #include <simgear/structure/exception.hxx>
  30 #include <simgear/misc/sg_path.hxx>
  31 #include <simgear/sg_inlines.h>
  32 #include <simgear/props/props_io.hxx>
  33
  34 #include <Main/fg_props.hxx>
  35 #include <Main/globals.hxx>
  36 #include <Main/util.hxx>
  37
  38 #include "xmlauto.hxx"
  39
  40 using std::cout;
  41 using std::endl;
  42
  43 void FGXMLAutoInput::parse( SGPropertyNode_ptr node, double aValue, double aOffset, double aScale )
  44 {
  45     value = aValue;
  46     property = NULL;
  47     offset = NULL;
  48     scale = NULL;
  49     min = NULL;
  50     max = NULL;
  51
  52     if( node == NULL )
  53         return;
  54
  55     SGPropertyNode * n;
  56
  57     if( (n = node->getChild("condition")) != NULL ) {
  58         _condition = sgReadCondition(node, n);
  59     }
  60
  61     if( (n = node->getChild( "scale" )) != NULL ) {
  62         scale = new FGXMLAutoInput( n, aScale );
  63     }
  64
  65     if( (n = node->getChild( "offset" )) != NULL ) {
  66         offset = new FGXMLAutoInput( n, aOffset );
  67     }
  68
  69     if( (n = node->getChild( "max" )) != NULL ) {
  70         max = new FGXMLAutoInput( n );
  71     }
  72
  73     if( (n = node->getChild( "min" )) != NULL ) {
  74         min = new FGXMLAutoInput( n );
  75     }
  76
  77     if( (n = node->getChild( "abs" )) != NULL ) {
  78       abs = n->getBoolValue();
  79     }
  80
  81     SGPropertyNode *valueNode = node->getChild( "value" );
  82     if ( valueNode != NULL ) {
  83         value = valueNode->getDoubleValue();
  84     }
  85
  86     n = node->getChild( "property" );
  87     // if no <property> element, check for <prop> element for backwards
  88     // compatibility
  89     if(  n == NULL )
  90         n = node->getChild( "prop" );
  91
  92     if (  n != NULL ) {
  93         property = fgGetNode(  n->getStringValue(), true );
  94         if ( valueNode != NULL ) {
  95             // initialize property with given value
  96             // if both <prop> and <value> exist
  97             double s = get_scale();
  98             if( s != 0 )
  99               property->setDoubleValue( (value - get_offset())/s );
 100             else
 101               property->setDoubleValue( 0 ); // if scale is zero, value*scale is zero
 102         }
 103     }
 104
 105     if ( n == NULL && valueNode == NULL ) {
 106         // no <value> element and no <prop> element, use text node
 107         const char * textnode = node->getStringValue();
 108         char * endp = NULL;
 109         // try to convert to a double value. If the textnode does not start with a number
 110         // endp will point to the beginning of the string. We assume this should be
 111         // a property name
 112         value = strtod( textnode, &endp );
 113         if( endp == textnode ) {
 114           property = fgGetNode( textnode, true );
 115         }
 116     }
 117 }
 118
 119 void FGXMLAutoInput::set_value( double aValue )
 120 {
 121     double s = get_scale();
 122     if( s != 0 )
 123         property->setDoubleValue( (aValue - get_offset())/s );
 124     else
 125         property->setDoubleValue( 0 ); // if scale is zero, value*scale is zero
 126 }
 127
 128 double FGXMLAutoInput::get_value()
 129 {
 130     if( property != NULL )
 131         value = property->getDoubleValue();
 132
 133     if( scale )
 134         value *= scale->get_value();
 135
 136     if( offset )
 137         value += offset->get_value();
 138
 139     if( min ) {
 140         double m = min->get_value();
 141         if( value < m )
 142             value = m;
 143     }
 144
 145     if( max ) {
 146         double m = max->get_value();
 147         if( value > m )
 148             value = m;
 149     }
 150
 151     return abs ? fabs(value) : value;
 152 }
 153
 154 FGXMLAutoComponent::FGXMLAutoComponent( SGPropertyNode * node ) :
 155       debug(false),
 156       name(""),
 157       enable_prop( NULL ),
 158       passive_mode( fgGetNode("/autopilot/locks/passive-mode", true) ),
 159       enable_value( NULL ),
 160       honor_passive( false ),
 161       enabled( false ),
 162       _condition( NULL ),
 163       feedback_if_disabled( false )
 164 {
 165     int i;
 166     SGPropertyNode *prop;
 167
 168     for ( i = 0; i < node->nChildren(); ++i ) {
 169         SGPropertyNode *child = node->getChild(i);
 170         string cname = child->getName();
 171         string cval = child->getStringValue();
 172         if ( cname == "name" ) {
 173             name = cval;
 174
 175         } else if ( cname == "feedback-if-disabled" ) {
 176             feedback_if_disabled = child->getBoolValue();
 177
 178         } else if ( cname == "debug" ) {
 179             debug = child->getBoolValue();
 180
 181         } else if ( cname == "enable" ) {
 182             if( (prop = child->getChild("condition")) != NULL ) {
 183               _condition = sgReadCondition(child, prop);
 184             } else {
 185                if ( (prop = child->getChild( "prop" )) != NULL ) {
 186                    enable_prop = fgGetNode( prop->getStringValue(), true );
 187                }
 188
 189                if ( (prop = child->getChild( "value" )) != NULL ) {
 190                    delete enable_value;
 191                    enable_value = new string(prop->getStringValue());
 192                }
 193             }
 194             if ( (prop = child->getChild( "honor-passive" )) != NULL ) {
 195                 honor_passive = prop->getBoolValue();
 196             }
 197
 198         } else if ( cname == "input" ) {
 199
 200               valueInput.push_back( new FGXMLAutoInput( child ) );
 201
 202         } else if ( cname == "reference" ) {
 203
 204               referenceInput.push_back( new FGXMLAutoInput( child ) );
 205
 206         } else if ( cname == "output" ) {
 207             // grab all <prop> and <property> childs
 208             int found = 0;
 209             // backwards compatibility: allow <prop> elements
 210             for( int i = 0; (prop = child->getChild("prop", i)) != NULL; i++ ) {
 211                 SGPropertyNode *tmp = fgGetNode( prop->getStringValue(), true );
 212                 output_list.push_back( tmp );
 213                 found++;
 214             }
 215             for( int i = 0; (prop = child->getChild("property", i)) != NULL; i++ ) {
 216                 SGPropertyNode *tmp = fgGetNode( prop->getStringValue(), true );
 217                 output_list.push_back( tmp );
 218                 found++;
 219             }
 220
 221             // no <prop> elements, text node of <output> is property name
 222             if( found == 0 )
 223                 output_list.push_back( fgGetNode(child->getStringValue(), true ) );
 224
 225         } else if ( cname == "config" ) {
 226             if( (prop = child->getChild("min")) != NULL ) {
 227               uminInput.push_back( new FGXMLAutoInput( prop ) );
 228             }
 229             if( (prop = child->getChild("u_min")) != NULL ) {
 230               uminInput.push_back( new FGXMLAutoInput( prop ) );
 231             }
 232             if( (prop = child->getChild("max")) != NULL ) {
 233               umaxInput.push_back( new FGXMLAutoInput( prop ) );
 234             }
 235             if( (prop = child->getChild("u_max")) != NULL ) {
 236               umaxInput.push_back( new FGXMLAutoInput( prop ) );
 237             }
 238         } else if ( cname == "min" ) {
 239             uminInput.push_back( new FGXMLAutoInput( child ) );
 240         } else if ( cname == "u_min" ) {
 241             uminInput.push_back( new FGXMLAutoInput( child ) );
 242         } else if ( cname == "max" ) {
 243             umaxInput.push_back( new FGXMLAutoInput( child ) );
 244         } else if ( cname == "u_max" ) {
 245             umaxInput.push_back( new FGXMLAutoInput( child ) );
 246         }
 247     }
 248 }
 249
 250 FGXMLAutoComponent::~FGXMLAutoComponent()
 251 {
 252     delete enable_value;
 253 }
 254
 255 bool FGXMLAutoComponent::isPropertyEnabled()
 256 {
 257     if( _condition )
 258         return _condition->test();
 259
 260     if( enable_prop ) {
 261         if( enable_value ) {
 262             return *enable_value == enable_prop->getStringValue();
 263         } else {
 264             return enable_prop->getBoolValue();
 265         }
 266     }
 267     return true;
 268 }
 269
 270 void FGXMLAutoComponent::do_feedback_if_disabled()
 271 {
 272     if( output_list.size() > 0 ) {
 273         FGXMLAutoInput * input = valueInput.get_active();
 274         if( input != NULL )
 275             input->set_value( output_list[0]->getDoubleValue() );
 276     }
 277 }
 278
 279 double FGXMLAutoComponent::clamp( double value )
 280 {
 281     // clamp, if either min or max is defined
 282     if( uminInput.size() + umaxInput.size() > 0 ) {
 283         double d = umaxInput.get_value( 0.0 );
 284         if( value > d ) value = d;
 285         d = uminInput.get_value( 0.0 );
 286         if( value < d ) value = d;
 287     }
 288     return value;
 289 }
 290
 291 FGPIDController::FGPIDController( SGPropertyNode *node ):
 292     FGXMLAutoComponent( node ),
 293     alpha( 0.1 ),
 294     beta( 1.0 ),
 295     gamma( 0.0 ),
 296     ep_n_1( 0.0 ),
 297     edf_n_1( 0.0 ),
 298     edf_n_2( 0.0 ),
 299     u_n_1( 0.0 ),
 300     desiredTs( 0.0 ),
 301     elapsedTime( 0.0 )
 302 {
 303     int i;
 304     for ( i = 0; i < node->nChildren(); ++i ) {
 305         SGPropertyNode *child = node->getChild(i);
 306         string cname = child->getName();
 307         string cval = child->getStringValue();
 308         if ( cname == "config" ) {
 309             SGPropertyNode *config;
 310
 311             if ( (config = child->getChild( "Ts" )) != NULL ) {
 312                 desiredTs = config->getDoubleValue();
 313             }
 314
 315             Kp.push_back( new FGXMLAutoInput( child->getChild( "Kp" ) ) );
 316             Ti.push_back( new FGXMLAutoInput( child->getChild( "Ti" ) ) );
 317             Td.push_back( new FGXMLAutoInput( child->getChild( "Td" ) ) );
 318
 319             config = child->getChild( "beta" );
 320             if ( config != NULL ) {
 321                 beta = config->getDoubleValue();
 322             }
 323
 324             config = child->getChild( "alpha" );
 325             if ( config != NULL ) {
 326                 alpha = config->getDoubleValue();
 327             }
 328
 329             config = child->getChild( "gamma" );
 330             if ( config != NULL ) {
 331                 gamma = config->getDoubleValue();
 332             }
 333
 334         } else {
 335             SG_LOG( SG_AUTOPILOT, SG_WARN, "Error in autopilot config logic" );
 336             if ( get_name().length() ) {
 337                 SG_LOG( SG_AUTOPILOT, SG_WARN, "Section = " << get_name() );
 338             }
 339         }
 340     }
 341 }
 342
 343
 344 /*
 345  * Roy Vegard Ovesen:
 346  *
 347  * Ok! Here is the PID controller algorithm that I would like to see
 348  * implemented:
 349  *
 350  *   delta_u_n = Kp * [ (ep_n - ep_n-1) + ((Ts/Ti)*e_n)
 351  *               + (Td/Ts)*(edf_n - 2*edf_n-1 + edf_n-2) ]
 352  *
 353  *   u_n = u_n-1 + delta_u_n
 354  *
 355  * where:
 356  *
 357  * delta_u : The incremental output
 358  * Kp      : Proportional gain
 359  * ep      : Proportional error with reference weighing
 360  *           ep = beta * r - y
 361  *           where:
 362  *           beta : Weighing factor
 363  *           r    : Reference (setpoint)
 364  *           y    : Process value, measured
 365  * e       : Error
 366  *           e = r - y
 367  * Ts      : Sampling interval
 368  * Ti      : Integrator time
 369  * Td      : Derivator time
 370  * edf     : Derivate error with reference weighing and filtering
 371  *           edf_n = edf_n-1 / ((Ts/Tf) + 1) + ed_n * (Ts/Tf) / ((Ts/Tf) + 1)
 372  *           where:
 373  *           Tf : Filter time
 374  *           Tf = alpha * Td , where alpha usually is set to 0.1
 375  *           ed : Unfiltered derivate error with reference weighing
 376  *             ed = gamma * r - y
 377  *             where:
 378  *             gamma : Weighing factor
 379  *
 380  * u       : absolute output
 381  *
 382  * Index n means the n'th value.
 383  *
 384  *
 385  * Inputs:
 386  * enabled ,
 387  * y_n , r_n , beta=1 , gamma=0 , alpha=0.1 ,
 388  * Kp , Ti , Td , Ts (is the sampling time available?)
 389  * u_min , u_max
 390  *
 391  * Output:
 392  * u_n
 393  */
 394
 395 void FGPIDController::update( double dt ) {
 396     double ep_n;            // proportional error with reference weighing
 397     double e_n;             // error
 398     double ed_n;            // derivative error
 399     double edf_n;           // derivative error filter
 400     double Tf;              // filter time
 401     double delta_u_n = 0.0; // incremental output
 402     double u_n = 0.0;       // absolute output
 403     double Ts;              // sampling interval (sec)
 404
 405     double u_min = uminInput.get_value();
 406     double u_max = umaxInput.get_value();
 407
 408     elapsedTime += dt;
 409     if ( elapsedTime <= desiredTs ) {
 410         // do nothing if time step is not positive (i.e. no time has
 411         // elapsed)
 412         return;
 413     }
 414     Ts = elapsedTime;
 415     elapsedTime = 0.0;
 416
 417     if ( isPropertyEnabled() ) {
 418         if ( !enabled ) {
 419             // first time being enabled, seed u_n with current
 420             // property tree value
 421             u_n = get_output_value();
 422             u_n_1 = u_n;
 423         }
 424         enabled = true;
 425     } else {
 426         enabled = false;
 427         do_feedback();
 428     }
 429
 430     if ( enabled && Ts > 0.0) {
 431         if ( debug ) cout << "Updating " << get_name()
 432                           << " Ts " << Ts << endl;
 433
 434         double y_n = valueInput.get_value();
 435         double r_n = referenceInput.get_value();
 436
 437         if ( debug ) cout << "  input = " << y_n << " ref = " << r_n << endl;
 438
 439         // Calculates proportional error:
 440         ep_n = beta * r_n - y_n;
 441         if ( debug ) cout << "  ep_n = " << ep_n;
 442         if ( debug ) cout << "  ep_n_1 = " << ep_n_1;
 443
 444         // Calculates error:
 445         e_n = r_n - y_n;
 446         if ( debug ) cout << " e_n = " << e_n;
 447
 448         // Calculates derivate error:
 449         ed_n = gamma * r_n - y_n;
 450         if ( debug ) cout << " ed_n = " << ed_n;
 451
 452         double td = Td.get_value();
 453         if ( td > 0.0 ) {
 454             // Calculates filter time:
 455             Tf = alpha * td;
 456             if ( debug ) cout << " Tf = " << Tf;
 457
 458             // Filters the derivate error:
 459             edf_n = edf_n_1 / (Ts/Tf + 1)
 460                 + ed_n * (Ts/Tf) / (Ts/Tf + 1);
 461             if ( debug ) cout << " edf_n = " << edf_n;
 462         } else {
 463             edf_n = ed_n;
 464         }
 465
 466         // Calculates the incremental output:
 467         double ti = Ti.get_value();
 468         if ( ti > 0.0 ) {
 469             delta_u_n = Kp.get_value() * ( (ep_n - ep_n_1)
 470                                + ((Ts/ti) * e_n)
 471                                + ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
 472         }
 473
 474         if ( debug ) {
 475             cout << " delta_u_n = " << delta_u_n << endl;
 476             cout << "P:" << Kp.get_value() * (ep_n - ep_n_1)
 477                  << " I:" << Kp.get_value() * ((Ts/ti) * e_n)
 478                  << " D:" << Kp.get_value() * ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2))
 479                  << endl;
 480         }
 481
 482         // Integrator anti-windup logic:
 483         if ( delta_u_n > (u_max - u_n_1) ) {
 484             delta_u_n = u_max - u_n_1;
 485             if ( debug ) cout << " max saturation " << endl;
 486         } else if ( delta_u_n < (u_min - u_n_1) ) {
 487             delta_u_n = u_min - u_n_1;
 488             if ( debug ) cout << " min saturation " << endl;
 489         }
 490
 491         // Calculates absolute output:
 492         u_n = u_n_1 + delta_u_n;
 493         if ( debug ) cout << "  output = " << u_n << endl;
 494
 495         // Updates indexed values;
 496         u_n_1   = u_n;
 497         ep_n_1  = ep_n;
 498         edf_n_2 = edf_n_1;
 499         edf_n_1 = edf_n;
 500
 501         set_output_value( u_n );
 502     } else if ( !enabled ) {
 503         ep_n  = 0.0;
 504         edf_n = 0.0;
 505         // Updates indexed values;
 506         u_n_1   = u_n;
 507         ep_n_1  = ep_n;
 508         edf_n_2 = edf_n_1;
 509         edf_n_1 = edf_n;
 510     }
 511 }
 512
 513
 514 FGPISimpleController::FGPISimpleController( SGPropertyNode *node ):
 515     FGXMLAutoComponent( node ),
 516     int_sum( 0.0 )
 517 {
 518     int i;
 519     for ( i = 0; i < node->nChildren(); ++i ) {
 520         SGPropertyNode *child = node->getChild(i);
 521         string cname = child->getName();
 522         string cval = child->getStringValue();
 523         if ( cname == "config" ) {
 524             Kp.push_back( new FGXMLAutoInput( child->getChild( "Kp" ) ) );
 525             Ki.push_back( new FGXMLAutoInput( child->getChild( "Ki" ) ) );
 526         } else {
 527             SG_LOG( SG_AUTOPILOT, SG_WARN, "Error in autopilot config logic" );
 528             if ( get_name().length() ) {
 529                 SG_LOG( SG_AUTOPILOT, SG_WARN, "Section = " << get_name() );
 530             }
 531         }
 532     }
 533 }
 534
 535
 536 void FGPISimpleController::update( double dt ) {
 537
 538     if ( isPropertyEnabled() ) {
 539         if ( !enabled ) {
 540             // we have just been enabled, zero out int_sum
 541             int_sum = 0.0;
 542         }
 543         enabled = true;
 544     } else {
 545         enabled = false;
 546         do_feedback();
 547     }
 548
 549     if ( enabled ) {
 550         if ( debug ) cout << "Updating " << get_name() << endl;
 551         double y_n = valueInput.get_value();
 552         double r_n = referenceInput.get_value();
 553
 554         double error = r_n - y_n;
 555         if ( debug ) cout << "input = " << y_n
 556                           << " reference = " << r_n
 557                           << " error = " << error
 558                           << endl;
 559
 560         double prop_comp = error * Kp.get_value();
 561         int_sum += error * Ki.get_value() * dt;
 562
 563
 564         if ( debug ) cout << "prop_comp = " << prop_comp
 565                           << " int_sum = " << int_sum << endl;
 566
 567         double output = prop_comp + int_sum;
 568         output = clamp( output );
 569         set_output_value( output );
 570         if ( debug ) cout << "output = " << output << endl;
 571     }
 572 }
 573
 574
 575 FGPredictor::FGPredictor ( SGPropertyNode *node ):
 576     FGXMLAutoComponent( node )
 577 {
 578     int i;
 579     for ( i = 0; i < node->nChildren(); ++i ) {
 580         SGPropertyNode *child = node->getChild(i);
 581         string cname = child->getName();
 582         if ( cname == "seconds" ) {
 583             seconds.push_back( new FGXMLAutoInput( child, 0 ) );
 584         } else if ( cname == "filter-gain" ) {
 585             filter_gain.push_back( new FGXMLAutoInput( child, 0 ) );
 586         }
 587     }
 588 }
 589
 590 void FGPredictor::update( double dt ) {
 591     /*
 592        Simple moving average filter converts input value to predicted value "seconds".
 593
 594        Smoothing as described by Curt Olson:
 595          gain would be valid in the range of 0 - 1.0
 596          1.0 would mean no filtering.
 597          0.0 would mean no input.
 598          0.5 would mean (1 part past value + 1 part current value) / 2
 599          0.1 would mean (9 parts past value + 1 part current value) / 10
 600          0.25 would mean (3 parts past value + 1 part current value) / 4
 601
 602     */
 603
 604     double ivalue = valueInput.get_value();
 605
 606     if ( isPropertyEnabled() ) {
 607         if ( !enabled ) {
 608             // first time being enabled
 609             last_value = ivalue;
 610         }
 611         enabled = true;
 612     } else {
 613         enabled = false;
 614         do_feedback();
 615     }
 616
 617     if ( enabled ) {
 618
 619         if ( dt > 0.0 ) {
 620             double current = (ivalue - last_value)/dt; // calculate current error change (per second)
 621             double average = dt < 1.0 ? ((1.0 - dt) * average + current * dt) : current;
 622
 623             // calculate output with filter gain adjustment
 624             double output = ivalue +
 625                (1.0 - filter_gain.get_value()) * (average * seconds.get_value()) +
 626                        filter_gain.get_value() * (current * seconds.get_value());
 627             output = clamp( output );
 628             set_output_value( output );
 629         }
 630         last_value = ivalue;
 631     }
 632 }
 633
 634
 635 FGDigitalFilter::FGDigitalFilter(SGPropertyNode *node):
 636     FGXMLAutoComponent( node ),
 637     filterType(none)
 638 {
 639     int i;
 640     for ( i = 0; i < node->nChildren(); ++i ) {
 641         SGPropertyNode *child = node->getChild(i);
 642         string cname = child->getName();
 643         string cval = child->getStringValue();
 644         if ( cname == "type" ) {
 645             if ( cval == "exponential" ) {
 646                 filterType = exponential;
 647             } else if (cval == "double-exponential") {
 648                 filterType = doubleExponential;
 649             } else if (cval == "moving-average") {
 650                 filterType = movingAverage;
 651             } else if (cval == "noise-spike") {
 652                 filterType = noiseSpike;
 653             } else if (cval == "gain") {
 654                 filterType = gain;
 655             } else if (cval == "reciprocal") {
 656                 filterType = reciprocal;
 657             }
 658         } else if ( cname == "filter-time" ) {
 659             TfInput.push_back( new FGXMLAutoInput( child, 1.0 ) );
 660             if( filterType == none ) filterType = exponential;
 661         } else if ( cname == "samples" ) {
 662             samplesInput.push_back( new FGXMLAutoInput( child, 1 ) );
 663             if( filterType == none ) filterType = movingAverage;
 664         } else if ( cname == "max-rate-of-change" ) {
 665             rateOfChangeInput.push_back( new FGXMLAutoInput( child, 1 ) );
 666             if( filterType == none ) filterType = noiseSpike;
 667         } else if ( cname == "gain" ) {
 668             gainInput.push_back( new FGXMLAutoInput( child, 1 ) );
 669             if( filterType == none ) filterType = gain;
 670         }
 671     }
 672
 673     output.resize(2, 0.0);
 674     input.resize(samplesInput.get_value() + 1, 0.0);
 675 }
 676
 677 void FGDigitalFilter::update(double dt)
 678 {
 679     if ( isPropertyEnabled() ) {
 680
 681         input.push_front(valueInput.get_value());
 682         input.resize(samplesInput.get_value() + 1, 0.0);
 683
 684         if ( !enabled ) {
 685             // first time being enabled, initialize output to the
 686             // value of the output property to avoid bumping.
 687             output.push_front(get_output_value());
 688         }
 689
 690         enabled = true;
 691     } else {
 692         enabled = false;
 693         do_feedback();
 694     }
 695
 696     if ( enabled && dt > 0.0 ) {
 697         /*
 698          * Exponential filter
 699          *
 700          * Output[n] = alpha*Input[n] + (1-alpha)*Output[n-1]
 701          *
 702          */
 703          if( debug ) cout << "Updating " << get_name()
 704                           << " dt " << dt << endl;
 705
 706         if (filterType == exponential)
 707         {
 708             double alpha = 1 / ((TfInput.get_value()/dt) + 1);
 709             output.push_front(alpha * input[0] +
 710                               (1 - alpha) * output[0]);
 711         }
 712         else if (filterType == doubleExponential)
 713         {
 714             double alpha = 1 / ((TfInput.get_value()/dt) + 1);
 715             output.push_front(alpha * alpha * input[0] +
 716                               2 * (1 - alpha) * output[0] -
 717                               (1 - alpha) * (1 - alpha) * output[1]);
 718         }
 719         else if (filterType == movingAverage)
 720         {
 721             output.push_front(output[0] +
 722                               (input[0] - input.back()) / samplesInput.get_value());
 723         }
 724         else if (filterType == noiseSpike)
 725         {
 726             double maxChange = rateOfChangeInput.get_value() * dt;
 727
 728             if ((output[0] - input[0]) > maxChange)
 729             {
 730                 output.push_front(output[0] - maxChange);
 731             }
 732             else if ((output[0] - input[0]) < -maxChange)
 733             {
 734                 output.push_front(output[0] + maxChange);
 735             }
 736             else if (fabs(input[0] - output[0]) <= maxChange)
 737             {
 738                 output.push_front(input[0]);
 739             }
 740         }
 741         else if (filterType == gain)
 742         {
 743             output[0] = gainInput.get_value() * input[0];
 744         }
 745         else if (filterType == reciprocal)
 746         {
 747             if (input[0] != 0.0) {
 748                 output[0] = gainInput.get_value() / input[0];
 749             }
 750         }
 751
 752         output[0] = clamp(output[0]) ;
 753         set_output_value( output[0] );
 754
 755         output.resize(2);
 756
 757         if (debug)
 758         {
 759             cout << "input:" << input[0]
 760                  << "\toutput:" << output[0] << endl;
 761         }
 762     }
 763 }
 764
 765
 766 FGXMLAutopilot::FGXMLAutopilot() {
 767 }
 768
 769
 770 FGXMLAutopilot::~FGXMLAutopilot() {
 771 }
 772
 773
 774 void FGXMLAutopilot::init() {
 775     config_props = fgGetNode( "/autopilot/new-config", true );
 776
 777     SGPropertyNode *path_n = fgGetNode("/sim/systems/autopilot/path");
 778
 779     if ( path_n ) {
 780         SGPath config( globals->get_fg_root() );
 781         config.append( path_n->getStringValue() );
 782
 783         SG_LOG( SG_ALL, SG_INFO, "Reading autopilot configuration from "
 784                 << config.str() );
 785         try {
 786             readProperties( config.str(), config_props );
 787
 788             if ( ! build() ) {
 789                 SG_LOG( SG_ALL, SG_ALERT,
 790                         "Detected an internal inconsistency in the autopilot");
 791                 SG_LOG( SG_ALL, SG_ALERT,
 792                         " configuration.  See earlier errors for" );
 793                 SG_LOG( SG_ALL, SG_ALERT,
 794                         " details.");
 795                 exit(-1);
 796             }
 797         } catch (const sg_exception& exc) {
 798             SG_LOG( SG_ALL, SG_ALERT, "Failed to load autopilot configuration: "
 799                     << config.str() );
 800         }
 801
 802     } else {
 803         SG_LOG( SG_ALL, SG_WARN,
 804                 "No autopilot configuration specified for this model!");
 805     }
 806 }
 807
 808
 809 void FGXMLAutopilot::reinit() {
 810     components.clear();
 811     init();
 812 }
 813
 814
 815 void FGXMLAutopilot::bind() {
 816 }
 817
 818 void FGXMLAutopilot::unbind() {
 819 }
 820
 821 bool FGXMLAutopilot::build() {
 822     SGPropertyNode *node;
 823     int i;
 824
 825     int count = config_props->nChildren();
 826     for ( i = 0; i < count; ++i ) {
 827         node = config_props->getChild(i);
 828         string name = node->getName();
 829         // cout << name << endl;
 830         if ( name == "pid-controller" ) {
 831             components.push_back( new FGPIDController( node ) );
 832         } else if ( name == "pi-simple-controller" ) {
 833             components.push_back( new FGPISimpleController( node ) );
 834         } else if ( name == "predict-simple" ) {
 835             components.push_back( new FGPredictor( node ) );
 836         } else if ( name == "filter" ) {
 837             components.push_back( new FGDigitalFilter( node ) );
 838         } else {
 839             SG_LOG( SG_ALL, SG_ALERT, "Unknown top level section: "
 840                     << name );
 841             return false;
 842         }
 843     }
 844
 845     return true;
 846 }
 847
 848
 849 /*
 850  * Update helper values
 851  */
 852 static void update_helper( double dt ) {
 853     // Estimate speed in 5,10 seconds
 854     static SGPropertyNode_ptr vel = fgGetNode( "/velocities/airspeed-kt", true );
 855     static SGPropertyNode_ptr lookahead5
 856         = fgGetNode( "/autopilot/internal/lookahead-5-sec-airspeed-kt", true );
 857     static SGPropertyNode_ptr lookahead10
 858         = fgGetNode( "/autopilot/internal/lookahead-10-sec-airspeed-kt", true );
 859
 860     static double average = 0.0; // average/filtered prediction
 861     static double v_last = 0.0;  // last velocity
 862
 863     double v = vel->getDoubleValue();
 864     double a = 0.0;
 865     if ( dt > 0.0 ) {
 866         a = (v - v_last) / dt;
 867
 868         if ( dt < 1.0 ) {
 869             average = (1.0 - dt) * average + dt * a;
 870         } else {
 871             average = a;
 872         }
 873
 874         lookahead5->setDoubleValue( v + average * 5.0 );
 875         lookahead10->setDoubleValue( v + average * 10.0 );
 876         v_last = v;
 877     }
 878
 879     // Calculate heading bug error normalized to +/- 180.0 (based on
 880     // DG indicated heading)
 881     static SGPropertyNode_ptr bug
 882         = fgGetNode( "/autopilot/settings/heading-bug-deg", true );
 883     static SGPropertyNode_ptr ind_hdg
 884         = fgGetNode( "/instrumentation/heading-indicator/indicated-heading-deg",
 885                      true );
 886     static SGPropertyNode_ptr ind_bug_error
 887         = fgGetNode( "/autopilot/internal/heading-bug-error-deg", true );
 888
 889     double diff = bug->getDoubleValue() - ind_hdg->getDoubleValue();
 890     if ( diff < -180.0 ) { diff += 360.0; }
 891     if ( diff > 180.0 ) { diff -= 360.0; }
 892     ind_bug_error->setDoubleValue( diff );
 893
 894     // Calculate heading bug error normalized to +/- 180.0 (based on
 895     // actual/nodrift magnetic-heading, i.e. a DG slaved to magnetic
 896     // compass.)
 897     static SGPropertyNode_ptr mag_hdg
 898         = fgGetNode( "/orientation/heading-magnetic-deg", true );
 899     static SGPropertyNode_ptr fdm_bug_error
 900         = fgGetNode( "/autopilot/internal/fdm-heading-bug-error-deg", true );
 901
 902     diff = bug->getDoubleValue() - mag_hdg->getDoubleValue();
 903     if ( diff < -180.0 ) { diff += 360.0; }
 904     if ( diff > 180.0 ) { diff -= 360.0; }
 905     fdm_bug_error->setDoubleValue( diff );
 906
 907     // Calculate true heading error normalized to +/- 180.0
 908     static SGPropertyNode_ptr target_true
 909         = fgGetNode( "/autopilot/settings/true-heading-deg", true );
 910     static SGPropertyNode_ptr true_hdg
 911         = fgGetNode( "/orientation/heading-deg", true );
 912     static SGPropertyNode_ptr true_track
 913         = fgGetNode( "/instrumentation/gps/indicated-track-true-deg", true );
 914     static SGPropertyNode_ptr true_error
 915         = fgGetNode( "/autopilot/internal/true-heading-error-deg", true );
 916
 917     diff = target_true->getDoubleValue() - true_hdg->getDoubleValue();
 918     if ( diff < -180.0 ) { diff += 360.0; }
 919     if ( diff > 180.0 ) { diff -= 360.0; }
 920     true_error->setDoubleValue( diff );
 921
 922     // Calculate nav1 target heading error normalized to +/- 180.0
 923     static SGPropertyNode_ptr target_nav1
 924         = fgGetNode( "/instrumentation/nav[0]/radials/target-auto-hdg-deg", true );
 925     static SGPropertyNode_ptr true_nav1
 926         = fgGetNode( "/autopilot/internal/nav1-heading-error-deg", true );
 927     static SGPropertyNode_ptr true_track_nav1
 928         = fgGetNode( "/autopilot/internal/nav1-track-error-deg", true );
 929
 930     diff = target_nav1->getDoubleValue() - true_hdg->getDoubleValue();
 931     if ( diff < -180.0 ) { diff += 360.0; }
 932     if ( diff > 180.0 ) { diff -= 360.0; }
 933     true_nav1->setDoubleValue( diff );
 934
 935     diff = target_nav1->getDoubleValue() - true_track->getDoubleValue();
 936     if ( diff < -180.0 ) { diff += 360.0; }
 937     if ( diff > 180.0 ) { diff -= 360.0; }
 938     true_track_nav1->setDoubleValue( diff );
 939
 940     // Calculate nav1 selected course error normalized to +/- 180.0
 941     // (based on DG indicated heading)
 942     static SGPropertyNode_ptr nav1_course_error
 943         = fgGetNode( "/autopilot/internal/nav1-course-error", true );
 944     static SGPropertyNode_ptr nav1_selected_course
 945         = fgGetNode( "/instrumentation/nav[0]/radials/selected-deg", true );
 946
 947     diff = nav1_selected_course->getDoubleValue() - ind_hdg->getDoubleValue();
 948 //    if ( diff < -180.0 ) { diff += 360.0; }
 949 //    if ( diff > 180.0 ) { diff -= 360.0; }
 950     SG_NORMALIZE_RANGE( diff, -180.0, 180.0 );
 951     nav1_course_error->setDoubleValue( diff );
 952
 953     // Calculate vertical speed in fpm
 954     static SGPropertyNode_ptr vs_fps
 955         = fgGetNode( "/velocities/vertical-speed-fps", true );
 956     static SGPropertyNode_ptr vs_fpm
 957         = fgGetNode( "/autopilot/internal/vert-speed-fpm", true );
 958
 959     vs_fpm->setDoubleValue( vs_fps->getDoubleValue() * 60.0 );
 960
 961
 962     // Calculate static port pressure rate in [inhg/s].
 963     // Used to determine vertical speed.
 964     static SGPropertyNode_ptr static_pressure
 965         = fgGetNode( "/systems/static[0]/pressure-inhg", true );
 966     static SGPropertyNode_ptr pressure_rate
 967         = fgGetNode( "/autopilot/internal/pressure-rate", true );
 968
 969     static double last_static_pressure = 0.0;
 970
 971     if ( dt > 0.0 ) {
 972         double current_static_pressure = static_pressure->getDoubleValue();
 973
 974         double current_pressure_rate =
 975             ( current_static_pressure - last_static_pressure ) / dt;
 976
 977         pressure_rate->setDoubleValue(current_pressure_rate);
 978
 979         last_static_pressure = current_static_pressure;
 980     }
 981
 982 }
 983
 984
 985 /*
 986  * Update the list of autopilot components
 987  */
 988
 989 void FGXMLAutopilot::update( double dt ) {
 990     update_helper( dt );
 991
 992     unsigned int i;
 993     for ( i = 0; i < components.size(); ++i ) {
 994         components[i]->update( dt );
 995     }
 996 }
 997