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