src/FDM/JSBSim/models/flight_control/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 Lesser 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 Lesser General Public License for more
  17  details.
  18
  19  You should have received a copy of the GNU Lesser 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 Lesser 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, Element* element) : FGFCSComponent(fcs, element)
  55 {
  56   Element *traverse_element, *setting_element;
  57   double tmpDetent;
  58   double tmpTime;
  59
  60   Detents.clear();
  61   TransitionTimes.clear();
  62
  63   Output = OutputPct = 0;
  64   DoScale = true;
  65
  66   if (element->FindElement("noscale")) DoScale = false;
  67
  68   traverse_element = element->FindElement("traverse");
  69   setting_element = traverse_element->FindElement("setting");
  70   while (setting_element) {
  71     tmpDetent = setting_element->FindElementValueAsNumber("position");
  72     tmpTime = setting_element->FindElementValueAsNumber("time");
  73     Detents.push_back(tmpDetent);
  74     TransitionTimes.push_back(tmpTime);
  75     setting_element = traverse_element->FindNextElement("setting");
  76   }
  77   NumDetents = Detents.size();
  78
  79   if (NumDetents <= 1) {
  80     cerr << "Kinematic component " << Name
  81          << " must have more than 1 setting element" << endl;
  82     exit(-1);
  83   }
  84
  85   FGFCSComponent::bind();
  86 //  treenode->Tie("output-norm", this, &FGKinemat::GetOutputPct );
  87
  88   Debug(0);
  89 }
  90
  91 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  92
  93 FGKinemat::~FGKinemat()
  94 {
  95   Debug(1);
  96 }
  97
  98 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  99
 100 bool FGKinemat::Run(void )
 101 {
 102   double dt = fcs->GetState()->Getdt();
 103
 104   Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
 105
 106   if (DoScale) Input *= Detents[NumDetents-1];
 107
 108   if (IsOutput) Output = OutputNode->getDoubleValue();
 109
 110   if (Input < Detents[0])
 111     Input = Detents[0];
 112   else if (Detents[NumDetents-1] < Input)
 113     Input = Detents[NumDetents-1];
 114
 115   // Process all detent intervals the movement traverses until either the
 116   // final value is reached or the time interval has finished.
 117   while ( 0.0 < dt && !EqualToRoundoff(Input, Output) ) {
 118
 119     // Find the area where Output is in
 120     int ind;
 121     for (ind = 1; (Input < Output) ? Detents[ind] < Output : Detents[ind] <= Output ; ++ind)
 122       if (NumDetents <= ind)
 123         break;
 124
 125     // A transition time of 0.0 means an infinite rate.
 126     // The output is reached in one step
 127     if (TransitionTimes[ind] <= 0.0) {
 128       Output = Input;
 129       break;
 130     } else {
 131       // Compute the rate in this area
 132       double Rate = (Detents[ind] - Detents[ind-1])/TransitionTimes[ind];
 133       // Compute the maximum input value inside this area
 134       double ThisInput = Input;
 135       if (ThisInput < Detents[ind-1])   ThisInput = Detents[ind-1];
 136       if (Detents[ind] < ThisInput)     ThisInput = Detents[ind];
 137       // Compute the time to reach the value in ThisInput
 138       double ThisDt = fabs((ThisInput-Output)/Rate);
 139
 140       // and clip to the timestep size
 141       if (dt < ThisDt) {
 142         ThisDt = dt;
 143         if (Output < Input)
 144           Output += ThisDt*Rate;
 145         else
 146           Output -= ThisDt*Rate;
 147       } else
 148         // Handle this case separate to make shure the termination condition
 149         // is met even in inexact arithmetics ...
 150         Output = ThisInput;
 151
 152       dt -= ThisDt;
 153     }
 154   }
 155
 156   OutputPct = (Output-Detents[0])/(Detents[NumDetents-1]-Detents[0]);
 157
 158   Clip();
 159   if (IsOutput) SetOutput();
 160
 161   return true;
 162 }
 163
 164 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 165 //    The bitmasked value choices are as follows:
 166 //    unset: In this case (the default) JSBSim would only print
 167 //       out the normally expected messages, essentially echoing
 168 //       the config files as they are read. If the environment
 169 //       variable is not set, debug_lvl is set to 1 internally
 170 //    0: This requests JSBSim not to output any messages
 171 //       whatsoever.
 172 //    1: This value explicity requests the normal JSBSim
 173 //       startup messages
 174 //    2: This value asks for a message to be printed out when
 175 //       a class is instantiated
 176 //    4: When this value is set, a message is displayed when a
 177 //       FGModel object executes its Run() method
 178 //    8: When this value is set, various runtime state variables
 179 //       are printed out periodically
 180 //    16: When set various parameters are sanity checked and
 181 //       a message is printed out when they go out of bounds
 182
 183 void FGKinemat::Debug(int from)
 184 {
 185   if (debug_lvl <= 0) return;
 186
 187   if (debug_lvl & 1) { // Standard console startup message output
 188     if (from == 0) { // Constructor
 189       cout << "      INPUT: " << InputNodes[0]->getName() << endl;
 190       cout << "      DETENTS: " << NumDetents << endl;
 191       for (int i=0;i<NumDetents;i++) {
 192         cout << "        " << Detents[i] << " " << TransitionTimes[i] << endl;
 193       }
 194       if (IsOutput) cout << "      OUTPUT: " << OutputNode->getName() << endl;
 195       if (!DoScale) cout << "      NOSCALE" << endl;
 196     }
 197   }
 198   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 199     if (from == 0) cout << "Instantiated: FGKinemat" << endl;
 200     if (from == 1) cout << "Destroyed:    FGKinemat" << endl;
 201   }
 202   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 203   }
 204   if (debug_lvl & 8 ) { // Runtime state variables
 205   }
 206   if (debug_lvl & 16) { // Sanity checking
 207   }
 208   if (debug_lvl & 64) {
 209     if (from == 0) { // Constructor
 210       cout << IdSrc << endl;
 211       cout << IdHdr << endl;
 212     }
 213   }
 214 }
 215 }