]> git.mxchange.org Git - flightgear.git/blob - src/FDM/JSBSim/models/flight_control/FGFCSComponent.cpp
Merge branch 'next' of git://gitorious.org/fg/flightgear into next
[flightgear.git] / src / FDM / JSBSim / models / flight_control / FGFCSComponent.cpp
1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2
3  Module:       FGFCSComponent.cpp
4  Author:       Jon S. Berndt
5  Date started: 11/1999
6
7  ------------- Copyright (C) 2000 -------------
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 "FGFCSComponent.h"
41 #include "input_output/FGPropertyManager.h"
42 #include "input_output/FGXMLElement.h"
43 #include <iostream>
44 #include <cstdlib>
45
46 using namespace std;
47
48 namespace JSBSim {
49
50 static const char *IdSrc = "$Id: FGFCSComponent.cpp,v 1.29 2010/09/07 00:40:03 jberndt Exp $";
51 static const char *IdHdr = ID_FCSCOMPONENT;
52
53 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
54 CLASS IMPLEMENTATION
55 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
56
57 FGFCSComponent::FGFCSComponent(FGFCS* _fcs, Element* element) : fcs(_fcs)
58 {
59   Element *input_element, *clip_el;
60   Input = Output = clipmin = clipmax = 0.0;
61   treenode = 0;
62   delay = index = 0;
63   ClipMinPropertyNode = ClipMaxPropertyNode = 0;
64   clipMinSign = clipMaxSign = 1.0;
65   IsOutput   = clip = false;
66   string input, clip_string;
67   dt = fcs->GetDt();
68
69   PropertyManager = fcs->GetPropertyManager();
70   if        (element->GetName() == string("lag_filter")) {
71     Type = "LAG_FILTER";
72   } else if (element->GetName() == string("lead_lag_filter")) {
73     Type = "LEAD_LAG_FILTER";
74   } else if (element->GetName() == string("washout_filter")) {
75     Type = "WASHOUT_FILTER";
76   } else if (element->GetName() == string("second_order_filter")) {
77     Type = "SECOND_ORDER_FILTER";
78   } else if (element->GetName() == string("integrator")) {
79     Type = "INTEGRATOR";
80   } else if (element->GetName() == string("summer")) {
81     Type = "SUMMER";
82   } else if (element->GetName() == string("pure_gain")) {
83     Type = "PURE_GAIN";
84   } else if (element->GetName() == string("scheduled_gain")) {
85     Type = "SCHEDULED_GAIN";
86   } else if (element->GetName() == string("aerosurface_scale")) {
87     Type = "AEROSURFACE_SCALE";
88   } else if (element->GetName() == string("switch")) {
89     Type = "SWITCH";
90   } else if (element->GetName() == string("kinematic")) {
91     Type = "KINEMATIC";
92   } else if (element->GetName() == string("deadband")) {
93     Type = "DEADBAND";
94   } else if (element->GetName() == string("fcs_function")) {
95     Type = "FCS_FUNCTION";
96   } else if (element->GetName() == string("pid")) {
97     Type = "PID";
98   } else if (element->GetName() == string("sensor")) {
99     Type = "SENSOR";
100   } else if (element->GetName() == string("accelerometer")) {
101     Type = "ACCELEROMETER";
102   } else if (element->GetName() == string("magnetometer")) {
103     Type = "MAGNETOMETER";
104   } else if (element->GetName() == string("gyro")) {
105     Type = "GYRO";
106   } else if (element->GetName() == string("actuator")) {
107     Type = "ACTUATOR";
108   } else { // illegal component in this channel
109     Type = "UNKNOWN";
110   }
111
112   Name = element->GetAttributeValue("name");
113
114   FGPropertyManager *tmp=0;
115
116   input_element = element->FindElement("input");
117   while (input_element) {
118     input = input_element->GetDataLine();
119     if (input[0] == '-') {
120       InputSigns.push_back(-1.0);
121       input.erase(0,1);
122     } else {
123       InputSigns.push_back( 1.0);
124     }
125     if (PropertyManager->HasNode(input)) {
126       tmp = PropertyManager->GetNode(input);
127     } else {
128       tmp = 0L;
129       // cerr << fgcyan << "In component: " + Name + " property "
130       //      + input + " is initially undefined." << reset << endl;
131     }
132     InputNodes.push_back( tmp );
133     InputNames.push_back( input );
134
135     input_element = element->FindNextElement("input");
136   }
137
138   Element *out_elem = element->FindElement("output");
139   while (out_elem) {
140     IsOutput = true;
141     string output_node_name = out_elem->GetDataLine();
142     FGPropertyManager* OutputNode = PropertyManager->GetNode( output_node_name, true );
143     OutputNodes.push_back(OutputNode);
144     if (!OutputNode) {
145       cerr << endl << "  Unable to process property: " << output_node_name << endl;
146       throw(string("Invalid output property name in flight control definition"));
147     }
148     out_elem = element->FindNextElement("output");
149   }
150
151   Element* delay_elem = element->FindElement("delay");
152   if ( delay_elem ) {
153     delay = (unsigned int)delay_elem->GetDataAsNumber();
154     string delayType = delay_elem->GetAttributeValue("type");
155     if (delayType.length() > 0) {
156       if (delayType == "time") {
157         delay = (int)(delay / dt);
158       } else if (delayType == "frames") {
159         // no op. the delay type of "frames" is assumed and is the default.
160       } else {
161         cerr << "Unallowed delay type" << endl;
162       }
163     } else {
164       delay = (int)(delay / dt);
165     }
166     output_array.resize(delay);
167     for (int i=0; i<delay; i++) output_array[i] = 0.0;
168   }
169
170   clip_el = element->FindElement("clipto");
171   if (clip_el) {
172     clip_string = clip_el->FindElementValue("min");
173     if (!is_number(clip_string)) { // it's a property
174       if (clip_string[0] == '-') {
175         clipMinSign = -1.0;
176         clip_string.erase(0,1);
177       }
178       ClipMinPropertyNode = PropertyManager->GetNode( clip_string );
179     } else {
180       clipmin = clip_el->FindElementValueAsNumber("min");
181     }
182     clip_string = clip_el->FindElementValue("max");
183     if (!is_number(clip_string)) { // it's a property
184       if (clip_string[0] == '-') {
185         clipMaxSign = -1.0;
186         clip_string.erase(0,1);
187       }
188       ClipMaxPropertyNode = PropertyManager->GetNode( clip_string );
189     } else {
190       clipmax = clip_el->FindElementValueAsNumber("max");
191     }
192     clip = true;
193   }
194
195   Debug(0);
196 }
197
198 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
199
200 FGFCSComponent::~FGFCSComponent()
201 {
202   Debug(1);
203 }
204
205 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
206
207 void FGFCSComponent::SetOutput(void)
208 {
209   for (unsigned int i=0; i<OutputNodes.size(); i++) OutputNodes[i]->setDoubleValue(Output);
210 }
211
212 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
213
214 bool FGFCSComponent::Run(void)
215 {
216   return true;
217 }
218
219 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
220
221 void FGFCSComponent::Delay(void)
222 {
223   output_array[index] = Output;
224   if (index == delay-1) index = 0;
225   else index++;
226   Output = output_array[index];
227 }
228
229 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
230
231 void FGFCSComponent::Clip(void)
232 {
233   if (clip) {
234     if (ClipMinPropertyNode != 0) clipmin = clipMinSign*ClipMinPropertyNode->getDoubleValue();
235     if (ClipMaxPropertyNode != 0) clipmax = clipMaxSign*ClipMaxPropertyNode->getDoubleValue();
236     if (Output > clipmax)      Output = clipmax;
237     else if (Output < clipmin) Output = clipmin;
238   }
239 }
240
241 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
242
243 void FGFCSComponent::LateBind(void)
244 {
245   FGPropertyManager* node = 0L;
246
247   for (unsigned int i=0; i<InputNodes.size(); i++) {
248     if (!InputNodes[i]) {
249       if (PropertyManager->HasNode(InputNames[i])) {
250         node = PropertyManager->GetNode(InputNames[i]);
251         InputNodes[i] = node;
252       } else {
253         throw(InputNames[i]);
254       }
255     }
256   }
257 }
258
259 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
260 //
261 // The old way of naming FCS components allowed upper or lower case, spaces, etc.
262 // but then the names were modified to fit into a property name heirarchy. This
263 // was confusing (it wasn't done intentionally - it was a carryover from the early
264 // design). We now support the direct naming of properties in the FCS component
265 // name attribute. The old way is supported in code at this time, but deprecated.
266
267 void FGFCSComponent::bind(void)
268 {
269   string tmp;
270   if (Name.find("/") == string::npos) {
271     tmp = "fcs/" + PropertyManager->mkPropertyName(Name, true);
272   } else {
273     tmp = Name;
274   }
275   PropertyManager->Tie( tmp, this, &FGFCSComponent::GetOutput);
276 }
277
278 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
279 //    The bitmasked value choices are as follows:
280 //    unset: In this case (the default) JSBSim would only print
281 //       out the normally expected messages, essentially echoing
282 //       the config files as they are read. If the environment
283 //       variable is not set, debug_lvl is set to 1 internally
284 //    0: This requests JSBSim not to output any messages
285 //       whatsoever.
286 //    1: This value explicity requests the normal JSBSim
287 //       startup messages
288 //    2: This value asks for a message to be printed out when
289 //       a class is instantiated
290 //    4: When this value is set, a message is displayed when a
291 //       FGModel object executes its Run() method
292 //    8: When this value is set, various runtime state variables
293 //       are printed out periodically
294 //    16: When set various parameters are sanity checked and
295 //       a message is printed out when they go out of bounds
296
297 void FGFCSComponent::Debug(int from)
298 {
299   string propsign="";
300
301   if (debug_lvl <= 0) return;
302
303   if (debug_lvl & 1) { // Standard console startup message output
304     if (from == 0) {
305       cout << endl << "    Loading Component \"" << Name
306                    << "\" of type: " << Type << endl;
307
308       if (clip) {
309         if (ClipMinPropertyNode != 0L) {
310           if (clipMinSign < 0.0) propsign="-";
311           cout << "      Minimum limit: " << propsign << ClipMinPropertyNode->GetName() << endl;
312           propsign="";
313         } else {
314           cout << "      Minimum limit: " << clipmin << endl;
315         }
316         if (ClipMaxPropertyNode != 0L) {
317           if (clipMaxSign < 0.0) propsign="-";
318           cout << "      Maximum limit: " << propsign << ClipMaxPropertyNode->GetName() << endl;
319           propsign="";
320         } else {
321           cout << "      Maximum limit: " << clipmax << endl;
322         }
323       }  
324       if (delay > 0) cout <<"      Frame delay: " << delay
325                                    << " frames (" << delay*dt << " sec)" << endl;
326     }
327   }
328   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
329     if (from == 0) cout << "Instantiated: FGFCSComponent" << endl;
330     if (from == 1) cout << "Destroyed:    FGFCSComponent" << endl;
331   }
332   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
333   }
334   if (debug_lvl & 8 ) { // Runtime state variables
335   }
336   if (debug_lvl & 16) { // Sanity checking
337   }
338   if (debug_lvl & 64) {
339     if (from == 0) { // Constructor
340       cout << IdSrc << endl;
341       cout << IdHdr << endl;
342     }
343   }
344 }
345 }