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