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