src/FDM/JSBSim/models/flight_control/FGPID.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Module:       FGPID.cpp
   4  Author:       Jon S. Berndt
   5  Date started: 6/17/2006
   6
   7  ------------- Copyright (C) 2006 Jon S. Berndt (jon@jsbsim.org) -------------
   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 HISTORY
  27 --------------------------------------------------------------------------------
  28 Initial code 6/17/2006 JSB
  29
  30 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  31 COMMENTS, REFERENCES,  and NOTES
  32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  33
  34 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  35 INCLUDES
  36 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  37
  38 #include "FGPID.h"
  39
  40 namespace JSBSim {
  41
  42 static const char *IdSrc = "$Id$";
  43 static const char *IdHdr = ID_PID;
  44
  45 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  46 CLASS IMPLEMENTATION
  47 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  48
  49 FGPID::FGPID(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
  50 {
  51   string kp_string, ki_string, kd_string;
  52   dt = fcs->GetState()->Getdt();
  53
  54   Kp = Ki = Kd = 0.0;
  55   KpPropertyNode = 0;
  56   KiPropertyNode = 0;
  57   KdPropertyNode = 0;
  58   KpPropertySign = 1.0;
  59   KiPropertySign = 1.0;
  60   KdPropertySign = 1.0;
  61   I_out_total = 0.0;
  62   Input_prev = Input_prev2 = 0.0;
  63   Trigger = 0;
  64
  65   if ( element->FindElement("kp") ) {
  66     kp_string = element->FindElementValue("kp");
  67     if (!is_number(kp_string)) { // property
  68       if (kp_string[0] == '-') {
  69        KpPropertySign = -1.0;
  70        kp_string.erase(0,1);
  71       }
  72       KpPropertyNode = PropertyManager->GetNode(kp_string);
  73     } else {
  74       Kp = element->FindElementValueAsNumber("kp");
  75     }
  76   }
  77
  78   if ( element->FindElement("ki") ) {
  79     ki_string = element->FindElementValue("ki");
  80     if (!is_number(ki_string)) { // property
  81       if (ki_string[0] == '-') {
  82        KiPropertySign = -1.0;
  83        ki_string.erase(0,1);
  84       }
  85       KiPropertyNode = PropertyManager->GetNode(ki_string);
  86     } else {
  87       Ki = element->FindElementValueAsNumber("ki");
  88     }
  89   }
  90
  91   if ( element->FindElement("kd") ) {
  92     kd_string = element->FindElementValue("kd");
  93     if (!is_number(kd_string)) { // property
  94       if (kd_string[0] == '-') {
  95        KdPropertySign = -1.0;
  96        kd_string.erase(0,1);
  97       }
  98       KdPropertyNode = PropertyManager->GetNode(kd_string);
  99     } else {
 100       Kd = element->FindElementValueAsNumber("kd");
 101     }
 102   }
 103
 104   if (element->FindElement("trigger")) {
 105     Trigger =  PropertyManager->GetNode(element->FindElementValue("trigger"));
 106   }
 107
 108   FGFCSComponent::bind();
 109
 110   Debug(0);
 111 }
 112
 113 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 114
 115 FGPID::~FGPID()
 116 {
 117   Debug(1);
 118 }
 119
 120 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 121
 122 bool FGPID::Run(void )
 123 {
 124   double I_out_delta = 0.0;
 125   double P_out, D_out;
 126
 127   Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
 128
 129   if (KpPropertyNode != 0) Kp = KpPropertyNode->getDoubleValue() * KpPropertySign;
 130   if (KiPropertyNode != 0) Ki = KiPropertyNode->getDoubleValue() * KiPropertySign;
 131   if (KdPropertyNode != 0) Kd = KdPropertyNode->getDoubleValue() * KdPropertySign;
 132
 133   P_out = Kp * Input;
 134   D_out = (Kd / dt) * (Input - Input_prev);
 135
 136   // Do not continue to integrate the input to the integrator if a wind-up
 137   // condition is sensed - that is, if the property pointed to by the trigger
 138   // element is non-zero. Reset the integrator to 0.0 if the Trigger value
 139   // is negative.
 140
 141   if (Trigger != 0) {
 142     double test = Trigger->getDoubleValue();
 143     if (fabs(test) < 0.000001) I_out_delta = Ki * dt * Input;  // Normal
 144     if (test < 0.0)            I_out_total = 0.0;  // Reset integrator to 0.0
 145   } else { // no anti-wind-up trigger defined
 146     I_out_delta = Ki * dt * Input;
 147   }
 148
 149   I_out_total += I_out_delta;
 150
 151   Output = P_out + I_out_total + D_out;
 152
 153   Input_prev = Input;
 154   Input_prev2 = Input_prev;
 155
 156   Clip();
 157   if (IsOutput) SetOutput();
 158
 159   return true;
 160 }
 161
 162 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 163 //    The bitmasked value choices are as follows:
 164 //    unset: In this case (the default) JSBSim would only print
 165 //       out the normally expected messages, essentially echoing
 166 //       the config files as they are read. If the environment
 167 //       variable is not set, debug_lvl is set to 1 internally
 168 //    0: This requests JSBSim not to output any messages
 169 //       whatsoever.
 170 //    1: This value explicity requests the normal JSBSim
 171 //       startup messages
 172 //    2: This value asks for a message to be printed out when
 173 //       a class is instantiated
 174 //    4: When this value is set, a message is displayed when a
 175 //       FGModel object executes its Run() method
 176 //    8: When this value is set, various runtime state variables
 177 //       are printed out periodically
 178 //    16: When set various parameters are sanity checked and
 179 //       a message is printed out when they go out of bounds
 180
 181 void FGPID::Debug(int from)
 182 {
 183   if (debug_lvl <= 0) return;
 184
 185   if (debug_lvl & 1) { // Standard console startup message output
 186     if (from == 0) { // Constructor
 187       if (InputSigns[0] < 0)
 188         cout << "      INPUT: -" << InputNodes[0]->getName() << endl;
 189       else
 190         cout << "      INPUT: " << InputNodes[0]->getName() << endl;
 191
 192       if (IsOutput) cout << "      OUTPUT: " << OutputNode->getName() << endl;
 193     }
 194   }
 195   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 196     if (from == 0) cout << "Instantiated: FGPID" << endl;
 197     if (from == 1) cout << "Destroyed:    FGPID" << endl;
 198   }
 199   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 200   }
 201   if (debug_lvl & 8 ) { // Runtime state variables
 202   }
 203   if (debug_lvl & 16) { // Sanity checking
 204   }
 205   if (debug_lvl & 64) {
 206     if (from == 0) { // Constructor
 207       cout << IdSrc << endl;
 208       cout << IdHdr << endl;
 209     }
 210   }
 211 }
 212 }