]> git.mxchange.org Git - flightgear.git/blob - src/FDM/JSBSim/models/flight_control/FGFCSComponent.cpp
Merge branch 'jmt/dialog'
[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$";
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     tmp = PropertyManager->GetNode(input);
126     if (tmp) {
127       InputNodes.push_back( tmp );
128     } else {
129       cerr << fgred << "  In component: " << Name << " unknown property "
130            << input << " referenced. Aborting" << reset << endl;
131       exit(-1);
132     }
133     input_element = element->FindNextElement("input");
134   }
135
136   Element *out_elem = element->FindElement("output");
137   while (out_elem) {
138     IsOutput = true;
139     string output_node_name = out_elem->GetDataLine();
140     FGPropertyManager* OutputNode = PropertyManager->GetNode( output_node_name, true );
141     OutputNodes.push_back(OutputNode);
142     if (!OutputNode) {
143       cerr << endl << "  Unable to process property: " << output_node_name << endl;
144       throw(string("Invalid output property name in flight control definition"));
145     }
146     out_elem = element->FindNextElement("output");
147   }
148
149   Element* delay_elem = element->FindElement("delay");
150   if ( delay_elem ) {
151     delay = (unsigned int)delay_elem->GetDataAsNumber();
152     string delayType = delay_elem->GetAttributeValue("type");
153     if (delayType.length() > 0) {
154       if (delayType == "time") {
155         delay = (int)(delay / dt);
156       } else if (delayType == "frames") {
157         // no op. the delay type of "frames" is assumed and is the default.
158       } else {
159         cerr << "Unallowed delay type" << endl;
160       }
161     } else {
162       delay = (int)(delay / dt);
163     }
164     output_array.resize(delay);
165     for (int i=0; i<delay; i++) output_array[i] = 0.0;
166   }
167
168   clip_el = element->FindElement("clipto");
169   if (clip_el) {
170     clip_string = clip_el->FindElementValue("min");
171     if (!is_number(clip_string)) { // it's a property
172       if (clip_string[0] == '-') {
173         clipMinSign = -1.0;
174         clip_string.erase(0,1);
175       }
176       ClipMinPropertyNode = PropertyManager->GetNode( clip_string );
177     } else {
178       clipmin = clip_el->FindElementValueAsNumber("min");
179     }
180     clip_string = clip_el->FindElementValue("max");
181     if (!is_number(clip_string)) { // it's a property
182       if (clip_string[0] == '-') {
183         clipMaxSign = -1.0;
184         clip_string.erase(0,1);
185       }
186       ClipMaxPropertyNode = PropertyManager->GetNode( clip_string );
187     } else {
188       clipmax = clip_el->FindElementValueAsNumber("max");
189     }
190     clip = true;
191   }
192
193   Debug(0);
194 }
195
196 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
197
198 FGFCSComponent::~FGFCSComponent()
199 {
200   Debug(1);
201 }
202
203 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
204
205 void FGFCSComponent::SetOutput(void)
206 {
207   for (unsigned int i=0; i<OutputNodes.size(); i++) OutputNodes[i]->setDoubleValue(Output);
208 }
209
210 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
211
212 bool FGFCSComponent::Run(void)
213 {
214   return true;
215 }
216
217 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
218
219 void FGFCSComponent::Delay(void)
220 {
221   output_array[index] = Output;
222   if (index == delay-1) index = 0;
223   else index++;
224   Output = output_array[index];
225 }
226
227 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
228
229 void FGFCSComponent::Clip(void)
230 {
231   if (clip) {
232     if (ClipMinPropertyNode != 0) clipmin = clipMinSign*ClipMinPropertyNode->getDoubleValue();
233     if (ClipMaxPropertyNode != 0) clipmax = clipMaxSign*ClipMaxPropertyNode->getDoubleValue();
234     if (Output > clipmax)      Output = clipmax;
235     else if (Output < clipmin) Output = clipmin;
236   }
237 }
238
239 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
240 //
241 // The old way of naming FCS components allowed upper or lower case, spaces, etc.
242 // but then the names were modified to fit into a property name heirarchy. This
243 // was confusing (it wasn't done intentionally - it was a carryover from the early
244 // design). We now support the direct naming of properties in the FCS component
245 // name attribute. The old way is supported in code at this time, but deprecated.
246
247 void FGFCSComponent::bind(void)
248 {
249   string tmp;
250   if (Name.find("/") == string::npos) {
251     tmp = "fcs/" + PropertyManager->mkPropertyName(Name, true);
252   } else {
253     tmp = Name;
254   }
255   PropertyManager->Tie( tmp, this, &FGFCSComponent::GetOutput);
256 }
257
258 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
259 //    The bitmasked value choices are as follows:
260 //    unset: In this case (the default) JSBSim would only print
261 //       out the normally expected messages, essentially echoing
262 //       the config files as they are read. If the environment
263 //       variable is not set, debug_lvl is set to 1 internally
264 //    0: This requests JSBSim not to output any messages
265 //       whatsoever.
266 //    1: This value explicity requests the normal JSBSim
267 //       startup messages
268 //    2: This value asks for a message to be printed out when
269 //       a class is instantiated
270 //    4: When this value is set, a message is displayed when a
271 //       FGModel object executes its Run() method
272 //    8: When this value is set, various runtime state variables
273 //       are printed out periodically
274 //    16: When set various parameters are sanity checked and
275 //       a message is printed out when they go out of bounds
276
277 void FGFCSComponent::Debug(int from)
278 {
279   string propsign="";
280
281   if (debug_lvl <= 0) return;
282
283   if (debug_lvl & 1) { // Standard console startup message output
284     if (from == 0) {
285       cout << endl << "    Loading Component \"" << Name
286                    << "\" of type: " << Type << endl;
287
288       if (clip) {
289         if (ClipMinPropertyNode != 0L) {
290           if (clipMinSign < 0.0) propsign="-";
291           cout << "      Minimum limit: " << propsign << ClipMinPropertyNode->GetName() << endl;
292           propsign="";
293         } else {
294           cout << "      Minimum limit: " << clipmin << endl;
295         }
296         if (ClipMaxPropertyNode != 0L) {
297           if (clipMaxSign < 0.0) propsign="-";
298           cout << "      Maximum limit: " << propsign << ClipMaxPropertyNode->GetName() << endl;
299           propsign="";
300         } else {
301           cout << "      Maximum limit: " << clipmax << endl;
302         }
303       }  
304       if (delay > 0) cout <<"      Frame delay: " << delay
305                                    << " frames (" << delay*dt << " sec)" << endl;
306     }
307   }
308   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
309     if (from == 0) cout << "Instantiated: FGFCSComponent" << endl;
310     if (from == 1) cout << "Destroyed:    FGFCSComponent" << endl;
311   }
312   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
313   }
314   if (debug_lvl & 8 ) { // Runtime state variables
315   }
316   if (debug_lvl & 16) { // Sanity checking
317   }
318   if (debug_lvl & 64) {
319     if (from == 0) { // Constructor
320       cout << IdSrc << endl;
321       cout << IdHdr << endl;
322     }
323   }
324 }
325 }