JSBSim Flight Dynamics Model  1.0 (02 March 2017)
An Open Source Flight Dynamics and Control Software Library in C++
FGFilter.cpp
1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2 
3  Module: FGFilter.cpp
4  Author: Jon S. Berndt
5  Date started: 11/2000
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 "FGFilter.h"
41 #include "input_output/FGXMLElement.h"
42 #include "input_output/FGPropertyManager.h"
43 
44 #include <iostream>
45 #include <string>
46 
47 using namespace std;
48 
49 namespace JSBSim {
50 
51 IDENT(IdSrc,"$Id: FGFilter.cpp,v 1.21 2015/09/27 20:26:23 bcoconni Exp $");
52 IDENT(IdHdr,ID_FILTER);
53 
54 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
55 CLASS IMPLEMENTATION
56 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
57 
58 FGFilter::FGFilter(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
59 {
60  Trigger = 0;
61  DynamicFilter = false;
62 
63  C[1] = C[2] = C[3] = C[4] = C[5] = C[6] = 0.0;
64  for (int i=1; i<7; i++) {
65  PropertySign[i] = 1.0;
66  PropertyNode[i] = 0L;
67  ReadFilterCoefficients(element, i);
68  }
69 
70  if (Type == "LAG_FILTER") FilterType = eLag ;
71  else if (Type == "LEAD_LAG_FILTER") FilterType = eLeadLag ;
72  else if (Type == "SECOND_ORDER_FILTER") FilterType = eOrder2 ;
73  else if (Type == "WASHOUT_FILTER") FilterType = eWashout ;
74  else if (Type == "INTEGRATOR") FilterType = eIntegrator ;
75  else FilterType = eUnknown ;
76 
77  if (element->FindElement("trigger")) {
78  Trigger = PropertyManager->GetNode(element->FindElementValue("trigger"));
79  }
80 
81  Initialize = true;
82 
83  CalculateDynamicFilters();
84 
85  FGFCSComponent::bind();
86 
87  Debug(0);
88 }
89 
90 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
91 
92 FGFilter::~FGFilter()
93 {
94  Debug(1);
95 }
96 
97 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
98 
99 void FGFilter::ResetPastStates(void)
100 {
101  FGFCSComponent::ResetPastStates();
102 
103  Input = 0.0; Initialize = true;
104 }
105 
106 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
107 
108 void FGFilter::ReadFilterCoefficients(Element* element, int index)
109 {
110  // index is known to be 1-7.
111  // A stringstream would be overkill, but also trying to avoid sprintf
112  string coefficient = "c0";
113  coefficient[1] += index;
114 
115  if ( element->FindElement(coefficient) ) {
116  string property_string = element->FindElementValue(coefficient);
117  if (!is_number(property_string)) { // property
118  if (property_string[0] == '-') {
119  PropertySign[index] = -1.0;
120  property_string.erase(0,1);
121  } else {
122  PropertySign[index] = 1.0;
123  }
124  PropertyNode[index] = PropertyManager->GetNode(property_string);
125  DynamicFilter = true;
126  } else {
127  C[index] = element->FindElementValueAsNumber(coefficient);
128  }
129  }
130 }
131 
132 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
133 
134 void FGFilter::CalculateDynamicFilters(void)
135 {
136  double denom;
137 
138  switch (FilterType) {
139  case eLag:
140  if (PropertyNode[1] != 0L) C[1] = PropertyNode[1]->getDoubleValue()*PropertySign[1];
141  denom = 2.00 + dt*C[1];
142  ca = dt*C[1] / denom;
143  cb = (2.00 - dt*C[1]) / denom;
144 
145  break;
146  case eLeadLag:
147  if (PropertyNode[1] != 0L) C[1] = PropertyNode[1]->getDoubleValue()*PropertySign[1];
148  if (PropertyNode[2] != 0L) C[2] = PropertyNode[2]->getDoubleValue()*PropertySign[2];
149  if (PropertyNode[3] != 0L) C[3] = PropertyNode[3]->getDoubleValue()*PropertySign[3];
150  if (PropertyNode[4] != 0L) C[4] = PropertyNode[4]->getDoubleValue()*PropertySign[4];
151  denom = 2.00*C[3] + dt*C[4];
152  ca = (2.00*C[1] + dt*C[2]) / denom;
153  cb = (dt*C[2] - 2.00*C[1]) / denom;
154  cc = (2.00*C[3] - dt*C[4]) / denom;
155  break;
156  case eOrder2:
157  if (PropertyNode[1] != 0L) C[1] = PropertyNode[1]->getDoubleValue()*PropertySign[1];
158  if (PropertyNode[2] != 0L) C[2] = PropertyNode[2]->getDoubleValue()*PropertySign[2];
159  if (PropertyNode[3] != 0L) C[3] = PropertyNode[3]->getDoubleValue()*PropertySign[3];
160  if (PropertyNode[4] != 0L) C[4] = PropertyNode[4]->getDoubleValue()*PropertySign[4];
161  if (PropertyNode[5] != 0L) C[5] = PropertyNode[5]->getDoubleValue()*PropertySign[5];
162  if (PropertyNode[6] != 0L) C[6] = PropertyNode[6]->getDoubleValue()*PropertySign[6];
163  denom = 4.0*C[4] + 2.0*C[5]*dt + C[6]*dt*dt;
164  ca = (4.0*C[1] + 2.0*C[2]*dt + C[3]*dt*dt) / denom;
165  cb = (2.0*C[3]*dt*dt - 8.0*C[1]) / denom;
166  cc = (4.0*C[1] - 2.0*C[2]*dt + C[3]*dt*dt) / denom;
167  cd = (2.0*C[6]*dt*dt - 8.0*C[4]) / denom;
168  ce = (4.0*C[4] - 2.0*C[5]*dt + C[6]*dt*dt) / denom;
169  break;
170  case eWashout:
171  if (PropertyNode[1] != 0L) C[1] = PropertyNode[1]->getDoubleValue()*PropertySign[1];
172  denom = 2.00 + dt*C[1];
173  ca = 2.00 / denom;
174  cb = (2.00 - dt*C[1]) / denom;
175  break;
176  case eIntegrator:
177  if (PropertyNode[1] != 0L) C[1] = PropertyNode[1]->getDoubleValue()*PropertySign[1];
178  ca = dt*C[1] / 2.00;
179  break;
180  case eUnknown:
181  cerr << "Unknown filter type" << endl;
182  break;
183  }
184 
185 }
186 
187 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
188 
189 bool FGFilter::Run(void)
190 {
191  if (Initialize) {
192 
193  PreviousOutput2 = PreviousInput2 = PreviousOutput1 = PreviousInput1 = Output = Input;
194  Initialize = false;
195 
196  } else {
197 
198  Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
199 
200  if (DynamicFilter) CalculateDynamicFilters();
201 
202  switch (FilterType) {
203  case eLag:
204  Output = Input * ca + PreviousInput1 * ca + PreviousOutput1 * cb;
205  break;
206  case eLeadLag:
207  Output = Input * ca + PreviousInput1 * cb + PreviousOutput1 * cc;
208  break;
209  case eOrder2:
210  Output = Input * ca + PreviousInput1 * cb + PreviousInput2 * cc
211  - PreviousOutput1 * cd - PreviousOutput2 * ce;
212  break;
213  case eWashout:
214  Output = Input * ca - PreviousInput1 * ca + PreviousOutput1 * cb;
215  break;
216  case eIntegrator:
217  if (Trigger != 0) {
218  double test = Trigger->getDoubleValue();
219  if (fabs(test) > 0.000001) {
220  Input = PreviousInput1 = PreviousInput2 = 0.0;
221  }
222  }
223  Output = Input * ca + PreviousInput1 * ca + PreviousOutput1;
224  break;
225  case eUnknown:
226  break;
227  }
228 
229  }
230 
231  PreviousOutput2 = PreviousOutput1;
232  PreviousOutput1 = Output;
233  PreviousInput2 = PreviousInput1;
234  PreviousInput1 = Input;
235 
236  Clip();
237  if (IsOutput) SetOutput();
238 
239  return true;
240 }
241 
242 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
243 // The bitmasked value choices are as follows:
244 // unset: In this case (the default) JSBSim would only print
245 // out the normally expected messages, essentially echoing
246 // the config files as they are read. If the environment
247 // variable is not set, debug_lvl is set to 1 internally
248 // 0: This requests JSBSim not to output any messages
249 // whatsoever.
250 // 1: This value explicity requests the normal JSBSim
251 // startup messages
252 // 2: This value asks for a message to be printed out when
253 // a class is instantiated
254 // 4: When this value is set, a message is displayed when a
255 // FGModel object executes its Run() method
256 // 8: When this value is set, various runtime state variables
257 // are printed out periodically
258 // 16: When set various parameters are sanity checked and
259 // a message is printed out when they go out of bounds
260 
261 void FGFilter::Debug(int from)
262 {
263  string sgn="";
264 
265  if (debug_lvl <= 0) return;
266 
267  if (debug_lvl & 1) { // Standard console startup message output
268  if (from == 0) { // Constructor
269  cout << " INPUT: " << InputNodes[0]->GetName() << endl;
270  switch (FilterType) {
271  case eLag:
272  if (PropertySign[1] < 0.0) sgn="-";
273  else sgn = "";
274  if (PropertyNode[1] == 0L) cout << " C[1]: " << C[1] << endl;
275  else cout << " C[1] is the value of property: " << sgn << PropertyNode[1]->GetName() << endl;
276  break;
277  case eLeadLag:
278  if (PropertySign[1] < 0.0) sgn="-";
279  else sgn = "";
280  if (PropertyNode[1] == 0L) cout << " C[1]: " << C[1] << endl;
281  else cout << " C[1] is the value of property: " << sgn << PropertyNode[1]->GetName() << endl;
282  if (PropertySign[2] < 0.0) sgn="-";
283  else sgn = "";
284  if (PropertyNode[2] == 0L) cout << " C[2]: " << C[2] << endl;
285  else cout << " C[2] is the value of property: " << sgn << PropertyNode[2]->GetName() << endl;
286  if (PropertySign[3] < 0.0) sgn="-";
287  else sgn = "";
288  if (PropertyNode[3] == 0L) cout << " C[3]: " << C[3] << endl;
289  else cout << " C[3] is the value of property: " << sgn << PropertyNode[3]->GetName() << endl;
290  if (PropertySign[4] < 0.0) sgn="-";
291  else sgn = "";
292  if (PropertyNode[4] == 0L) cout << " C[4]: " << C[4] << endl;
293  else cout << " C[4] is the value of property: " << sgn << PropertyNode[4]->GetName() << endl;
294  break;
295  case eOrder2:
296  if (PropertySign[1] < 0.0) sgn="-";
297  else sgn = "";
298  if (PropertyNode[1] == 0L) cout << " C[1]: " << C[1] << endl;
299  else cout << " C[1] is the value of property: " << sgn << PropertyNode[1]->GetName() << endl;
300  if (PropertySign[2] < 0.0) sgn="-";
301  else sgn = "";
302  if (PropertyNode[2] == 0L) cout << " C[2]: " << C[2] << endl;
303  else cout << " C[2] is the value of property: " << sgn << PropertyNode[2]->GetName() << endl;
304  if (PropertySign[3] < 0.0) sgn="-";
305  else sgn = "";
306  if (PropertyNode[3] == 0L) cout << " C[3]: " << C[3] << endl;
307  else cout << " C[3] is the value of property: " << sgn << PropertyNode[3]->GetName() << endl;
308  if (PropertySign[4] < 0.0) sgn="-";
309  else sgn = "";
310  if (PropertyNode[4] == 0L) cout << " C[4]: " << C[4] << endl;
311  else cout << " C[4] is the value of property: " << sgn << PropertyNode[4]->GetName() << endl;
312  if (PropertySign[5] < 0.0) sgn="-";
313  else sgn = "";
314  if (PropertyNode[5] == 0L) cout << " C[5]: " << C[5] << endl;
315  else cout << " C[5] is the value of property: " << sgn << PropertyNode[5]->GetName() << endl;
316  if (PropertySign[6] < 0.0) sgn="-";
317  else sgn = "";
318  if (PropertyNode[6] == 0L) cout << " C[6]: " << C[6] << endl;
319  else cout << " C[6] is the value of property: " << sgn << PropertyNode[6]->GetName() << endl;
320  break;
321  case eWashout:
322  if (PropertySign[1] < 0.0) sgn="-";
323  else sgn = "";
324  if (PropertyNode[1] == 0L) cout << " C[1]: " << C[1] << endl;
325  else cout << " C[1] is the value of property: " << sgn << PropertyNode[1]->GetName() << endl;
326  break;
327  case eIntegrator:
328  if (PropertySign[1] < 0.0) sgn="-";
329  else sgn = "";
330  if (PropertyNode[1] == 0L) cout << " C[1]: " << C[1] << endl;
331  else cout << " C[1] is the value of property: " << sgn << PropertyNode[1]->GetName() << endl;
332  break;
333  case eUnknown:
334  break;
335  }
336  if (IsOutput) {
337  for (unsigned int i=0; i<OutputNodes.size(); i++)
338  cout << " OUTPUT: " << OutputNodes[i]->getName() << endl;
339  }
340  }
341  }
342  if (debug_lvl & 2 ) { // Instantiation/Destruction notification
343  if (from == 0) cout << "Instantiated: FGFilter" << endl;
344  if (from == 1) cout << "Destroyed: FGFilter" << endl;
345  }
346  if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
347  }
348  if (debug_lvl & 8 ) { // Runtime state variables
349  }
350  if (debug_lvl & 16) { // Sanity checking
351  }
352  if (debug_lvl & 64) {
353  if (from == 0) { // Constructor
354  cout << IdSrc << endl;
355  cout << IdHdr << endl;
356  }
357  }
358 }
359 }
STL namespace.