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