FlightGear next
pidcontroller.cxx
Go to the documentation of this file.
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
26using namespace FGXMLAutopilot;
27
28using std::endl;
29using std::cout;
30
31PIDController::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 startup_current( false ),
43 startup_its( 0 ),
44 iteration( 0 )
45{
46}
47
48/*
49 * Roy Vegard Ovesen:
50 *
51 * Ok! Here is the PID controller algorithm that I would like to see
52 * implemented:
53 *
54 * delta_u_n = Kp * [ (ep_n - ep_n-1) + ((Ts/Ti)*e_n)
55 * + (Td/Ts)*(edf_n - 2*edf_n-1 + edf_n-2) ]
56 *
57 * u_n = u_n-1 + delta_u_n
58 *
59 * where:
60 *
61 * delta_u : The incremental output
62 * Kp : Proportional gain
63 * ep : Proportional error with reference weighing
64 * ep = beta * r - y
65 * where:
66 * beta : Weighing factor
67 * r : Reference (setpoint)
68 * y : Process value, measured
69 * e : Error
70 * e = r - y
71 * Ts : Sampling interval
72 * Ti : Integrator time
73 * Td : Derivator time
74 * edf : Derivate error with reference weighing and filtering
75 * edf_n = edf_n-1 / ((Ts/Tf) + 1) + ed_n * (Ts/Tf) / ((Ts/Tf) + 1)
76 * where:
77 * Tf : Filter time
78 * Tf = alpha * Td , where alpha usually is set to 0.1
79 * ed : Unfiltered derivate error with reference weighing
80 * ed = gamma * r - y
81 * where:
82 * gamma : Weighing factor
83 *
84 * u : absolute output
85 *
86 * Index n means the n'th value.
87 *
88 *
89 * Inputs:
90 * enabled ,
91 * y_n , r_n , beta=1 , gamma=0 , alpha=0.1 ,
92 * Kp , Ti , Td , Ts (is the sampling time available?)
93 * u_min , u_max
94 *
95 * Output:
96 * u_n
97 */
98
99void PIDController::update( bool firstTime, double dt )
100{
101 elapsedTime += dt;
102 if (firstTime) {
103 iteration = 0;
104 /* We always initialise edf_n_1 to zero, regardless of startup_its. */
105 edf_n_1 = 0;
106 }
107 else if (elapsedTime <= desiredTs ) {
108 // do nothing if not enough time has passed.
109 return;
110 }
111
112 iteration += 1;
113
114 /* We are going to do an iteration so reset elapsedTime. */
115 double Ts = elapsedTime;
116 elapsedTime = 0.0;
117
118 /* Read generic things from our AnalogComponent base class. */
119 double y_n = _valueInput.get_value(); // input.
120 double r_n = _referenceInput.get_value(); // reference.
121 double u_min = _minInput.get_value(); // minimum output.
122 double u_max = _maxInput.get_value(); // maximum output.
123
124 /* Read things specific to PIDController. */
125 double td = Td.get_value(); // derivative time.
126 double ti = Ti.get_value(); // (reciprocal?) integral time.
127
128 /*
129 Now do the PID calculations.
130 */
131
132 double ep_n = beta * r_n - y_n; // proportional error.
133 double e_n = r_n - y_n; // error.
134 double ed_n = gamma * r_n - y_n; // derivate error.
135 double Tf = alpha * td; // filter time.
136
137 double edf_n = 0.0; // derivate error.
138 if (td > 0.0) {
139 edf_n = edf_n_1 / (Ts/Tf + 1)
140 + ed_n * (Ts/Tf) / (Ts/Tf + 1);
141 }
142
143 if (firstTime) {
144 if (startup_current) {
145 /* Seed our historical state with current values. This avoids spurious
146 large terms in calculation of delta_u_n below. */
147 ep_n_1 = ep_n;
148 edf_n_2 = edf_n;
149 edf_n_1 = edf_n;
150 }
151 else {
152 // Default behaviour.
153 ep_n_1 = 0;
154 edf_n_2 = 0;
155 // edf_n_1 is already set to zero above.
156 }
157 u_n_1 = get_output_value();
158 }
159
160 double delta_u_n = 0.0; // incremental output
161 if ( ti > 0.0 ) {
162 delta_u_n = Kp.get_value() * (
163 (ep_n - ep_n_1)
164 + ((Ts/ti) * e_n)
165 + ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2))
166 );
167 }
168
169 const char* saturation = "";
170 double u_n; // absolute output
171
172 if (iteration > startup_its) {
173
174 /* Update the output, clipping to u_min..u_max. */
175
176 u_n = u_n_1 + delta_u_n;
177
178 if (u_n > u_max) {
179 u_n = u_max;
180 saturation = " max_saturation";
181 }
182 else if (u_n < u_min) {
183 u_n = u_min;
184 saturation = " min_saturation";
185 }
186 set_output_value( u_n );
187 }
188 else {
189 /* Do not change the output. Instead get the current output value for
190 use in setting our historical state below. */
191 if (_debug) {
192 cout << subsystemId()
193 << ": doing nothing."
194 << " startup_its=" << startup_its
195 << " iteration=" << iteration
196 << std::endl;
197 }
198 u_n = get_output_value();
199 }
200
201 if ( _debug ) {
202 cout
203 << "Updating " << subsystemId()
204 << " startup_its=" << startup_its
205 << " startup_current=" << startup_current
206 << " firstTime=" << firstTime
207 << " iteration=" << iteration
208 << " Ts=" << Ts
209 << " input=" << y_n
210 << " ref=" << r_n
211 << " ep_n=" << ep_n
212 << " ep_n_1=" << ep_n_1
213 << " e_n=" << e_n
214 << " ed_n=" << ed_n
215 << " Tf=" << Tf
216 << " edf_n=" << edf_n
217 << " edf_n_1=" << edf_n_1
218 << " edf_n_2=" << edf_n_2
219 << " ti=" << ti
220 << " delta_u_n=" << delta_u_n
221 << " P=" << Kp.get_value() * (ep_n - ep_n_1)
222 << " I=" << Kp.get_value() * ((Ts/ti) * e_n)
223 << " D=" << Kp.get_value() * ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2))
224 << saturation
225 << " u_n_1=" << u_n_1
226 << " delta_u_n=" << delta_u_n
227 << " output=" << u_n
228 << std::endl;
229 }
230
231 // Updates indexed values;
232 u_n_1 = u_n;
233 ep_n_1 = ep_n;
234 edf_n_2 = edf_n_1;
235 edf_n_1 = edf_n;
236}
237
238//------------------------------------------------------------------------------
239bool PIDController::configure( SGPropertyNode& cfg_node,
240 const std::string& cfg_name,
241 SGPropertyNode& prop_root )
242{
243 if( cfg_name == "config" ) {
244 Component::configure(prop_root, cfg_node);
245 return true;
246 }
247
248 if (cfg_name == "Ts") {
249 desiredTs = cfg_node.getDoubleValue();
250 return true;
251 }
252
253 if (cfg_name == "Kp") {
254 Kp.push_back(new simgear::Value(prop_root, cfg_node));
255 return true;
256 }
257
258 if (cfg_name == "Ti") {
259 Ti.push_back(new simgear::Value(prop_root, cfg_node));
260 return true;
261 }
262
263 if (cfg_name == "Td") {
264 Td.push_back(new simgear::Value(prop_root, cfg_node));
265 return true;
266 }
267
268 if (cfg_name == "beta") {
269 beta = cfg_node.getDoubleValue();
270 return true;
271 }
272
273 if (cfg_name == "alpha") {
274 alpha = cfg_node.getDoubleValue();
275 return true;
276 }
277
278 if (cfg_name == "gamma") {
279 gamma = cfg_node.getDoubleValue();
280 return true;
281 }
282
283 if (cfg_name == "startup-its") {
284 startup_its = cfg_node.getIntValue();
285 return true;
286 }
287
288 if (cfg_name == "startup-current") {
289 startup_current = cfg_node.getBoolValue();
290 return true;
291 }
292
293 return AnalogComponent::configure(cfg_node, cfg_name, prop_root);
294}
295
296
297// Register the subsystem.
298SGSubsystemMgr::Registrant<PIDController> registrantPIDController;
FGXMLAutopilot::AnalogComponent::_valueInput
simgear::ValueList _valueInput
the value input
Definition analogcomponent.hxx:57
FGXMLAutopilot::AnalogComponent::configure
bool configure(SGPropertyNode &cfg_node, const std::string &cfg_name, SGPropertyNode &prop_root) override
This method configures this analog component from a property node.
Definition analogcomponent.cxx:54
FGXMLAutopilot::AnalogComponent::AnalogComponent
AnalogComponent()
A constructor for an analog component.
Definition analogcomponent.cxx:30
FGXMLAutopilot::AnalogComponent::_referenceInput
simgear::ValueList _referenceInput
the reference input
Definition analogcomponent.hxx:62
FGXMLAutopilot::AnalogComponent::_minInput
simgear::ValueList _minInput
the minimum output clamp input
Definition analogcomponent.hxx:67
FGXMLAutopilot::AnalogComponent::get_output_value
double get_output_value() const
return the current double value of the output property
Definition analogcomponent.hxx:120
FGXMLAutopilot::AnalogComponent::_maxInput
simgear::ValueList _maxInput
the maximum output clamp input
Definition analogcomponent.hxx:72
FGXMLAutopilot::Component::configure
virtual bool configure(SGPropertyNode &cfg_node, const std::string &cfg_name, SGPropertyNode &prop_root)
Definition component.cxx:67
FGXMLAutopilot::Component::_debug
bool _debug
debug flag, true if this component should generate some useful output on every iteration
Definition component.hxx:66
FGXMLAutopilot::PIDController::update
void update(bool firstTime, double dt) override
pure virtual function to be implemented by the derived classes.
Definition pidcontroller.cxx:99
FGXMLAutopilot::PIDController::configure
virtual bool configure(SGPropertyNode &cfg_node, const std::string &cfg_name, SGPropertyNode &prop_root)
This method configures this analog component from a property node.
Definition pidcontroller.cxx:239
registrantPIDController
SGSubsystemMgr::Registrant< PIDController > registrantPIDController
Definition pidcontroller.cxx:298