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