JSBSim Flight Dynamics Model  1.0 (02 March 2017)
An Open Source Flight Dynamics and Control Software Library in C++
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 "input_output/FGXMLElement.h"
42 #include "models/FGFCS.h"
43 #include <iostream>
44 #include <cstdlib>
45 
46 using namespace std;
47 
48 namespace JSBSim {
49 
50 IDENT(IdSrc,"$Id: FGKinemat.cpp,v 1.16 2016/06/12 14:47:46 bcoconni Exp $");
51 IDENT(IdHdr,ID_FLAPS);
52 
53 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
54 CLASS IMPLEMENTATION
55 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
56 
57 FGKinemat::FGKinemat(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
58 {
59  Element *traverse_element, *setting_element;
60  double tmpDetent;
61  double tmpTime;
62 
63  Detents.clear();
64  TransitionTimes.clear();
65 
66  Output = 0;
67  DoScale = true;
68 
69  if (element->FindElement("noscale")) DoScale = false;
70 
71  traverse_element = element->FindElement("traverse");
72  setting_element = traverse_element->FindElement("setting");
73  while (setting_element) {
74  tmpDetent = setting_element->FindElementValueAsNumber("position");
75  tmpTime = setting_element->FindElementValueAsNumber("time");
76  Detents.push_back(tmpDetent);
77  TransitionTimes.push_back(tmpTime);
78  setting_element = traverse_element->FindNextElement("setting");
79  }
80 
81  if (Detents.size() <= 1) {
82  cerr << "Kinematic component " << Name
83  << " must have more than 1 setting element" << endl;
84  exit(-1);
85  }
86 
87  FGFCSComponent::bind();
88 
89  Debug(0);
90 }
91 
92 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
93 
95 {
96  Debug(1);
97 }
98 
99 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
100 
101 bool FGKinemat::Run(void )
102 {
103  double dt0 = dt;
104 
105  Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
106 
107  if (DoScale) Input *= Detents.back();
108 
109  if (IsOutput) Output = OutputNodes[0]->getDoubleValue();
110 
111  Input = Constrain(Detents.front(), Input, Detents.back());
112 
113  if (fcs->GetTrimStatus())
114  // When trimming the output must be reached in one step
115  Output = Input;
116  else {
117  // Process all detent intervals the movement traverses until either the
118  // final value is reached or the time interval has finished.
119  while ( dt0 > 0.0 && !EqualToRoundoff(Input, Output) ) {
120 
121  // Find the area where Output is in
122  unsigned int ind;
123  for (ind = 1; (Input < Output) ? Detents[ind] < Output : Detents[ind] <= Output ; ++ind)
124  if (ind >= Detents.size())
125  break;
126 
127  // A transition time of 0.0 means an infinite rate.
128  // The output is reached in one step
129  if (TransitionTimes[ind] <= 0.0) {
130  Output = Input;
131  break;
132  } else {
133  // Compute the rate in this area
134  double Rate = (Detents[ind] - Detents[ind-1])/TransitionTimes[ind];
135  // Compute the maximum input value inside this area
136  double ThisInput = Constrain(Detents[ind-1], Input, 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 (dt0 < ThisDt) {
142  ThisDt = dt0;
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  dt0 -= ThisDt;
153  }
154  }
155  }
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: " << Detents.size() << endl;
190  for (unsigned int i=0;i<Detents.size();i++) {
191  cout << " " << Detents[i] << " " << TransitionTimes[i] << endl;
192  }
193  if (IsOutput) {
194  for (unsigned int i=0; i<OutputNodes.size(); i++)
195  cout << " OUTPUT: " << OutputNodes[i]->getName() << endl;
196  }
197  if (!DoScale) cout << " NOSCALE" << endl;
198  }
199  }
200  if (debug_lvl & 2 ) { // Instantiation/Destruction notification
201  if (from == 0) cout << "Instantiated: FGKinemat" << endl;
202  if (from == 1) cout << "Destroyed: FGKinemat" << endl;
203  }
204  if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
205  }
206  if (debug_lvl & 8 ) { // Runtime state variables
207  }
208  if (debug_lvl & 16) { // Sanity checking
209  }
210  if (debug_lvl & 64) {
211  if (from == 0) { // Constructor
212  cout << IdSrc << endl;
213  cout << IdHdr << endl;
214  }
215  }
216 }
217 }
~FGKinemat()
Destructor.
Definition: FGKinemat.cpp:94
static double Constrain(double min, double value, double max)
Constrain a value between a minimum and a maximum value.
Definition: FGJSBBase.h:332
STL namespace.
Element * FindElement(const std::string &el="")
Searches for a specified element.
double FindElementValueAsNumber(const std::string &el="")
Searches for the named element and returns the data belonging to it as a number.
static bool EqualToRoundoff(double a, double b)
Finite precision comparison.
Definition: FGJSBBase.h:300
Encapsulates the Flight Control System (FCS) functionality.
Definition: FGFCS.h:193
Element * FindNextElement(const std::string &el="")
Searches for the next element as specified.
bool Run(void)
Run method, overrides FGModel::Run().
Definition: FGKinemat.cpp:101
Base class for JSBSim Flight Control System Components.