[Jsbsim-cvslogs] CVS: JSBSim/src/models/flight_control FGFilter.cpp, 1.5, 1.6 FGFilter.h, 1.6, 1.7

[Jon S. B. <jb...@us...>]

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);
  };