1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
4 Author: Tony Peden, for flight control system authored by Jon S. Berndt
7 ------------- Copyright (C) 2000 Anthony K. Peden -------------
9 This program is free software; you can redistribute it and/or modify it under
10 the terms of the GNU General Public License as published by the Free Software
11 Foundation; either version 2 of the License, or (at your option) any later
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 General Public License for more
19 You should have received a copy of the GNU 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.
23 Further information about the GNU General Public License can also be found on
24 the world wide web at http://www.gnu.org.
26 FUNCTIONAL DESCRIPTION
27 --------------------------------------------------------------------------------
30 --------------------------------------------------------------------------------
32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
33 COMMENTS, REFERENCES, and NOTES
34 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
36 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
43 #include "FGKinemat.h"
47 static const char *IdSrc = "$Id$";
48 static const char *IdHdr = ID_FLAPS;
50 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
52 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
54 FGKinemat::FGKinemat(FGFCS* fcs, FGConfigFile* AC_cfg) : FGFCSComponent(fcs),
57 string token,sOutputIdx;
62 TransitionTimes.clear();
67 Type = AC_cfg->GetValue("TYPE");
68 Name = AC_cfg->GetValue("NAME");
69 AC_cfg->GetNextConfigLine();
71 while ((token = AC_cfg->GetValue()) != string("/COMPONENT")) {
73 if (token == "INPUT") {
74 token = AC_cfg->GetValue("INPUT");
75 if( InputNodes.size() > 0 ) {
76 cerr << "Kinemat can only accept one input" << endl;
79 InputNodes.push_back( resolveSymbol(token) );
82 } else if ( token == "DETENTS" ) {
83 *AC_cfg >> NumDetents;
85 cerr << "Kinemat must have at least 2 DETENTS" << endl;
87 for (int i=0;i<NumDetents;i++) {
90 Detents.push_back(tmpDetent);
91 TransitionTimes.push_back(tmpTime);
93 } else if (token == "OUTPUT") {
96 *AC_cfg >> sOutputIdx;
97 OutputNode = PropertyManager->GetNode(sOutputIdx, true);
98 } else if (token == "NOSCALE") {
103 FGFCSComponent::bind();
104 treenode->Tie("output-norm", this, &FGKinemat::GetOutputPct );
109 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
111 FGKinemat::~FGKinemat()
116 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
118 bool FGKinemat::Run(void )
120 double dt = fcs->GetState()->Getdt();
122 Input = InputNodes[0]->getDoubleValue();
124 if (DoScale) Input *= Detents[NumDetents-1];
126 Output = OutputNode->getDoubleValue();
128 if (Input < Detents[0])
130 else if (Detents[NumDetents-1] < Input)
131 Input = Detents[NumDetents-1];
133 // Process all detent intervals the movement traverses until either the
134 // final value is reached or the time interval has finished.
135 while ( 0.0 < dt && !EqualToRoundoff(Input, Output) ) {
137 // Find the area where Output is in
139 for (ind = 1; (Input < Output) ? Detents[ind] < Output : Detents[ind] <= Output ; ++ind)
140 if (NumDetents <= ind)
143 // A transition time of 0.0 means an infinite rate.
144 // The output is reached in one step
145 if (TransitionTimes[ind] <= 0.0) {
149 // Compute the rate in this area
150 double Rate = (Detents[ind] - Detents[ind-1])/TransitionTimes[ind];
151 // Compute the maximum input value inside this area
152 double ThisInput = Input;
153 if (ThisInput < Detents[ind-1]) ThisInput = Detents[ind-1];
154 if (Detents[ind] < ThisInput) ThisInput = Detents[ind];
155 // Compute the time to reach the value in ThisInput
156 double ThisDt = fabs((ThisInput-Output)/Rate);
158 // and clip to the timestep size
162 Output += ThisDt*Rate;
164 Output -= ThisDt*Rate;
166 // Handle this case separate to make shure the termination condition
167 // is met even in inexact arithmetics ...
174 OutputPct = (Output-Detents[0])/(Detents[NumDetents-1]-Detents[0]);
176 if (IsOutput) SetOutput();
181 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
183 void FGKinemat::convert(void)
186 cout << " <component name=\"" << Name << "\" type=\"" << Type << "\">" << endl;
188 cout << " <input>" << (InputNodes[0]->GetFullyQualifiedName()).substr(12) << "</input>" << endl;
190 cout << " <traverse>" << endl;
191 for (int i=0; i<Detents.size(); i++) {
192 cout << " <setting>" << endl;
193 cout << " <position>" << Detents[i] << "</position>" << endl;
194 cout << " <time>" << TransitionTimes[i] << "</time>" << endl;
195 cout << " </setting>" << endl;
197 cout << " </traverse>" << endl;
200 cout << " <output>" << (OutputNode->GetFullyQualifiedName()).substr(12) << "</output>" << endl;
202 cout << " </component>" << endl;
205 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
206 // The bitmasked value choices are as follows:
207 // unset: In this case (the default) JSBSim would only print
208 // out the normally expected messages, essentially echoing
209 // the config files as they are read. If the environment
210 // variable is not set, debug_lvl is set to 1 internally
211 // 0: This requests JSBSim not to output any messages
213 // 1: This value explicity requests the normal JSBSim
215 // 2: This value asks for a message to be printed out when
216 // a class is instantiated
217 // 4: When this value is set, a message is displayed when a
218 // FGModel object executes its Run() method
219 // 8: When this value is set, various runtime state variables
220 // are printed out periodically
221 // 16: When set various parameters are sanity checked and
222 // a message is printed out when they go out of bounds
224 void FGKinemat::Debug(int from)
226 if (debug_lvl <= 0) return;
228 if (debug_lvl & 1) { // Standard console startup message output
229 if (from == 0) { // Constructor
230 cout << " INPUT: " << InputNodes[0]->getName() << endl;
231 cout << " DETENTS: " << NumDetents << endl;
232 for (int i=0;i<NumDetents;i++) {
233 cout << " " << Detents[i] << " " << TransitionTimes[i] << endl;
235 if (IsOutput) cout << " OUTPUT: " << OutputNode->getName() << endl;
236 if (!DoScale) cout << " NOSCALE" << endl;
239 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
240 if (from == 0) cout << "Instantiated: FGKinemat" << endl;
241 if (from == 1) cout << "Destroyed: FGKinemat" << endl;
243 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
245 if (debug_lvl & 8 ) { // Runtime state variables
247 if (debug_lvl & 16) { // Sanity checking
249 if (debug_lvl & 64) {
250 if (from == 0) { // Constructor
251 cout << IdSrc << endl;
252 cout << IdHdr << endl;