src/FDM/JSBSim/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 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 General Public License for more
  19  details.
  20
  21  You should have received a copy of the GNU 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 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 "FGState.h"
  42 #include "FGFDMExec.h"
  43 #include "FGAtmosphere.h"
  44 #include "FGAircraft.h"
  45 #include "FGTranslation.h"
  46 #include "FGRotation.h"
  47 #include "FGPosition.h"
  48 #include "FGAuxiliary.h"
  49 #include "FGOutput.h"
  50
  51 #include "filtersjb/FGFilter.h"
  52 #include "filtersjb/FGDeadBand.h"
  53 #include "filtersjb/FGGain.h"
  54 #include "filtersjb/FGGradient.h"
  55 #include "filtersjb/FGSwitch.h"
  56 #include "filtersjb/FGSummer.h"
  57 #include "filtersjb/FGKinemat.h"
  58
  59 static const char *IdSrc = "$Id$";
  60 static const char *IdHdr = ID_FCS;
  61
  62 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  63 CLASS IMPLEMENTATION
  64 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  65
  66 FGFCS::FGFCS(FGFDMExec* fdmex) : FGModel(fdmex)
  67 {
  68   Name = "FGFCS";
  69
  70   DaCmd = DeCmd = DrCmd = DfCmd = DsbCmd = DspCmd = 0.0;
  71   PTrimCmd = YTrimCmd = RTrimCmd = 0.0;
  72   DaPos = DePos = DrPos = DfPos = DsbPos = DspPos = 0.0;
  73   GearCmd = GearPos = 1; // default to gear down
  74   LeftBrake = RightBrake = CenterBrake = 0.0;
  75
  76   Debug(0);
  77 }
  78
  79 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  80
  81 FGFCS::~FGFCS()
  82 {
  83   ThrottleCmd.clear();
  84   ThrottlePos.clear();
  85   MixtureCmd.clear();
  86   MixturePos.clear();
  87   PropAdvanceCmd.clear();
  88   PropAdvance.clear();
  89
  90   unsigned int i;
  91
  92   for (i=0;i<Components.size();i++) delete Components[i];
  93   Debug(1);
  94 }
  95
  96 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  97
  98 bool FGFCS::Run(void)
  99 {
 100   unsigned int i;
 101
 102   if (!FGModel::Run()) {
 103     for (i=0; i<ThrottlePos.size(); i++) ThrottlePos[i] = ThrottleCmd[i];
 104     for (i=0; i<MixturePos.size(); i++) MixturePos[i] = MixtureCmd[i];
 105     for (i=0; i<PropAdvance.size(); i++) PropAdvance[i] = PropAdvanceCmd[i];
 106     for (i=0; i<Components.size(); i++)  Components[i]->Run();
 107   } else {
 108   }
 109
 110   return false;
 111 }
 112
 113 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 114
 115 void FGFCS::SetThrottleCmd(int engineNum, double setting)
 116 {
 117   unsigned int ctr;
 118
 119   if (engineNum < (int)ThrottlePos.size()) {
 120     if (engineNum < 0) {
 121       for (ctr=0;ctr<ThrottleCmd.size();ctr++) ThrottleCmd[ctr] = setting;
 122     } else {
 123       ThrottleCmd[engineNum] = setting;
 124     }
 125   } else {
 126     cerr << "Throttle " << engineNum << " does not exist! " << ThrottleCmd.size()
 127          << " engines exist, but attempted throttle command is for engine "
 128          << engineNum << endl;
 129   }
 130 }
 131
 132 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 133
 134 void FGFCS::SetThrottlePos(int engineNum, double setting)
 135 {
 136   unsigned int ctr;
 137
 138   if (engineNum < (int)ThrottlePos.size()) {
 139     if (engineNum < 0) {
 140       for (ctr=0;ctr<ThrottlePos.size();ctr++) ThrottlePos[ctr] = setting;
 141     } else {
 142       ThrottlePos[engineNum] = setting;
 143     }
 144   } else {
 145     cerr << "Throttle " << engineNum << " does not exist! " << ThrottlePos.size()
 146          << " engines exist, but attempted throttle position setting is for engine "
 147          << engineNum << endl;
 148   }
 149 }
 150
 151 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 152
 153 double FGFCS::GetThrottleCmd(int engineNum)
 154 {
 155   if (engineNum < (int)ThrottlePos.size()) {
 156     if (engineNum < 0) {
 157        cerr << "Cannot get throttle value for ALL engines" << endl;
 158     } else {
 159       return ThrottleCmd[engineNum];
 160     }
 161   } else {
 162     cerr << "Throttle " << engineNum << " does not exist! " << ThrottleCmd.size()
 163          << " engines exist, but throttle setting for engine " << engineNum
 164          << " is selected" << endl;
 165   }
 166   return 0.0;
 167 }
 168
 169 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 170
 171 double FGFCS::GetThrottlePos(int engineNum)
 172 {
 173   if (engineNum < (int)ThrottlePos.size()) {
 174     if (engineNum < 0) {
 175        cerr << "Cannot get throttle value for ALL engines" << endl;
 176     } else {
 177       return ThrottlePos[engineNum];
 178     }
 179   } else {
 180     cerr << "Throttle " << engineNum << " does not exist! " << ThrottlePos.size()
 181          << " engines exist, but attempted throttle position setting is for engine "
 182          << engineNum << endl;
 183   }
 184   return 0.0;
 185 }
 186
 187 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 188
 189 void FGFCS::SetMixtureCmd(int engineNum, double setting)
 190 {
 191   unsigned int ctr;
 192
 193   if (engineNum < (int)ThrottlePos.size()) {
 194     if (engineNum < 0) {
 195       for (ctr=0;ctr<MixtureCmd.size();ctr++) MixtureCmd[ctr] = setting;
 196     } else {
 197       MixtureCmd[engineNum] = setting;
 198     }
 199   }
 200 }
 201
 202 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 203
 204 void FGFCS::SetMixturePos(int engineNum, double setting)
 205 {
 206   unsigned int ctr;
 207
 208   if (engineNum < (int)ThrottlePos.size()) {
 209     if (engineNum < 0) {
 210       for (ctr=0;ctr<=MixtureCmd.size();ctr++) MixturePos[ctr] = MixtureCmd[ctr];
 211     } else {
 212       MixturePos[engineNum] = setting;
 213     }
 214   }
 215 }
 216
 217 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 218
 219 void FGFCS::SetPropAdvanceCmd(int engineNum, double setting)
 220 {
 221   unsigned int ctr;
 222
 223   if (engineNum < (int)ThrottlePos.size()) {
 224     if (engineNum < 0) {
 225       for (ctr=0;ctr<PropAdvanceCmd.size();ctr++) PropAdvanceCmd[ctr] = setting;
 226     } else {
 227       PropAdvanceCmd[engineNum] = setting;
 228     }
 229   }
 230 }
 231
 232 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 233
 234 void FGFCS::SetPropAdvance(int engineNum, double setting)
 235 {
 236   unsigned int ctr;
 237
 238   if (engineNum < (int)ThrottlePos.size()) {
 239     if (engineNum < 0) {
 240       for (ctr=0;ctr<=PropAdvanceCmd.size();ctr++) PropAdvance[ctr] = PropAdvanceCmd[ctr];
 241     } else {
 242       PropAdvance[engineNum] = setting;
 243     }
 244   }
 245 }
 246
 247 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 248
 249 bool FGFCS::Load(FGConfigFile* AC_cfg)
 250 {
 251   string token;
 252
 253   Name = Name + ":" + AC_cfg->GetValue("NAME");
 254   if (debug_lvl > 0) cout << "    Control System Name: " << Name << endl;
 255   AC_cfg->GetNextConfigLine();
 256   while ((token = AC_cfg->GetValue()) != string("/FLIGHT_CONTROL")) {
 257     if (token == "COMPONENT") {
 258       token = AC_cfg->GetValue("TYPE");
 259       if (debug_lvl > 0) cout << "    Loading Component \""
 260                               << AC_cfg->GetValue("NAME")
 261                               << "\" of type: " << token << endl;
 262       if ((token == "LAG_FILTER") ||
 263           (token == "LEAD_LAG_FILTER") ||
 264           (token == "SECOND_ORDER_FILTER") ||
 265           (token == "WASHOUT_FILTER") ||
 266           (token == "INTEGRATOR") ) {
 267         Components.push_back(new FGFilter(this, AC_cfg));
 268       } else if ((token == "PURE_GAIN") ||
 269                  (token == "SCHEDULED_GAIN") ||
 270                  (token == "AEROSURFACE_SCALE") ) {
 271
 272         Components.push_back(new FGGain(this, AC_cfg));
 273
 274       } else if (token == "SUMMER") {
 275         Components.push_back(new FGSummer(this, AC_cfg));
 276       } else if (token == "DEADBAND") {
 277         Components.push_back(new FGDeadBand(this, AC_cfg));
 278       } else if (token == "GRADIENT") {
 279         Components.push_back(new FGGradient(this, AC_cfg));
 280       } else if (token == "SWITCH") {
 281         Components.push_back(new FGSwitch(this, AC_cfg));
 282       } else if (token == "KINEMAT") {
 283         Components.push_back(new FGKinemat(this, AC_cfg));
 284       } else {
 285         cerr << "Unknown token [" << token << "] in FCS portion of config file" << endl;
 286         return false;
 287       }
 288       AC_cfg->GetNextConfigLine();
 289     }
 290   }
 291   return true;
 292 }
 293
 294 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 295
 296 double FGFCS::GetComponentOutput(eParam idx) {
 297   return Components[idx]->GetOutput();
 298 }
 299
 300 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 301
 302 string FGFCS::GetComponentName(int idx) {
 303   return Components[idx]->GetName();
 304 }
 305
 306 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 307
 308 double FGFCS::GetBrake(FGLGear::BrakeGroup bg)
 309 {
 310   switch (bg) {
 311   case FGLGear::bgLeft:
 312     return LeftBrake;
 313   case FGLGear::bgRight:
 314     return RightBrake;
 315   case FGLGear::bgCenter:
 316     return CenterBrake;
 317   default:
 318     cerr << "GetBrake asked to return a bogus brake value" << endl;
 319   }
 320   return 0.0;
 321 }
 322
 323 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 324
 325 string FGFCS::GetComponentStrings(void)
 326 {
 327   unsigned int comp;
 328   string CompStrings = "";
 329   bool firstime = true;
 330
 331   for (comp = 0; comp < Components.size(); comp++) {
 332     if (firstime) firstime = false;
 333     else          CompStrings += ", ";
 334
 335     CompStrings += Components[comp]->GetName();
 336   }
 337
 338   return CompStrings;
 339 }
 340
 341 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 342
 343 string FGFCS::GetComponentValues(void)
 344 {
 345   unsigned int comp;
 346   string CompValues = "";
 347   char buffer[10];
 348   bool firstime = true;
 349
 350   for (comp = 0; comp < Components.size(); comp++) {
 351     if (firstime) firstime = false;
 352     else          CompValues += ", ";
 353
 354     sprintf(buffer, "%9.6f", Components[comp]->GetOutput());
 355     CompValues += string(buffer);
 356   }
 357
 358   return CompValues;
 359 }
 360
 361 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 362
 363 void FGFCS::AddThrottle(void)
 364 {
 365   ThrottleCmd.push_back(0.0);
 366   ThrottlePos.push_back(0.0);
 367   MixtureCmd.push_back(0.0);     // assume throttle and mixture are coupled
 368   MixturePos.push_back(0.0);
 369   PropAdvanceCmd.push_back(0.0); // assume throttle and prop pitch are coupled
 370   PropAdvance.push_back(0.0);
 371 }
 372
 373 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 374 //    The bitmasked value choices are as follows:
 375 //    unset: In this case (the default) JSBSim would only print
 376 //       out the normally expected messages, essentially echoing
 377 //       the config files as they are read. If the environment
 378 //       variable is not set, debug_lvl is set to 1 internally
 379 //    0: This requests JSBSim not to output any messages
 380 //       whatsoever.
 381 //    1: This value explicity requests the normal JSBSim
 382 //       startup messages
 383 //    2: This value asks for a message to be printed out when
 384 //       a class is instantiated
 385 //    4: When this value is set, a message is displayed when a
 386 //       FGModel object executes its Run() method
 387 //    8: When this value is set, various runtime state variables
 388 //       are printed out periodically
 389 //    16: When set various parameters are sanity checked and
 390 //       a message is printed out when they go out of bounds
 391
 392 void FGFCS::Debug(int from)
 393 {
 394   if (debug_lvl <= 0) return;
 395
 396   if (debug_lvl & 1) { // Standard console startup message output
 397     if (from == 0) { // Constructor
 398
 399     }
 400   }
 401   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 402     if (from == 0) cout << "Instantiated: FGFCS" << endl;
 403     if (from == 1) cout << "Destroyed:    FGFCS" << endl;
 404   }
 405   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 406   }
 407   if (debug_lvl & 8 ) { // Runtime state variables
 408   }
 409   if (debug_lvl & 16) { // Sanity checking
 410   }
 411   if (debug_lvl & 64) {
 412     if (from == 0) { // Constructor
 413       cout << IdSrc << endl;
 414       cout << IdHdr << endl;
 415     }
 416   }
 417 }
 418