src/FDM/JSBSim/models/FGFCS.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Module:       FGFCS.cpp
   4  Author:       Jon Berndt
   5  Date started: 12/12/98
   6  Purpose:      Model the flight controls
   7  Called by:    FDMExec
   8
   9  ------------- Copyright (C) 1999  Jon S. Berndt (jsb@hal-pc.org) -------------
  10
  11  This program is free software; you can redistribute it and/or modify it under
  12  the terms of the GNU Lesser General Public License as published by the Free Software
  13  Foundation; either version 2 of the License, or (at your option) any later
  14  version.
  15
  16  This program is distributed in the hope that it will be useful, but WITHOUT
  17  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  18  FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
  19  details.
  20
  21  You should have received a copy of the GNU Lesser General Public License along with
  22  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  23  Place - Suite 330, Boston, MA  02111-1307, USA.
  24
  25  Further information about the GNU Lesser General Public License can also be found on
  26  the world wide web at http://www.gnu.org.
  27
  28 FUNCTIONAL DESCRIPTION
  29 --------------------------------------------------------------------------------
  30 This class models the flight controls for a specific airplane
  31
  32 HISTORY
  33 --------------------------------------------------------------------------------
  34 12/12/98   JSB   Created
  35
  36 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  37 INCLUDES
  38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  39
  40 #include "FGFCS.h"
  41 #include <FGFDMExec.h>
  42 #include <input_output/FGPropertyManager.h>
  43 #include <fstream>
  44
  45 #include <models/flight_control/FGFilter.h>
  46 #include <models/flight_control/FGDeadBand.h>
  47 #include <models/flight_control/FGGain.h>
  48 #include <models/flight_control/FGPID.h>
  49 #include <models/flight_control/FGGradient.h>
  50 #include <models/flight_control/FGSwitch.h>
  51 #include <models/flight_control/FGSummer.h>
  52 #include <models/flight_control/FGKinemat.h>
  53 #include <models/flight_control/FGFCSFunction.h>
  54 #include <models/flight_control/FGSensor.h>
  55 #include <models/flight_control/FGActuator.h>
  56
  57 namespace JSBSim {
  58
  59 static const char *IdSrc = "$Id$";
  60 static const char *IdHdr = ID_FCS;
  61
  62 #if defined(WIN32) && !defined(__CYGWIN__)
  63 #define snprintf _snprintf
  64 #endif
  65
  66 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  67 CLASS IMPLEMENTATION
  68 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  69
  70 FGFCS::FGFCS(FGFDMExec* fdmex) : FGModel(fdmex)
  71 {
  72   int i;
  73   Name = "FGFCS";
  74
  75   DaCmd = DeCmd = DrCmd = DsCmd = DfCmd = DsbCmd = DspCmd = 0;
  76   PTrimCmd = YTrimCmd = RTrimCmd = 0.0;
  77   GearCmd = GearPos = 1; // default to gear down
  78   LeftBrake = RightBrake = CenterBrake = 0.0;
  79   TailhookPos = WingFoldPos = 0.0;
  80
  81   bind();
  82   for (i=0;i<NForms;i++) {
  83     DePos[i] = DaLPos[i] = DaRPos[i] = DrPos[i] = 0.0;
  84     DfPos[i] = DsbPos[i] = DspPos[i] = 0.0;
  85   }
  86
  87   Debug(0);
  88 }
  89
  90 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  91
  92 FGFCS::~FGFCS()
  93 {
  94   ThrottleCmd.clear();
  95   ThrottlePos.clear();
  96   MixtureCmd.clear();
  97   MixturePos.clear();
  98   PropAdvanceCmd.clear();
  99   PropAdvance.clear();
 100   SteerPosDeg.clear();
 101   PropFeatherCmd.clear();
 102   PropFeather.clear();
 103
 104   unsigned int i;
 105
 106   for (i=0;i<sensors.size();i++) delete sensors[i];
 107   sensors.clear();
 108   for (i=0;i<APComponents.size();i++) delete APComponents[i];
 109   APComponents.clear();
 110   for (i=0;i<FCSComponents.size();i++) delete FCSComponents[i];
 111   FCSComponents.clear();
 112   for (i=0;i<Systems.size();i++) delete Systems[i];
 113   Systems.clear();
 114
 115   for (unsigned int i=0; i<interface_properties.size(); i++) delete interface_properties[i];
 116   interface_properties.clear();
 117
 118   Debug(1);
 119 }
 120
 121 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 122
 123 bool FGFCS::InitModel(void)
 124 {
 125   unsigned int i;
 126
 127   if (!FGModel::InitModel()) return false;
 128
 129   for (i=0; i<ThrottlePos.size(); i++) ThrottlePos[i] = 0.0;
 130   for (i=0; i<MixturePos.size(); i++) MixturePos[i] = 0.0;
 131   for (i=0; i<ThrottleCmd.size(); i++) ThrottleCmd[i] = 0.0;
 132   for (i=0; i<MixtureCmd.size(); i++) MixtureCmd[i] = 0.0;
 133   for (i=0; i<PropAdvance.size(); i++) PropAdvance[i] = 0.0;
 134   for (i=0; i<PropFeather.size(); i++) PropFeather[i] = 0.0;
 135
 136   DaCmd = DeCmd = DrCmd = DsCmd = DfCmd = DsbCmd = DspCmd = 0;
 137   PTrimCmd = YTrimCmd = RTrimCmd = 0.0;
 138   TailhookPos = WingFoldPos = 0.0;
 139
 140   for (i=0;i<NForms;i++) {
 141     DePos[i] = DaLPos[i] = DaRPos[i] = DrPos[i] = 0.0;
 142     DfPos[i] = DsbPos[i] = DspPos[i] = 0.0;
 143   }
 144
 145   return true;
 146 }
 147
 148 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 149 // Notes: In this logic the default engine commands are set. This is simply a
 150 // sort of safe-mode method in case the user has not defined control laws for
 151 // throttle, mixture, and prop-advance. The throttle, mixture, and prop advance
 152 // positions are set equal to the respective commands. Any control logic that is
 153 // actually present in the flight_control or autopilot section will override
 154 // these simple assignments.
 155
 156 bool FGFCS::Run(void)
 157 {
 158   unsigned int i;
 159
 160   if (FGModel::Run()) return true; // fast exit if nothing to do
 161   if (FDMExec->Holding()) return false;
 162
 163   for (i=0; i<ThrottlePos.size(); i++) ThrottlePos[i] = ThrottleCmd[i];
 164   for (i=0; i<MixturePos.size(); i++) MixturePos[i] = MixtureCmd[i];
 165   for (i=0; i<PropAdvance.size(); i++) PropAdvance[i] = PropAdvanceCmd[i];
 166   for (i=0; i<PropFeather.size(); i++) PropFeather[i] = PropFeatherCmd[i];
 167
 168   // Set the default steering angle
 169   for (i=0; i<SteerPosDeg.size(); i++) {
 170     FGLGear* gear = GroundReactions->GetGearUnit(i);
 171     SteerPosDeg[i] = gear->GetDefaultSteerAngle( GetDsCmd() );
 172   }
 173
 174   // Cycle through the sensor, systems, autopilot, and flight control components
 175   // Execute Sensors
 176   for (i=0; i<sensors.size(); i++) sensors[i]->Run();
 177
 178   // Execute Systems in order
 179   for (i=0; i<Systems.size(); i++) Systems[i]->Run();
 180
 181   // Execute Autopilot
 182   for (i=0; i<APComponents.size(); i++) APComponents[i]->Run();
 183
 184   // Execute Flight Control System
 185   for (i=0; i<FCSComponents.size(); i++) FCSComponents[i]->Run();
 186
 187   return false;
 188 }
 189
 190 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 191
 192 void FGFCS::SetDaLPos( int form , double pos )
 193 {
 194   switch(form) {
 195   case ofRad:
 196     DaLPos[ofRad] = pos;
 197     DaLPos[ofDeg] = pos*radtodeg;
 198     break;
 199   case ofDeg:
 200     DaLPos[ofRad] = pos*degtorad;
 201     DaLPos[ofDeg] = pos;
 202     break;
 203   case ofNorm:
 204     DaLPos[ofNorm] = pos;
 205   }
 206   DaLPos[ofMag] = fabs(DaLPos[ofRad]);
 207 }
 208
 209 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 210
 211 void FGFCS::SetDaRPos( int form , double pos )
 212 {
 213   switch(form) {
 214   case ofRad:
 215     DaRPos[ofRad] = pos;
 216     DaRPos[ofDeg] = pos*radtodeg;
 217     break;
 218   case ofDeg:
 219     DaRPos[ofRad] = pos*degtorad;
 220     DaRPos[ofDeg] = pos;
 221     break;
 222   case ofNorm:
 223     DaRPos[ofNorm] = pos;
 224   }
 225   DaRPos[ofMag] = fabs(DaRPos[ofRad]);
 226 }
 227
 228 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 229
 230 void FGFCS::SetDePos( int form , double pos )
 231 {
 232   switch(form) {
 233   case ofRad:
 234     DePos[ofRad] = pos;
 235     DePos[ofDeg] = pos*radtodeg;
 236     break;
 237   case ofDeg:
 238     DePos[ofRad] = pos*degtorad;
 239     DePos[ofDeg] = pos;
 240     break;
 241   case ofNorm:
 242     DePos[ofNorm] = pos;
 243   }
 244   DePos[ofMag] = fabs(DePos[ofRad]);
 245 }
 246
 247 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 248
 249 void FGFCS::SetDrPos( int form , double pos )
 250 {
 251   switch(form) {
 252   case ofRad:
 253     DrPos[ofRad] = pos;
 254     DrPos[ofDeg] = pos*radtodeg;
 255     break;
 256   case ofDeg:
 257     DrPos[ofRad] = pos*degtorad;
 258     DrPos[ofDeg] = pos;
 259     break;
 260   case ofNorm:
 261     DrPos[ofNorm] = pos;
 262   }
 263   DrPos[ofMag] = fabs(DrPos[ofRad]);
 264 }
 265
 266 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 267
 268 void FGFCS::SetDfPos( int form , double pos )
 269 {
 270   switch(form) {
 271   case ofRad:
 272     DfPos[ofRad] = pos;
 273     DfPos[ofDeg] = pos*radtodeg;
 274     break;
 275   case ofDeg:
 276     DfPos[ofRad] = pos*degtorad;
 277     DfPos[ofDeg] = pos;
 278     break;
 279   case ofNorm:
 280     DfPos[ofNorm] = pos;
 281   }
 282   DfPos[ofMag] = fabs(DfPos[ofRad]);
 283 }
 284
 285 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 286
 287 void FGFCS::SetDsbPos( int form , double pos )
 288 {
 289   switch(form) {
 290   case ofRad:
 291     DsbPos[ofRad] = pos;
 292     DsbPos[ofDeg] = pos*radtodeg;
 293     break;
 294   case ofDeg:
 295     DsbPos[ofRad] = pos*degtorad;
 296     DsbPos[ofDeg] = pos;
 297     break;
 298   case ofNorm:
 299     DsbPos[ofNorm] = pos;
 300   }
 301   DsbPos[ofMag] = fabs(DsbPos[ofRad]);
 302 }
 303
 304 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 305
 306 void FGFCS::SetDspPos( int form , double pos )
 307 {
 308   switch(form) {
 309   case ofRad:
 310     DspPos[ofRad] = pos;
 311     DspPos[ofDeg] = pos*radtodeg;
 312     break;
 313   case ofDeg:
 314     DspPos[ofRad] = pos*degtorad;
 315     DspPos[ofDeg] = pos;
 316     break;
 317   case ofNorm:
 318     DspPos[ofNorm] = pos;
 319   }
 320   DspPos[ofMag] = fabs(DspPos[ofRad]);
 321 }
 322
 323 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 324
 325 void FGFCS::SetThrottleCmd(int engineNum, double setting)
 326 {
 327   unsigned int ctr;
 328
 329   if (engineNum < (int)ThrottlePos.size()) {
 330     if (engineNum < 0) {
 331       for (ctr=0;ctr<ThrottleCmd.size();ctr++) ThrottleCmd[ctr] = setting;
 332     } else {
 333       ThrottleCmd[engineNum] = setting;
 334     }
 335   } else {
 336     cerr << "Throttle " << engineNum << " does not exist! " << ThrottleCmd.size()
 337          << " engines exist, but attempted throttle command is for engine "
 338          << engineNum << endl;
 339   }
 340 }
 341
 342 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 343
 344 void FGFCS::SetThrottlePos(int engineNum, double setting)
 345 {
 346   unsigned int ctr;
 347
 348   if (engineNum < (int)ThrottlePos.size()) {
 349     if (engineNum < 0) {
 350       for (ctr=0;ctr<ThrottlePos.size();ctr++) ThrottlePos[ctr] = setting;
 351     } else {
 352       ThrottlePos[engineNum] = setting;
 353     }
 354   } else {
 355     cerr << "Throttle " << engineNum << " does not exist! " << ThrottlePos.size()
 356          << " engines exist, but attempted throttle position setting is for engine "
 357          << engineNum << endl;
 358   }
 359 }
 360
 361 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 362
 363 double FGFCS::GetThrottleCmd(int engineNum) const
 364 {
 365   if (engineNum < (int)ThrottlePos.size()) {
 366     if (engineNum < 0) {
 367        cerr << "Cannot get throttle value for ALL engines" << endl;
 368     } else {
 369       return ThrottleCmd[engineNum];
 370     }
 371   } else {
 372     cerr << "Throttle " << engineNum << " does not exist! " << ThrottleCmd.size()
 373          << " engines exist, but throttle setting for engine " << engineNum
 374          << " is selected" << endl;
 375   }
 376   return 0.0;
 377 }
 378
 379 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 380
 381 double FGFCS::GetThrottlePos(int engineNum) const
 382 {
 383   if (engineNum < (int)ThrottlePos.size()) {
 384     if (engineNum < 0) {
 385        cerr << "Cannot get throttle value for ALL engines" << endl;
 386     } else {
 387       return ThrottlePos[engineNum];
 388     }
 389   } else {
 390     cerr << "Throttle " << engineNum << " does not exist! " << ThrottlePos.size()
 391          << " engines exist, but attempted throttle position setting is for engine "
 392          << engineNum << endl;
 393   }
 394   return 0.0;
 395 }
 396
 397 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 398
 399 void FGFCS::SetMixtureCmd(int engineNum, double setting)
 400 {
 401   unsigned int ctr;
 402
 403   if (engineNum < (int)ThrottlePos.size()) {
 404     if (engineNum < 0) {
 405       for (ctr=0;ctr<MixtureCmd.size();ctr++) MixtureCmd[ctr] = setting;
 406     } else {
 407       MixtureCmd[engineNum] = setting;
 408     }
 409   }
 410 }
 411
 412 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 413
 414 void FGFCS::SetMixturePos(int engineNum, double setting)
 415 {
 416   unsigned int ctr;
 417
 418   if (engineNum < (int)ThrottlePos.size()) {
 419     if (engineNum < 0) {
 420       for (ctr=0;ctr<=MixtureCmd.size();ctr++) MixturePos[ctr] = MixtureCmd[ctr];
 421     } else {
 422       MixturePos[engineNum] = setting;
 423     }
 424   }
 425 }
 426
 427 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 428
 429 void FGFCS::SetPropAdvanceCmd(int engineNum, double setting)
 430 {
 431   unsigned int ctr;
 432
 433   if (engineNum < (int)ThrottlePos.size()) {
 434     if (engineNum < 0) {
 435       for (ctr=0;ctr<PropAdvanceCmd.size();ctr++) PropAdvanceCmd[ctr] = setting;
 436     } else {
 437       PropAdvanceCmd[engineNum] = setting;
 438     }
 439   }
 440 }
 441
 442 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 443
 444 void FGFCS::SetPropAdvance(int engineNum, double setting)
 445 {
 446   unsigned int ctr;
 447
 448   if (engineNum < (int)ThrottlePos.size()) {
 449     if (engineNum < 0) {
 450       for (ctr=0;ctr<=PropAdvanceCmd.size();ctr++) PropAdvance[ctr] = PropAdvanceCmd[ctr];
 451     } else {
 452       PropAdvance[engineNum] = setting;
 453     }
 454   }
 455 }
 456
 457 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 458
 459 void FGFCS::SetFeatherCmd(int engineNum, bool setting)
 460 {
 461   unsigned int ctr;
 462
 463   if (engineNum < (int)ThrottlePos.size()) {
 464     if (engineNum < 0) {
 465       for (ctr=0;ctr<PropFeatherCmd.size();ctr++) PropFeatherCmd[ctr] = setting;
 466     } else {
 467       PropFeatherCmd[engineNum] = setting;
 468     }
 469   }
 470 }
 471
 472 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 473
 474 void FGFCS::SetPropFeather(int engineNum, bool setting)
 475 {
 476   unsigned int ctr;
 477
 478   if (engineNum < (int)ThrottlePos.size()) {
 479     if (engineNum < 0) {
 480       for (ctr=0;ctr<=PropFeatherCmd.size();ctr++) PropFeather[ctr] = PropFeatherCmd[ctr];
 481     } else {
 482       PropFeather[engineNum] = setting;
 483     }
 484   }
 485 }
 486
 487 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 488
 489 bool FGFCS::Load(Element* el, SystemType systype)
 490 {
 491   string name, file, fname="", interface_property_string, parent_name;
 492   vector <FGFCSComponent*> *Components;
 493   Element *component_element, *property_element, *sensor_element;
 494   Element *channel_element;
 495
 496   Components=0;
 497
 498   string separator = "/";
 499
 500 // ToDo: The handling of name and file attributes could be improved, here,
 501 //       considering that a name can be in the external file, as well.
 502
 503   name = el->GetAttributeValue("name");
 504
 505   if (name.empty()) {
 506     fname = el->GetAttributeValue("file");
 507     if (systype == stSystem) {
 508       file = FindSystemFullPathname(fname);
 509     } else {
 510       file = FDMExec->GetFullAircraftPath() + separator + fname + ".xml";
 511     }
 512     if (fname.empty()) {
 513       cerr << "FCS, Autopilot, or system does not appear to be defined inline nor in a file" << endl;
 514       return false;
 515     } else {
 516       document = LoadXMLDocument(file);
 517       name = document->GetAttributeValue("name");
 518     }
 519   } else {
 520     document = el;
 521   }
 522
 523   if (document->GetName() == "autopilot") {
 524     Components = &APComponents;
 525     Name = "Autopilot: " + document->GetAttributeValue("name");
 526   } else if (document->GetName() == "flight_control") {
 527     Components = &FCSComponents;
 528     Name = "FCS: " + document->GetAttributeValue("name");
 529   } else if (document->GetName() == "system") {
 530     Components = &Systems;
 531     Name = "System: " + document->GetAttributeValue("name");
 532   }
 533   Debug(2);
 534
 535   if (document->GetName() == "flight_control") bindModel();
 536
 537   // Interface properties from any autopilot, flight control, or other system are
 538   // all stored in the interface properties array.
 539
 540   property_element = document->FindElement("property");
 541   if (property_element) cout << endl << "    Declared properties" << endl << endl;
 542   while (property_element) {
 543     double value=0.0;
 544     if ( ! property_element->GetAttributeValue("value").empty())
 545       value = property_element->GetAttributeValueAsNumber("value");
 546     interface_properties.push_back(new double(value));
 547     interface_property_string = property_element->GetDataLine();
 548     PropertyManager->Tie(interface_property_string, interface_properties.back());
 549     cout << "      " << interface_property_string << " (initial value: " << value << ")" << endl;
 550     property_element = document->FindNextElement("property");
 551   }
 552
 553   // After reading interface properties in a file, read properties in the local
 554   // flight_control, autopiot, or system element. This allows general-purpose
 555   // systems to be defined in a file, with overrides or initial loaded constants
 556   // supplied in the relevant element of the aircraft configuration file.
 557
 558   if (!fname.empty()) {
 559     property_element = el->FindElement("property");
 560     if (property_element && debug_lvl > 0) cout << endl << "    Declared properties" << endl << endl;
 561     while (property_element) {
 562       double value=0.0;
 563       if ( ! property_element->GetAttributeValue("value").empty())
 564         value = property_element->GetAttributeValueAsNumber("value");
 565
 566       interface_property_string = property_element->GetDataLine();
 567
 568       FGPropertyManager* node = PropertyManager->GetNode(interface_property_string);
 569       if (node) {
 570         cout << "      " << "Overriding value for property " << interface_property_string
 571              << " (old value: " << node->getDoubleValue() << "  new value: " << value << ")" << endl;
 572         node->setDoubleValue(value);
 573       } else {
 574         interface_properties.push_back(new double(value));
 575         PropertyManager->Tie(interface_property_string, interface_properties.back());
 576         if (debug_lvl > 0)
 577           cout << "      " << interface_property_string << " (initial value: " << value << ")" << endl;
 578       }
 579
 580
 581       property_element = el->FindNextElement("property");
 582     }
 583   }
 584
 585   // Any sensor elements that are outside of a channel (in either the autopilot
 586   // or the flight_control, or even any possible "system") are placed into the global
 587   // "sensors" array, and are executed prior to any autopilot, flight control, or
 588   // system.
 589
 590   sensor_element = document->FindElement("sensor");
 591   while (sensor_element) {
 592     try {
 593       sensors.push_back(new FGSensor(this, sensor_element));
 594     } catch (string s) {
 595       cerr << highint << fgred << endl << "  " << s << endl;
 596       return false;
 597     }
 598     sensor_element = document->FindNextElement("sensor");
 599   }
 600
 601   channel_element = document->FindElement("channel");
 602   while (channel_element) {
 603
 604     if (debug_lvl > 0)
 605       cout << endl << highint << fgblue << "    Channel "
 606          << normint << channel_element->GetAttributeValue("name") << reset << endl;
 607
 608     component_element = channel_element->GetElement();
 609     while (component_element) {
 610       try {
 611         if ((component_element->GetName() == string("lag_filter")) ||
 612             (component_element->GetName() == string("lead_lag_filter")) ||
 613             (component_element->GetName() == string("washout_filter")) ||
 614             (component_element->GetName() == string("second_order_filter")) ||
 615             (component_element->GetName() == string("integrator")) )
 616         {
 617           Components->push_back(new FGFilter(this, component_element));
 618         } else if ((component_element->GetName() == string("pure_gain")) ||
 619                    (component_element->GetName() == string("scheduled_gain")) ||
 620                    (component_element->GetName() == string("aerosurface_scale")))
 621         {
 622           Components->push_back(new FGGain(this, component_element));
 623         } else if (component_element->GetName() == string("summer")) {
 624           Components->push_back(new FGSummer(this, component_element));
 625         } else if (component_element->GetName() == string("deadband")) {
 626           Components->push_back(new FGDeadBand(this, component_element));
 627         } else if (component_element->GetName() == string("switch")) {
 628           Components->push_back(new FGSwitch(this, component_element));
 629         } else if (component_element->GetName() == string("kinematic")) {
 630           Components->push_back(new FGKinemat(this, component_element));
 631         } else if (component_element->GetName() == string("fcs_function")) {
 632           Components->push_back(new FGFCSFunction(this, component_element));
 633         } else if (component_element->GetName() == string("pid")) {
 634           Components->push_back(new FGPID(this, component_element));
 635         } else if (component_element->GetName() == string("actuator")) {
 636           Components->push_back(new FGActuator(this, component_element));
 637         } else if (component_element->GetName() == string("sensor")) {
 638           Components->push_back(new FGSensor(this, component_element));
 639         } else {
 640           cerr << "Unknown FCS component: " << component_element->GetName() << endl;
 641         }
 642       } catch(string s) {
 643         cerr << highint << fgred << endl << "  " << s << endl;
 644         cerr << reset << endl;
 645         return false;
 646       }
 647       component_element = channel_element->GetNextElement();
 648     }
 649     channel_element = document->FindNextElement("channel");
 650   }
 651
 652   ResetParser();
 653
 654   return true;
 655 }
 656
 657 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 658
 659 double FGFCS::GetBrake(FGLGear::BrakeGroup bg)
 660 {
 661   switch (bg) {
 662   case FGLGear::bgLeft:
 663     return LeftBrake;
 664   case FGLGear::bgRight:
 665     return RightBrake;
 666   case FGLGear::bgCenter:
 667     return CenterBrake;
 668   default:
 669     cerr << "GetBrake asked to return a bogus brake value" << endl;
 670   }
 671   return 0.0;
 672 }
 673
 674 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 675
 676 string FGFCS::FindSystemFullPathname(string system_filename)
 677 {
 678   string fullpath, localpath;
 679   string systemPath = FDMExec->GetSystemsPath();
 680   string aircraftPath = FDMExec->GetFullAircraftPath();
 681   ifstream system_file;
 682
 683   string separator = "/";
 684
 685   fullpath = systemPath + separator;
 686   localpath = aircraftPath + separator + "Systems" + separator;
 687
 688   system_file.open(string(fullpath + system_filename + ".xml").c_str());
 689   if ( !system_file.is_open()) {
 690     system_file.open(string(localpath + system_filename + ".xml").c_str());
 691       if ( !system_file.is_open()) {
 692         cerr << " Could not open system file: " << system_filename << " in path "
 693              << fullpath << " or " << localpath << endl;
 694         return string("");
 695       } else {
 696         return string(localpath + system_filename + ".xml");
 697       }
 698   }
 699   return string(fullpath + system_filename + ".xml");
 700 }
 701
 702 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 703
 704 ifstream* FGFCS::FindSystemFile(string system_filename)
 705 {
 706   string fullpath, localpath;
 707   string systemPath = FDMExec->GetSystemsPath();
 708   string aircraftPath = FDMExec->GetFullAircraftPath();
 709   ifstream* system_file = new ifstream();
 710
 711   string separator = "/";
 712
 713   fullpath = systemPath + separator;
 714   localpath = aircraftPath + separator + "Systems" + separator;
 715
 716   system_file->open(string(fullpath + system_filename + ".xml").c_str());
 717   if ( !system_file->is_open()) {
 718     system_file->open(string(localpath + system_filename + ".xml").c_str());
 719       if ( !system_file->is_open()) {
 720         cerr << " Could not open system file: " << system_filename << " in path "
 721              << fullpath << " or " << localpath << endl;
 722       }
 723   }
 724   return system_file;
 725 }
 726
 727 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 728
 729 string FGFCS::GetComponentStrings(string delimeter)
 730 {
 731   unsigned int comp;
 732   string CompStrings = "";
 733   bool firstime = true;
 734   int total_count=0;
 735
 736   for (unsigned int i=0; i<Systems.size(); i++) {
 737     if (firstime) firstime = false;
 738     else          CompStrings += delimeter;
 739
 740     CompStrings += Systems[i]->GetName();
 741     total_count++;
 742   }
 743
 744   for (comp = 0; comp < APComponents.size(); comp++)
 745   {
 746     if (firstime) firstime = false;
 747     else          CompStrings += delimeter;
 748
 749     CompStrings += APComponents[comp]->GetName();
 750     total_count++;
 751   }
 752
 753   for (comp = 0; comp < FCSComponents.size(); comp++) {
 754     if (firstime) firstime = false;
 755     else          CompStrings += delimeter;
 756
 757     CompStrings += FCSComponents[comp]->GetName();
 758     total_count++;
 759   }
 760
 761   return CompStrings;
 762 }
 763
 764 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 765
 766 string FGFCS::GetComponentValues(string delimeter)
 767 {
 768   unsigned int comp;
 769   string CompValues = "";
 770   char buffer[100];
 771   bool firstime = true;
 772   int total_count=0;
 773
 774   for (unsigned int i=0; i<Systems.size(); i++) {
 775     if (firstime) firstime = false;
 776     else          CompValues += delimeter;
 777
 778     snprintf(buffer, 100, "%9.6f", Systems[i]->GetOutput());
 779     CompValues += string(buffer);
 780     total_count++;
 781   }
 782
 783   for (comp = 0; comp < APComponents.size(); comp++) {
 784     if (firstime) firstime = false;
 785     else          CompValues += delimeter;
 786
 787     sprintf(buffer, "%9.6f", APComponents[comp]->GetOutput());
 788     CompValues += string(buffer);
 789     total_count++;
 790   }
 791
 792   for (comp = 0; comp < FCSComponents.size(); comp++) {
 793     if (firstime) firstime = false;
 794     else          CompValues += delimeter;
 795
 796     sprintf(buffer, "%9.6f", FCSComponents[comp]->GetOutput());
 797     CompValues += string(buffer);
 798     total_count++;
 799   }
 800
 801   CompValues += "\0";
 802   return CompValues;
 803 }
 804
 805 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 806
 807 void FGFCS::AddThrottle(void)
 808 {
 809   ThrottleCmd.push_back(0.0);
 810   ThrottlePos.push_back(0.0);
 811   MixtureCmd.push_back(0.0);     // assume throttle and mixture are coupled
 812   MixturePos.push_back(0.0);
 813   PropAdvanceCmd.push_back(0.0); // assume throttle and prop pitch are coupled
 814   PropAdvance.push_back(0.0);
 815   PropFeatherCmd.push_back(false);
 816   PropFeather.push_back(false);
 817
 818   unsigned int num = (unsigned int)ThrottleCmd.size()-1;
 819   bindThrottle(num);
 820 }
 821
 822 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 823
 824 void FGFCS::AddGear(void)
 825 {
 826   SteerPosDeg.push_back(0.0);
 827 }
 828
 829 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 830
 831 double FGFCS::GetDt(void)
 832 {
 833   return FDMExec->GetDeltaT()*rate;
 834 }
 835
 836 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 837
 838 void FGFCS::bind(void)
 839 {
 840   PropertyManager->Tie("fcs/aileron-cmd-norm", this, &FGFCS::GetDaCmd, &FGFCS::SetDaCmd);
 841   PropertyManager->Tie("fcs/elevator-cmd-norm", this, &FGFCS::GetDeCmd, &FGFCS::SetDeCmd);
 842   PropertyManager->Tie("fcs/rudder-cmd-norm", this, &FGFCS::GetDrCmd, &FGFCS::SetDrCmd);
 843   PropertyManager->Tie("fcs/flap-cmd-norm", this, &FGFCS::GetDfCmd, &FGFCS::SetDfCmd);
 844   PropertyManager->Tie("fcs/speedbrake-cmd-norm", this, &FGFCS::GetDsbCmd, &FGFCS::SetDsbCmd);
 845   PropertyManager->Tie("fcs/spoiler-cmd-norm", this, &FGFCS::GetDspCmd, &FGFCS::SetDspCmd);
 846   PropertyManager->Tie("fcs/pitch-trim-cmd-norm", this, &FGFCS::GetPitchTrimCmd, &FGFCS::SetPitchTrimCmd);
 847   PropertyManager->Tie("fcs/roll-trim-cmd-norm", this, &FGFCS::GetRollTrimCmd, &FGFCS::SetRollTrimCmd);
 848   PropertyManager->Tie("fcs/yaw-trim-cmd-norm", this, &FGFCS::GetYawTrimCmd, &FGFCS::SetYawTrimCmd);
 849
 850   PropertyManager->Tie("fcs/left-aileron-pos-rad", this, ofRad, &FGFCS::GetDaLPos, &FGFCS::SetDaLPos);
 851   PropertyManager->Tie("fcs/left-aileron-pos-deg", this, ofDeg, &FGFCS::GetDaLPos, &FGFCS::SetDaLPos);
 852   PropertyManager->Tie("fcs/left-aileron-pos-norm", this, ofNorm, &FGFCS::GetDaLPos, &FGFCS::SetDaLPos);
 853   PropertyManager->Tie("fcs/mag-left-aileron-pos-rad", this, ofMag, &FGFCS::GetDaLPos);
 854
 855   PropertyManager->Tie("fcs/right-aileron-pos-rad", this, ofRad, &FGFCS::GetDaRPos, &FGFCS::SetDaRPos);
 856   PropertyManager->Tie("fcs/right-aileron-pos-deg", this, ofDeg, &FGFCS::GetDaRPos, &FGFCS::SetDaRPos);
 857   PropertyManager->Tie("fcs/right-aileron-pos-norm", this, ofNorm, &FGFCS::GetDaRPos, &FGFCS::SetDaRPos);
 858   PropertyManager->Tie("fcs/mag-right-aileron-pos-rad", this, ofMag, &FGFCS::GetDaRPos);
 859
 860   PropertyManager->Tie("fcs/elevator-pos-rad", this, ofRad, &FGFCS::GetDePos, &FGFCS::SetDePos);
 861   PropertyManager->Tie("fcs/elevator-pos-deg", this, ofDeg, &FGFCS::GetDePos, &FGFCS::SetDePos);
 862   PropertyManager->Tie("fcs/elevator-pos-norm", this, ofNorm, &FGFCS::GetDePos, &FGFCS::SetDePos);
 863   PropertyManager->Tie("fcs/mag-elevator-pos-rad", this, ofMag, &FGFCS::GetDePos);
 864
 865   PropertyManager->Tie("fcs/rudder-pos-rad", this,ofRad, &FGFCS::GetDrPos, &FGFCS::SetDrPos);
 866   PropertyManager->Tie("fcs/rudder-pos-deg", this,ofDeg, &FGFCS::GetDrPos, &FGFCS::SetDrPos);
 867   PropertyManager->Tie("fcs/rudder-pos-norm", this,ofNorm, &FGFCS::GetDrPos, &FGFCS::SetDrPos);
 868   PropertyManager->Tie("fcs/mag-rudder-pos-rad", this,ofMag, &FGFCS::GetDrPos);
 869
 870   PropertyManager->Tie("fcs/flap-pos-rad", this,ofRad, &FGFCS::GetDfPos, &FGFCS::SetDfPos);
 871   PropertyManager->Tie("fcs/flap-pos-deg", this,ofDeg, &FGFCS::GetDfPos, &FGFCS::SetDfPos);
 872   PropertyManager->Tie("fcs/flap-pos-norm", this,ofNorm, &FGFCS::GetDfPos, &FGFCS::SetDfPos);
 873
 874   PropertyManager->Tie("fcs/speedbrake-pos-rad", this,ofRad, &FGFCS::GetDsbPos, &FGFCS::SetDsbPos);
 875   PropertyManager->Tie("fcs/speedbrake-pos-deg", this,ofDeg, &FGFCS::GetDsbPos, &FGFCS::SetDsbPos);
 876   PropertyManager->Tie("fcs/speedbrake-pos-norm", this,ofNorm, &FGFCS::GetDsbPos, &FGFCS::SetDsbPos);
 877   PropertyManager->Tie("fcs/mag-speedbrake-pos-rad", this,ofMag, &FGFCS::GetDsbPos);
 878
 879   PropertyManager->Tie("fcs/spoiler-pos-rad", this, ofRad, &FGFCS::GetDspPos, &FGFCS::SetDspPos);
 880   PropertyManager->Tie("fcs/spoiler-pos-deg", this, ofDeg, &FGFCS::GetDspPos, &FGFCS::SetDspPos);
 881   PropertyManager->Tie("fcs/spoiler-pos-norm", this, ofNorm, &FGFCS::GetDspPos, &FGFCS::SetDspPos);
 882   PropertyManager->Tie("fcs/mag-spoiler-pos-rad", this, ofMag, &FGFCS::GetDspPos);
 883
 884   PropertyManager->Tie("gear/gear-pos-norm", this, &FGFCS::GetGearPos, &FGFCS::SetGearPos);
 885   PropertyManager->Tie("gear/gear-cmd-norm", this, &FGFCS::GetGearCmd, &FGFCS::SetGearCmd);
 886   PropertyManager->Tie("fcs/left-brake-cmd-norm", this, &FGFCS::GetLBrake, &FGFCS::SetLBrake);
 887   PropertyManager->Tie("fcs/right-brake-cmd-norm", this, &FGFCS::GetRBrake, &FGFCS::SetRBrake);
 888   PropertyManager->Tie("fcs/center-brake-cmd-norm", this, &FGFCS::GetCBrake, &FGFCS::SetCBrake);
 889   PropertyManager->Tie("fcs/steer-cmd-norm", this, &FGFCS::GetDsCmd, &FGFCS::SetDsCmd);
 890
 891   PropertyManager->Tie("gear/tailhook-pos-norm", this, &FGFCS::GetTailhookPos, &FGFCS::SetTailhookPos);
 892   PropertyManager->Tie("fcs/wing-fold-pos-norm", this, &FGFCS::GetWingFoldPos, &FGFCS::SetWingFoldPos);
 893 }
 894
 895 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 896 // Technically, this function should probably bind propulsion type specific controls
 897 // rather than mixture and prop-advance.
 898 //
 899
 900 void FGFCS::bindThrottle(unsigned int num)
 901 {
 902   char tmp[80];
 903
 904   snprintf(tmp, 80, "fcs/throttle-cmd-norm[%u]",num);
 905   PropertyManager->Tie( tmp, this, num, &FGFCS::GetThrottleCmd,
 906                                         &FGFCS::SetThrottleCmd);
 907   snprintf(tmp, 80, "fcs/throttle-pos-norm[%u]",num);
 908   PropertyManager->Tie( tmp, this, num, &FGFCS::GetThrottlePos,
 909                                         &FGFCS::SetThrottlePos);
 910   snprintf(tmp, 80, "fcs/mixture-cmd-norm[%u]",num);
 911   PropertyManager->Tie( tmp, this, num, &FGFCS::GetMixtureCmd,
 912                                         &FGFCS::SetMixtureCmd);
 913   snprintf(tmp, 80, "fcs/mixture-pos-norm[%u]",num);
 914   PropertyManager->Tie( tmp, this, num, &FGFCS::GetMixturePos,
 915                                         &FGFCS::SetMixturePos);
 916   snprintf(tmp, 80, "fcs/advance-cmd-norm[%u]",num);
 917   PropertyManager->Tie( tmp, this, num, &FGFCS::GetPropAdvanceCmd,
 918                                         &FGFCS::SetPropAdvanceCmd);
 919   snprintf(tmp, 80, "fcs/advance-pos-norm[%u]", num);
 920   PropertyManager->Tie( tmp, this, num, &FGFCS::GetPropAdvance,
 921                                         &FGFCS::SetPropAdvance);
 922   snprintf(tmp, 80, "fcs/feather-cmd-norm[%u]", num);
 923   PropertyManager->Tie( tmp, this, num, &FGFCS::GetFeatherCmd,
 924                                         &FGFCS::SetFeatherCmd);
 925   snprintf(tmp, 80, "fcs/feather-pos-norm[%u]", num);
 926   PropertyManager->Tie( tmp, this, num, &FGFCS::GetPropFeather,
 927                                         &FGFCS::SetPropFeather);
 928 }
 929
 930 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 931
 932 void FGFCS::bindModel(void)
 933 {
 934   unsigned int i;
 935   char tmp[80];
 936
 937   for (i=0; i<SteerPosDeg.size(); i++) {
 938     if (GroundReactions->GetGearUnit(i)->GetSteerable()) {
 939       snprintf(tmp,80,"fcs/steer-pos-deg[%u]",i);
 940       PropertyManager->Tie( tmp, this, i, &FGFCS::GetSteerPosDeg, &FGFCS::SetSteerPosDeg);
 941     }
 942   }
 943 }
 944
 945 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 946 //    The bitmasked value choices are as follows:
 947 //    unset: In this case (the default) JSBSim would only print
 948 //       out the normally expected messages, essentially echoing
 949 //       the config files as they are read. If the environment
 950 //       variable is not set, debug_lvl is set to 1 internally
 951 //    0: This requests JSBSim not to output any messages
 952 //       whatsoever.
 953 //    1: This value explicity requests the normal JSBSim
 954 //       startup messages
 955 //    2: This value asks for a message to be printed out when
 956 //       a class is instantiated
 957 //    4: When this value is set, a message is displayed when a
 958 //       FGModel object executes its Run() method
 959 //    8: When this value is set, various runtime state variables
 960 //       are printed out periodically
 961 //    16: When set various parameters are sanity checked and
 962 //       a message is printed out when they go out of bounds
 963
 964 void FGFCS::Debug(int from)
 965 {
 966   if (debug_lvl <= 0) return;
 967
 968   if (debug_lvl & 1) { // Standard console startup message output
 969     if (from == 2) { // Loader
 970       cout << endl << "  Flight Control (" << Name << ")" << endl;
 971     }
 972   }
 973   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 974     if (from == 0) cout << "Instantiated: FGFCS" << endl;
 975     if (from == 1) cout << "Destroyed:    FGFCS" << endl;
 976   }
 977   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 978   }
 979   if (debug_lvl & 8 ) { // Runtime state variables
 980   }
 981   if (debug_lvl & 16) { // Sanity checking
 982   }
 983   if (debug_lvl & 64) {
 984     if (from == 0) { // Constructor
 985       cout << IdSrc << endl;
 986       cout << IdHdr << endl;
 987     }
 988   }
 989 }
 990
 991 }