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;
31 PIDController::PIDController():
48 * Ok! Here is the PID controller algorithm that I would like to see
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) ]
54 * u_n = u_n-1 + delta_u_n
58 * delta_u : The incremental output
59 * Kp : Proportional gain
60 * ep : Proportional error with reference weighing
63 * beta : Weighing factor
64 * r : Reference (setpoint)
65 * y : Process value, measured
68 * Ts : Sampling interval
69 * Ti : Integrator 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)
75 * Tf = alpha * Td , where alpha usually is set to 0.1
76 * ed : Unfiltered derivate error with reference weighing
79 * gamma : Weighing factor
83 * Index n means the n'th value.
88 * y_n , r_n , beta=1 , gamma=0 , alpha=0.1 ,
89 * Kp , Ti , Td , Ts (is the sampling time available?)
96 void PIDController::update( bool firstTime, double dt )
99 double u_n = 0.0; // absolute output
101 double u_min = _minInput.get_value();
102 double u_max = _maxInput.get_value();
105 if( elapsedTime <= desiredTs ) {
106 // do nothing if time step is not positive (i.e. no time has
110 double Ts = elapsedTime; // sampling interval (sec)
114 // first time being enabled, seed u_n with current
115 // property tree value
117 edf_n_2 = edf_n_1 = edf_n = 0.0;
118 u_n = get_output_value();
122 if( Ts > SGLimitsd::min()) {
123 if( _debug ) cout << "Updating " << get_name()
124 << " Ts " << Ts << endl;
126 double y_n = _valueInput.get_value();
127 double r_n = _referenceInput.get_value();
129 if ( _debug ) cout << " input = " << y_n << " ref = " << r_n << endl;
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;
137 double e_n = r_n - y_n;
138 if ( _debug ) cout << " e_n = " << e_n;
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);
150 double td = Td.get_value();
151 if ( td > 0.0 ) { // do we need to calcluate derivative error?
153 // Calculates derivate error:
154 double ed_n = gamma * r_n - y_n;
155 if ( _debug ) cout << " ed_n = " << ed_n;
157 // Calculates filter time:
158 double Tf = alpha * td;
159 if ( _debug ) cout << " Tf = " << Tf;
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;
168 _debug_node->setDoubleValue("ed_n", ed_n);
169 _debug_node->setDoubleValue("Tf", Tf);
170 _debug_node->setDoubleValue("edf_n", edf_n);
173 edf_n_2 = edf_n_1 = edf_n = 0.0;
176 // Calculates the incremental output:
177 double ti = Ti.get_value();
178 double delta_u_n = 0.0; // incremental output
180 delta_u_n = Kp.get_value() * ( (ep_n - ep_n_1)
182 + ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
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));
189 cout << " delta_u_n = " << delta_u_n << endl;
190 cout << "P:" << Pn << " I:" << In << " D:" << Dn << endl;
194 _debug_node->setDoubleValue("Pn", Pn);
195 _debug_node->setDoubleValue("In", In);
196 _debug_node->setDoubleValue("Dn", Dn);
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;
209 // Calculates absolute output:
210 u_n = u_n_1 + delta_u_n;
211 if ( _debug ) cout << " output = " << u_n << endl;
214 _debug_node->setDoubleValue("dU", delta_u_n);
216 // Updates indexed values;
222 set_output_value( u_n );
226 bool PIDController::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
228 SG_LOG( SG_AUTOPILOT, SG_BULK, "PIDController::configure(" << nodeName << ")" << endl );
230 if( AnalogComponent::configure( nodeName, configNode ) )
233 if( nodeName == "config" ) {
234 Component::configure( configNode );
238 if (nodeName == "Ts") {
239 desiredTs = configNode->getDoubleValue();
243 if (nodeName == "Kp") {
244 Kp.push_back( new InputValue(configNode) );
248 if (nodeName == "Ti") {
249 Ti.push_back( new InputValue(configNode) );
253 if (nodeName == "Td") {
254 Td.push_back( new InputValue(configNode) );
258 if (nodeName == "beta") {
259 beta = configNode->getDoubleValue();
263 if (nodeName == "alpha") {
264 alpha = configNode->getDoubleValue();
268 if (nodeName == "gamma") {
269 gamma = configNode->getDoubleValue();
273 SG_LOG( SG_AUTOPILOT, SG_BULK, "PIDController::configure(" << nodeName << ") [unhandled]" << endl );