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       _condition( NULL ),
 156       enable_prop( NULL ),
 157       enable_value( NULL ),
 158       passive_mode( fgGetNode("/autopilot/locks/passive-mode", true) ),
 159       honor_passive( false ),
 160       name(""),
 161       feedback_if_disabled( false ),
 162       debug(false),
 163       enabled( 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     average(0.0)
 578 {
 579     int i;
 580     for ( i = 0; i < node->nChildren(); ++i ) {
 581         SGPropertyNode *child = node->getChild(i);
 582         string cname = child->getName();
 583         if ( cname == "seconds" ) {
 584             seconds.push_back( new FGXMLAutoInput( child, 0 ) );
 585         } else if ( cname == "filter-gain" ) {
 586             filter_gain.push_back( new FGXMLAutoInput( child, 0 ) );
 587         }
 588     }
 589 }
 590
 591 void FGPredictor::update( double dt ) {
 592     /*
 593        Simple moving average filter converts input value to predicted value "seconds".
 594
 595        Smoothing as described by Curt Olson:
 596          gain would be valid in the range of 0 - 1.0
 597          1.0 would mean no filtering.
 598          0.0 would mean no input.
 599          0.5 would mean (1 part past value + 1 part current value) / 2
 600          0.1 would mean (9 parts past value + 1 part current value) / 10
 601          0.25 would mean (3 parts past value + 1 part current value) / 4
 602
 603     */
 604
 605     double ivalue = valueInput.get_value();
 606
 607     if ( isPropertyEnabled() ) {
 608         if ( !enabled ) {
 609             // first time being enabled
 610             last_value = ivalue;
 611         }
 612         enabled = true;
 613     } else {
 614         enabled = false;
 615         do_feedback();
 616     }
 617
 618     if ( enabled ) {
 619
 620         if ( dt > 0.0 ) {
 621             double current = (ivalue - last_value)/dt; // calculate current error change (per second)
 622             average = dt < 1.0 ? ((1.0 - dt) * average + current * dt) : current;
 623
 624             // calculate output with filter gain adjustment
 625             double output = ivalue +
 626                (1.0 - filter_gain.get_value()) * (average * seconds.get_value()) +
 627                        filter_gain.get_value() * (current * seconds.get_value());
 628             output = clamp( output );
 629             set_output_value( output );
 630         }
 631         last_value = ivalue;
 632     }
 633 }
 634
 635
 636 FGDigitalFilter::FGDigitalFilter(SGPropertyNode *node):
 637     FGXMLAutoComponent( node ),
 638     filterType(none)
 639 {
 640     int i;
 641     for ( i = 0; i < node->nChildren(); ++i ) {
 642         SGPropertyNode *child = node->getChild(i);
 643         string cname = child->getName();
 644         string cval = child->getStringValue();
 645         if ( cname == "type" ) {
 646             if ( cval == "exponential" ) {
 647                 filterType = exponential;
 648             } else if (cval == "double-exponential") {
 649                 filterType = doubleExponential;
 650             } else if (cval == "moving-average") {
 651                 filterType = movingAverage;
 652             } else if (cval == "noise-spike") {
 653                 filterType = noiseSpike;
 654             } else if (cval == "gain") {
 655                 filterType = gain;
 656             } else if (cval == "reciprocal") {
 657                 filterType = reciprocal;
 658             }
 659         } else if ( cname == "filter-time" ) {
 660             TfInput.push_back( new FGXMLAutoInput( child, 1.0 ) );
 661             if( filterType == none ) filterType = exponential;
 662         } else if ( cname == "samples" ) {
 663             samplesInput.push_back( new FGXMLAutoInput( child, 1 ) );
 664             if( filterType == none ) filterType = movingAverage;
 665         } else if ( cname == "max-rate-of-change" ) {
 666             rateOfChangeInput.push_back( new FGXMLAutoInput( child, 1 ) );
 667             if( filterType == none ) filterType = noiseSpike;
 668         } else if ( cname == "gain" ) {
 669             gainInput.push_back( new FGXMLAutoInput( child, 1 ) );
 670             if( filterType == none ) filterType = gain;
 671         }
 672     }
 673
 674     output.resize(2, 0.0);
 675     input.resize(samplesInput.get_value() + 1, 0.0);
 676 }
 677
 678 void FGDigitalFilter::update(double dt)
 679 {
 680     if ( isPropertyEnabled() ) {
 681
 682         input.push_front(valueInput.get_value());
 683         input.resize(samplesInput.get_value() + 1, 0.0);
 684
 685         if ( !enabled ) {
 686             // first time being enabled, initialize output to the
 687             // value of the output property to avoid bumping.
 688             output.push_front(get_output_value());
 689         }
 690
 691         enabled = true;
 692     } else {
 693         enabled = false;
 694         do_feedback();
 695     }
 696
 697     if ( enabled && dt > 0.0 ) {
 698         /*
 699          * Exponential filter
 700          *
 701          * Output[n] = alpha*Input[n] + (1-alpha)*Output[n-1]
 702          *
 703          */
 704          if( debug ) cout << "Updating " << get_name()
 705                           << " dt " << dt << endl;
 706
 707         if (filterType == exponential)
 708         {
 709             double alpha = 1 / ((TfInput.get_value()/dt) + 1);
 710             output.push_front(alpha * input[0] +
 711                               (1 - alpha) * output[0]);
 712         }
 713         else if (filterType == doubleExponential)
 714         {
 715             double alpha = 1 / ((TfInput.get_value()/dt) + 1);
 716             output.push_front(alpha * alpha * input[0] +
 717                               2 * (1 - alpha) * output[0] -
 718                               (1 - alpha) * (1 - alpha) * output[1]);
 719         }
 720         else if (filterType == movingAverage)
 721         {
 722             output.push_front(output[0] +
 723                               (input[0] - input.back()) / samplesInput.get_value());
 724         }
 725         else if (filterType == noiseSpike)
 726         {
 727             double maxChange = rateOfChangeInput.get_value() * dt;
 728
 729             if ((output[0] - input[0]) > maxChange)
 730             {
 731                 output.push_front(output[0] - maxChange);
 732             }
 733             else if ((output[0] - input[0]) < -maxChange)
 734             {
 735                 output.push_front(output[0] + maxChange);
 736             }
 737             else if (fabs(input[0] - output[0]) <= maxChange)
 738             {
 739                 output.push_front(input[0]);
 740             }
 741         }
 742         else if (filterType == gain)
 743         {
 744             output[0] = gainInput.get_value() * input[0];
 745         }
 746         else if (filterType == reciprocal)
 747         {
 748             if (input[0] != 0.0) {
 749                 output[0] = gainInput.get_value() / input[0];
 750             }
 751         }
 752
 753         output[0] = clamp(output[0]) ;
 754         set_output_value( output[0] );
 755
 756         output.resize(2);
 757
 758         if (debug)
 759         {
 760             cout << "input:" << input[0]
 761                  << "\toutput:" << output[0] << endl;
 762         }
 763     }
 764 }
 765
 766
 767 FGXMLAutopilot::FGXMLAutopilot() {
 768 }
 769
 770
 771 FGXMLAutopilot::~FGXMLAutopilot() {
 772 }
 773
 774
 775 void FGXMLAutopilot::init() {
 776     config_props = fgGetNode( "/autopilot/new-config", true );
 777
 778     SGPropertyNode *path_n = fgGetNode("/sim/systems/autopilot/path");
 779
 780     if ( path_n ) {
 781         SGPath config( globals->get_fg_root() );
 782         config.append( path_n->getStringValue() );
 783
 784         SG_LOG( SG_ALL, SG_INFO, "Reading autopilot configuration from "
 785                 << config.str() );
 786         try {
 787             readProperties( config.str(), config_props );
 788
 789             if ( ! build() ) {
 790                 SG_LOG( SG_ALL, SG_ALERT,
 791                         "Detected an internal inconsistency in the autopilot");
 792                 SG_LOG( SG_ALL, SG_ALERT,
 793                         " configuration.  See earlier errors for" );
 794                 SG_LOG( SG_ALL, SG_ALERT,
 795                         " details.");
 796                 exit(-1);
 797             }
 798         } catch (const sg_exception&) {
 799             SG_LOG( SG_ALL, SG_ALERT, "Failed to load autopilot configuration: "
 800                     << config.str() );
 801         }
 802
 803     } else {
 804         SG_LOG( SG_ALL, SG_WARN,
 805                 "No autopilot configuration specified for this model!");
 806     }
 807 }
 808
 809
 810 void FGXMLAutopilot::reinit() {
 811     components.clear();
 812     init();
 813 }
 814
 815
 816 void FGXMLAutopilot::bind() {
 817 }
 818
 819 void FGXMLAutopilot::unbind() {
 820 }
 821
 822 bool FGXMLAutopilot::build() {
 823     SGPropertyNode *node;
 824     int i;
 825
 826     int count = config_props->nChildren();
 827     for ( i = 0; i < count; ++i ) {
 828         node = config_props->getChild(i);
 829         string name = node->getName();
 830         // cout << name << endl;
 831         if ( name == "pid-controller" ) {
 832             components.push_back( new FGPIDController( node ) );
 833         } else if ( name == "pi-simple-controller" ) {
 834             components.push_back( new FGPISimpleController( node ) );
 835         } else if ( name == "predict-simple" ) {
 836             components.push_back( new FGPredictor( node ) );
 837         } else if ( name == "filter" ) {
 838             components.push_back( new FGDigitalFilter( node ) );
 839         } else {
 840             SG_LOG( SG_ALL, SG_ALERT, "Unknown top level section: "
 841                     << name );
 842             return false;
 843         }
 844     }
 845
 846     return true;
 847 }
 848
 849
 850 /*
 851  * Update helper values
 852  */
 853 static void update_helper( double dt ) {
 854     // Estimate speed in 5,10 seconds
 855     static SGPropertyNode_ptr vel = fgGetNode( "/velocities/airspeed-kt", true );
 856     static SGPropertyNode_ptr lookahead5
 857         = fgGetNode( "/autopilot/internal/lookahead-5-sec-airspeed-kt", true );
 858     static SGPropertyNode_ptr lookahead10
 859         = fgGetNode( "/autopilot/internal/lookahead-10-sec-airspeed-kt", true );
 860
 861     static double average = 0.0; // average/filtered prediction
 862     static double v_last = 0.0;  // last velocity
 863
 864     double v = vel->getDoubleValue();
 865     double a = 0.0;
 866     if ( dt > 0.0 ) {
 867         a = (v - v_last) / dt;
 868
 869         if ( dt < 1.0 ) {
 870             average = (1.0 - dt) * average + dt * a;
 871         } else {
 872             average = a;
 873         }
 874
 875         lookahead5->setDoubleValue( v + average * 5.0 );
 876         lookahead10->setDoubleValue( v + average * 10.0 );
 877         v_last = v;
 878     }
 879
 880     // Calculate heading bug error normalized to +/- 180.0 (based on
 881     // DG indicated heading)
 882     static SGPropertyNode_ptr bug
 883         = fgGetNode( "/autopilot/settings/heading-bug-deg", true );
 884     static SGPropertyNode_ptr ind_hdg
 885         = fgGetNode( "/instrumentation/heading-indicator/indicated-heading-deg",
 886                      true );
 887     static SGPropertyNode_ptr ind_bug_error
 888         = fgGetNode( "/autopilot/internal/heading-bug-error-deg", true );
 889
 890     double diff = bug->getDoubleValue() - ind_hdg->getDoubleValue();
 891     if ( diff < -180.0 ) { diff += 360.0; }
 892     if ( diff > 180.0 ) { diff -= 360.0; }
 893     ind_bug_error->setDoubleValue( diff );
 894
 895     // Calculate heading bug error normalized to +/- 180.0 (based on
 896     // actual/nodrift magnetic-heading, i.e. a DG slaved to magnetic
 897     // compass.)
 898     static SGPropertyNode_ptr mag_hdg
 899         = fgGetNode( "/orientation/heading-magnetic-deg", true );
 900     static SGPropertyNode_ptr fdm_bug_error
 901         = fgGetNode( "/autopilot/internal/fdm-heading-bug-error-deg", true );
 902
 903     diff = bug->getDoubleValue() - mag_hdg->getDoubleValue();
 904     if ( diff < -180.0 ) { diff += 360.0; }
 905     if ( diff > 180.0 ) { diff -= 360.0; }
 906     fdm_bug_error->setDoubleValue( diff );
 907
 908     // Calculate true heading error normalized to +/- 180.0
 909     static SGPropertyNode_ptr target_true
 910         = fgGetNode( "/autopilot/settings/true-heading-deg", true );
 911     static SGPropertyNode_ptr true_hdg
 912         = fgGetNode( "/orientation/heading-deg", true );
 913     static SGPropertyNode_ptr true_track
 914         = fgGetNode( "/instrumentation/gps/indicated-track-true-deg", true );
 915     static SGPropertyNode_ptr true_error
 916         = fgGetNode( "/autopilot/internal/true-heading-error-deg", true );
 917
 918     diff = target_true->getDoubleValue() - true_hdg->getDoubleValue();
 919     if ( diff < -180.0 ) { diff += 360.0; }
 920     if ( diff > 180.0 ) { diff -= 360.0; }
 921     true_error->setDoubleValue( diff );
 922
 923     // Calculate nav1 target heading error normalized to +/- 180.0
 924     static SGPropertyNode_ptr target_nav1
 925         = fgGetNode( "/instrumentation/nav[0]/radials/target-auto-hdg-deg", true );
 926     static SGPropertyNode_ptr true_nav1
 927         = fgGetNode( "/autopilot/internal/nav1-heading-error-deg", true );
 928     static SGPropertyNode_ptr true_track_nav1
 929         = fgGetNode( "/autopilot/internal/nav1-track-error-deg", true );
 930
 931     diff = target_nav1->getDoubleValue() - true_hdg->getDoubleValue();
 932     if ( diff < -180.0 ) { diff += 360.0; }
 933     if ( diff > 180.0 ) { diff -= 360.0; }
 934     true_nav1->setDoubleValue( diff );
 935
 936     diff = target_nav1->getDoubleValue() - true_track->getDoubleValue();
 937     if ( diff < -180.0 ) { diff += 360.0; }
 938     if ( diff > 180.0 ) { diff -= 360.0; }
 939     true_track_nav1->setDoubleValue( diff );
 940
 941     // Calculate nav1 selected course error normalized to +/- 180.0
 942     // (based on DG indicated heading)
 943     static SGPropertyNode_ptr nav1_course_error
 944         = fgGetNode( "/autopilot/internal/nav1-course-error", true );
 945     static SGPropertyNode_ptr nav1_selected_course
 946         = fgGetNode( "/instrumentation/nav[0]/radials/selected-deg", true );
 947
 948     diff = nav1_selected_course->getDoubleValue() - ind_hdg->getDoubleValue();
 949 //    if ( diff < -180.0 ) { diff += 360.0; }
 950 //    if ( diff > 180.0 ) { diff -= 360.0; }
 951     SG_NORMALIZE_RANGE( diff, -180.0, 180.0 );
 952     nav1_course_error->setDoubleValue( diff );
 953
 954     // Calculate vertical speed in fpm
 955     static SGPropertyNode_ptr vs_fps
 956         = fgGetNode( "/velocities/vertical-speed-fps", true );
 957     static SGPropertyNode_ptr vs_fpm
 958         = fgGetNode( "/autopilot/internal/vert-speed-fpm", true );
 959
 960     vs_fpm->setDoubleValue( vs_fps->getDoubleValue() * 60.0 );
 961
 962
 963     // Calculate static port pressure rate in [inhg/s].
 964     // Used to determine vertical speed.
 965     static SGPropertyNode_ptr static_pressure
 966         = fgGetNode( "/systems/static[0]/pressure-inhg", true );
 967     static SGPropertyNode_ptr pressure_rate
 968         = fgGetNode( "/autopilot/internal/pressure-rate", true );
 969
 970     static double last_static_pressure = 0.0;
 971
 972     if ( dt > 0.0 ) {
 973         double current_static_pressure = static_pressure->getDoubleValue();
 974
 975         double current_pressure_rate =
 976             ( current_static_pressure - last_static_pressure ) / dt;
 977
 978         pressure_rate->setDoubleValue(current_pressure_rate);
 979
 980         last_static_pressure = current_static_pressure;
 981     }
 982
 983 }
 984
 985
 986 /*
 987  * Update the list of autopilot components
 988  */
 989
 990 void FGXMLAutopilot::update( double dt ) {
 991     update_helper( dt );
 992
 993     unsigned int i;
 994     for ( i = 0; i < components.size(); ++i ) {
 995         components[i]->update( dt );
 996     }
 997 }
 998