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         }
 285     }
 286 }
 287
 288
 289 void FGPIDController::update_old( double dt ) {
 290     if (enable_prop != NULL && enable_prop->getStringValue() == enable_value) {
 291         if ( !enabled ) {
 292             // we have just been enabled, zero out int_sum
 293             int_sum = 0.0;
 294         }
 295         enabled = true;
 296     } else {
 297         enabled = false;
 298     }
 299
 300     if ( enabled ) {
 301         if ( debug ) cout << "Updating " << name << endl;
 302         double input = 0.0;
 303         if ( input_prop != NULL ) {
 304             input = input_prop->getDoubleValue();
 305         }
 306
 307         double r_n = 0.0;
 308         if ( r_n_prop != NULL ) {
 309             r_n = r_n_prop->getDoubleValue();
 310         } else {
 311             r_n = r_n_value;
 312         }
 313
 314         double error = r_n - input;
 315         if ( one_eighty ) {
 316             while ( error < -180.0 ) { error += 360.0; }
 317             while ( error > 180.0 ) { error -= 360.0; }
 318         }
 319         if ( debug ) cout << "input = " << input
 320                           << " reference = " << r_n
 321                           << " error = " << error
 322                           << endl;
 323
 324         double prop_comp = 0.0;
 325         double offset = 0.0;
 326         if ( offset_prop != NULL ) {
 327             offset = offset_prop->getDoubleValue();
 328             if ( debug ) cout << "offset = " << offset << endl;
 329         } else {
 330             offset = offset_value;
 331         }
 332
 333         if ( proportional ) {
 334             prop_comp = error * factor + offset;
 335         }
 336
 337         if ( integral ) {
 338             int_sum += error * gain * dt;
 339         } else {
 340             int_sum = 0.0;
 341         }
 342
 343         if ( debug ) cout << "prop_comp = " << prop_comp
 344                           << " int_sum = " << int_sum << endl;
 345
 346         double output = prop_comp + int_sum;
 347
 348         if ( clamp ) {
 349             if ( output < u_min ) {
 350                 output = u_min;
 351             }
 352             if ( output > u_max ) {
 353                 output = u_max;
 354             }
 355         }
 356         if ( debug ) cout << "output = " << output << endl;
 357
 358         unsigned int i;
 359         for ( i = 0; i < output_list.size(); ++i ) {
 360             output_list[i]->setDoubleValue( output );
 361         }
 362     }
 363 }
 364
 365
 366 /*
 367  * Roy Vegard Ovesen:
 368  *
 369  * Ok! Here is the PID controller algorithm that I would like to see
 370  * implemented:
 371  *
 372  *   delta_u_n = Kp * [ (ep_n - ep_n-1) + ((Ts/Ti)*e_n)
 373  *               + (Td/Ts)*(edf_n - 2*edf_n-1 + edf_n-2) ]
 374  *
 375  *   u_n = u_n-1 + delta_u_n
 376  *
 377  * where:
 378  *
 379  * delta_u : The incremental output
 380  * Kp      : Proportional gain
 381  * ep      : Proportional error with reference weighing
 382  *           ep = beta * r - y
 383  *           where:
 384  *           beta : Weighing factor
 385  *           r    : Reference (setpoint)
 386  *           y    : Process value, measured
 387  * e       : Error
 388  *           e = r - y
 389  * Ts      : Sampling interval
 390  * Ti      : Integrator time
 391  * Td      : Derivator time
 392  * edf     : Derivate error with reference weighing and filtering
 393  *           edf_n = edf_n-1 / ((Ts/Tf) + 1) + ed_n * (Ts/Tf) / ((Ts/Tf) + 1)
 394  *           where:
 395  *           Tf : Filter time
 396  *           Tf = alpha * Td , where alpha usually is set to 0.1
 397  *           ed : Unfiltered derivate error with reference weighing
 398  *             ed = gamma * r - y
 399  *             where:
 400  *             gamma : Weighing factor
 401  *
 402  * u       : absolute output
 403  *
 404  * Index n means the n'th value.
 405  *
 406  *
 407  * Inputs:
 408  * enabled ,
 409  * y_n , r_n , beta=1 , gamma=0 , alpha=0.1 ,
 410  * Kp , Ti , Td , Ts (is the sampling time available?)
 411  * u_min , u_max
 412  *
 413  * Output:
 414  * u_n
 415  */
 416
 417 void FGPIDController::update( double dt ) {
 418     double ep_n;         // proportional error with reference weighing
 419     double e_n;          // error
 420     double ed_n;         // derivative error
 421     double edf_n;        // derivative error filter
 422     double Tf;           // filter time
 423     double delta_u_n;    // incremental output
 424     double u_n;          // absolute output
 425     double Ts = dt;      // Sampling interval (sec)
 426
 427     if ( Ts <= 0.0 ) {
 428         // do nothing if time step is not positive (i.e. no time has
 429         // elapsed)
 430         return;
 431     }
 432
 433     if (enable_prop != NULL && enable_prop->getStringValue() == enable_value) {
 434         enabled = true;
 435     } else {
 436         enabled = false;
 437     }
 438
 439     if ( enabled ) {
 440         if ( debug ) cout << "Updating " << name << endl;
 441
 442         double y_n = 0.0;
 443         if ( input_prop != NULL ) {
 444             y_n = input_prop->getDoubleValue();
 445         }
 446
 447         double r_n = 0.0;
 448         if ( r_n_prop != NULL ) {
 449             r_n = r_n_prop->getDoubleValue();
 450         } else {
 451             r_n = r_n_value;
 452         }
 453
 454         if ( debug ) cout << "  input = " << y_n << " ref = " << r_n << endl;
 455
 456         // Calculates proportional error:
 457         ep_n = beta * r_n - y_n;
 458         if ( debug ) cout << "  ep_n = " << ep_n;
 459         if ( debug ) cout << "  ep_n_1 = " << ep_n_1;
 460
 461         // Calculates error:
 462         e_n = r_n - y_n;
 463         if ( debug ) cout << " e_n = " << e_n;
 464
 465         // Calculates derivate error:
 466         ed_n = gamma * r_n - y_n;
 467         if ( debug ) cout << " ed_n = " << ed_n;
 468
 469         // Calculates filter time:
 470         Tf = alpha * Td;
 471         if ( debug ) cout << " Tf = " << Tf;
 472
 473         // Filters the derivate error:
 474         edf_n = edf_n_1 / (Ts/Tf + 1)
 475             + ed_n * (Ts/Tf) / (Ts/Tf + 1);
 476         if ( debug ) cout << " edf_n = " << edf_n;
 477
 478         // Calculates the incremental output:
 479         delta_u_n = Kp * ( (ep_n - ep_n_1)
 480                            + ((Ts/Ti) * e_n)
 481                            + ((Td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
 482         if ( debug ) cout << " delta_u_n = " << delta_u_n << endl;
 483
 484         // Integrator anti-windup logic:
 485         if ( delta_u_n > (u_max - u_n_1) ) {
 486             delta_u_n = 0;
 487         } else if ( delta_u_n < (u_min - u_n_1) ) {
 488             delta_u_n = 0;
 489         }
 490
 491         // Calculates absolute output:
 492         u_n = u_n_1 + delta_u_n;
 493         if ( debug ) cout << "  output = " << u_n << endl;
 494
 495         // Updates indexed values;
 496         u_n_1   = u_n;
 497         ep_n_1  = ep_n;
 498         edf_n_2 = edf_n_1;
 499         edf_n_1 = edf_n;
 500
 501         unsigned int i;
 502         for ( i = 0; i < output_list.size(); ++i ) {
 503             output_list[i]->setDoubleValue( u_n );
 504         }
 505     } else if ( !enabled ) {
 506         u_n   = 0.0;
 507         ep_n  = 0.0;
 508         edf_n = 0.0;
 509         // Updates indexed values;
 510         u_n_1   = u_n;
 511         ep_n_1  = ep_n;
 512         edf_n_2 = edf_n_1;
 513         edf_n_1 = edf_n;
 514     }
 515 }
 516
 517
 518 FGXMLAutopilot::FGXMLAutopilot() {
 519 }
 520
 521
 522 FGXMLAutopilot::~FGXMLAutopilot() {
 523 }
 524
 525
 526 void FGXMLAutopilot::init() {
 527     config_props = fgGetNode( "/autopilot/new-config", true );
 528
 529     SGPropertyNode *path_n = fgGetNode("/sim/systems/autopilot/path");
 530
 531     if ( path_n ) {
 532         SGPath config( globals->get_fg_root() );
 533         config.append( path_n->getStringValue() );
 534
 535         SG_LOG( SG_ALL, SG_INFO, "Reading autopilot configuration from "
 536                 << config.str() );
 537         try {
 538             readProperties( config.str(), config_props );
 539
 540             if ( ! build() ) {
 541                 SG_LOG( SG_ALL, SG_ALERT,
 542                         "Detected an internal inconsistancy in the autopilot");
 543                 SG_LOG( SG_ALL, SG_ALERT,
 544                         " configuration.  See earlier errors for" );
 545                 SG_LOG( SG_ALL, SG_ALERT,
 546                         " details.");
 547                 exit(-1);
 548             }
 549         } catch (const sg_exception& exc) {
 550             SG_LOG( SG_ALL, SG_ALERT, "Failed to load autopilot configuration: "
 551                     << config.str() );
 552         }
 553
 554     } else {
 555         SG_LOG( SG_ALL, SG_WARN,
 556                 "No autopilot configuration specified for this model!");
 557     }
 558 }
 559
 560
 561 void FGXMLAutopilot::reinit() {
 562     components.clear();
 563     build();
 564 }
 565
 566
 567 void FGXMLAutopilot::bind() {
 568 }
 569
 570 void FGXMLAutopilot::unbind() {
 571 }
 572
 573 bool FGXMLAutopilot::build() {
 574     SGPropertyNode *node;
 575     int i;
 576
 577     int count = config_props->nChildren();
 578     for ( i = 0; i < count; ++i ) {
 579         node = config_props->getChild(i);
 580         string name = node->getName();
 581         // cout << name << endl;
 582         if ( name == "pid-controller" ) {
 583             FGXMLAutoComponent *c = new FGPIDController( node );
 584             components.push_back( c );
 585         } else {
 586             SG_LOG( SG_ALL, SG_ALERT, "Unknown top level section: "
 587                     << name );
 588             return false;
 589         }
 590     }
 591
 592     return true;
 593 }
 594
 595
 596 /*
 597  * Update helper values
 598  */
 599 static void update_helper( double dt ) {
 600     // Estimate speed in 5,10 seconds
 601     static SGPropertyNode *vel = fgGetNode( "/velocities/airspeed-kt", true );
 602     static SGPropertyNode *lookahead5
 603         = fgGetNode( "/autopilot/internal/lookahead-5-sec-airspeed-kt", true );
 604     static SGPropertyNode *lookahead10
 605         = fgGetNode( "/autopilot/internal/lookahead-10-sec-airspeed-kt", true );
 606
 607     static double average = 0.0; // average/filtered prediction
 608     static double v_last = 0.0;  // last velocity
 609
 610     double v = vel->getDoubleValue();
 611     double a = 0.0;
 612     if ( dt > 0.0 ) {
 613         a = (v - v_last) / dt;
 614
 615         if ( dt < 1.0 ) {
 616             average = (1.0 - dt) * average + dt * a;
 617         } else {
 618             average = a;
 619         }
 620
 621         lookahead5->setDoubleValue( v + average * 5.0 );
 622         lookahead10->setDoubleValue( v + average * 10.0 );
 623         v_last = v;
 624     }
 625
 626     // Calculate heading bug error normalized to +/- 180.0
 627     static SGPropertyNode *bug
 628         = fgGetNode( "/autopilot/settings/heading-bug-deg", true );
 629     static SGPropertyNode *ind_hdg
 630         = fgGetNode( "/instrumentation/heading-indicator/indicated-heading-deg",
 631                      true );
 632     static SGPropertyNode *bug_error
 633         = fgGetNode( "/autopilot/internal/heading-bug-error-deg", true );
 634
 635     double diff = bug->getDoubleValue() - ind_hdg->getDoubleValue();
 636     if ( diff < -180.0 ) { diff += 360.0; }
 637     if ( diff > 180.0 ) { diff -= 360.0; }
 638     bug_error->setDoubleValue( diff );
 639
 640     // Calculate true heading error normalized to +/- 180.0
 641     static SGPropertyNode *target_true
 642         = fgGetNode( "/autopilot/settings/true-heading-deg", true );
 643     static SGPropertyNode *true_hdg
 644         = fgGetNode( "/orientation/heading-deg", true );
 645     static SGPropertyNode *true_error
 646         = fgGetNode( "/autopilot/internal/true-heading-error-deg", true );
 647
 648     diff = target_true->getDoubleValue() - true_hdg->getDoubleValue();
 649     if ( diff < -180.0 ) { diff += 360.0; }
 650     if ( diff > 180.0 ) { diff -= 360.0; }
 651     true_error->setDoubleValue( diff );
 652
 653     // Calculate nav1 target heading error normalized to +/- 180.0
 654     static SGPropertyNode *target_nav1
 655         = fgGetNode( "/radios/nav[0]/radials/target-auto-hdg-deg", true );
 656     static SGPropertyNode *true_nav1
 657         = fgGetNode( "/autopilot/internal/nav1-heading-error-deg", true );
 658
 659     diff = target_nav1->getDoubleValue() - true_hdg->getDoubleValue();
 660     if ( diff < -180.0 ) { diff += 360.0; }
 661     if ( diff > 180.0 ) { diff -= 360.0; }
 662     true_nav1->setDoubleValue( diff );
 663 }
 664
 665
 666 /*
 667  * Update the list of autopilot components
 668  */
 669
 670 void FGXMLAutopilot::update( double dt ) {
 671     update_helper( dt );
 672
 673     unsigned int i;
 674     for ( i = 0; i < components.size(); ++i ) {
 675         components[i]->update( dt );
 676     }
 677 }