src/FDM/JSBSim/filtersjb/FGKinemat.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Module:       FGKinemat.cpp
   4  Author:       Tony Peden, for flight control system authored by Jon S. Berndt
   5  Date started: 12/02/01
   6
   7  ------------- Copyright (C) 2000 Anthony K. Peden -------------
   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 "FGKinemat.h"
  41
  42 static const char *IdSrc = "$Id$";
  43 static const char *IdHdr = ID_FLAPS;
  44
  45 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  46 CLASS IMPLEMENTATION
  47 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  48
  49 FGKinemat::FGKinemat(FGFCS* fcs, FGConfigFile* AC_cfg) : FGFCSComponent(fcs),
  50 AC_cfg(AC_cfg) {
  51   string token;
  52   double tmpDetent;
  53   double tmpTime;
  54
  55   Detents.clear();
  56   TransitionTimes.clear();
  57
  58   Type = AC_cfg->GetValue("TYPE");
  59   Name = AC_cfg->GetValue("NAME");
  60   AC_cfg->GetNextConfigLine();
  61
  62   while ((token = AC_cfg->GetValue()) != string("/COMPONENT")) {
  63     *AC_cfg >> token;
  64     if (token == "ID") {
  65       *AC_cfg >> ID;
  66     } else if (token == "INPUT") {
  67       token = AC_cfg->GetValue("INPUT");
  68       if (token.find("FG_") != token.npos) {
  69         *AC_cfg >> token;
  70         InputIdx = fcs->GetState()->GetParameterIndex(token);
  71         InputType = itPilotAC;
  72       }
  73     } else if ( token == "DETENTS" ) {
  74       *AC_cfg >> NumDetents;
  75       for(int i=0;i<NumDetents;i++) {
  76         *AC_cfg >> tmpDetent;
  77         *AC_cfg >> tmpTime;
  78         Detents.push_back(tmpDetent);
  79         TransitionTimes.push_back(tmpTime);
  80       }
  81     } else if (token == "OUTPUT") {
  82
  83       IsOutput = true;
  84       *AC_cfg >> sOutputIdx;
  85       OutputIdx = fcs->GetState()->GetParameterIndex(sOutputIdx);
  86     }
  87   }
  88
  89   if (debug_lvl > 1) {
  90     cout << "      ID: " << ID << endl;
  91     cout << "      INPUT: " << InputIdx << endl;
  92     cout << "      DETENTS: " << NumDetents << endl;
  93     for(int i=0;i<NumDetents;i++) {
  94       cout << "        " << Detents[i] << " " << TransitionTimes[i] << endl;
  95     }
  96     if (IsOutput) cout << "      OUTPUT: " <<sOutputIdx << endl;
  97   }
  98
  99   if (debug_lvl & 2) cout << "Instantiated: FGKinemat" << endl;
 100 }
 101
 102 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 103
 104 FGKinemat::~FGKinemat()
 105 {
 106   if (debug_lvl & 2) cout << "Destroyed:    FGKinemat" << endl;
 107 }
 108
 109 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 110
 111 bool FGKinemat::Run(void ) {
 112   double dt=fcs->GetState()->Getdt();
 113   double output_transit_rate=0;
 114
 115   FGFCSComponent::Run(); // call the base class for initialization of Input
 116   InputCmd = Input*Detents[NumDetents-1];
 117   OutputPos = fcs->GetState()->GetParameter(OutputIdx);
 118
 119   if(InputCmd < Detents[0]) {
 120     fi=0;
 121     InputCmd=Detents[0];
 122     lastInputCmd=InputCmd;
 123     OutputPos=Detents[0];
 124     Output=OutputPos;
 125   } else if(InputCmd > Detents[NumDetents-1]) {
 126     fi=NumDetents-1;
 127     InputCmd=Detents[fi];
 128     lastInputCmd=InputCmd;
 129     OutputPos=Detents[fi];
 130     Output=OutputPos;
 131   } else {
 132     //cout << "FGKinemat::Run Handle: " << InputCmd << " Position: " << OutputPos << endl;
 133     if(dt <= 0)
 134       OutputPos=InputCmd;
 135     else {
 136       if(InputCmd != lastInputCmd) {
 137
 138         InTransit=1;
 139       }
 140       if(InTransit) {
 141
 142         //fprintf(stderr,"InputCmd: %g, OutputPos: %g\n",InputCmd,OutputPos);
 143         fi=0;
 144         while(Detents[fi] < InputCmd) {
 145           fi++;
 146         }
 147         if(OutputPos < InputCmd) {
 148           if(TransitionTimes[fi] > 0)
 149             output_transit_rate=(Detents[fi] - Detents[fi-1])/TransitionTimes[fi];
 150           else
 151             output_transit_rate=(Detents[fi] - Detents[fi-1])/5;
 152         } else {
 153           if(TransitionTimes[fi+1] > 0)
 154             output_transit_rate=(Detents[fi] - Detents[fi+1])/TransitionTimes[fi+1];
 155           else
 156             output_transit_rate=(Detents[fi] - Detents[fi+1])/5;
 157         }
 158         if(fabs(OutputPos - InputCmd) > dt*output_transit_rate)
 159           OutputPos+=output_transit_rate*dt;
 160         else {
 161           InTransit=0;
 162           OutputPos=InputCmd;
 163         }
 164       }
 165     }
 166     lastInputCmd=InputCmd;
 167     Output=OutputPos;
 168   }
 169   //cout << "FGKinemat::Run Handle: " << InputCmd << " Position: " << OutputPos << " Output: " << Output << endl;
 170   if (IsOutput) {
 171     //cout << "Calling SetOutput()" << endl;
 172     SetOutput();
 173   }
 174   //cout << "Out FGKinemat::Run" << endl;
 175   return true;
 176 }
 177
 178 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 179
 180 void FGKinemat::Debug(void)
 181 {
 182     //TODO: Add your source code here
 183 }
 184