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