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/FGFlaps.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 = PTrimCmd = 0.0;
  71   DaPos = DePos = DrPos = DfPos = DsbPos = DspPos = 0.0;
  72   LeftBrake = RightBrake = CenterBrake = 0.0;
  73
  74   if (debug_lvl & 2) cout << "Instantiated: " << Name << endl;
  75 }
  76
  77 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  78
  79 FGFCS::~FGFCS()
  80 {
  81   ThrottleCmd.clear();
  82   ThrottlePos.clear();
  83   MixtureCmd.clear();
  84   MixturePos.clear();
  85
  86   unsigned int i;
  87
  88   for(i=0;i<Components.size();i++) delete Components[i];
  89   if (debug_lvl & 2) cout << "Destroyed:    FGFCS" << endl;
  90 }
  91
  92 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  93
  94 bool FGFCS::Run(void)
  95 {
  96   unsigned int i;
  97
  98   if (!FGModel::Run()) {
  99     for (i=0; i<ThrottlePos.size(); i++) ThrottlePos[i] = ThrottleCmd[i];
 100     for (i=0; i<MixturePos.size(); i++) MixturePos[i] = MixtureCmd[i];
 101     for (i=0; i<Components.size(); i++)  Components[i]->Run();
 102   } else {
 103   }
 104
 105   return false;
 106 }
 107
 108 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 109
 110 void FGFCS::SetThrottleCmd(int engineNum, float setting)
 111 {
 112   unsigned int ctr;
 113
 114   if ((int)ThrottleCmd.size() > engineNum) {
 115     if (engineNum < 0) {
 116       for (ctr=0;ctr<=ThrottleCmd.size();ctr++) ThrottleCmd[ctr] = setting;
 117     } else {
 118       ThrottleCmd[engineNum] = setting;
 119     }
 120   } else {
 121     cerr << "Throttle " << engineNum << " does not exist! " << ThrottleCmd.size()
 122          << " engines exist, but attempted throttle command is for engine "
 123          << engineNum << endl;
 124   }
 125 }
 126
 127 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 128
 129 void FGFCS::SetThrottlePos(int engineNum, float setting)
 130 {
 131   unsigned int ctr;
 132
 133   if ((int)ThrottlePos.size() > engineNum) {
 134     if (engineNum < 0) {
 135       for (ctr=0;ctr<=ThrottlePos.size();ctr++) ThrottlePos[ctr] = setting;
 136     } else {
 137       ThrottlePos[engineNum] = setting;
 138     }
 139   } else {
 140     cerr << "Throttle " << engineNum << " does not exist! " << ThrottlePos.size()
 141          << " engines exist, but attempted throttle position setting is for engine "
 142          << engineNum << endl;
 143   }
 144 }
 145
 146 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 147
 148 float FGFCS::GetThrottleCmd(int engineNum)
 149 {
 150   if ((int)ThrottleCmd.size() > engineNum) {
 151     if (engineNum < 0) {
 152        cerr << "Cannot get throttle value for ALL engines" << endl;
 153     } else {
 154       return ThrottleCmd[engineNum];
 155     }
 156   } else {
 157     cerr << "Throttle " << engineNum << " does not exist! " << ThrottleCmd.size()
 158          << " engines exist, but throttle setting for engine " << engineNum
 159                << " is selected" << endl;
 160   }
 161 }
 162
 163 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 164
 165 float FGFCS::GetThrottlePos(int engineNum)
 166 {
 167   if ((int)ThrottlePos.size() > engineNum) {
 168     if (engineNum < 0) {
 169        cerr << "Cannot get throttle value for ALL engines" << endl;
 170     } else {
 171       return ThrottlePos[engineNum];
 172     }
 173   } else {
 174     cerr << "Throttle " << engineNum << " does not exist! " << ThrottlePos.size()
 175          << " engines exist, but attempted throttle position setting is for engine "
 176          << engineNum << endl;
 177   }
 178 }
 179
 180 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 181
 182 void FGFCS::SetMixtureCmd(int engineNum, float setting)
 183 {
 184   unsigned int ctr;
 185
 186   if (engineNum < 0) {
 187     for (ctr=0;ctr<MixtureCmd.size();ctr++) MixtureCmd[ctr] = setting;
 188   } else {
 189     MixtureCmd[engineNum] = setting;
 190   }
 191 }
 192
 193 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 194
 195 void FGFCS::SetMixturePos(int engineNum, float setting)
 196 {
 197   unsigned int ctr;
 198
 199   if (engineNum < 0) {
 200     for (ctr=0;ctr<=MixtureCmd.size();ctr++) MixturePos[ctr] = MixtureCmd[ctr];
 201   } else {
 202     MixturePos[engineNum] = setting;
 203   }
 204 }
 205
 206 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 207
 208 bool FGFCS::Load(FGConfigFile* AC_cfg)
 209 {
 210   string token;
 211
 212   Name = Name + ":" + AC_cfg->GetValue("NAME");
 213   if (debug_lvl > 0) cout << "    Control System Name: " << Name << endl;
 214   AC_cfg->GetNextConfigLine();
 215   while ((token = AC_cfg->GetValue()) != "/FLIGHT_CONTROL") {
 216     if (token == "COMPONENT") {
 217       token = AC_cfg->GetValue("TYPE");
 218       if (debug_lvl > 0) cout << "    Loading Component \""
 219                               << AC_cfg->GetValue("NAME")
 220                                                 << "\" of type: " << token << endl;
 221       if ((token == "LAG_FILTER") ||
 222           (token == "LEAD_LAG_FILTER") ||
 223           (token == "SECOND_ORDER_FILTER") ||
 224           (token == "WASHOUT_FILTER") ||
 225           (token == "INTEGRATOR") ) {
 226         Components.push_back(new FGFilter(this, AC_cfg));
 227       } else if ((token == "PURE_GAIN") ||
 228                  (token == "SCHEDULED_GAIN") ||
 229                  (token == "AEROSURFACE_SCALE") ) {
 230
 231         Components.push_back(new FGGain(this, AC_cfg));
 232
 233       } else if (token == "SUMMER") {
 234         Components.push_back(new FGSummer(this, AC_cfg));
 235       } else if (token == "DEADBAND") {
 236         Components.push_back(new FGDeadBand(this, AC_cfg));
 237       } else if (token == "GRADIENT") {
 238         Components.push_back(new FGGradient(this, AC_cfg));
 239       } else if (token == "SWITCH") {
 240         Components.push_back(new FGSwitch(this, AC_cfg));
 241       } else if (token == "FLAPS") {
 242         Components.push_back(new FGFlaps(this, AC_cfg));
 243       } else {
 244         cerr << "Unknown token [" << token << "] in FCS portion of config file" << endl;
 245         return false;
 246       }
 247       AC_cfg->GetNextConfigLine();
 248     }
 249   }
 250   return true;
 251 }
 252
 253 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 254
 255 float FGFCS::GetComponentOutput(eParam idx) {
 256   return Components[idx]->GetOutput();
 257 }
 258
 259 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 260
 261 string FGFCS::GetComponentName(int idx) {
 262   return Components[idx]->GetName();
 263 }
 264
 265 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 266
 267 float FGFCS::GetBrake(FGLGear::BrakeGroup bg) {
 268   switch (bg) {
 269   case FGLGear::bgLeft:
 270     return LeftBrake;
 271   case FGLGear::bgRight:
 272     return RightBrake;
 273   case FGLGear::bgCenter:
 274     return CenterBrake;
 275   default:
 276     cerr << "GetBrake asked to return a bogus brake value" << endl;
 277   }
 278   return 0.0;
 279 }
 280
 281 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 282
 283 string FGFCS::GetComponentStrings(void)
 284 {
 285   unsigned int comp;
 286
 287   string CompStrings = "";
 288   bool firstime = true;
 289
 290   for (comp = 0; comp < Components.size(); comp++) {
 291     if (firstime) firstime = false;
 292     else          CompStrings += ", ";
 293
 294     CompStrings += Components[comp]->GetName();
 295   }
 296
 297   return CompStrings;
 298 }
 299
 300 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 301
 302 string FGFCS::GetComponentValues(void)
 303 {
 304   unsigned int comp;
 305
 306   string CompValues = "";
 307   char buffer[10];
 308   bool firstime = true;
 309
 310   for (comp = 0; comp < Components.size(); comp++) {
 311     if (firstime) firstime = false;
 312     else          CompValues += ", ";
 313
 314     sprintf(buffer, "%9.6f", Components[comp]->GetOutput());
 315     CompValues += string(buffer);
 316   }
 317
 318   return CompValues;
 319 }
 320
 321 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 322
 323 void FGFCS::AddThrottle(void)
 324 {
 325   ThrottleCmd.push_back(0.0);
 326   ThrottlePos.push_back(0.0);
 327   MixtureCmd.push_back(0.0);    // assume throttle and mixture are coupled
 328   MixturePos.push_back(0.0);
 329 }
 330
 331 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 332
 333 void FGFCS::Debug(void)
 334 {
 335     //TODO: Add your source code here
 336 }
 337