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