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