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