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