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  - curt@flightgear.org
   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., 675 Mass Ave, Cambridge, MA 02139, USA.
  20 //
  21 // $Id$
  22
  23
  24 #include <simgear/structure/exception.hxx>
  25 #include <simgear/misc/sg_path.hxx>
  26
  27 #include <Main/fg_props.hxx>
  28 #include <Main/globals.hxx>
  29
  30 #include "xmlauto.hxx"
  31
  32
  33 FGPIDController::FGPIDController( SGPropertyNode *node, bool old ):
  34     proportional( false ),
  35     factor( 0.0 ),
  36     offset_prop( NULL ),
  37     offset_value( 0.0 ),
  38     integral( false ),
  39     gain( 0.0 ),
  40     int_sum( 0.0 ),
  41     one_eighty( false ),
  42     clamp( false ),
  43     debug( false ),
  44     y_n( 0.0 ),
  45     r_n( 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 {
  59     int i;
  60     for ( i = 0; i < node->nChildren(); ++i ) {
  61         SGPropertyNode *child = node->getChild(i);
  62         string cname = child->getName();
  63         string cval = child->getStringValue();
  64         if ( cname == "name" ) {
  65             name = cval;
  66         } else if ( cname == "enable" ) {
  67             // cout << "parsing enable" << endl;
  68             SGPropertyNode *prop = child->getChild( "prop" );
  69             if ( prop != NULL ) {
  70                 // cout << "prop = " << prop->getStringValue() << endl;
  71                 enable_prop = fgGetNode( prop->getStringValue(), true );
  72             } else {
  73                 // cout << "no prop child" << endl;
  74             }
  75             SGPropertyNode *val = child->getChild( "value" );
  76             if ( val != NULL ) {
  77                 enable_value = val->getStringValue();
  78             }
  79         } else if ( cname == "debug" ) {
  80             debug = child->getBoolValue();
  81         } else if ( cname == "input" ) {
  82             SGPropertyNode *prop = child->getChild( "prop" );
  83             if ( prop != NULL ) {
  84                 input_prop = fgGetNode( prop->getStringValue(), true );
  85             }
  86         } else if ( cname == "reference" ) {
  87             SGPropertyNode *prop = child->getChild( "prop" );
  88             if ( prop != NULL ) {
  89                 r_n_prop = fgGetNode( prop->getStringValue(), true );
  90             } else {
  91                 prop = child->getChild( "value" );
  92                 if ( prop != NULL ) {
  93                     r_n_value = prop->getDoubleValue();
  94                 }
  95             }
  96         } else if ( cname == "output" ) {
  97             int i = 0;
  98             SGPropertyNode *prop;
  99             while ( (prop = child->getChild("prop", i)) != NULL ) {
 100                 SGPropertyNode *tmp = fgGetNode( prop->getStringValue(), true );
 101                 output_list.push_back( tmp );
 102                 i++;
 103             }
 104             prop = child->getChild( "clamp" );
 105             if ( prop != NULL ) {
 106                 clamp = true;
 107
 108                 SGPropertyNode *tmp;
 109
 110                 tmp = prop->getChild( "min" );
 111                 if ( tmp != NULL ) {
 112                     u_min = tmp->getDoubleValue();
 113                     // cout << "min = " << u_min << endl;
 114                 }
 115
 116                 tmp = prop->getChild( "max" );
 117                 if ( tmp != NULL ) {
 118                     u_max = tmp->getDoubleValue();
 119                     // cout << "max = " << u_max << endl;
 120                 }
 121             }
 122         } else if ( cname == "proportional" ) {
 123             proportional = true;
 124
 125             SGPropertyNode *prop;
 126
 127             prop = child->getChild( "pre" );
 128             if ( prop != NULL ) {
 129                 prop = prop->getChild( "one-eighty" );
 130                 if ( prop != NULL && prop->getBoolValue() ) {
 131                     one_eighty = true;
 132                 }
 133             }
 134
 135             prop = child->getChild( "factor" );
 136             if ( prop != NULL ) {
 137                 factor = prop->getDoubleValue();
 138             }
 139
 140             prop = child->getChild( "offset" );
 141             if ( prop != NULL ) {
 142                 SGPropertyNode *sub = prop->getChild( "prop" );
 143                 if ( sub != NULL ) {
 144                     offset_prop = fgGetNode( sub->getStringValue(), true );
 145                     // cout << "offset prop = " << sub->getStringValue() << endl;
 146                 } else {
 147                     sub = prop->getChild( "value" );
 148                     if ( sub != NULL ) {
 149                         offset_value = sub->getDoubleValue();
 150                         // cout << "offset value = " << offset_value << endl;
 151                     }
 152                 }
 153             }
 154         } else if ( cname == "integral" ) {
 155             integral = true;
 156
 157             SGPropertyNode *prop;
 158             prop = child->getChild( "gain" );
 159             if ( prop != NULL ) {
 160                 gain = prop->getDoubleValue();
 161             }
 162         } else {
 163             SG_LOG( SG_AUTOPILOT, SG_WARN, "Error in autopilot config logic" );
 164         }
 165     }
 166 }
 167
 168
 169 FGPIDController::FGPIDController( SGPropertyNode *node ):
 170     proportional( false ),
 171     factor( 0.0 ),
 172     offset_prop( NULL ),
 173     offset_value( 0.0 ),
 174     integral( false ),
 175     gain( 0.0 ),
 176     int_sum( 0.0 ),
 177     one_eighty( false ),
 178     clamp( false ),
 179     debug( false ),
 180     y_n( 0.0 ),
 181     r_n( 0.0 ),
 182     Kp( 0.0 ),
 183     alpha( 0.1 ),
 184     beta( 1.0 ),
 185     gamma( 0.0 ),
 186     Ti( 0.0 ),
 187     Td( 0.0 ),
 188     u_min( 0.0 ),
 189     u_max( 0.0 ),
 190     ep_n_1( 0.0 ),
 191     edf_n_1( 0.0 ),
 192     edf_n_2( 0.0 ),
 193     u_n_1( 0.0 )
 194 {
 195     int i;
 196     for ( i = 0; i < node->nChildren(); ++i ) {
 197         SGPropertyNode *child = node->getChild(i);
 198         string cname = child->getName();
 199         string cval = child->getStringValue();
 200         if ( cname == "name" ) {
 201             name = cval;
 202         } else if ( cname == "debug" ) {
 203             debug = child->getBoolValue();
 204         } else if ( cname == "enable" ) {
 205             // cout << "parsing enable" << endl;
 206             SGPropertyNode *prop = child->getChild( "prop" );
 207             if ( prop != NULL ) {
 208                 // cout << "prop = " << prop->getStringValue() << endl;
 209                 enable_prop = fgGetNode( prop->getStringValue(), true );
 210             } else {
 211                 // cout << "no prop child" << endl;
 212             }
 213             SGPropertyNode *val = child->getChild( "value" );
 214             if ( val != NULL ) {
 215                 enable_value = val->getStringValue();
 216             }
 217         } else if ( cname == "input" ) {
 218             SGPropertyNode *prop = child->getChild( "prop" );
 219             if ( prop != NULL ) {
 220                 input_prop = fgGetNode( prop->getStringValue(), true );
 221             }
 222         } else if ( cname == "reference" ) {
 223             SGPropertyNode *prop = child->getChild( "prop" );
 224             if ( prop != NULL ) {
 225                 r_n_prop = fgGetNode( prop->getStringValue(), true );
 226             } else {
 227                 prop = child->getChild( "value" );
 228                 if ( prop != NULL ) {
 229                     r_n = prop->getDoubleValue();
 230                 }
 231             }
 232         } else if ( cname == "output" ) {
 233             int i = 0;
 234             SGPropertyNode *prop;
 235             while ( (prop = child->getChild("prop", i)) != NULL ) {
 236                 SGPropertyNode *tmp = fgGetNode( prop->getStringValue(), true );
 237                 output_list.push_back( tmp );
 238                 i++;
 239             }
 240         } else if ( cname == "config" ) {
 241             SGPropertyNode *prop;
 242
 243             prop = child->getChild( "Kp" );
 244             if ( prop != NULL ) {
 245                 Kp = prop->getDoubleValue();
 246             }
 247
 248             prop = child->getChild( "beta" );
 249             if ( prop != NULL ) {
 250                 beta = prop->getDoubleValue();
 251             }
 252
 253             prop = child->getChild( "alpha" );
 254             if ( prop != NULL ) {
 255                 alpha = prop->getDoubleValue();
 256             }
 257
 258             prop = child->getChild( "gamma" );
 259             if ( prop != NULL ) {
 260                 gamma = prop->getDoubleValue();
 261             }
 262
 263             prop = child->getChild( "Ti" );
 264             if ( prop != NULL ) {
 265                 Ti = prop->getDoubleValue();
 266             }
 267
 268             prop = child->getChild( "Td" );
 269             if ( prop != NULL ) {
 270                 Td = prop->getDoubleValue();
 271             }
 272
 273             prop = child->getChild( "u_min" );
 274             if ( prop != NULL ) {
 275                 u_min = prop->getDoubleValue();
 276             }
 277
 278             prop = child->getChild( "u_max" );
 279             if ( prop != NULL ) {
 280                 u_max = prop->getDoubleValue();
 281             }
 282         } else {
 283             SG_LOG( SG_AUTOPILOT, SG_WARN, "Error in autopilot config logic" );
 284             if ( name.length() ) {
 285                 SG_LOG( SG_AUTOPILOT, SG_WARN, "Section = " << name );
 286             }
 287         }
 288     }
 289 }
 290
 291
 292 void FGPIDController::update_old( double dt ) {
 293     if (enable_prop != NULL && enable_prop->getStringValue() == enable_value) {
 294         if ( !enabled ) {
 295             // we have just been enabled, zero out int_sum
 296             int_sum = 0.0;
 297         }
 298         enabled = true;
 299     } else {
 300         enabled = false;
 301     }
 302
 303     if ( enabled ) {
 304         if ( debug ) cout << "Updating " << name << endl;
 305         double input = 0.0;
 306         if ( input_prop != NULL ) {
 307             input = input_prop->getDoubleValue();
 308         }
 309
 310         double r_n = 0.0;
 311         if ( r_n_prop != NULL ) {
 312             r_n = r_n_prop->getDoubleValue();
 313         } else {
 314             r_n = r_n_value;
 315         }
 316
 317         double error = r_n - input;
 318         if ( one_eighty ) {
 319             while ( error < -180.0 ) { error += 360.0; }
 320             while ( error > 180.0 ) { error -= 360.0; }
 321         }
 322         if ( debug ) cout << "input = " << input
 323                           << " reference = " << r_n
 324                           << " error = " << error
 325                           << endl;
 326
 327         double prop_comp = 0.0;
 328         double offset = 0.0;
 329         if ( offset_prop != NULL ) {
 330             offset = offset_prop->getDoubleValue();
 331             if ( debug ) cout << "offset = " << offset << endl;
 332         } else {
 333             offset = offset_value;
 334         }
 335
 336         if ( proportional ) {
 337             prop_comp = error * factor + offset;
 338         }
 339
 340         if ( integral ) {
 341             int_sum += error * gain * dt;
 342         } else {
 343             int_sum = 0.0;
 344         }
 345
 346         if ( debug ) cout << "prop_comp = " << prop_comp
 347                           << " int_sum = " << int_sum << endl;
 348
 349         double output = prop_comp + int_sum;
 350
 351         if ( clamp ) {
 352             if ( output < u_min ) {
 353                 output = u_min;
 354             }
 355             if ( output > u_max ) {
 356                 output = u_max;
 357             }
 358         }
 359         if ( debug ) cout << "output = " << output << endl;
 360
 361         unsigned int i;
 362         for ( i = 0; i < output_list.size(); ++i ) {
 363             output_list[i]->setDoubleValue( output );
 364         }
 365     }
 366 }
 367
 368
 369 /*
 370  * Roy Vegard Ovesen:
 371  *
 372  * Ok! Here is the PID controller algorithm that I would like to see
 373  * implemented:
 374  *
 375  *   delta_u_n = Kp * [ (ep_n - ep_n-1) + ((Ts/Ti)*e_n)
 376  *               + (Td/Ts)*(edf_n - 2*edf_n-1 + edf_n-2) ]
 377  *
 378  *   u_n = u_n-1 + delta_u_n
 379  *
 380  * where:
 381  *
 382  * delta_u : The incremental output
 383  * Kp      : Proportional gain
 384  * ep      : Proportional error with reference weighing
 385  *           ep = beta * r - y
 386  *           where:
 387  *           beta : Weighing factor
 388  *           r    : Reference (setpoint)
 389  *           y    : Process value, measured
 390  * e       : Error
 391  *           e = r - y
 392  * Ts      : Sampling interval
 393  * Ti      : Integrator time
 394  * Td      : Derivator time
 395  * edf     : Derivate error with reference weighing and filtering
 396  *           edf_n = edf_n-1 / ((Ts/Tf) + 1) + ed_n * (Ts/Tf) / ((Ts/Tf) + 1)
 397  *           where:
 398  *           Tf : Filter time
 399  *           Tf = alpha * Td , where alpha usually is set to 0.1
 400  *           ed : Unfiltered derivate error with reference weighing
 401  *             ed = gamma * r - y
 402  *             where:
 403  *             gamma : Weighing factor
 404  *
 405  * u       : absolute output
 406  *
 407  * Index n means the n'th value.
 408  *
 409  *
 410  * Inputs:
 411  * enabled ,
 412  * y_n , r_n , beta=1 , gamma=0 , alpha=0.1 ,
 413  * Kp , Ti , Td , Ts (is the sampling time available?)
 414  * u_min , u_max
 415  *
 416  * Output:
 417  * u_n
 418  */
 419
 420 void FGPIDController::update( double dt ) {
 421     double ep_n;         // proportional error with reference weighing
 422     double e_n;          // error
 423     double ed_n;         // derivative error
 424     double edf_n;        // derivative error filter
 425     double Tf;           // filter time
 426     double delta_u_n;    // incremental output
 427     double u_n;          // absolute output
 428     double Ts = dt;      // Sampling interval (sec)
 429
 430     if ( Ts <= 0.0 ) {
 431         // do nothing if time step is not positive (i.e. no time has
 432         // elapsed)
 433         return;
 434     }
 435
 436     if (enable_prop != NULL && enable_prop->getStringValue() == enable_value) {
 437         enabled = true;
 438     } else {
 439         enabled = false;
 440     }
 441
 442     if ( enabled ) {
 443         if ( debug ) cout << "Updating " << name << endl;
 444
 445         double y_n = 0.0;
 446         if ( input_prop != NULL ) {
 447             y_n = input_prop->getDoubleValue();
 448         }
 449
 450         double r_n = 0.0;
 451         if ( r_n_prop != NULL ) {
 452             r_n = r_n_prop->getDoubleValue();
 453         } else {
 454             r_n = r_n_value;
 455         }
 456
 457         if ( debug ) cout << "  input = " << y_n << " ref = " << r_n << endl;
 458
 459         // Calculates proportional error:
 460         ep_n = beta * r_n - y_n;
 461         if ( debug ) cout << "  ep_n = " << ep_n;
 462         if ( debug ) cout << "  ep_n_1 = " << ep_n_1;
 463
 464         // Calculates error:
 465         e_n = r_n - y_n;
 466         if ( debug ) cout << " e_n = " << e_n;
 467
 468         // Calculates derivate error:
 469         ed_n = gamma * r_n - y_n;
 470         if ( debug ) cout << " ed_n = " << ed_n;
 471
 472         // Calculates filter time:
 473         Tf = alpha * Td;
 474         if ( debug ) cout << " Tf = " << Tf;
 475
 476         // Filters the derivate error:
 477         edf_n = edf_n_1 / (Ts/Tf + 1)
 478             + ed_n * (Ts/Tf) / (Ts/Tf + 1);
 479         if ( debug ) cout << " edf_n = " << edf_n;
 480
 481         // Calculates the incremental output:
 482         delta_u_n = Kp * ( (ep_n - ep_n_1)
 483                            + ((Ts/Ti) * e_n)
 484                            + ((Td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
 485         if ( debug ) cout << " delta_u_n = " << delta_u_n << endl;
 486
 487         // Integrator anti-windup logic:
 488         if ( delta_u_n > (u_max - u_n_1) ) {
 489             delta_u_n = 0;
 490         } else if ( delta_u_n < (u_min - u_n_1) ) {
 491             delta_u_n = 0;
 492         }
 493
 494         // Calculates absolute output:
 495         u_n = u_n_1 + delta_u_n;
 496         if ( debug ) cout << "  output = " << u_n << endl;
 497
 498         // Updates indexed values;
 499         u_n_1   = u_n;
 500         ep_n_1  = ep_n;
 501         edf_n_2 = edf_n_1;
 502         edf_n_1 = edf_n;
 503
 504         unsigned int i;
 505         for ( i = 0; i < output_list.size(); ++i ) {
 506             output_list[i]->setDoubleValue( u_n );
 507         }
 508     } else if ( !enabled ) {
 509         u_n   = 0.0;
 510         ep_n  = 0.0;
 511         edf_n = 0.0;
 512         // Updates indexed values;
 513         u_n_1   = u_n;
 514         ep_n_1  = ep_n;
 515         edf_n_2 = edf_n_1;
 516         edf_n_1 = edf_n;
 517     }
 518 }
 519
 520
 521 FGXMLAutopilot::FGXMLAutopilot() {
 522 }
 523
 524
 525 FGXMLAutopilot::~FGXMLAutopilot() {
 526 }
 527
 528
 529 void FGXMLAutopilot::init() {
 530     config_props = fgGetNode( "/autopilot/new-config", true );
 531
 532     SGPropertyNode *path_n = fgGetNode("/sim/systems/autopilot/path");
 533
 534     if ( path_n ) {
 535         SGPath config( globals->get_fg_root() );
 536         config.append( path_n->getStringValue() );
 537
 538         SG_LOG( SG_ALL, SG_INFO, "Reading autopilot configuration from "
 539                 << config.str() );
 540         try {
 541             readProperties( config.str(), config_props );
 542
 543             if ( ! build() ) {
 544                 SG_LOG( SG_ALL, SG_ALERT,
 545                         "Detected an internal inconsistancy in the autopilot");
 546                 SG_LOG( SG_ALL, SG_ALERT,
 547                         " configuration.  See earlier errors for" );
 548                 SG_LOG( SG_ALL, SG_ALERT,
 549                         " details.");
 550                 exit(-1);
 551             }
 552         } catch (const sg_exception& exc) {
 553             SG_LOG( SG_ALL, SG_ALERT, "Failed to load autopilot configuration: "
 554                     << config.str() );
 555         }
 556
 557     } else {
 558         SG_LOG( SG_ALL, SG_WARN,
 559                 "No autopilot configuration specified for this model!");
 560     }
 561 }
 562
 563
 564 void FGXMLAutopilot::reinit() {
 565     components.clear();
 566     build();
 567 }
 568
 569
 570 void FGXMLAutopilot::bind() {
 571 }
 572
 573 void FGXMLAutopilot::unbind() {
 574 }
 575
 576 bool FGXMLAutopilot::build() {
 577     SGPropertyNode *node;
 578     int i;
 579
 580     int count = config_props->nChildren();
 581     for ( i = 0; i < count; ++i ) {
 582         node = config_props->getChild(i);
 583         string name = node->getName();
 584         // cout << name << endl;
 585         if ( name == "pid-controller" ) {
 586             FGXMLAutoComponent *c = new FGPIDController( node );
 587             components.push_back( c );
 588         } else {
 589             SG_LOG( SG_ALL, SG_ALERT, "Unknown top level section: "
 590                     << name );
 591             return false;
 592         }
 593     }
 594
 595     return true;
 596 }
 597
 598
 599 /*
 600  * Update helper values
 601  */
 602 static void update_helper( double dt ) {
 603     // Estimate speed in 5,10 seconds
 604     static SGPropertyNode *vel = fgGetNode( "/velocities/airspeed-kt", true );
 605     static SGPropertyNode *lookahead5
 606         = fgGetNode( "/autopilot/internal/lookahead-5-sec-airspeed-kt", true );
 607     static SGPropertyNode *lookahead10
 608         = fgGetNode( "/autopilot/internal/lookahead-10-sec-airspeed-kt", true );
 609
 610     static double average = 0.0; // average/filtered prediction
 611     static double v_last = 0.0;  // last velocity
 612
 613     double v = vel->getDoubleValue();
 614     double a = 0.0;
 615     if ( dt > 0.0 ) {
 616         a = (v - v_last) / dt;
 617
 618         if ( dt < 1.0 ) {
 619             average = (1.0 - dt) * average + dt * a;
 620         } else {
 621             average = a;
 622         }
 623
 624         lookahead5->setDoubleValue( v + average * 5.0 );
 625         lookahead10->setDoubleValue( v + average * 10.0 );
 626         v_last = v;
 627     }
 628
 629     // Calculate heading bug error normalized to +/- 180.0
 630     static SGPropertyNode *bug
 631         = fgGetNode( "/autopilot/settings/heading-bug-deg", true );
 632     static SGPropertyNode *ind_hdg
 633         = fgGetNode( "/instrumentation/heading-indicator/indicated-heading-deg",
 634                      true );
 635     static SGPropertyNode *bug_error
 636         = fgGetNode( "/autopilot/internal/heading-bug-error-deg", true );
 637
 638     double diff = bug->getDoubleValue() - ind_hdg->getDoubleValue();
 639     if ( diff < -180.0 ) { diff += 360.0; }
 640     if ( diff > 180.0 ) { diff -= 360.0; }
 641     bug_error->setDoubleValue( diff );
 642
 643     // Calculate true heading error normalized to +/- 180.0
 644     static SGPropertyNode *target_true
 645         = fgGetNode( "/autopilot/settings/true-heading-deg", true );
 646     static SGPropertyNode *true_hdg
 647         = fgGetNode( "/orientation/heading-deg", true );
 648     static SGPropertyNode *true_error
 649         = fgGetNode( "/autopilot/internal/true-heading-error-deg", true );
 650
 651     diff = target_true->getDoubleValue() - true_hdg->getDoubleValue();
 652     if ( diff < -180.0 ) { diff += 360.0; }
 653     if ( diff > 180.0 ) { diff -= 360.0; }
 654     true_error->setDoubleValue( diff );
 655
 656     // Calculate nav1 target heading error normalized to +/- 180.0
 657     static SGPropertyNode *target_nav1
 658         = fgGetNode( "/radios/nav[0]/radials/target-auto-hdg-deg", true );
 659     static SGPropertyNode *true_nav1
 660         = fgGetNode( "/autopilot/internal/nav1-heading-error-deg", true );
 661
 662     diff = target_nav1->getDoubleValue() - true_hdg->getDoubleValue();
 663     if ( diff < -180.0 ) { diff += 360.0; }
 664     if ( diff > 180.0 ) { diff -= 360.0; }
 665     true_nav1->setDoubleValue( diff );
 666 }
 667
 668
 669 /*
 670  * Update the list of autopilot components
 671  */
 672
 673 void FGXMLAutopilot::update( double dt ) {
 674     update_helper( dt );
 675
 676     unsigned int i;
 677     for ( i = 0; i < components.size(); ++i ) {
 678         components[i]->update( dt );
 679     }
 680 }