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