src/Autopilot/pidcontroller.cxx

   1 // pidcontroller.cxx - implementation of PID controller
   2 //
   3 // Written by Torsten Dreyer
   4 // Based heavily on work created by Curtis Olson, started January 2004.
   5 //
   6 // Copyright (C) 2004  Curtis L. Olson  - http://www.flightgear.org/~curt
   7 // Copyright (C) 2010  Torsten Dreyer - Torsten (at) t3r (dot) de
   8 //
   9 // This program is free software; you can redistribute it and/or
  10 // modify it under the terms of the GNU General Public License as
  11 // published by the Free Software Foundation; either version 2 of the
  12 // License, or (at your option) any later version.
  13 //
  14 // This program is distributed in the hope that it will be useful, but
  15 // WITHOUT ANY WARRANTY; without even the implied warranty of
  16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  17 // General Public License for more details.
  18 //
  19 // You should have received a copy of the GNU General Public License
  20 // along with this program; if not, write to the Free Software
  21 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  22 //
  23
  24 #include "pidcontroller.hxx"
  25
  26 using namespace FGXMLAutopilot;
  27
  28 using std::endl;
  29 using std::cout;
  30
  31 PIDController::PIDController():
  32     AnalogComponent(),
  33     alpha( 0.1 ),
  34     beta( 1.0 ),
  35     gamma( 0.0 ),
  36     ep_n_1( 0.0 ),
  37     edf_n_1( 0.0 ),
  38     edf_n_2( 0.0 ),
  39     u_n_1( 0.0 ),
  40     desiredTs( 0.0 ),
  41     elapsedTime( 0.0 )
  42 {
  43 }
  44
  45 /*
  46  * Roy Vegard Ovesen:
  47  *
  48  * Ok! Here is the PID controller algorithm that I would like to see
  49  * implemented:
  50  *
  51  *   delta_u_n = Kp * [ (ep_n - ep_n-1) + ((Ts/Ti)*e_n)
  52  *               + (Td/Ts)*(edf_n - 2*edf_n-1 + edf_n-2) ]
  53  *
  54  *   u_n = u_n-1 + delta_u_n
  55  *
  56  * where:
  57  *
  58  * delta_u : The incremental output
  59  * Kp      : Proportional gain
  60  * ep      : Proportional error with reference weighing
  61  *           ep = beta * r - y
  62  *           where:
  63  *           beta : Weighing factor
  64  *           r    : Reference (setpoint)
  65  *           y    : Process value, measured
  66  * e       : Error
  67  *           e = r - y
  68  * Ts      : Sampling interval
  69  * Ti      : Integrator time
  70  * Td      : Derivator time
  71  * edf     : Derivate error with reference weighing and filtering
  72  *           edf_n = edf_n-1 / ((Ts/Tf) + 1) + ed_n * (Ts/Tf) / ((Ts/Tf) + 1)
  73  *           where:
  74  *           Tf : Filter time
  75  *           Tf = alpha * Td , where alpha usually is set to 0.1
  76  *           ed : Unfiltered derivate error with reference weighing
  77  *             ed = gamma * r - y
  78  *             where:
  79  *             gamma : Weighing factor
  80  *
  81  * u       : absolute output
  82  *
  83  * Index n means the n'th value.
  84  *
  85  *
  86  * Inputs:
  87  * enabled ,
  88  * y_n , r_n , beta=1 , gamma=0 , alpha=0.1 ,
  89  * Kp , Ti , Td , Ts (is the sampling time available?)
  90  * u_min , u_max
  91  *
  92  * Output:
  93  * u_n
  94  */
  95
  96 void PIDController::update( bool firstTime, double dt )
  97 {
  98     double edf_n = 0.0;
  99     double u_n = 0.0;       // absolute output
 100
 101     double u_min = _minInput.get_value();
 102     double u_max = _maxInput.get_value();
 103
 104     elapsedTime += dt;
 105     if( elapsedTime <= desiredTs ) {
 106         // do nothing if time step is not positive (i.e. no time has
 107         // elapsed)
 108         return;
 109     }
 110     double Ts = elapsedTime; // sampling interval (sec)
 111     elapsedTime = 0.0;
 112
 113     if( firstTime ) {
 114       // first time being enabled, seed u_n with current
 115       // property tree value
 116       ep_n_1 = 0.0;
 117       edf_n_2 = edf_n_1 = edf_n = 0.0;
 118       u_n = get_output_value();
 119       u_n_1 = u_n;
 120     }
 121
 122     if( Ts > SGLimitsd::min()) {
 123         if( _debug ) cout <<  "Updating " << get_name()
 124                           << " Ts " << Ts << endl;
 125
 126         double y_n = _valueInput.get_value();
 127         double r_n = _referenceInput.get_value();
 128
 129         if ( _debug ) cout << "  input = " << y_n << " ref = " << r_n << endl;
 130
 131         // Calculates proportional error:
 132         double ep_n = beta * r_n - y_n;
 133         if ( _debug ) cout << "  ep_n = " << ep_n;
 134         if ( _debug ) cout << "  ep_n_1 = " << ep_n_1;
 135
 136         // Calculates error:
 137         double e_n = r_n - y_n;
 138         if ( _debug ) cout << " e_n = " << e_n;
 139
 140         if( _debug_node )
 141         {
 142           _debug_node->setDoubleValue("Ts", Ts);
 143           _debug_node->setDoubleValue("y_n", y_n);
 144           _debug_node->setDoubleValue("r_n", r_n);
 145           _debug_node->setDoubleValue("ep_n", ep_n);
 146           _debug_node->setDoubleValue("ep_n_1", ep_n_1);
 147           _debug_node->setDoubleValue("e_n", e_n);
 148         }
 149
 150         double td = Td.get_value();
 151         if ( td > 0.0 ) { // do we need to calcluate derivative error?
 152
 153           // Calculates derivate error:
 154             double ed_n = gamma * r_n - y_n;
 155             if ( _debug ) cout << " ed_n = " << ed_n;
 156
 157             // Calculates filter time:
 158             double Tf = alpha * td;
 159             if ( _debug ) cout << " Tf = " << Tf;
 160
 161             // Filters the derivate error:
 162             edf_n = edf_n_1 / (Ts/Tf + 1)
 163                 + ed_n * (Ts/Tf) / (Ts/Tf + 1);
 164             if ( _debug ) cout << " edf_n = " << edf_n;
 165
 166             if( _debug_node )
 167             {
 168               _debug_node->setDoubleValue("ed_n", ed_n);
 169               _debug_node->setDoubleValue("Tf", Tf);
 170               _debug_node->setDoubleValue("edf_n", edf_n);
 171             }
 172         } else {
 173             edf_n_2 = edf_n_1 = edf_n = 0.0;
 174         }
 175
 176         // Calculates the incremental output:
 177         double ti = Ti.get_value();
 178         double delta_u_n = 0.0; // incremental output
 179         if ( ti > 0.0 ) {
 180             delta_u_n = Kp.get_value() * ( (ep_n - ep_n_1)
 181                                + ((Ts/ti) * e_n)
 182                                + ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
 183
 184           double Pn = Kp.get_value() * (ep_n - ep_n_1),
 185                  In = Kp.get_value() * ((Ts/ti) * e_n),
 186                  Dn = Kp.get_value() * ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2));
 187           if ( _debug )
 188           {
 189               cout << " delta_u_n = " << delta_u_n << endl;
 190               cout << "P:" << Pn << " I:" << In << " D:" << Dn << endl;
 191           }
 192           if( _debug_node )
 193           {
 194             _debug_node->setDoubleValue("Pn", Pn);
 195             _debug_node->setDoubleValue("In", In);
 196             _debug_node->setDoubleValue("Dn", Dn);
 197           }
 198         }
 199
 200         // Integrator anti-windup logic:
 201         if ( delta_u_n > (u_max - u_n_1) ) {
 202             delta_u_n = u_max - u_n_1;
 203             if ( _debug ) cout << " max saturation " << endl;
 204         } else if ( delta_u_n < (u_min - u_n_1) ) {
 205             delta_u_n = u_min - u_n_1;
 206             if ( _debug ) cout << " min saturation " << endl;
 207         }
 208
 209         // Calculates absolute output:
 210         u_n = u_n_1 + delta_u_n;
 211         if ( _debug ) cout << "  output = " << u_n << endl;
 212
 213         if( _debug_node )
 214           _debug_node->setDoubleValue("dU", delta_u_n);
 215
 216         // Updates indexed values;
 217         u_n_1   = u_n;
 218         ep_n_1  = ep_n;
 219         edf_n_2 = edf_n_1;
 220         edf_n_1 = edf_n;
 221
 222         set_output_value( u_n );
 223     }
 224 }
 225
 226 bool PIDController::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
 227 {
 228   SG_LOG( SG_AUTOPILOT, SG_BULK, "PIDController::configure(" << nodeName << ")" << endl );
 229
 230   if( AnalogComponent::configure( nodeName, configNode ) )
 231     return true;
 232
 233   if( nodeName == "config" ) {
 234     Component::configure( configNode );
 235     return true;
 236   }
 237
 238   if (nodeName == "Ts") {
 239     desiredTs = configNode->getDoubleValue();
 240     return true;
 241   }
 242
 243   if (nodeName == "Kp") {
 244     Kp.push_back( new InputValue(configNode) );
 245     return true;
 246   }
 247
 248   if (nodeName == "Ti") {
 249     Ti.push_back( new InputValue(configNode) );
 250     return true;
 251   }
 252
 253   if (nodeName == "Td") {
 254     Td.push_back( new InputValue(configNode) );
 255     return true;
 256   }
 257
 258   if (nodeName == "beta") {
 259     beta = configNode->getDoubleValue();
 260     return true;
 261   }
 262
 263   if (nodeName == "alpha") {
 264     alpha = configNode->getDoubleValue();
 265     return true;
 266   }
 267
 268   if (nodeName == "gamma") {
 269     gamma = configNode->getDoubleValue();
 270     return true;
 271   }
 272
 273   SG_LOG( SG_AUTOPILOT, SG_BULK, "PIDController::configure(" << nodeName << ") [unhandled]" << endl );
 274   return false;
 275 }
 276