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   double denom;
  54
  55   dt = fcs->GetState()->Getdt();
  56   Trigger = 0;
  57
  58   C1 = C2 = C3 = C4 = C5 = C6 = 0.0;
  59
  60   if      (Type == "LAG_FILTER")          FilterType = eLag        ;
  61   else if (Type == "LEAD_LAG_FILTER")     FilterType = eLeadLag    ;
  62   else if (Type == "SECOND_ORDER_FILTER") FilterType = eOrder2     ;
  63   else if (Type == "WASHOUT_FILTER")      FilterType = eWashout    ;
  64   else if (Type == "INTEGRATOR")          FilterType = eIntegrator ;
  65   else                                    FilterType = eUnknown    ;
  66
  67   if (element->FindElement("c1")) C1 = element->FindElementValueAsNumber("c1");
  68   if (element->FindElement("c2")) C2 = element->FindElementValueAsNumber("c2");
  69   if (element->FindElement("c3")) C3 = element->FindElementValueAsNumber("c3");
  70   if (element->FindElement("c4")) C4 = element->FindElementValueAsNumber("c4");
  71   if (element->FindElement("c5")) C5 = element->FindElementValueAsNumber("c5");
  72   if (element->FindElement("c6")) C6 = element->FindElementValueAsNumber("c6");
  73   if (element->FindElement("trigger")) {
  74     Trigger =  PropertyManager->GetNode(element->FindElementValue("trigger"));
  75   }
  76
  77   Initialize = true;
  78
  79   switch (FilterType) {
  80     case eLag:
  81       denom = 2.00 + dt*C1;
  82       ca = dt*C1 / denom;
  83       cb = (2.00 - dt*C1) / denom;
  84       break;
  85     case eLeadLag:
  86       denom = 2.00*C3 + dt*C4;
  87       ca = (2.00*C1 + dt*C2) / denom;
  88       cb = (dt*C2 - 2.00*C1) / denom;
  89       cc = (2.00*C3 - dt*C4) / denom;
  90       break;
  91     case eOrder2:
  92       denom = 4.0*C4 + 2.0*C5*dt + C6*dt*dt;
  93       ca = (4.0*C1 + 2.0*C2*dt + C3*dt*dt) / denom;
  94       cb = (2.0*C3*dt*dt - 8.0*C1) / denom;
  95       cc = (4.0*C1 - 2.0*C2*dt + C3*dt*dt) / denom;
  96       cd = (2.0*C6*dt*dt - 8.0*C4) / denom;
  97       ce = (4.0*C4 - 2.0*C5*dt + C6*dt*dt) / denom;
  98       break;
  99     case eWashout:
 100       denom = 2.00 + dt*C1;
 101       ca = 2.00 / denom;
 102       cb = (2.00 - dt*C1) / denom;
 103       break;
 104     case eIntegrator:
 105       ca = dt*C1 / 2.00;
 106       break;
 107     case eUnknown:
 108       cerr << "Unknown filter type" << endl;
 109     break;
 110   }
 111   FGFCSComponent::bind();
 112
 113   Debug(0);
 114 }
 115
 116 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 117
 118 FGFilter::~FGFilter()
 119 {
 120   Debug(1);
 121 }
 122
 123 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 124
 125 bool FGFilter::Run(void)
 126 {
 127   double test = 0.0;
 128
 129   if (Initialize) {
 130
 131     PreviousOutput1 = PreviousInput1 = Output = Input;
 132     Initialize = false;
 133
 134   } else {
 135
 136     Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
 137     switch (FilterType) {
 138       case eLag:
 139         Output = Input * ca + PreviousInput1 * ca + PreviousOutput1 * cb;
 140         break;
 141       case eLeadLag:
 142         Output = Input * ca + PreviousInput1 * cb + PreviousOutput1 * cc;
 143         break;
 144       case eOrder2:
 145         Output = Input * ca + PreviousInput1 * cb + PreviousInput2 * cc
 146                             - PreviousOutput1 * cd - PreviousOutput2 * ce;
 147         break;
 148       case eWashout:
 149         Output = Input * ca - PreviousInput1 * ca + PreviousOutput1 * cb;
 150         break;
 151       case eIntegrator:
 152         if (Trigger != 0) {
 153           test = Trigger->getDoubleValue();
 154           if (fabs(test) > 0.000001) {
 155             Input  = PreviousInput1 = PreviousInput2 = 0.0;
 156           }
 157         }
 158         Output = Input * ca + PreviousInput1 * ca + PreviousOutput1;
 159         break;
 160       case eUnknown:
 161         break;
 162     }
 163
 164   }
 165
 166   PreviousOutput2 = PreviousOutput1;
 167   PreviousOutput1 = Output;
 168   PreviousInput2  = PreviousInput1;
 169   PreviousInput1  = Input;
 170
 171   Clip();
 172   if (IsOutput) SetOutput();
 173
 174   return true;
 175 }
 176
 177 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 178 //    The bitmasked value choices are as follows:
 179 //    unset: In this case (the default) JSBSim would only print
 180 //       out the normally expected messages, essentially echoing
 181 //       the config files as they are read. If the environment
 182 //       variable is not set, debug_lvl is set to 1 internally
 183 //    0: This requests JSBSim not to output any messages
 184 //       whatsoever.
 185 //    1: This value explicity requests the normal JSBSim
 186 //       startup messages
 187 //    2: This value asks for a message to be printed out when
 188 //       a class is instantiated
 189 //    4: When this value is set, a message is displayed when a
 190 //       FGModel object executes its Run() method
 191 //    8: When this value is set, various runtime state variables
 192 //       are printed out periodically
 193 //    16: When set various parameters are sanity checked and
 194 //       a message is printed out when they go out of bounds
 195
 196 void FGFilter::Debug(int from)
 197 {
 198   if (debug_lvl <= 0) return;
 199
 200   if (debug_lvl & 1) { // Standard console startup message output
 201     if (from == 0) { // Constructor
 202       cout << "      INPUT: " << InputNodes[0]->getName() << endl;
 203       cout << "      C1: " << C1 << endl;
 204       cout << "      C2: " << C2 << endl;
 205       cout << "      C3: " << C3 << endl;
 206       cout << "      C4: " << C4 << endl;
 207       cout << "      C5: " << C5 << endl;
 208       cout << "      C6: " << C6 << endl;
 209       if (IsOutput) cout << "      OUTPUT: " << OutputNode->getName() << endl;
 210     }
 211   }
 212   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 213     if (from == 0) cout << "Instantiated: FGFilter" << endl;
 214     if (from == 1) cout << "Destroyed:    FGFilter" << endl;
 215   }
 216   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 217   }
 218   if (debug_lvl & 8 ) { // Runtime state variables
 219   }
 220   if (debug_lvl & 16) { // Sanity checking
 221   }
 222   if (debug_lvl & 64) {
 223     if (from == 0) { // Constructor
 224       cout << IdSrc << endl;
 225       cout << IdHdr << endl;
 226     }
 227   }
 228 }
 229 }