src/FDM/JSBSim/filtersjb/FGFlaps.cpp

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