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, double setting)
 111 {
 112   unsigned int ctr;
 113
 114   if (engineNum < (int)ThrottlePos.size()) {
 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, double setting)
 130 {
 131   unsigned int ctr;
 132
 133   if (engineNum < (int)ThrottlePos.size()) {
 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 double FGFCS::GetThrottleCmd(int engineNum)
 149 {
 150   if (engineNum < (int)ThrottlePos.size()) {
 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 double FGFCS::GetThrottlePos(int engineNum)
 166 {
 167   if (engineNum < (int)ThrottlePos.size()) {
 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, double setting)
 183 {
 184   unsigned int ctr;
 185
 186   if (engineNum < (int)ThrottlePos.size()) {
 187     if (engineNum < 0) {
 188       for (ctr=0;ctr<MixtureCmd.size();ctr++) MixtureCmd[ctr] = setting;
 189     } else {
 190       MixtureCmd[engineNum] = setting;
 191     }
 192   }
 193 }
 194
 195 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 196
 197 void FGFCS::SetMixturePos(int engineNum, double setting)
 198 {
 199   unsigned int ctr;
 200
 201   if (engineNum < (int)ThrottlePos.size()) {
 202     if (engineNum < 0) {
 203       for (ctr=0;ctr<=MixtureCmd.size();ctr++) MixturePos[ctr] = MixtureCmd[ctr];
 204     } else {
 205       MixturePos[engineNum] = setting;
 206     }
 207   }
 208 }
 209
 210 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 211
 212 bool FGFCS::Load(FGConfigFile* AC_cfg)
 213 {
 214   string token;
 215
 216   Name = Name + ":" + AC_cfg->GetValue("NAME");
 217   if (debug_lvl > 0) cout << "    Control System Name: " << Name << endl;
 218   AC_cfg->GetNextConfigLine();
 219   while ((token = AC_cfg->GetValue()) != string("/FLIGHT_CONTROL")) {
 220     if (token == "COMPONENT") {
 221       token = AC_cfg->GetValue("TYPE");
 222       if (debug_lvl > 0) cout << "    Loading Component \""
 223                               << AC_cfg->GetValue("NAME")
 224                                                 << "\" of type: " << token << endl;
 225       if ((token == "LAG_FILTER") ||
 226           (token == "LEAD_LAG_FILTER") ||
 227           (token == "SECOND_ORDER_FILTER") ||
 228           (token == "WASHOUT_FILTER") ||
 229           (token == "INTEGRATOR") ) {
 230         Components.push_back(new FGFilter(this, AC_cfg));
 231       } else if ((token == "PURE_GAIN") ||
 232                  (token == "SCHEDULED_GAIN") ||
 233                  (token == "AEROSURFACE_SCALE") ) {
 234
 235         Components.push_back(new FGGain(this, AC_cfg));
 236
 237       } else if (token == "SUMMER") {
 238         Components.push_back(new FGSummer(this, AC_cfg));
 239       } else if (token == "DEADBAND") {
 240         Components.push_back(new FGDeadBand(this, AC_cfg));
 241       } else if (token == "GRADIENT") {
 242         Components.push_back(new FGGradient(this, AC_cfg));
 243       } else if (token == "SWITCH") {
 244         Components.push_back(new FGSwitch(this, AC_cfg));
 245       } else if (token == "FLAPS") {
 246         Components.push_back(new FGFlaps(this, AC_cfg));
 247       } else {
 248         cerr << "Unknown token [" << token << "] in FCS portion of config file" << endl;
 249         return false;
 250       }
 251       AC_cfg->GetNextConfigLine();
 252     }
 253   }
 254   return true;
 255 }
 256
 257 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 258
 259 double FGFCS::GetComponentOutput(eParam idx) {
 260   return Components[idx]->GetOutput();
 261 }
 262
 263 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 264
 265 string FGFCS::GetComponentName(int idx) {
 266   return Components[idx]->GetName();
 267 }
 268
 269 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 270
 271 double FGFCS::GetBrake(FGLGear::BrakeGroup bg) {
 272   switch (bg) {
 273   case FGLGear::bgLeft:
 274     return LeftBrake;
 275   case FGLGear::bgRight:
 276     return RightBrake;
 277   case FGLGear::bgCenter:
 278     return CenterBrake;
 279   default:
 280     cerr << "GetBrake asked to return a bogus brake value" << endl;
 281   }
 282   return 0.0;
 283 }
 284
 285 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 286
 287 string FGFCS::GetComponentStrings(void)
 288 {
 289   unsigned int comp;
 290
 291   string CompStrings = "";
 292   bool firstime = true;
 293
 294   for (comp = 0; comp < Components.size(); comp++) {
 295     if (firstime) firstime = false;
 296     else          CompStrings += ", ";
 297
 298     CompStrings += Components[comp]->GetName();
 299   }
 300
 301   return CompStrings;
 302 }
 303
 304 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 305
 306 string FGFCS::GetComponentValues(void)
 307 {
 308   unsigned int comp;
 309
 310   string CompValues = "";
 311   char buffer[10];
 312   bool firstime = true;
 313
 314   for (comp = 0; comp < Components.size(); comp++) {
 315     if (firstime) firstime = false;
 316     else          CompValues += ", ";
 317
 318     sprintf(buffer, "%9.6f", Components[comp]->GetOutput());
 319     CompValues += string(buffer);
 320   }
 321
 322   return CompValues;
 323 }
 324
 325 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 326
 327 void FGFCS::AddThrottle(void)
 328 {
 329   ThrottleCmd.push_back(0.0);
 330   ThrottlePos.push_back(0.0);
 331   MixtureCmd.push_back(0.0);    // assume throttle and mixture are coupled
 332   MixturePos.push_back(0.0);
 333 }
 334
 335 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 336
 337 void FGFCS::Debug(void)
 338 {
 339     //TODO: Add your source code here
 340 }
 341