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