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