]> git.mxchange.org Git - flightgear.git/blob - src/FDM/JSBSim/models/flight_control/FGKinemat.cpp
sync. with JSBSim again
[flightgear.git] / 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 }