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