From: Jon S. B. <jb...@us...> - 2007-09-18 03:19:11
|
Update of /cvsroot/jsbsim/JSBSim/src/models/flight_control In directory sc8-pr-cvs6.sourceforge.net:/tmp/cvs-serv6813/src/models/flight_control Modified Files: FGFilter.cpp FGFilter.h Log Message: Added ability to specify a property for a filter coefficient, thus resulting in a dynamic filter Index: FGFilter.cpp =================================================================== RCS file: /cvsroot/jsbsim/JSBSim/src/models/flight_control/FGFilter.cpp,v retrieving revision 1.5 retrieving revision 1.6 diff -C2 -r1.5 -r1.6 *** FGFilter.cpp 30 Aug 2006 12:04:35 -0000 1.5 --- FGFilter.cpp 18 Sep 2007 03:19:04 -0000 1.6 *************** *** 51,60 **** FGFilter::FGFilter(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element) { - double denom; - dt = fcs->GetState()->Getdt(); Trigger = 0; ! C1 = C2 = C3 = C4 = C5 = C6 = 0.0; if (Type == "LAG_FILTER") FilterType = eLag ; --- 51,60 ---- FGFilter::FGFilter(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element) { dt = fcs->GetState()->Getdt(); Trigger = 0; + DynamicFilter = false; ! C[1] = C[2] = C[3] = C[4] = C[5] = C[6] = 0.0; ! for (int i=0; i<7; i++) PropertyNode[i] = 0L; if (Type == "LAG_FILTER") FilterType = eLag ; *************** *** 65,74 **** else FilterType = eUnknown ; ! if (element->FindElement("c1")) C1 = element->FindElementValueAsNumber("c1"); ! if (element->FindElement("c2")) C2 = element->FindElementValueAsNumber("c2"); ! if (element->FindElement("c3")) C3 = element->FindElementValueAsNumber("c3"); ! if (element->FindElement("c4")) C4 = element->FindElementValueAsNumber("c4"); ! if (element->FindElement("c5")) C5 = element->FindElementValueAsNumber("c5"); ! if (element->FindElement("c6")) C6 = element->FindElementValueAsNumber("c6"); if (element->FindElement("trigger")) { Trigger = PropertyManager->GetNode(element->FindElementValue("trigger")); --- 65,75 ---- else FilterType = eUnknown ; ! ReadFilterCoefficients(element, 1); ! ReadFilterCoefficients(element, 2); ! ReadFilterCoefficients(element, 3); ! ReadFilterCoefficients(element, 4); ! ReadFilterCoefficients(element, 5); ! ReadFilterCoefficients(element, 6); ! if (element->FindElement("trigger")) { Trigger = PropertyManager->GetNode(element->FindElementValue("trigger")); *************** *** 77,107 **** Initialize = true; switch (FilterType) { case eLag: ! denom = 2.00 + dt*C1; ! ca = dt*C1 / denom; ! cb = (2.00 - dt*C1) / denom; break; case eLeadLag: ! denom = 2.00*C3 + dt*C4; ! ca = (2.00*C1 + dt*C2) / denom; ! cb = (dt*C2 - 2.00*C1) / denom; ! cc = (2.00*C3 - dt*C4) / denom; break; case eOrder2: ! denom = 4.0*C4 + 2.0*C5*dt + C6*dt*dt; ! ca = (4.0*C1 + 2.0*C2*dt + C3*dt*dt) / denom; ! cb = (2.0*C3*dt*dt - 8.0*C1) / denom; ! cc = (4.0*C1 - 2.0*C2*dt + C3*dt*dt) / denom; ! cd = (2.0*C6*dt*dt - 8.0*C4) / denom; ! ce = (4.0*C4 - 2.0*C5*dt + C6*dt*dt) / denom; break; case eWashout: ! denom = 2.00 + dt*C1; ca = 2.00 / denom; ! cb = (2.00 - dt*C1) / denom; break; case eIntegrator: ! ca = dt*C1 / 2.00; break; case eUnknown: --- 78,165 ---- Initialize = true; + CalculateDynamicFilters(); + + FGFCSComponent::bind(); + + Debug(0); + } + + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + FGFilter::~FGFilter() + { + Debug(1); + } + + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + void FGFilter::ReadFilterCoefficients(Element* element, int index) + { + char buf[3]; + sprintf(buf, "c%d", index); + string coefficient = string(buf); + string property_string=""; + + if ( element->FindElement(coefficient) ) { + property_string = element->FindElementValue(coefficient); + if (property_string.find_first_not_of("+-.0123456789Ee") != string::npos) { // property + if (property_string[0] == '-') { + PropertySign[index] = -1.0; + property_string.erase(0,1); + } + PropertyNode[index] = PropertyManager->GetNode(property_string); + DynamicFilter = true; + } else { + C[index] = element->FindElementValueAsNumber(coefficient); + } + } + } + + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + void FGFilter::CalculateDynamicFilters(void) + { + double denom; + switch (FilterType) { case eLag: ! if (PropertyNode[1] != 0L) C[1] = PropertyNode[1]->getDoubleValue(); ! denom = 2.00 + dt*C[1]; ! ca = dt*C[1] / denom; ! cb = (2.00 - dt*C[1]) / denom; break; case eLeadLag: ! if (PropertyNode[1] != 0L) C[1] = PropertyNode[1]->getDoubleValue(); ! if (PropertyNode[2] != 0L) C[2] = PropertyNode[2]->getDoubleValue(); ! if (PropertyNode[3] != 0L) C[3] = PropertyNode[3]->getDoubleValue(); ! if (PropertyNode[4] != 0L) C[4] = PropertyNode[4]->getDoubleValue(); ! denom = 2.00*C[3] + dt*C[4]; ! ca = (2.00*C[1] + dt*C[2]) / denom; ! cb = (dt*C[2] - 2.00*C[1]) / denom; ! cc = (2.00*C[3] - dt*C[4]) / denom; break; case eOrder2: ! if (PropertyNode[1] != 0L) C[1] = PropertyNode[1]->getDoubleValue(); ! if (PropertyNode[2] != 0L) C[2] = PropertyNode[2]->getDoubleValue(); ! if (PropertyNode[3] != 0L) C[3] = PropertyNode[3]->getDoubleValue(); ! if (PropertyNode[4] != 0L) C[4] = PropertyNode[4]->getDoubleValue(); ! if (PropertyNode[5] != 0L) C[5] = PropertyNode[5]->getDoubleValue(); ! if (PropertyNode[6] != 0L) C[6] = PropertyNode[6]->getDoubleValue(); ! denom = 4.0*C[4] + 2.0*C[5]*dt + C[6]*dt*dt; ! ca = (4.0*C[1] + 2.0*C[2]*dt + C[3]*dt*dt) / denom; ! cb = (2.0*C[3]*dt*dt - 8.0*C[1]) / denom; ! cc = (4.0*C[1] - 2.0*C[2]*dt + C[3]*dt*dt) / denom; ! cd = (2.0*C[6]*dt*dt - 8.0*C[4]) / denom; ! ce = (4.0*C[4] - 2.0*C[5]*dt + C[6]*dt*dt) / denom; break; case eWashout: ! if (PropertyNode[1] != 0L) C[1] = PropertyNode[1]->getDoubleValue(); ! denom = 2.00 + dt*C[1]; ca = 2.00 / denom; ! cb = (2.00 - dt*C[1]) / denom; break; case eIntegrator: ! if (PropertyNode[1] != 0L) C[1] = PropertyNode[1]->getDoubleValue(); ! ca = dt*C[1] / 2.00; break; case eUnknown: *************** *** 109,122 **** break; } - FGFCSComponent::bind(); - Debug(0); - } - - //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - - FGFilter::~FGFilter() - { - Debug(1); } --- 167,171 ---- *************** *** 135,138 **** --- 184,190 ---- Input = InputNodes[0]->getDoubleValue() * InputSigns[0]; + + if (DynamicFilter) CalculateDynamicFilters(); + switch (FilterType) { case eLag: *************** *** 201,210 **** if (from == 0) { // Constructor cout << " INPUT: " << InputNodes[0]->getName() << endl; ! cout << " C1: " << C1 << endl; ! cout << " C2: " << C2 << endl; ! cout << " C3: " << C3 << endl; ! cout << " C4: " << C4 << endl; ! cout << " C5: " << C5 << endl; ! cout << " C6: " << C6 << endl; if (IsOutput) cout << " OUTPUT: " << OutputNode->getName() << endl; } --- 253,294 ---- if (from == 0) { // Constructor cout << " INPUT: " << InputNodes[0]->getName() << endl; ! switch (FilterType) { ! case eLag: ! if (PropertyNode[1] == 0L) cout << " C[1]: " << C[1] << endl; ! else cout << " C[1] is the value of property: " << PropertyNode[1]->GetName() << endl; ! break; ! case eLeadLag: ! if (PropertyNode[1] == 0L) cout << " C[1]: " << C[1] << endl; ! else cout << " C[1] is the value of property: " << PropertyNode[1]->GetName() << endl; ! if (PropertyNode[2] == 0L) cout << " C[2]: " << C[2] << endl; ! else cout << " C[2] is the value of property: " << PropertyNode[2]->GetName() << endl; ! if (PropertyNode[3] == 0L) cout << " C[3]: " << C[3] << endl; ! else cout << " C[3] is the value of property: " << PropertyNode[3]->GetName() << endl; ! if (PropertyNode[4] == 0L) cout << " C[4]: " << C[4] << endl; ! else cout << " C[4] is the value of property: " << PropertyNode[4]->GetName() << endl; ! break; ! case eOrder2: ! if (PropertyNode[1] == 0L) cout << " C[1]: " << C[1] << endl; ! else cout << " C[1] is the value of property: " << PropertyNode[1]->GetName() << endl; ! if (PropertyNode[2] == 0L) cout << " C[2]: " << C[2] << endl; ! else cout << " C[2] is the value of property: " << PropertyNode[2]->GetName() << endl; ! if (PropertyNode[3] == 0L) cout << " C[3]: " << C[3] << endl; ! else cout << " C[3] is the value of property: " << PropertyNode[3]->GetName() << endl; ! if (PropertyNode[4] == 0L) cout << " C[4]: " << C[4] << endl; ! else cout << " C[4] is the value of property: " << PropertyNode[4]->GetName() << endl; ! if (PropertyNode[5] == 0L) cout << " C[5]: " << C[5] << endl; ! else cout << " C[5] is the value of property: " << PropertyNode[5]->GetName() << endl; ! if (PropertyNode[6] == 0L) cout << " C[6]: " << C[6] << endl; ! else cout << " C[6] is the value of property: " << PropertyNode[6]->GetName() << endl; ! break; ! case eWashout: ! if (PropertyNode[1] == 0L) cout << " C[1]: " << C[1] << endl; ! else cout << " C[1] is the value of property: " << PropertyNode[1]->GetName() << endl; ! break; ! case eIntegrator: ! if (PropertyNode[1] == 0L) cout << " C[1]: " << C[1] << endl; ! else cout << " C[1] is the value of property: " << PropertyNode[1]->GetName() << endl; ! break; ! } if (IsOutput) cout << " OUTPUT: " << OutputNode->getName() << endl; } Index: FGFilter.h =================================================================== RCS file: /cvsroot/jsbsim/JSBSim/src/models/flight_control/FGFilter.h,v retrieving revision 1.6 retrieving revision 1.7 diff -C2 -r1.6 -r1.7 *** FGFilter.h 30 Aug 2006 12:04:35 -0000 1.6 --- FGFilter.h 18 Sep 2007 03:19:04 -0000 1.7 *************** *** 65,74 **** <typename name="name"> <input> property </input> ! <c1> value </c1> ! [<c2> value </c2>] ! [<c3> value </c3>] ! [<c4> value </c4>] ! [<c5> value </c5>] ! [<c6> value </c6>] [<clipto> <min> {[-]property name | value} </min> --- 65,74 ---- <typename name="name"> <input> property </input> ! <c1> value|property </c1> ! [<c2> value|property </c2>] ! [<c3> value|property </c3>] ! [<c4> value|property </c4>] ! [<c5> value|property </c5>] ! [<c6> value|property </c6>] [<clipto> <min> {[-]property name | value} </min> *************** *** 92,96 **** <lag_filter name="name"> <input> property </input> ! <c1> value </c1> [<clipto> <min> {[-]property name | value} </min> --- 92,96 ---- <lag_filter name="name"> <input> property </input> ! <c1> value|property </c1> [<clipto> <min> {[-]property name | value} </min> *************** *** 131,138 **** <lead_lag_filter name="name"> <input> property </input> ! <c1> value <c/1> ! <c2> value <c/2> ! <c3> value <c/3> ! <c4> value <c/4> [<clipto> <min> {[-]property name | value} </min> --- 131,138 ---- <lead_lag_filter name="name"> <input> property </input> ! <c1> value|property <c/1> ! <c2> value|property <c/2> ! <c3> value|property <c/3> ! <c4> value|property <c/4> [<clipto> <min> {[-]property name | value} </min> *************** *** 178,187 **** <second_order_filter name="name"> <input> property </input> ! <c1> value </c1> ! <c2> value </c2> ! <c3> value </c3> ! <c4> value </c4> ! <c5> value </c5> ! <c6> value </c6> [<clipto> <min> {[-]property name | value} </min> --- 178,187 ---- <second_order_filter name="name"> <input> property </input> ! <c1> value|property </c1> ! <c2> value|property </c2> ! <c3> value|property </c3> ! <c4> value|property </c4> ! <c5> value|property </c5> ! <c6> value|property </c6> [<clipto> <min> {[-]property name | value} </min> *************** *** 205,209 **** <integrator name="name"> <input> property </input> ! <c1> value </c1> [<trigger> property </trigger>] [<clipto> --- 205,209 ---- <integrator name="name"> <input> property </input> ! <c1> value|property </c1> [<trigger> property </trigger>] [<clipto> *************** *** 255,264 **** double cd; double ce; ! double C1; ! double C2; ! double C3; ! double C4; ! double C5; ! double C6; double PreviousInput1; double PreviousInput2; --- 255,260 ---- double cd; double ce; ! double C[7]; // There are 6 coefficients, indexing is "1" based. ! double PropertySign[7]; double PreviousInput1; double PreviousInput2; *************** *** 266,269 **** --- 262,269 ---- double PreviousOutput2; FGPropertyManager* Trigger; + FGPropertyManager* PropertyNode[7]; + void CalculateDynamicFilters(void); + void ReadFilterCoefficients(Element* el, int index); + bool DynamicFilter; void Debug(int from); }; |