src/FDM/JSBSim/filtersjb/FGGain.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Module:       FGGain.cpp
   4  Author:       Jon S. Berndt
   5  Date started: 4/2000
   6
   7  ------------- Copyright (C) 2000 -------------
   8
   9  This program is free software; you can redistribute it and/or modify it under
  10  the terms of the GNU General Public License as published by the Free Software
  11  Foundation; either version 2 of the License, or (at your option) any later
  12  version.
  13
  14  This program is distributed in the hope that it will be useful, but WITHOUT
  15  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  16  FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
  17  details.
  18
  19  You should have received a copy of the GNU General Public License along with
  20  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  21  Place - Suite 330, Boston, MA  02111-1307, USA.
  22
  23  Further information about the GNU General Public License can also be found on
  24  the world wide web at http://www.gnu.org.
  25
  26 FUNCTIONAL DESCRIPTION
  27 --------------------------------------------------------------------------------
  28
  29 HISTORY
  30 --------------------------------------------------------------------------------
  31
  32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  33 COMMENTS, REFERENCES,  and NOTES
  34 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  35
  36 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  37 INCLUDES
  38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  39
  40 #include "FGGain.h"
  41
  42 static const char *IdSrc = "$Header$";
  43 static const char *IdHdr = ID_GAIN;
  44
  45 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  46 CLASS IMPLEMENTATION
  47 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  48
  49 // *****************************************************************************
  50 //  Function:   Constructor
  51 //  Purpose:    Builds a Gain-type of FCS component.
  52 //  Parameters: void
  53 //  Comments:   Types are PURE_GAIN, SCHEDULED_GAIN, and AEROSURFACE_SCALE
  54
  55 FGGain::FGGain(FGFCS* fcs, FGConfigFile* AC_cfg) : FGFCSComponent(fcs),
  56                                                    AC_cfg(AC_cfg)
  57 {
  58   string token;
  59   string strScheduledBy;
  60
  61   lookup = NULL;
  62   Schedule.clear();
  63   Gain = 1.000;
  64   Min = Max = 0;
  65   ScheduledBy = FG_UNDEF;
  66
  67   Type = AC_cfg->GetValue("TYPE");
  68   Name = AC_cfg->GetValue("NAME");
  69   AC_cfg->GetNextConfigLine();
  70
  71   while ((token = AC_cfg->GetValue()) != "/COMPONENT") {
  72     *AC_cfg >> token;
  73     if (token == "ID") {
  74       *AC_cfg >> ID;
  75       cout << "      ID: " << ID << endl;
  76     } else if (token == "INPUT") {
  77       token = AC_cfg->GetValue("INPUT");
  78       cout << "      INPUT: " << token << endl;
  79       if (token.find("FG_") != token.npos) {
  80         *AC_cfg >> token;
  81         InputIdx = fcs->GetState()->GetParameterIndex(token);
  82         InputType = itPilotAC;
  83       } else {
  84         *AC_cfg >> InputIdx;
  85         InputType = itFCS;
  86       }
  87     } else if (token == "GAIN") {
  88       *AC_cfg >> Gain;
  89       cout << "      GAIN: " << Gain << endl;
  90     } else if (token == "MIN") {
  91       *AC_cfg >> Min;
  92       cout << "      MIN: " << Min << endl;
  93     } else if (token == "MAX") {
  94       *AC_cfg >> Max;
  95       cout << "      MAX: " << Max << endl;
  96     } else if (token == "SCHEDULED_BY") {
  97       token = AC_cfg->GetValue("SCHEDULED_BY");
  98       if (token.find("FG_") != token.npos) {
  99         *AC_cfg >> strScheduledBy;
 100         ScheduledBy = fcs->GetState()->GetParameterIndex(strScheduledBy);
 101         cout << "      Scheduled by parameter: " << token << endl;
 102       } else {
 103         *AC_cfg >> ScheduledBy;
 104         cout << "      Scheduled by FCS output: " << ScheduledBy << endl;
 105       }
 106     } else if (token == "OUTPUT") {
 107       IsOutput = true;
 108       *AC_cfg >> sOutputIdx;
 109       OutputIdx = fcs->GetState()->GetParameterIndex(sOutputIdx);
 110       cout << "      OUTPUT: " << sOutputIdx << endl;
 111     } else {
 112       AC_cfg->ResetLineIndexToZero();
 113       lookup = new float[2];
 114       *AC_cfg >> lookup[0] >> lookup[1];
 115       cout << "        " << lookup[0] << "  " << lookup[1] << endl;
 116       Schedule.push_back(lookup);
 117     }
 118   }
 119 }
 120
 121 // *****************************************************************************
 122 //  Function:   Run
 123 //  Purpose:
 124 //  Parameters: void
 125 //  Comments:
 126
 127 bool FGGain::Run(void )
 128 {
 129   float SchedGain = 1.0;
 130
 131   FGFCSComponent::Run(); // call the base class for initialization of Input
 132
 133   if (Type == "PURE_GAIN") {
 134
 135     Output = Gain * Input;
 136
 137   } else if (Type == "SCHEDULED_GAIN") {
 138
 139     float LookupVal = fcs->GetState()->GetParameter(ScheduledBy);
 140     unsigned int last = Schedule.size()-1;
 141     float lowVal = Schedule[0][0], hiVal = Schedule[last][0];
 142     float factor = 1.0;
 143
 144     if (LookupVal <= lowVal) Output = Gain * Schedule[0][1] * Input;
 145     else if (LookupVal >= hiVal) Output = Gain * Schedule[last][1] * Input;
 146     else {
 147       for (unsigned int ctr = 1; ctr < last; ctr++) {
 148         if (LookupVal < Schedule[ctr][0]) {
 149           hiVal = Schedule[ctr][0];
 150           lowVal = Schedule[ctr-1][0];
 151           factor = (LookupVal - lowVal) / (hiVal - lowVal);
 152           SchedGain = Schedule[ctr-1][1] + factor*(Schedule[ctr][1] - Schedule[ctr-1][1]);
 153           Output = Gain * SchedGain * Input;
 154           break;
 155         }
 156       }
 157     }
 158
 159   } else if (Type == "AEROSURFACE_SCALE") {
 160
 161     if (Output >= 0.0) Output = Input * Max;
 162     else Output = Input * (-Min);
 163
 164     Output *= Gain;
 165   }
 166
 167   if (IsOutput) SetOutput();
 168
 169   return true;
 170 }
 171