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