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