41 #include "input_output/FGXMLElement.h" 42 #include "input_output/FGPropertyManager.h" 51 IDENT(IdSrc,
"$Id: FGFilter.cpp,v 1.21 2015/09/27 20:26:23 bcoconni Exp $");
52 IDENT(IdHdr,ID_FILTER);
58 FGFilter::FGFilter(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
61 DynamicFilter =
false;
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;
67 ReadFilterCoefficients(element, i);
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 ;
77 if (element->FindElement(
"trigger")) {
78 Trigger = PropertyManager->GetNode(element->FindElementValue(
"trigger"));
83 CalculateDynamicFilters();
85 FGFCSComponent::bind();
99 void FGFilter::ResetPastStates(
void)
101 FGFCSComponent::ResetPastStates();
103 Input = 0.0; Initialize =
true;
108 void FGFilter::ReadFilterCoefficients(Element* element,
int index)
112 string coefficient =
"c0";
113 coefficient[1] += index;
115 if ( element->FindElement(coefficient) ) {
116 string property_string = element->FindElementValue(coefficient);
117 if (!is_number(property_string)) {
118 if (property_string[0] ==
'-') {
119 PropertySign[index] = -1.0;
120 property_string.erase(0,1);
122 PropertySign[index] = 1.0;
124 PropertyNode[index] = PropertyManager->GetNode(property_string);
125 DynamicFilter =
true;
127 C[index] = element->FindElementValueAsNumber(coefficient);
134 void FGFilter::CalculateDynamicFilters(
void)
138 switch (FilterType) {
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;
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;
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;
171 if (PropertyNode[1] != 0L) C[1] = PropertyNode[1]->getDoubleValue()*PropertySign[1];
172 denom = 2.00 + dt*C[1];
174 cb = (2.00 - dt*C[1]) / denom;
177 if (PropertyNode[1] != 0L) C[1] = PropertyNode[1]->getDoubleValue()*PropertySign[1];
181 cerr <<
"Unknown filter type" << endl;
189 bool FGFilter::Run(
void)
193 PreviousOutput2 = PreviousInput2 = PreviousOutput1 = PreviousInput1 = Output = Input;
198 Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
200 if (DynamicFilter) CalculateDynamicFilters();
202 switch (FilterType) {
204 Output = Input * ca + PreviousInput1 * ca + PreviousOutput1 * cb;
207 Output = Input * ca + PreviousInput1 * cb + PreviousOutput1 * cc;
210 Output = Input * ca + PreviousInput1 * cb + PreviousInput2 * cc
211 - PreviousOutput1 * cd - PreviousOutput2 * ce;
214 Output = Input * ca - PreviousInput1 * ca + PreviousOutput1 * cb;
218 double test = Trigger->getDoubleValue();
219 if (fabs(test) > 0.000001) {
220 Input = PreviousInput1 = PreviousInput2 = 0.0;
223 Output = Input * ca + PreviousInput1 * ca + PreviousOutput1;
231 PreviousOutput2 = PreviousOutput1;
232 PreviousOutput1 = Output;
233 PreviousInput2 = PreviousInput1;
234 PreviousInput1 = Input;
237 if (IsOutput) SetOutput();
261 void FGFilter::Debug(
int from)
265 if (debug_lvl <= 0)
return;
269 cout <<
" INPUT: " << InputNodes[0]->GetName() << endl;
270 switch (FilterType) {
272 if (PropertySign[1] < 0.0) 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;
278 if (PropertySign[1] < 0.0) 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=
"-";
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=
"-";
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=
"-";
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;
296 if (PropertySign[1] < 0.0) 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=
"-";
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=
"-";
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=
"-";
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=
"-";
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=
"-";
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;
322 if (PropertySign[1] < 0.0) 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;
328 if (PropertySign[1] < 0.0) 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;
337 for (
unsigned int i=0; i<OutputNodes.size(); i++)
338 cout <<
" OUTPUT: " << OutputNodes[i]->getName() << endl;
342 if (debug_lvl & 2 ) {
343 if (from == 0) cout <<
"Instantiated: FGFilter" << endl;
344 if (from == 1) cout <<
"Destroyed: FGFilter" << endl;
346 if (debug_lvl & 4 ) {
348 if (debug_lvl & 8 ) {
350 if (debug_lvl & 16) {
352 if (debug_lvl & 64) {
354 cout << IdSrc << endl;
355 cout << IdHdr << endl;