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