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   int i;
  69   Name = "FGFCS";
  70
  71   DaCmd = DeCmd = DrCmd = DfCmd = DsbCmd = DspCmd = 0.0;
  72   PTrimCmd = YTrimCmd = RTrimCmd = 0.0;
  73   DaLPos = DaRPos = DePos = DrPos = DfPos = DsbPos = DspPos = 0.0;
  74   GearCmd = GearPos = 1; // default to gear down
  75   LeftBrake = RightBrake = CenterBrake = 0.0;
  76   DoNormalize=true;
  77
  78   for(i=0;i<6;i++) { ToNormalize[i]=-1;}
  79   Debug(0);
  80 }
  81
  82 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  83
  84 FGFCS::~FGFCS()
  85 {
  86   ThrottleCmd.clear();
  87   ThrottlePos.clear();
  88   MixtureCmd.clear();
  89   MixturePos.clear();
  90   PropAdvanceCmd.clear();
  91   PropAdvance.clear();
  92
  93   unsigned int i;
  94
  95   for (i=0;i<Components.size();i++) delete Components[i];
  96   Debug(1);
  97 }
  98
  99 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 100
 101 bool FGFCS::Run(void)
 102 {
 103   unsigned int i;
 104
 105   if (!FGModel::Run()) {
 106     for (i=0; i<ThrottlePos.size(); i++) ThrottlePos[i] = ThrottleCmd[i];
 107     for (i=0; i<MixturePos.size(); i++) MixturePos[i] = MixtureCmd[i];
 108     for (i=0; i<PropAdvance.size(); i++) PropAdvance[i] = PropAdvanceCmd[i];
 109     for (i=0; i<Components.size(); i++)  Components[i]->Run();
 110     if(DoNormalize) Normalize();
 111   } else {
 112   }
 113
 114   return false;
 115 }
 116
 117 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 118
 119 void FGFCS::SetThrottleCmd(int engineNum, double setting)
 120 {
 121   unsigned int ctr;
 122
 123   if (engineNum < (int)ThrottlePos.size()) {
 124     if (engineNum < 0) {
 125       for (ctr=0;ctr<ThrottleCmd.size();ctr++) ThrottleCmd[ctr] = setting;
 126     } else {
 127       ThrottleCmd[engineNum] = setting;
 128     }
 129   } else {
 130     cerr << "Throttle " << engineNum << " does not exist! " << ThrottleCmd.size()
 131          << " engines exist, but attempted throttle command is for engine "
 132          << engineNum << endl;
 133   }
 134 }
 135
 136 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 137
 138 void FGFCS::SetThrottlePos(int engineNum, double setting)
 139 {
 140   unsigned int ctr;
 141
 142   if (engineNum < (int)ThrottlePos.size()) {
 143     if (engineNum < 0) {
 144       for (ctr=0;ctr<ThrottlePos.size();ctr++) ThrottlePos[ctr] = setting;
 145     } else {
 146       ThrottlePos[engineNum] = setting;
 147     }
 148   } else {
 149     cerr << "Throttle " << engineNum << " does not exist! " << ThrottlePos.size()
 150          << " engines exist, but attempted throttle position setting is for engine "
 151          << engineNum << endl;
 152   }
 153 }
 154
 155 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 156
 157 double FGFCS::GetThrottleCmd(int engineNum)
 158 {
 159   if (engineNum < (int)ThrottlePos.size()) {
 160     if (engineNum < 0) {
 161        cerr << "Cannot get throttle value for ALL engines" << endl;
 162     } else {
 163       return ThrottleCmd[engineNum];
 164     }
 165   } else {
 166     cerr << "Throttle " << engineNum << " does not exist! " << ThrottleCmd.size()
 167          << " engines exist, but throttle setting for engine " << engineNum
 168          << " is selected" << endl;
 169   }
 170   return 0.0;
 171 }
 172
 173 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 174
 175 double FGFCS::GetThrottlePos(int engineNum)
 176 {
 177   if (engineNum < (int)ThrottlePos.size()) {
 178     if (engineNum < 0) {
 179        cerr << "Cannot get throttle value for ALL engines" << endl;
 180     } else {
 181       return ThrottlePos[engineNum];
 182     }
 183   } else {
 184     cerr << "Throttle " << engineNum << " does not exist! " << ThrottlePos.size()
 185          << " engines exist, but attempted throttle position setting is for engine "
 186          << engineNum << endl;
 187   }
 188   return 0.0;
 189 }
 190
 191 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 192
 193 void FGFCS::SetMixtureCmd(int engineNum, double setting)
 194 {
 195   unsigned int ctr;
 196
 197   if (engineNum < (int)ThrottlePos.size()) {
 198     if (engineNum < 0) {
 199       for (ctr=0;ctr<MixtureCmd.size();ctr++) MixtureCmd[ctr] = setting;
 200     } else {
 201       MixtureCmd[engineNum] = setting;
 202     }
 203   }
 204 }
 205
 206 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 207
 208 void FGFCS::SetMixturePos(int engineNum, double setting)
 209 {
 210   unsigned int ctr;
 211
 212   if (engineNum < (int)ThrottlePos.size()) {
 213     if (engineNum < 0) {
 214       for (ctr=0;ctr<=MixtureCmd.size();ctr++) MixturePos[ctr] = MixtureCmd[ctr];
 215     } else {
 216       MixturePos[engineNum] = setting;
 217     }
 218   }
 219 }
 220
 221 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 222
 223 void FGFCS::SetPropAdvanceCmd(int engineNum, double setting)
 224 {
 225   unsigned int ctr;
 226
 227   if (engineNum < (int)ThrottlePos.size()) {
 228     if (engineNum < 0) {
 229       for (ctr=0;ctr<PropAdvanceCmd.size();ctr++) PropAdvanceCmd[ctr] = setting;
 230     } else {
 231       PropAdvanceCmd[engineNum] = setting;
 232     }
 233   }
 234 }
 235
 236 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 237
 238 void FGFCS::SetPropAdvance(int engineNum, double setting)
 239 {
 240   unsigned int ctr;
 241
 242   if (engineNum < (int)ThrottlePos.size()) {
 243     if (engineNum < 0) {
 244       for (ctr=0;ctr<=PropAdvanceCmd.size();ctr++) PropAdvance[ctr] = PropAdvanceCmd[ctr];
 245     } else {
 246       PropAdvance[engineNum] = setting;
 247     }
 248   }
 249 }
 250
 251 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 252
 253 bool FGFCS::Load(FGConfigFile* AC_cfg)
 254 {
 255   string token;
 256   unsigned i;
 257
 258   Name = Name + ":" + AC_cfg->GetValue("NAME");
 259   if (debug_lvl > 0) cout << "    Control System Name: " << Name << endl;
 260   if( AC_cfg->GetValue("NORMALIZE") == "FALSE") {
 261       DoNormalize=false;
 262       cout << "    Automatic Control Surface Normalization Disabled" << endl;
 263   }
 264   AC_cfg->GetNextConfigLine();
 265   while ((token = AC_cfg->GetValue()) != string("/FLIGHT_CONTROL")) {
 266     if (token == "COMPONENT") {
 267       token = AC_cfg->GetValue("TYPE");
 268       if (debug_lvl > 0) cout << endl << "    Loading Component \""
 269                               << AC_cfg->GetValue("NAME")
 270                               << "\" of type: " << token << endl;
 271       if ((token == "LAG_FILTER") ||
 272           (token == "LEAD_LAG_FILTER") ||
 273           (token == "SECOND_ORDER_FILTER") ||
 274           (token == "WASHOUT_FILTER") ||
 275           (token == "INTEGRATOR") ) {
 276         Components.push_back(new FGFilter(this, AC_cfg));
 277       } else if ((token == "PURE_GAIN") ||
 278                  (token == "SCHEDULED_GAIN") ||
 279                  (token == "AEROSURFACE_SCALE") ) {
 280
 281         Components.push_back(new FGGain(this, AC_cfg));
 282
 283       } else if (token == "SUMMER") {
 284         Components.push_back(new FGSummer(this, AC_cfg));
 285       } else if (token == "DEADBAND") {
 286         Components.push_back(new FGDeadBand(this, AC_cfg));
 287       } else if (token == "GRADIENT") {
 288         Components.push_back(new FGGradient(this, AC_cfg));
 289       } else if (token == "SWITCH") {
 290         Components.push_back(new FGSwitch(this, AC_cfg));
 291       } else if (token == "KINEMAT") {
 292         Components.push_back(new FGKinemat(this, AC_cfg));
 293       } else {
 294         cerr << "Unknown token [" << token << "] in FCS portion of config file" << endl;
 295         return false;
 296       }
 297       AC_cfg->GetNextConfigLine();
 298     }
 299   }
 300   //collect information for normalizing control surfaces
 301
 302   for(i=0;i<Components.size();i++) {
 303
 304     if(Components[i]->GetType() == "AEROSURFACE_SCALE"
 305         || Components[i]->GetType() == "KINEMAT"  ) {
 306       if( Components[i]->GetOutputIdx() == FG_ELEVATOR_POS ) {
 307         ToNormalize[iNDe]=i;
 308       } else if ( Components[i]->GetOutputIdx() == FG_LEFT_AILERON_POS
 309                       || Components[i]->GetOutputIdx() == FG_AILERON_POS ) {
 310         ToNormalize[iNDaL]=i;
 311       } else if ( Components[i]->GetOutputIdx() == FG_RIGHT_AILERON_POS ) {
 312         ToNormalize[iNDaR]=i;
 313       } else if ( Components[i]->GetOutputIdx() == FG_RUDDER_POS ) {
 314         ToNormalize[iNDr]=i;
 315       } else if ( Components[i]->GetOutputIdx() == FG_SPDBRAKE_POS ) {
 316         ToNormalize[iNDsb]=i;
 317       } else if ( Components[i]->GetOutputIdx() == FG_SPOILERS_POS ) {
 318         ToNormalize[iNDsp]=i;
 319       } else if ( Components[i]->GetOutputIdx() == FG_FLAPS_POS ) {
 320         ToNormalize[iNDf]=i;
 321       }
 322     }
 323   }
 324
 325   return true;
 326 }
 327
 328 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 329
 330 double FGFCS::GetComponentOutput(eParam idx) {
 331   return Components[idx]->GetOutput();
 332 }
 333
 334 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 335
 336 string FGFCS::GetComponentName(int idx) {
 337   return Components[idx]->GetName();
 338 }
 339
 340 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 341
 342 double FGFCS::GetBrake(FGLGear::BrakeGroup bg)
 343 {
 344   switch (bg) {
 345   case FGLGear::bgLeft:
 346     return LeftBrake;
 347   case FGLGear::bgRight:
 348     return RightBrake;
 349   case FGLGear::bgCenter:
 350     return CenterBrake;
 351   default:
 352     cerr << "GetBrake asked to return a bogus brake value" << endl;
 353   }
 354   return 0.0;
 355 }
 356
 357 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 358
 359 string FGFCS::GetComponentStrings(void)
 360 {
 361   unsigned int comp;
 362   string CompStrings = "";
 363   bool firstime = true;
 364
 365   for (comp = 0; comp < Components.size(); comp++) {
 366     if (firstime) firstime = false;
 367     else          CompStrings += ", ";
 368
 369     CompStrings += Components[comp]->GetName();
 370   }
 371
 372   return CompStrings;
 373 }
 374
 375 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 376
 377 string FGFCS::GetComponentValues(void)
 378 {
 379   unsigned int comp;
 380   string CompValues = "";
 381   char buffer[10];
 382   bool firstime = true;
 383
 384   for (comp = 0; comp < Components.size(); comp++) {
 385     if (firstime) firstime = false;
 386     else          CompValues += ", ";
 387
 388     sprintf(buffer, "%9.6f", Components[comp]->GetOutput());
 389     CompValues += string(buffer);
 390   }
 391
 392   return CompValues;
 393 }
 394
 395 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 396
 397 void FGFCS::AddThrottle(void)
 398 {
 399   ThrottleCmd.push_back(0.0);
 400   ThrottlePos.push_back(0.0);
 401   MixtureCmd.push_back(0.0);     // assume throttle and mixture are coupled
 402   MixturePos.push_back(0.0);
 403   PropAdvanceCmd.push_back(0.0); // assume throttle and prop pitch are coupled
 404   PropAdvance.push_back(0.0);
 405 }
 406
 407 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 408
 409 void FGFCS::Normalize(void) {
 410   if( ToNormalize[iNDe] > -1 )
 411     DePosN = Components[ToNormalize[iNDe]]->GetOutputPct();
 412
 413   if( ToNormalize[iNDaL] > -1 )
 414     DaLPosN = Components[ToNormalize[iNDaL]]->GetOutputPct();
 415
 416   if( ToNormalize[iNDaR] > -1 )
 417     DaRPosN = Components[ToNormalize[iNDaR]]->GetOutputPct();
 418
 419   if( ToNormalize[iNDr] > -1 )
 420     DrPosN = Components[ToNormalize[iNDr]]->GetOutputPct();
 421
 422   if( ToNormalize[iNDsb] > -1 )
 423     DsbPosN = Components[ToNormalize[iNDsb]]->GetOutputPct();
 424
 425   if( ToNormalize[iNDsp] > -1 )
 426     DspPosN = Components[ToNormalize[iNDsp]]->GetOutputPct();
 427
 428   if( ToNormalize[iNDf] > -1 )
 429     DfPosN = Components[ToNormalize[iNDf]]->GetOutputPct();
 430
 431 }
 432
 433 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 434 //    The bitmasked value choices are as follows:
 435 //    unset: In this case (the default) JSBSim would only print
 436 //       out the normally expected messages, essentially echoing
 437 //       the config files as they are read. If the environment
 438 //       variable is not set, debug_lvl is set to 1 internally
 439 //    0: This requests JSBSim not to output any messages
 440 //       whatsoever.
 441 //    1: This value explicity requests the normal JSBSim
 442 //       startup messages
 443 //    2: This value asks for a message to be printed out when
 444 //       a class is instantiated
 445 //    4: When this value is set, a message is displayed when a
 446 //       FGModel object executes its Run() method
 447 //    8: When this value is set, various runtime state variables
 448 //       are printed out periodically
 449 //    16: When set various parameters are sanity checked and
 450 //       a message is printed out when they go out of bounds
 451
 452 void FGFCS::Debug(int from)
 453 {
 454   if (debug_lvl <= 0) return;
 455
 456   if (debug_lvl & 1) { // Standard console startup message output
 457     if (from == 0) { // Constructor
 458
 459     }
 460   }
 461   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 462     if (from == 0) cout << "Instantiated: FGFCS" << endl;
 463     if (from == 1) cout << "Destroyed:    FGFCS" << endl;
 464   }
 465   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 466   }
 467   if (debug_lvl & 8 ) { // Runtime state variables
 468   }
 469   if (debug_lvl & 16) { // Sanity checking
 470   }
 471   if (debug_lvl & 64) {
 472     if (from == 0) { // Constructor
 473       cout << IdSrc << endl;
 474       cout << IdHdr << endl;
 475     }
 476   }
 477 }
 478