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 <math.h>
  41 #include <float.h>
  42
  43 #include "FGKinemat.h"
  44
  45 namespace JSBSim {
  46
  47 static const char *IdSrc = "$Id$";
  48 static const char *IdHdr = ID_FLAPS;
  49
  50 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  51 CLASS IMPLEMENTATION
  52 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  53
  54 FGKinemat::FGKinemat(FGFCS* fcs, FGConfigFile* AC_cfg) : FGFCSComponent(fcs),
  55                                                          AC_cfg(AC_cfg)
  56 {
  57   string token,sOutputIdx;
  58   double tmpDetent;
  59   double tmpTime;
  60
  61   Detents.clear();
  62   TransitionTimes.clear();
  63
  64   OutputPct=0;
  65   DoScale = true;
  66
  67   Type = AC_cfg->GetValue("TYPE");
  68   Name = AC_cfg->GetValue("NAME");
  69   AC_cfg->GetNextConfigLine();
  70
  71   while ((token = AC_cfg->GetValue()) != string("/COMPONENT")) {
  72     *AC_cfg >> token;
  73     if (token == "INPUT") {
  74       token = AC_cfg->GetValue("INPUT");
  75       if( InputNodes.size() > 0 ) {
  76         cerr << "Kinemat can only accept one input" << endl;
  77       } else  {
  78         *AC_cfg >> token;
  79         InputNodes.push_back( resolveSymbol(token) );
  80       }
  81
  82     } else if ( token == "DETENTS" ) {
  83       *AC_cfg >> NumDetents;
  84       if (NumDetents < 2) {
  85         cerr << "Kinemat must have at least 2 DETENTS" << endl;
  86       }
  87       for (int i=0;i<NumDetents;i++) {
  88         *AC_cfg >> tmpDetent;
  89         *AC_cfg >> tmpTime;
  90         Detents.push_back(tmpDetent);
  91         TransitionTimes.push_back(tmpTime);
  92       }
  93     } else if (token == "OUTPUT") {
  94
  95       IsOutput = true;
  96       *AC_cfg >> sOutputIdx;
  97       OutputNode = PropertyManager->GetNode(sOutputIdx, true);
  98     } else if (token == "NOSCALE") {
  99
 100       DoScale = false;
 101     }
 102   }
 103   FGFCSComponent::bind();
 104   treenode->Tie("output-norm", this, &FGKinemat::GetOutputPct );
 105
 106   Debug(0);
 107 }
 108
 109 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 110
 111 FGKinemat::~FGKinemat()
 112 {
 113   Debug(1);
 114 }
 115
 116 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 117
 118 bool FGKinemat::Run(void )
 119 {
 120   double dt = fcs->GetState()->Getdt();
 121
 122   Input = InputNodes[0]->getDoubleValue();
 123
 124   if (DoScale)  Input *= Detents[NumDetents-1];
 125
 126   Output = OutputNode->getDoubleValue();
 127
 128   if (Input < Detents[0])
 129     Input = Detents[0];
 130   else if (Detents[NumDetents-1] < Input)
 131     Input = Detents[NumDetents-1];
 132
 133   // Process all detent intervals the movement traverses until either the
 134   // final value is reached or the time interval has finished.
 135   while (0.0 < dt && Input != Output) {
 136
 137     // Find the area where Output is in
 138     int ind;
 139     for (ind = 1; (Input < Output) ? Detents[ind] < Output : Detents[ind] <= Output ; ++ind)
 140       if (NumDetents <= ind)
 141         break;
 142
 143     // A transition time of 0.0 means an infinite rate.
 144     // The output is reached in one step
 145     if (TransitionTimes[ind] <= 0.0) {
 146       Output = Input;
 147       break;
 148     } else {
 149       // Compute the rate in this area
 150       double Rate = (Detents[ind] - Detents[ind-1])/TransitionTimes[ind];
 151       // Compute the maximum input value inside this area
 152       double ThisInput = Input;
 153       if (ThisInput < Detents[ind-1])   ThisInput = Detents[ind-1];
 154       if (Detents[ind] < ThisInput)     ThisInput = Detents[ind];
 155       // Compute the time to reach the value in ThisInput
 156       double ThisDt = fabs((ThisInput-Output)/Rate);
 157       // and clip to the timestep size
 158       if (dt < ThisDt) ThisDt = dt;
 159       dt -= ThisDt;
 160       // Do the output calculation
 161       if (Output < Input)
 162         Output += ThisDt*Rate;
 163       else
 164         Output -= ThisDt*Rate;
 165     }
 166   }
 167
 168   OutputPct = (Output-Detents[0])/(Detents[NumDetents-1]-Detents[0]);
 169
 170   if (IsOutput) SetOutput();
 171
 172   return true;
 173 }
 174
 175 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 176 //    The bitmasked value choices are as follows:
 177 //    unset: In this case (the default) JSBSim would only print
 178 //       out the normally expected messages, essentially echoing
 179 //       the config files as they are read. If the environment
 180 //       variable is not set, debug_lvl is set to 1 internally
 181 //    0: This requests JSBSim not to output any messages
 182 //       whatsoever.
 183 //    1: This value explicity requests the normal JSBSim
 184 //       startup messages
 185 //    2: This value asks for a message to be printed out when
 186 //       a class is instantiated
 187 //    4: When this value is set, a message is displayed when a
 188 //       FGModel object executes its Run() method
 189 //    8: When this value is set, various runtime state variables
 190 //       are printed out periodically
 191 //    16: When set various parameters are sanity checked and
 192 //       a message is printed out when they go out of bounds
 193
 194 void FGKinemat::Debug(int from)
 195 {
 196   if (debug_lvl <= 0) return;
 197
 198   if (debug_lvl & 1) { // Standard console startup message output
 199     if (from == 0) { // Constructor
 200       cout << "      INPUT: " << InputNodes[0]->getName() << endl;
 201       cout << "      DETENTS: " << NumDetents << endl;
 202       for (int i=0;i<NumDetents;i++) {
 203         cout << "        " << Detents[i] << " " << TransitionTimes[i] << endl;
 204       }
 205       if (IsOutput) cout << "      OUTPUT: " << OutputNode->getName() << endl;
 206       if (!DoScale) cout << "      NOSCALE" << endl;
 207     }
 208   }
 209   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 210     if (from == 0) cout << "Instantiated: FGKinemat" << endl;
 211     if (from == 1) cout << "Destroyed:    FGKinemat" << endl;
 212   }
 213   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 214   }
 215   if (debug_lvl & 8 ) { // Runtime state variables
 216   }
 217   if (debug_lvl & 16) { // Sanity checking
 218   }
 219   if (debug_lvl & 64) {
 220     if (from == 0) { // Constructor
 221       cout << IdSrc << endl;
 222       cout << IdHdr << endl;
 223     }
 224   }
 225 }
 226 }