src/FDM/JSBSim/models/flight_control/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 static const char *IdSrc = "$Id: FGFilter.cpp,v 1.15 2009/10/24 22:59:30 jberndt Exp $";
  52 static const char *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::ReadFilterCoefficients(Element* element, int index)
 100 {
 101   // index is known to be 1-7.
 102   // A stringstream would be overkill, but also trying to avoid sprintf
 103   string coefficient = "c0";
 104   coefficient[1] += index;
 105
 106   if ( element->FindElement(coefficient) ) {
 107     string property_string = element->FindElementValue(coefficient);
 108     if (!is_number(property_string)) { // property
 109       if (property_string[0] == '-') {
 110        PropertySign[index] = -1.0;
 111        property_string.erase(0,1);
 112       } else {
 113        PropertySign[index] = 1.0;
 114       }
 115       PropertyNode[index] = PropertyManager->GetNode(property_string);
 116       DynamicFilter = true;
 117     } else {
 118       C[index] = element->FindElementValueAsNumber(coefficient);
 119     }
 120   }
 121 }
 122
 123 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 124
 125 void FGFilter::CalculateDynamicFilters(void)
 126 {
 127   double denom;
 128
 129   switch (FilterType) {
 130     case eLag:
 131       if (PropertyNode[1] != 0L) C[1] = PropertyNode[1]->getDoubleValue()*PropertySign[1];
 132       denom = 2.00 + dt*C[1];
 133       ca = dt*C[1] / denom;
 134       cb = (2.00 - dt*C[1]) / denom;
 135
 136       break;
 137     case eLeadLag:
 138       if (PropertyNode[1] != 0L) C[1] = PropertyNode[1]->getDoubleValue()*PropertySign[1];
 139       if (PropertyNode[2] != 0L) C[2] = PropertyNode[2]->getDoubleValue()*PropertySign[2];
 140       if (PropertyNode[3] != 0L) C[3] = PropertyNode[3]->getDoubleValue()*PropertySign[3];
 141       if (PropertyNode[4] != 0L) C[4] = PropertyNode[4]->getDoubleValue()*PropertySign[4];
 142       denom = 2.00*C[3] + dt*C[4];
 143       ca = (2.00*C[1] + dt*C[2]) / denom;
 144       cb = (dt*C[2] - 2.00*C[1]) / denom;
 145       cc = (2.00*C[3] - dt*C[4]) / denom;
 146       break;
 147     case eOrder2:
 148       if (PropertyNode[1] != 0L) C[1] = PropertyNode[1]->getDoubleValue()*PropertySign[1];
 149       if (PropertyNode[2] != 0L) C[2] = PropertyNode[2]->getDoubleValue()*PropertySign[2];
 150       if (PropertyNode[3] != 0L) C[3] = PropertyNode[3]->getDoubleValue()*PropertySign[3];
 151       if (PropertyNode[4] != 0L) C[4] = PropertyNode[4]->getDoubleValue()*PropertySign[4];
 152       if (PropertyNode[5] != 0L) C[5] = PropertyNode[5]->getDoubleValue()*PropertySign[5];
 153       if (PropertyNode[6] != 0L) C[6] = PropertyNode[6]->getDoubleValue()*PropertySign[6];
 154       denom = 4.0*C[4] + 2.0*C[5]*dt + C[6]*dt*dt;
 155       ca = (4.0*C[1] + 2.0*C[2]*dt + C[3]*dt*dt) / denom;
 156       cb = (2.0*C[3]*dt*dt - 8.0*C[1]) / denom;
 157       cc = (4.0*C[1] - 2.0*C[2]*dt + C[3]*dt*dt) / denom;
 158       cd = (2.0*C[6]*dt*dt - 8.0*C[4]) / denom;
 159       ce = (4.0*C[4] - 2.0*C[5]*dt + C[6]*dt*dt) / denom;
 160       break;
 161     case eWashout:
 162       if (PropertyNode[1] != 0L) C[1] = PropertyNode[1]->getDoubleValue()*PropertySign[1];
 163       denom = 2.00 + dt*C[1];
 164       ca = 2.00 / denom;
 165       cb = (2.00 - dt*C[1]) / denom;
 166       break;
 167     case eIntegrator:
 168       if (PropertyNode[1] != 0L) C[1] = PropertyNode[1]->getDoubleValue()*PropertySign[1];
 169       ca = dt*C[1] / 2.00;
 170       break;
 171     case eUnknown:
 172       cerr << "Unknown filter type" << endl;
 173     break;
 174   }
 175
 176 }
 177
 178 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 179
 180 bool FGFilter::Run(void)
 181 {
 182   double test = 0.0;
 183
 184   if (Initialize) {
 185
 186     PreviousOutput1 = PreviousInput1 = Output = Input;
 187     Initialize = false;
 188
 189   } else {
 190
 191     Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
 192
 193     if (DynamicFilter) CalculateDynamicFilters();
 194
 195     switch (FilterType) {
 196       case eLag:
 197         Output = Input * ca + PreviousInput1 * ca + PreviousOutput1 * cb;
 198         break;
 199       case eLeadLag:
 200         Output = Input * ca + PreviousInput1 * cb + PreviousOutput1 * cc;
 201         break;
 202       case eOrder2:
 203         Output = Input * ca + PreviousInput1 * cb + PreviousInput2 * cc
 204                             - PreviousOutput1 * cd - PreviousOutput2 * ce;
 205         break;
 206       case eWashout:
 207         Output = Input * ca - PreviousInput1 * ca + PreviousOutput1 * cb;
 208         break;
 209       case eIntegrator:
 210         if (Trigger != 0) {
 211           test = Trigger->getDoubleValue();
 212           if (fabs(test) > 0.000001) {
 213             Input  = PreviousInput1 = PreviousInput2 = 0.0;
 214           }
 215         }
 216         Output = Input * ca + PreviousInput1 * ca + PreviousOutput1;
 217         break;
 218       case eUnknown:
 219         break;
 220     }
 221
 222   }
 223
 224   PreviousOutput2 = PreviousOutput1;
 225   PreviousOutput1 = Output;
 226   PreviousInput2  = PreviousInput1;
 227   PreviousInput1  = Input;
 228
 229   Clip();
 230   if (IsOutput) SetOutput();
 231
 232   return true;
 233 }
 234
 235 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 236 //    The bitmasked value choices are as follows:
 237 //    unset: In this case (the default) JSBSim would only print
 238 //       out the normally expected messages, essentially echoing
 239 //       the config files as they are read. If the environment
 240 //       variable is not set, debug_lvl is set to 1 internally
 241 //    0: This requests JSBSim not to output any messages
 242 //       whatsoever.
 243 //    1: This value explicity requests the normal JSBSim
 244 //       startup messages
 245 //    2: This value asks for a message to be printed out when
 246 //       a class is instantiated
 247 //    4: When this value is set, a message is displayed when a
 248 //       FGModel object executes its Run() method
 249 //    8: When this value is set, various runtime state variables
 250 //       are printed out periodically
 251 //    16: When set various parameters are sanity checked and
 252 //       a message is printed out when they go out of bounds
 253
 254 void FGFilter::Debug(int from)
 255 {
 256   string sgn="";
 257
 258   if (debug_lvl <= 0) return;
 259
 260   if (debug_lvl & 1) { // Standard console startup message output
 261     if (from == 0) { // Constructor
 262       cout << "      INPUT: " << InputNodes[0]->getName() << endl;
 263         switch (FilterType) {
 264         case eLag:
 265           if (PropertySign[1] < 0.0) sgn="-";
 266           else sgn = "";
 267           if (PropertyNode[1] == 0L) cout << "      C[1]: " << C[1] << endl;
 268           else cout << "      C[1] is the value of property: " << sgn << PropertyNode[1]->GetName() << endl;
 269           break;
 270         case eLeadLag:
 271           if (PropertySign[1] < 0.0) sgn="-";
 272           else sgn = "";
 273           if (PropertyNode[1] == 0L) cout << "      C[1]: " << C[1] << endl;
 274           else cout << "      C[1] is the value of property: " << sgn << PropertyNode[1]->GetName() << endl;
 275           if (PropertySign[2] < 0.0) sgn="-";
 276           else sgn = "";
 277           if (PropertyNode[2] == 0L) cout << "      C[2]: " << C[2] << endl;
 278           else cout << "      C[2] is the value of property: " << sgn << PropertyNode[2]->GetName() << endl;
 279           if (PropertySign[3] < 0.0) sgn="-";
 280           else sgn = "";
 281           if (PropertyNode[3] == 0L) cout << "      C[3]: " << C[3] << endl;
 282           else cout << "      C[3] is the value of property: " << sgn << PropertyNode[3]->GetName() << endl;
 283           if (PropertySign[4] < 0.0) sgn="-";
 284           else sgn = "";
 285           if (PropertyNode[4] == 0L) cout << "      C[4]: " << C[4] << endl;
 286           else cout << "      C[4] is the value of property: " << sgn << PropertyNode[4]->GetName() << endl;
 287           break;
 288         case eOrder2:
 289           if (PropertySign[1] < 0.0) sgn="-";
 290           else sgn = "";
 291           if (PropertyNode[1] == 0L) cout << "      C[1]: " << C[1] << endl;
 292           else cout << "      C[1] is the value of property: " << sgn << PropertyNode[1]->GetName() << endl;
 293           if (PropertySign[2] < 0.0) sgn="-";
 294           else sgn = "";
 295           if (PropertyNode[2] == 0L) cout << "      C[2]: " << C[2] << endl;
 296           else cout << "      C[2] is the value of property: " << sgn << PropertyNode[2]->GetName() << endl;
 297           if (PropertySign[3] < 0.0) sgn="-";
 298           else sgn = "";
 299           if (PropertyNode[3] == 0L) cout << "      C[3]: " << C[3] << endl;
 300           else cout << "      C[3] is the value of property: " << sgn << PropertyNode[3]->GetName() << endl;
 301           if (PropertySign[4] < 0.0) sgn="-";
 302           else sgn = "";
 303           if (PropertyNode[4] == 0L) cout << "      C[4]: " << C[4] << endl;
 304           else cout << "      C[4] is the value of property: " << sgn << PropertyNode[4]->GetName() << endl;
 305           if (PropertySign[5] < 0.0) sgn="-";
 306           else sgn = "";
 307           if (PropertyNode[5] == 0L) cout << "      C[5]: " << C[5] << endl;
 308           else cout << "      C[5] is the value of property: " << sgn << PropertyNode[5]->GetName() << endl;
 309           if (PropertySign[6] < 0.0) sgn="-";
 310           else sgn = "";
 311           if (PropertyNode[6] == 0L) cout << "      C[6]: " << C[6] << endl;
 312           else cout << "      C[6] is the value of property: " << sgn << PropertyNode[6]->GetName() << endl;
 313           break;
 314         case eWashout:
 315           if (PropertySign[1] < 0.0) sgn="-";
 316           else sgn = "";
 317           if (PropertyNode[1] == 0L) cout << "      C[1]: " << C[1] << endl;
 318           else cout << "      C[1] is the value of property: " << sgn << PropertyNode[1]->GetName() << endl;
 319           break;
 320         case eIntegrator:
 321           if (PropertySign[1] < 0.0) sgn="-";
 322           else sgn = "";
 323           if (PropertyNode[1] == 0L) cout << "      C[1]: " << C[1] << endl;
 324           else cout << "      C[1] is the value of property: " << sgn << PropertyNode[1]->GetName() << endl;
 325           break;
 326         case eUnknown:
 327           break;
 328        }
 329       if (IsOutput) {
 330         for (unsigned int i=0; i<OutputNodes.size(); i++)
 331           cout << "      OUTPUT: " << OutputNodes[i]->getName() << endl;
 332       }
 333     }
 334   }
 335   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 336     if (from == 0) cout << "Instantiated: FGFilter" << endl;
 337     if (from == 1) cout << "Destroyed:    FGFilter" << endl;
 338   }
 339   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 340   }
 341   if (debug_lvl & 8 ) { // Runtime state variables
 342   }
 343   if (debug_lvl & 16) { // Sanity checking
 344   }
 345   if (debug_lvl & 64) {
 346     if (from == 0) { // Constructor
 347       cout << IdSrc << endl;
 348       cout << IdHdr << endl;
 349     }
 350   }
 351 }
 352 }