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             SGPropertyNode *prop = child->getChild( "prop" );
 831             if ( prop != NULL ) {
 832                 enable_prop = fgGetNode( prop->getStringValue(), true );
 833             }
 834             SGPropertyNode *val = child->getChild( "value" );
 835             if ( val != NULL ) {
 836                 enable_value = val->getStringValue();
 837             }
 838             SGPropertyNode *pass = child->getChild( "honor-passive" );
 839             if ( pass != NULL ) {
 840                 honor_passive = pass->getBoolValue();
 841             }
 842         } else if ( cname == "type" ) {
 843             if ( cval == "exponential" ) {
 844                 filterType = exponential;
 845             } else if (cval == "double-exponential") {
 846                 filterType = doubleExponential;
 847             } else if (cval == "moving-average") {
 848                 filterType = movingAverage;
 849             } else if (cval == "noise-spike") {
 850                 filterType = noiseSpike;
 851             } else if (cval == "gain") {
 852                 filterType = gain;
 853             } else if (cval == "reciprocal") {
 854                 filterType = reciprocal;
 855             }
 856         } else if ( cname == "input" ) {
 857             input_prop = fgGetNode( child->getStringValue(), true );
 858         } else if ( cname == "filter-time" ) {
 859             Tf = child->getDoubleValue();
 860         } else if ( cname == "samples" ) {
 861             samples = child->getIntValue();
 862         } else if ( cname == "max-rate-of-change" ) {
 863             rateOfChange = child->getDoubleValue();
 864         } else if ( cname == "gain" ) {
 865             SGPropertyNode *val = child->getChild( "value" );
 866             if ( val != NULL ) {
 867                 gainFactor = val->getDoubleValue();
 868             }
 869             SGPropertyNode *prop = child->getChild( "prop" );
 870             if ( prop != NULL ) {
 871                 gain_prop = fgGetNode( prop->getStringValue(), true );
 872                 gain_prop->setDoubleValue(gainFactor);
 873             }
 874         } else if ( cname == "u_min" ) {
 875             output_min_clamp = child->getDoubleValue();
 876         } else if ( cname == "u_max" ) {
 877             output_max_clamp = child->getDoubleValue();
 878         } else if ( cname == "output" ) {
 879             SGPropertyNode *tmp = fgGetNode( child->getStringValue(), true );
 880             output_list.push_back( tmp );
 881         }
 882     }
 883
 884     output.resize(2, 0.0);
 885     input.resize(samples + 1, 0.0);
 886 }
 887
 888 void FGDigitalFilter::update(double dt)
 889 {
 890     if ( (input_prop != NULL &&
 891           enable_prop != NULL &&
 892           enable_prop->getStringValue() == enable_value) ||
 893          (enable_prop == NULL &&
 894           input_prop != NULL) ) {
 895
 896         input.push_front(input_prop->getDoubleValue());
 897         input.resize(samples + 1, 0.0);
 898
 899         if ( !enabled ) {
 900             // first time being enabled, initialize output to the
 901             // value of the output property to avoid bumping.
 902             output.push_front(output_list[0]->getDoubleValue());
 903         }
 904
 905         enabled = true;
 906     } else {
 907         enabled = false;
 908     }
 909
 910     if ( enabled && dt > 0.0 ) {
 911         /*
 912          * Exponential filter
 913          *
 914          * Output[n] = alpha*Input[n] + (1-alpha)*Output[n-1]
 915          *
 916          */
 917
 918         if (filterType == exponential)
 919         {
 920             double alpha = 1 / ((Tf/dt) + 1);
 921             output.push_front(alpha * input[0] +
 922                               (1 - alpha) * output[0]);
 923         }
 924         else if (filterType == doubleExponential)
 925         {
 926             double alpha = 1 / ((Tf/dt) + 1);
 927             output.push_front(alpha * alpha * input[0] +
 928                               2 * (1 - alpha) * output[0] -
 929                               (1 - alpha) * (1 - alpha) * output[1]);
 930         }
 931         else if (filterType == movingAverage)
 932         {
 933             output.push_front(output[0] +
 934                               (input[0] - input.back()) / samples);
 935         }
 936         else if (filterType == noiseSpike)
 937         {
 938             double maxChange = rateOfChange * dt;
 939
 940             if ((output[0] - input[0]) > maxChange)
 941             {
 942                 output.push_front(output[0] - maxChange);
 943             }
 944             else if ((output[0] - input[0]) < -maxChange)
 945             {
 946                 output.push_front(output[0] + maxChange);
 947             }
 948             else if (fabs(input[0] - output[0]) <= maxChange)
 949             {
 950                 output.push_front(input[0]);
 951             }
 952         }
 953         else if (filterType == gain)
 954         {
 955             if (gain_prop != NULL) {
 956                 gainFactor = gain_prop->getDoubleValue();
 957             }
 958             output[0] = gainFactor * input[0];
 959         }
 960         else if (filterType == reciprocal)
 961         {
 962             if (gain_prop != NULL) {
 963                 gainFactor = gain_prop->getDoubleValue();
 964             }
 965             if (input[0] != 0.0) {
 966                 output[0] = gainFactor / input[0];
 967             }
 968         }
 969
 970         if (output[0] < output_min_clamp) {
 971             output[0] = output_min_clamp;
 972         }
 973         else if (output[0] > output_max_clamp) {
 974             output[0] = output_max_clamp;
 975         }
 976
 977         unsigned int i;
 978         for ( i = 0; i < output_list.size(); ++i ) {
 979             output_list[i]->setDoubleValue( output[0] );
 980         }
 981         output.resize(2);
 982
 983         if (debug)
 984         {
 985             cout << "input:" << input[0]
 986                  << "\toutput:" << output[0] << endl;
 987         }
 988     }
 989 }
 990
 991
 992 FGXMLAutopilot::FGXMLAutopilot() {
 993 }
 994
 995
 996 FGXMLAutopilot::~FGXMLAutopilot() {
 997 }
 998
 999
1000 void FGXMLAutopilot::init() {
1001     config_props = fgGetNode( "/autopilot/new-config", true );
1002
1003     SGPropertyNode *path_n = fgGetNode("/sim/systems/autopilot/path");
1004
1005     if ( path_n ) {
1006         SGPath config( globals->get_fg_root() );
1007         config.append( path_n->getStringValue() );
1008
1009         SG_LOG( SG_ALL, SG_INFO, "Reading autopilot configuration from "
1010                 << config.str() );
1011         try {
1012             readProperties( config.str(), config_props );
1013
1014             if ( ! build() ) {
1015                 SG_LOG( SG_ALL, SG_ALERT,
1016                         "Detected an internal inconsistency in the autopilot");
1017                 SG_LOG( SG_ALL, SG_ALERT,
1018                         " configuration.  See earlier errors for" );
1019                 SG_LOG( SG_ALL, SG_ALERT,
1020                         " details.");
1021                 exit(-1);
1022             }
1023         } catch (const sg_exception& exc) {
1024             SG_LOG( SG_ALL, SG_ALERT, "Failed to load autopilot configuration: "
1025                     << config.str() );
1026         }
1027
1028     } else {
1029         SG_LOG( SG_ALL, SG_WARN,
1030                 "No autopilot configuration specified for this model!");
1031     }
1032 }
1033
1034
1035 void FGXMLAutopilot::reinit() {
1036     components.clear();
1037     init();
1038 }
1039
1040
1041 void FGXMLAutopilot::bind() {
1042 }
1043
1044 void FGXMLAutopilot::unbind() {
1045 }
1046
1047 bool FGXMLAutopilot::build() {
1048     SGPropertyNode *node;
1049     int i;
1050
1051     int count = config_props->nChildren();
1052     for ( i = 0; i < count; ++i ) {
1053         node = config_props->getChild(i);
1054         string name = node->getName();
1055         // cout << name << endl;
1056         if ( name == "pid-controller" ) {
1057             components.push_back( new FGPIDController( node ) );
1058         } else if ( name == "pi-simple-controller" ) {
1059             components.push_back( new FGPISimpleController( node ) );
1060         } else if ( name == "predict-simple" ) {
1061             components.push_back( new FGPredictor( node ) );
1062         } else if ( name == "filter" ) {
1063             components.push_back( new FGDigitalFilter( node ) );
1064         } else {
1065             SG_LOG( SG_ALL, SG_ALERT, "Unknown top level section: "
1066                     << name );
1067             return false;
1068         }
1069     }
1070
1071     return true;
1072 }
1073
1074
1075 /*
1076  * Update helper values
1077  */
1078 static void update_helper( double dt ) {
1079     // Estimate speed in 5,10 seconds
1080     static SGPropertyNode_ptr vel = fgGetNode( "/velocities/airspeed-kt", true );
1081     static SGPropertyNode_ptr lookahead5
1082         = fgGetNode( "/autopilot/internal/lookahead-5-sec-airspeed-kt", true );
1083     static SGPropertyNode_ptr lookahead10
1084         = fgGetNode( "/autopilot/internal/lookahead-10-sec-airspeed-kt", true );
1085
1086     static double average = 0.0; // average/filtered prediction
1087     static double v_last = 0.0;  // last velocity
1088
1089     double v = vel->getDoubleValue();
1090     double a = 0.0;
1091     if ( dt > 0.0 ) {
1092         a = (v - v_last) / dt;
1093
1094         if ( dt < 1.0 ) {
1095             average = (1.0 - dt) * average + dt * a;
1096         } else {
1097             average = a;
1098         }
1099
1100         lookahead5->setDoubleValue( v + average * 5.0 );
1101         lookahead10->setDoubleValue( v + average * 10.0 );
1102         v_last = v;
1103     }
1104
1105     // Calculate heading bug error normalized to +/- 180.0 (based on
1106     // DG indicated heading)
1107     static SGPropertyNode_ptr bug
1108         = fgGetNode( "/autopilot/settings/heading-bug-deg", true );
1109     static SGPropertyNode_ptr ind_hdg
1110         = fgGetNode( "/instrumentation/heading-indicator/indicated-heading-deg",
1111                      true );
1112     static SGPropertyNode_ptr ind_bug_error
1113         = fgGetNode( "/autopilot/internal/heading-bug-error-deg", true );
1114
1115     double diff = bug->getDoubleValue() - ind_hdg->getDoubleValue();
1116     if ( diff < -180.0 ) { diff += 360.0; }
1117     if ( diff > 180.0 ) { diff -= 360.0; }
1118     ind_bug_error->setDoubleValue( diff );
1119
1120     // Calculate heading bug error normalized to +/- 180.0 (based on
1121     // actual/nodrift magnetic-heading, i.e. a DG slaved to magnetic
1122     // compass.)
1123     static SGPropertyNode_ptr mag_hdg
1124         = fgGetNode( "/orientation/heading-magnetic-deg", true );
1125     static SGPropertyNode_ptr fdm_bug_error
1126         = fgGetNode( "/autopilot/internal/fdm-heading-bug-error-deg", true );
1127
1128     diff = bug->getDoubleValue() - mag_hdg->getDoubleValue();
1129     if ( diff < -180.0 ) { diff += 360.0; }
1130     if ( diff > 180.0 ) { diff -= 360.0; }
1131     fdm_bug_error->setDoubleValue( diff );
1132
1133     // Calculate true heading error normalized to +/- 180.0
1134     static SGPropertyNode_ptr target_true
1135         = fgGetNode( "/autopilot/settings/true-heading-deg", true );
1136     static SGPropertyNode_ptr true_hdg
1137         = fgGetNode( "/orientation/heading-deg", true );
1138     static SGPropertyNode_ptr true_track
1139         = fgGetNode( "/instrumentation/gps/indicated-track-true-deg", true );
1140     static SGPropertyNode_ptr true_error
1141         = fgGetNode( "/autopilot/internal/true-heading-error-deg", true );
1142
1143     diff = target_true->getDoubleValue() - true_hdg->getDoubleValue();
1144     if ( diff < -180.0 ) { diff += 360.0; }
1145     if ( diff > 180.0 ) { diff -= 360.0; }
1146     true_error->setDoubleValue( diff );
1147
1148     // Calculate nav1 target heading error normalized to +/- 180.0
1149     static SGPropertyNode_ptr target_nav1
1150         = fgGetNode( "/instrumentation/nav[0]/radials/target-auto-hdg-deg", true );
1151     static SGPropertyNode_ptr true_nav1
1152         = fgGetNode( "/autopilot/internal/nav1-heading-error-deg", true );
1153     static SGPropertyNode_ptr true_track_nav1
1154         = fgGetNode( "/autopilot/internal/nav1-track-error-deg", true );
1155
1156     diff = target_nav1->getDoubleValue() - true_hdg->getDoubleValue();
1157     if ( diff < -180.0 ) { diff += 360.0; }
1158     if ( diff > 180.0 ) { diff -= 360.0; }
1159     true_nav1->setDoubleValue( diff );
1160
1161     diff = target_nav1->getDoubleValue() - true_track->getDoubleValue();
1162     if ( diff < -180.0 ) { diff += 360.0; }
1163     if ( diff > 180.0 ) { diff -= 360.0; }
1164     true_track_nav1->setDoubleValue( diff );
1165
1166     // Calculate nav1 selected course error normalized to +/- 180.0
1167     // (based on DG indicated heading)
1168     static SGPropertyNode_ptr nav1_course_error
1169         = fgGetNode( "/autopilot/internal/nav1-course-error", true );
1170     static SGPropertyNode_ptr nav1_selected_course
1171         = fgGetNode( "/instrumentation/nav[0]/radials/selected-deg", true );
1172
1173     diff = nav1_selected_course->getDoubleValue() - ind_hdg->getDoubleValue();
1174 //    if ( diff < -180.0 ) { diff += 360.0; }
1175 //    if ( diff > 180.0 ) { diff -= 360.0; }
1176     SG_NORMALIZE_RANGE( diff, -180.0, 180.0 );
1177     nav1_course_error->setDoubleValue( diff );
1178
1179     // Calculate vertical speed in fpm
1180     static SGPropertyNode_ptr vs_fps
1181         = fgGetNode( "/velocities/vertical-speed-fps", true );
1182     static SGPropertyNode_ptr vs_fpm
1183         = fgGetNode( "/autopilot/internal/vert-speed-fpm", true );
1184
1185     vs_fpm->setDoubleValue( vs_fps->getDoubleValue() * 60.0 );
1186
1187
1188     // Calculate static port pressure rate in [inhg/s].
1189     // Used to determine vertical speed.
1190     static SGPropertyNode_ptr static_pressure
1191         = fgGetNode( "/systems/static[0]/pressure-inhg", true );
1192     static SGPropertyNode_ptr pressure_rate
1193         = fgGetNode( "/autopilot/internal/pressure-rate", true );
1194
1195     static double last_static_pressure = 0.0;
1196
1197     if ( dt > 0.0 ) {
1198         double current_static_pressure = static_pressure->getDoubleValue();
1199
1200         double current_pressure_rate =
1201             ( current_static_pressure - last_static_pressure ) / dt;
1202
1203         pressure_rate->setDoubleValue(current_pressure_rate);
1204
1205         last_static_pressure = current_static_pressure;
1206     }
1207
1208 }
1209
1210
1211 /*
1212  * Update the list of autopilot components
1213  */
1214
1215 void FGXMLAutopilot::update( double dt ) {
1216     update_helper( dt );
1217
1218     unsigned int i;
1219     for ( i = 0; i < components.size(); ++i ) {
1220         components[i]->update( dt );
1221     }
1222 }
1223