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 && !EqualToRoundoff(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
 158       // and clip to the timestep size
 159       if (dt < ThisDt) {
 160         ThisDt = dt;
 161         if (Output < Input)
 162           Output += ThisDt*Rate;
 163         else
 164           Output -= ThisDt*Rate;
 165       } else
 166         // Handle this case separate to make shure the termination condition
 167         // is met even in inexact arithmetics ...
 168         Output = ThisInput;
 169
 170       dt -= ThisDt;
 171     }
 172   }
 173
 174   OutputPct = (Output-Detents[0])/(Detents[NumDetents-1]-Detents[0]);
 175
 176   if (IsOutput) SetOutput();
 177
 178   return true;
 179 }
 180
 181 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 182 //    The bitmasked value choices are as follows:
 183 //    unset: In this case (the default) JSBSim would only print
 184 //       out the normally expected messages, essentially echoing
 185 //       the config files as they are read. If the environment
 186 //       variable is not set, debug_lvl is set to 1 internally
 187 //    0: This requests JSBSim not to output any messages
 188 //       whatsoever.
 189 //    1: This value explicity requests the normal JSBSim
 190 //       startup messages
 191 //    2: This value asks for a message to be printed out when
 192 //       a class is instantiated
 193 //    4: When this value is set, a message is displayed when a
 194 //       FGModel object executes its Run() method
 195 //    8: When this value is set, various runtime state variables
 196 //       are printed out periodically
 197 //    16: When set various parameters are sanity checked and
 198 //       a message is printed out when they go out of bounds
 199
 200 void FGKinemat::Debug(int from)
 201 {
 202   if (debug_lvl <= 0) return;
 203
 204   if (debug_lvl & 1) { // Standard console startup message output
 205     if (from == 0) { // Constructor
 206       cout << "      INPUT: " << InputNodes[0]->getName() << endl;
 207       cout << "      DETENTS: " << NumDetents << endl;
 208       for (int i=0;i<NumDetents;i++) {
 209         cout << "        " << Detents[i] << " " << TransitionTimes[i] << endl;
 210       }
 211       if (IsOutput) cout << "      OUTPUT: " << OutputNode->getName() << endl;
 212       if (!DoScale) cout << "      NOSCALE" << endl;
 213     }
 214   }
 215   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 216     if (from == 0) cout << "Instantiated: FGKinemat" << endl;
 217     if (from == 1) cout << "Destroyed:    FGKinemat" << endl;
 218   }
 219   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 220   }
 221   if (debug_lvl & 8 ) { // Runtime state variables
 222   }
 223   if (debug_lvl & 16) { // Sanity checking
 224   }
 225   if (debug_lvl & 64) {
 226     if (from == 0) { // Constructor
 227       cout << IdSrc << endl;
 228       cout << IdHdr << endl;
 229     }
 230   }
 231 }
 232 }