Branch data Line data Source code
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: FGKinemat.cpp,v 1.11 2010/08/21 22:56:11 jberndt Exp $";
50 : : static const char *IdHdr = ID_FLAPS;
51 : :
52 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
53 : : CLASS IMPLEMENTATION
54 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
55 : :
56 : 0 : FGKinemat::FGKinemat(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
57 : : {
58 : : Element *traverse_element, *setting_element;
59 : : double tmpDetent;
60 : : double tmpTime;
61 : :
62 : 0 : Detents.clear();
63 : 0 : TransitionTimes.clear();
64 : :
65 : 0 : Output = OutputPct = 0;
66 : 0 : DoScale = true;
67 : :
68 [ # # ][ # # ]: 0 : if (element->FindElement("noscale")) DoScale = false;
69 : :
70 : 0 : traverse_element = element->FindElement("traverse");
71 : 0 : setting_element = traverse_element->FindElement("setting");
72 [ # # ][ # # ]: 0 : while (setting_element) {
73 : 0 : tmpDetent = setting_element->FindElementValueAsNumber("position");
74 : 0 : tmpTime = setting_element->FindElementValueAsNumber("time");
75 : 0 : Detents.push_back(tmpDetent);
76 : 0 : TransitionTimes.push_back(tmpTime);
77 : 0 : setting_element = traverse_element->FindNextElement("setting");
78 : : }
79 : 0 : NumDetents = Detents.size();
80 : :
81 [ # # ][ # # ]: 0 : if (NumDetents <= 1) {
82 : : cerr << "Kinematic component " << Name
83 : 0 : << " must have more than 1 setting element" << endl;
84 : 0 : exit(-1);
85 : : }
86 : :
87 : 0 : FGFCSComponent::bind();
88 : : // treenode->Tie("output-norm", this, &FGKinemat::GetOutputPct );
89 : :
90 : 0 : Debug(0);
91 : 0 : }
92 : :
93 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
94 : :
95 : 0 : FGKinemat::~FGKinemat()
96 : : {
97 : 0 : Debug(1);
98 [ # # ][ # # ]: 0 : }
[ # # ]
99 : :
100 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
101 : :
102 : 0 : bool FGKinemat::Run(void )
103 : : {
104 : 0 : double dt0 = dt;
105 : :
106 : 0 : Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
107 : :
108 [ # # ]: 0 : if (DoScale) Input *= Detents[NumDetents-1];
109 : :
110 [ # # ]: 0 : if (IsOutput) Output = OutputNodes[0]->getDoubleValue();
111 : :
112 [ # # ]: 0 : if (Input < Detents[0])
113 : 0 : Input = Detents[0];
114 [ # # ]: 0 : else if (Detents[NumDetents-1] < Input)
115 : 0 : 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 [ # # ][ # # ]: 0 : while ( dt0 > 0.0 && !EqualToRoundoff(Input, Output) ) {
[ # # ]
120 : :
121 : : // Find the area where Output is in
122 : : int ind;
123 [ # # ][ # # ]: 0 : for (ind = 1; (Input < Output) ? Detents[ind] < Output : Detents[ind] <= Output ; ++ind)
124 [ # # ]: 0 : if (NumDetents <= ind)
125 : 0 : break;
126 : :
127 : : // A transition time of 0.0 means an infinite rate.
128 : : // The output is reached in one step
129 [ # # ]: 0 : if (TransitionTimes[ind] <= 0.0) {
130 : 0 : Output = Input;
131 : 0 : break;
132 : : } else {
133 : : // Compute the rate in this area
134 : 0 : double Rate = (Detents[ind] - Detents[ind-1])/TransitionTimes[ind];
135 : : // Compute the maximum input value inside this area
136 : 0 : double ThisInput = Input;
137 [ # # ]: 0 : if (ThisInput < Detents[ind-1]) ThisInput = Detents[ind-1];
138 [ # # ]: 0 : if (Detents[ind] < ThisInput) ThisInput = Detents[ind];
139 : : // Compute the time to reach the value in ThisInput
140 : 0 : double ThisDt = fabs((ThisInput-Output)/Rate);
141 : :
142 : : // and clip to the timestep size
143 [ # # ]: 0 : if (dt0 < ThisDt) {
144 : 0 : ThisDt = dt0;
145 [ # # ]: 0 : if (Output < Input)
146 : 0 : Output += ThisDt*Rate;
147 : : else
148 : 0 : Output -= ThisDt*Rate;
149 : : } else
150 : : // Handle this case separate to make shure the termination condition
151 : : // is met even in inexact arithmetics ...
152 : 0 : Output = ThisInput;
153 : :
154 : 0 : dt0 -= ThisDt;
155 : : }
156 : : }
157 : :
158 : 0 : OutputPct = (Output-Detents[0])/(Detents[NumDetents-1]-Detents[0]);
159 : :
160 : 0 : Clip();
161 [ # # ]: 0 : if (IsOutput) SetOutput();
162 : :
163 : 0 : 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 : 0 : void FGKinemat::Debug(int from)
186 : : {
187 [ # # ]: 0 : if (debug_lvl <= 0) return;
188 : :
189 [ # # ]: 0 : if (debug_lvl & 1) { // Standard console startup message output
190 [ # # ]: 0 : if (from == 0) { // Constructor
191 : 0 : cout << " INPUT: " << InputNames[0] << endl;
192 : 0 : cout << " DETENTS: " << NumDetents << endl;
193 [ # # ]: 0 : for (int i=0;i<NumDetents;i++) {
194 : 0 : cout << " " << Detents[i] << " " << TransitionTimes[i] << endl;
195 : : }
196 [ # # ]: 0 : if (IsOutput) {
197 [ # # ]: 0 : for (unsigned int i=0; i<OutputNodes.size(); i++)
198 : 0 : cout << " OUTPUT: " << OutputNodes[i]->getName() << endl;
199 : : }
200 [ # # ]: 0 : if (!DoScale) cout << " NOSCALE" << endl;
201 : : }
202 : : }
203 [ # # ]: 0 : if (debug_lvl & 2 ) { // Instantiation/Destruction notification
204 [ # # ]: 0 : if (from == 0) cout << "Instantiated: FGKinemat" << endl;
205 [ # # ]: 0 : if (from == 1) cout << "Destroyed: FGKinemat" << endl;
206 : : }
207 : 0 : if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
208 : : }
209 : 0 : if (debug_lvl & 8 ) { // Runtime state variables
210 : : }
211 : 0 : if (debug_lvl & 16) { // Sanity checking
212 : : }
213 [ # # ]: 0 : if (debug_lvl & 64) {
214 [ # # ]: 0 : if (from == 0) { // Constructor
215 : 0 : cout << IdSrc << endl;
216 : 0 : cout << IdHdr << endl;
217 : : }
218 : : }
219 : : }
220 [ + + ][ + - ]: 12 : }
|
