1 // pidcontroller.cxx - implementation of PID controller
3 // Written by Torsten Dreyer
4 // Based heavily on work created by Curtis Olson, started January 2004.
6 // Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
7 // Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
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.
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.
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.
24 #include "pidcontroller.hxx"
26 using namespace FGXMLAutopilot;
28 PIDController::PIDController():
45 * Ok! Here is the PID controller algorithm that I would like to see
48 * delta_u_n = Kp * [ (ep_n - ep_n-1) + ((Ts/Ti)*e_n)
49 * + (Td/Ts)*(edf_n - 2*edf_n-1 + edf_n-2) ]
51 * u_n = u_n-1 + delta_u_n
55 * delta_u : The incremental output
56 * Kp : Proportional gain
57 * ep : Proportional error with reference weighing
60 * beta : Weighing factor
61 * r : Reference (setpoint)
62 * y : Process value, measured
65 * Ts : Sampling interval
66 * Ti : Integrator time
68 * edf : Derivate error with reference weighing and filtering
69 * edf_n = edf_n-1 / ((Ts/Tf) + 1) + ed_n * (Ts/Tf) / ((Ts/Tf) + 1)
72 * Tf = alpha * Td , where alpha usually is set to 0.1
73 * ed : Unfiltered derivate error with reference weighing
76 * gamma : Weighing factor
80 * Index n means the n'th value.
85 * y_n , r_n , beta=1 , gamma=0 , alpha=0.1 ,
86 * Kp , Ti , Td , Ts (is the sampling time available?)
93 void PIDController::update( bool firstTime, double dt )
96 double delta_u_n = 0.0; // incremental output
97 double u_n = 0.0; // absolute output
99 double u_min = _minInput.get_value();
100 double u_max = _maxInput.get_value();
103 if( elapsedTime <= desiredTs ) {
104 // do nothing if time step is not positive (i.e. no time has
108 double Ts = elapsedTime; // sampling interval (sec)
112 // first time being enabled, seed u_n with current
113 // property tree value
115 edf_n_2 = edf_n_1 = edf_n = 0.0;
116 u_n = get_output_value();
120 if( Ts > SGLimitsd::min()) {
121 if( _debug ) cout << "Updating " << get_name()
122 << " Ts " << Ts << endl;
124 double y_n = _valueInput.get_value();
125 double r_n = _referenceInput.get_value();
127 if ( _debug ) cout << " input = " << y_n << " ref = " << r_n << endl;
129 // Calculates proportional error:
130 double ep_n = beta * r_n - y_n;
131 if ( _debug ) cout << " ep_n = " << ep_n;
132 if ( _debug ) cout << " ep_n_1 = " << ep_n_1;
135 double e_n = r_n - y_n;
136 if ( _debug ) cout << " e_n = " << e_n;
138 double td = Td.get_value();
139 if ( td > 0.0 ) { // do we need to calcluate derivative error?
141 // Calculates derivate error:
142 double ed_n = gamma * r_n - y_n;
143 if ( _debug ) cout << " ed_n = " << ed_n;
145 // Calculates filter time:
146 double Tf = alpha * td;
147 if ( _debug ) cout << " Tf = " << Tf;
149 // Filters the derivate error:
150 edf_n = edf_n_1 / (Ts/Tf + 1)
151 + ed_n * (Ts/Tf) / (Ts/Tf + 1);
152 if ( _debug ) cout << " edf_n = " << edf_n;
154 edf_n_2 = edf_n_1 = edf_n = 0.0;
157 // Calculates the incremental output:
158 double ti = Ti.get_value();
160 delta_u_n = Kp.get_value() * ( (ep_n - ep_n_1)
162 + ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
165 cout << " delta_u_n = " << delta_u_n << endl;
166 cout << "P:" << Kp.get_value() * (ep_n - ep_n_1)
167 << " I:" << Kp.get_value() * ((Ts/ti) * e_n)
168 << " D:" << Kp.get_value() * ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2))
173 // Integrator anti-windup logic:
174 if ( delta_u_n > (u_max - u_n_1) ) {
175 delta_u_n = u_max - u_n_1;
176 if ( _debug ) cout << " max saturation " << endl;
177 } else if ( delta_u_n < (u_min - u_n_1) ) {
178 delta_u_n = u_min - u_n_1;
179 if ( _debug ) cout << " min saturation " << endl;
182 // Calculates absolute output:
183 u_n = u_n_1 + delta_u_n;
184 if ( _debug ) cout << " output = " << u_n << endl;
186 // Updates indexed values;
192 set_output_value( u_n );
196 bool PIDController::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
198 SG_LOG( SG_AUTOPILOT, SG_BULK, "PIDController::configure(" << nodeName << ")" << endl );
200 if( AnalogComponent::configure( nodeName, configNode ) )
203 if( nodeName == "config" ) {
204 Component::configure( configNode );
208 if (nodeName == "Ts") {
209 desiredTs = configNode->getDoubleValue();
213 if (nodeName == "Kp") {
214 Kp.push_back( new InputValue(configNode) );
218 if (nodeName == "Ti") {
219 Ti.push_back( new InputValue(configNode) );
223 if (nodeName == "Td") {
224 Td.push_back( new InputValue(configNode) );
228 if (nodeName == "beta") {
229 beta = configNode->getDoubleValue();
233 if (nodeName == "alpha") {
234 alpha = configNode->getDoubleValue();
238 if (nodeName == "gamma") {
239 gamma = configNode->getDoubleValue();
243 SG_LOG( SG_AUTOPILOT, SG_BULK, "PIDController::configure(" << nodeName << ") [unhandled]" << endl );