src/FDM/JSBSim/FGFCS.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
   2  \r
   3  Module:       FGFCS.cpp\r
   4  Author:       Jon Berndt\r
   5  Date started: 12/12/98\r
   6  Purpose:      Model the flight controls\r
   7  Called by:    FDMExec\r
   8  \r
   9  ------------- Copyright (C) 1999  Jon S. Berndt (jsb@hal-pc.org) -------------\r
  10  \r
  11  This program is free software; you can redistribute it and/or modify it under\r
  12  the terms of the GNU General Public License as published by the Free Software\r
  13  Foundation; either version 2 of the License, or (at your option) any later\r
  14  version.\r
  15  \r
  16  This program is distributed in the hope that it will be useful, but WITHOUT\r
  17  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS\r
  18  FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more\r
  19  details.\r
  20  \r
  21  You should have received a copy of the GNU General Public License along with\r
  22  this program; if not, write to the Free Software Foundation, Inc., 59 Temple\r
  23  Place - Suite 330, Boston, MA  02111-1307, USA.\r
  24  \r
  25  Further information about the GNU General Public License can also be found on\r
  26  the world wide web at http://www.gnu.org.\r
  27  \r
  28 FUNCTIONAL DESCRIPTION\r
  29 --------------------------------------------------------------------------------\r
  30 This class models the flight controls for a specific airplane\r
  31  \r
  32 HISTORY\r
  33 --------------------------------------------------------------------------------\r
  34 12/12/98   JSB   Created\r
  35  \r
  36 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
  37 INCLUDES\r
  38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
  39 \r
  40 #include "FGDefs.h"\r
  41 \r
  42 #include "FGFCS.h"\r
  43 #include "FGState.h"\r
  44 #include "FGFDMExec.h"\r
  45 #include "FGAtmosphere.h"\r
  46 #include "FGAircraft.h"\r
  47 #include "FGTranslation.h"\r
  48 #include "FGRotation.h"\r
  49 #include "FGPosition.h"\r
  50 #include "FGAuxiliary.h"\r
  51 #include "FGOutput.h"\r
  52 \r
  53 #include "filtersjb/FGFilter.h"\r
  54 #include "filtersjb/FGDeadBand.h"\r
  55 #include "filtersjb/FGGain.h"\r
  56 #include "filtersjb/FGGradient.h"\r
  57 #include "filtersjb/FGSwitch.h"\r
  58 #include "filtersjb/FGSummer.h"\r
  59 #include "filtersjb/FGFlaps.h"\r
  60 \r
  61 static const char *IdSrc = "$Id$";\r
  62 static const char *IdHdr = ID_FCS;\r
  63 \r
  64 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
  65 CLASS IMPLEMENTATION\r
  66 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
  67 \r
  68 FGFCS::FGFCS(FGFDMExec* fdmex) : FGModel(fdmex)\r
  69 {\r
  70   Name = "FGFCS";\r
  71 \r
  72   DaCmd = DeCmd = DrCmd = DfCmd = DsbCmd = DspCmd = PTrimCmd = 0.0;\r
  73   DaPos = DePos = DrPos = DfPos = DsbPos = DspPos = 0.0;\r
  74   LeftBrake = RightBrake = CenterBrake = 0.0;\r
  75 \r
  76   if (debug_lvl & 2) cout << "Instantiated: " << Name << endl;\r
  77 }\r
  78 \r
  79 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
  80 \r
  81 FGFCS::~FGFCS()\r
  82 {\r
  83   ThrottleCmd.clear();\r
  84   ThrottlePos.clear();\r
  85   MixtureCmd.clear();\r
  86   MixturePos.clear();\r
  87 \r
  88   unsigned int i;\r
  89 \r
  90   for(i=0;i<Components.size();i++) delete Components[i];\r
  91   if (debug_lvl & 2) cout << "Destroyed:    FGFCS" << endl;\r
  92 }\r
  93 \r
  94 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
  95 \r
  96 bool FGFCS::Run(void)\r
  97 {\r
  98   unsigned int i;\r
  99 \r
 100   if (!FGModel::Run()) {\r
 101     for (i=0; i<ThrottlePos.size(); i++) ThrottlePos[i] = ThrottleCmd[i];\r
 102     for (i=0; i<MixturePos.size(); i++) MixturePos[i] = MixtureCmd[i];\r
 103     for (i=0; i<Components.size(); i++)  Components[i]->Run();\r
 104   } else {\r
 105   }\r
 106 \r
 107   return false;\r
 108 }\r
 109 \r
 110 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
 111 \r
 112 void FGFCS::SetThrottleCmd(int engineNum, float setting)\r
 113 {\r
 114   unsigned int ctr;\r
 115 \r
 116   if ((int)ThrottleCmd.size() > engineNum) {\r
 117     if (engineNum < 0) {\r
 118       for (ctr=0;ctr<=ThrottleCmd.size();ctr++) ThrottleCmd[ctr] = setting;\r
 119     } else {\r
 120       ThrottleCmd[engineNum] = setting;\r
 121     }\r
 122   } else {\r
 123     cerr << "Throttle " << engineNum << " does not exist! " << ThrottleCmd.size()\r
 124          << " engines exist, but attempted throttle command is for engine "\r
 125          << engineNum << endl;\r
 126   }\r
 127 }\r
 128 \r
 129 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
 130 \r
 131 void FGFCS::SetThrottlePos(int engineNum, float setting)\r
 132 {\r
 133   unsigned int ctr;\r
 134 \r
 135   if ((int)ThrottlePos.size() > engineNum) {\r
 136     if (engineNum < 0) {\r
 137       for (ctr=0;ctr<=ThrottlePos.size();ctr++) ThrottlePos[ctr] = setting;\r
 138     } else {\r
 139       ThrottlePos[engineNum] = setting;\r
 140     }\r
 141   } else {\r
 142     cerr << "Throttle " << engineNum << " does not exist! " << ThrottlePos.size()\r
 143          << " engines exist, but attempted throttle position setting is for engine "\r
 144          << engineNum << endl;\r
 145   }\r
 146 }\r
 147 \r
 148 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
 149 \r
 150 float FGFCS::GetThrottleCmd(int engineNum)\r
 151 {\r
 152   if ((int)ThrottleCmd.size() > engineNum) {\r
 153     if (engineNum < 0) {\r
 154        cerr << "Cannot get throttle value for ALL engines" << endl;\r
 155     } else {\r
 156       return ThrottleCmd[engineNum];\r
 157     }\r
 158   } else {\r
 159     cerr << "Throttle " << engineNum << " does not exist! " << ThrottleCmd.size()\r
 160          << " engines exist, but throttle setting for engine " << engineNum\r
 161                << " is selected" << endl;\r
 162   }\r
 163 }\r
 164 \r
 165 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
 166 \r
 167 float FGFCS::GetThrottlePos(int engineNum)\r
 168 {\r
 169   if ((int)ThrottlePos.size() > engineNum) {\r
 170     if (engineNum < 0) {\r
 171        cerr << "Cannot get throttle value for ALL engines" << endl;\r
 172     } else {\r
 173       return ThrottlePos[engineNum];\r
 174     }\r
 175   } else {\r
 176     cerr << "Throttle " << engineNum << " does not exist! " << ThrottlePos.size()\r
 177          << " engines exist, but attempted throttle position setting is for engine "\r
 178          << engineNum << endl;\r
 179   }\r
 180 }\r
 181 \r
 182 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
 183 \r
 184 void FGFCS::SetMixtureCmd(int engineNum, float setting)\r
 185 {\r
 186   unsigned int ctr;\r
 187 \r
 188   if (engineNum < 0) {\r
 189     for (ctr=0;ctr<MixtureCmd.size();ctr++) MixtureCmd[ctr] = setting;\r
 190   } else {\r
 191     MixtureCmd[engineNum] = setting;\r
 192   }\r
 193 }\r
 194 \r
 195 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
 196 \r
 197 void FGFCS::SetMixturePos(int engineNum, float setting)\r
 198 {\r
 199   unsigned int ctr;\r
 200 \r
 201   if (engineNum < 0) {\r
 202     for (ctr=0;ctr<=MixtureCmd.size();ctr++) MixturePos[ctr] = MixtureCmd[ctr];\r
 203   } else {\r
 204     MixturePos[engineNum] = setting;\r
 205   }\r
 206 }\r
 207 \r
 208 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
 209 \r
 210 bool FGFCS::Load(FGConfigFile* AC_cfg)\r
 211 {\r
 212   string token;\r
 213 \r
 214   Name = Name + ":" + AC_cfg->GetValue("NAME");\r
 215   if (debug_lvl > 0) cout << "    Control System Name: " << Name << endl;\r
 216   AC_cfg->GetNextConfigLine();\r
 217   while ((token = AC_cfg->GetValue()) != "/FLIGHT_CONTROL") {\r
 218     if (token == "COMPONENT") {\r
 219       token = AC_cfg->GetValue("TYPE");\r
 220       if (debug_lvl > 0) cout << "    Loading Component \""\r
 221                               << AC_cfg->GetValue("NAME")\r
 222                                                 << "\" of type: " << token << endl;\r
 223       if ((token == "LAG_FILTER") ||\r
 224           (token == "LEAD_LAG_FILTER") ||\r
 225           (token == "SECOND_ORDER_FILTER") ||\r
 226           (token == "WASHOUT_FILTER") ||\r
 227           (token == "INTEGRATOR") ) {\r
 228         Components.push_back(new FGFilter(this, AC_cfg));\r
 229       } else if ((token == "PURE_GAIN") ||\r
 230                  (token == "SCHEDULED_GAIN") ||\r
 231                  (token == "AEROSURFACE_SCALE") ) {\r
 232 \r
 233         Components.push_back(new FGGain(this, AC_cfg));\r
 234 \r
 235       } else if (token == "SUMMER") {\r
 236         Components.push_back(new FGSummer(this, AC_cfg));\r
 237       } else if (token == "DEADBAND") {\r
 238         Components.push_back(new FGDeadBand(this, AC_cfg));\r
 239       } else if (token == "GRADIENT") {\r
 240         Components.push_back(new FGGradient(this, AC_cfg));\r
 241       } else if (token == "SWITCH") {\r
 242         Components.push_back(new FGSwitch(this, AC_cfg));\r
 243       } else if (token == "FLAPS") {\r
 244         Components.push_back(new FGFlaps(this, AC_cfg));\r
 245       } else {\r
 246         cerr << "Unknown token [" << token << "] in FCS portion of config file" << endl;\r
 247         return false;\r
 248       }\r
 249       AC_cfg->GetNextConfigLine();\r
 250     }\r
 251   }\r
 252   return true;\r
 253 }\r
 254 \r
 255 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
 256 \r
 257 float FGFCS::GetComponentOutput(eParam idx) {\r
 258   return Components[idx]->GetOutput();\r
 259 }\r
 260 \r
 261 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
 262 \r
 263 string FGFCS::GetComponentName(int idx) {\r
 264   return Components[idx]->GetName();\r
 265 }\r
 266 \r
 267 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
 268 \r
 269 float FGFCS::GetBrake(FGLGear::BrakeGroup bg) {\r
 270   switch (bg) {\r
 271   case FGLGear::bgLeft:\r
 272     return LeftBrake;\r
 273   case FGLGear::bgRight:\r
 274     return RightBrake;\r
 275   case FGLGear::bgCenter:\r
 276     return CenterBrake;\r
 277   default:\r
 278     cerr << "GetBrake asked to return a bogus brake value" << endl;\r
 279   }\r
 280   return 0.0;\r
 281 }\r
 282 \r
 283 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
 284 \r
 285 string FGFCS::GetComponentStrings(void)\r
 286 {\r
 287   unsigned int comp;\r
 288 \r
 289   string CompStrings = "";\r
 290   bool firstime = true;\r
 291 \r
 292   for (comp = 0; comp < Components.size(); comp++) {\r
 293     if (firstime) firstime = false;\r
 294     else          CompStrings += ", ";\r
 295 \r
 296     CompStrings += Components[comp]->GetName();\r
 297   }\r
 298 \r
 299   return CompStrings;\r
 300 }\r
 301 \r
 302 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
 303 \r
 304 string FGFCS::GetComponentValues(void)\r
 305 {\r
 306   unsigned int comp;\r
 307 \r
 308   string CompValues = "";\r
 309   char buffer[10];\r
 310   bool firstime = true;\r
 311 \r
 312   for (comp = 0; comp < Components.size(); comp++) {\r
 313     if (firstime) firstime = false;\r
 314     else          CompValues += ", ";\r
 315 \r
 316     sprintf(buffer, "%9.6f", Components[comp]->GetOutput());\r
 317     CompValues += string(buffer);\r
 318   }\r
 319 \r
 320   return CompValues;\r
 321 }\r
 322 \r
 323 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
 324 \r
 325 void FGFCS::AddThrottle(void)\r
 326 {\r
 327   ThrottleCmd.push_back(0.0);\r
 328   ThrottlePos.push_back(0.0);\r
 329   MixtureCmd.push_back(0.0);    // assume throttle and mixture are coupled\r
 330   MixturePos.push_back(0.0);\r
 331 }\r
 332 \r
 333 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
 334 \r
 335 void FGFCS::Debug(void)\r
 336 {\r
 337     //TODO: Add your source code here\r
 338 }\r
 339 \r