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