]> git.mxchange.org Git - flightgear.git/blob - src/Autopilot/pidcontroller.cxx
Kill off platformDesktopPath entirely
[flightgear.git] / 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         double td = Td.get_value();
141         if ( td > 0.0 ) { // do we need to calcluate derivative error?
142
143           // Calculates derivate error:
144             double ed_n = gamma * r_n - y_n;
145             if ( _debug ) cout << " ed_n = " << ed_n;
146
147             // Calculates filter time:
148             double Tf = alpha * td;
149             if ( _debug ) cout << " Tf = " << Tf;
150
151             // Filters the derivate error:
152             edf_n = edf_n_1 / (Ts/Tf + 1)
153                 + ed_n * (Ts/Tf) / (Ts/Tf + 1);
154             if ( _debug ) cout << " edf_n = " << edf_n;
155         } else {
156             edf_n_2 = edf_n_1 = edf_n = 0.0;
157         }
158
159         // Calculates the incremental output:
160         double ti = Ti.get_value();
161         double delta_u_n = 0.0; // incremental output
162         if ( ti > 0.0 ) {
163             delta_u_n = Kp.get_value() * ( (ep_n - ep_n_1)
164                                + ((Ts/ti) * e_n)
165                                + ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
166
167           if ( _debug ) {
168               cout << " delta_u_n = " << delta_u_n << endl;
169               cout << "P:" << Kp.get_value() * (ep_n - ep_n_1)
170                    << " I:" << Kp.get_value() * ((Ts/ti) * e_n)
171                    << " D:" << Kp.get_value() * ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2))
172                    << endl;
173         }
174         }
175
176         // Integrator anti-windup logic:
177         if ( delta_u_n > (u_max - u_n_1) ) {
178             delta_u_n = u_max - u_n_1;
179             if ( _debug ) cout << " max saturation " << endl;
180         } else if ( delta_u_n < (u_min - u_n_1) ) {
181             delta_u_n = u_min - u_n_1;
182             if ( _debug ) cout << " min saturation " << endl;
183         }
184
185         // Calculates absolute output:
186         u_n = u_n_1 + delta_u_n;
187         if ( _debug ) cout << "  output = " << u_n << endl;
188
189         // Updates indexed values;
190         u_n_1   = u_n;
191         ep_n_1  = ep_n;
192         edf_n_2 = edf_n_1;
193         edf_n_1 = edf_n;
194
195         set_output_value( u_n );
196     }
197 }
198
199 bool PIDController::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
200 {
201   SG_LOG( SG_AUTOPILOT, SG_BULK, "PIDController::configure(" << nodeName << ")" << endl );
202
203   if( AnalogComponent::configure( nodeName, configNode ) )
204     return true;
205
206   if( nodeName == "config" ) {
207     Component::configure( configNode );
208     return true;
209   }
210
211   if (nodeName == "Ts") {
212     desiredTs = configNode->getDoubleValue();
213     return true;
214   } 
215
216   if (nodeName == "Kp") {
217     Kp.push_back( new InputValue(configNode) );
218     return true;
219   } 
220
221   if (nodeName == "Ti") {
222     Ti.push_back( new InputValue(configNode) );
223     return true;
224   } 
225
226   if (nodeName == "Td") {
227     Td.push_back( new InputValue(configNode) );
228     return true;
229   } 
230
231   if (nodeName == "beta") {
232     beta = configNode->getDoubleValue();
233     return true;
234   } 
235
236   if (nodeName == "alpha") {
237     alpha = configNode->getDoubleValue();
238     return true;
239   } 
240
241   if (nodeName == "gamma") {
242     gamma = configNode->getDoubleValue();
243     return true;
244   } 
245
246   SG_LOG( SG_AUTOPILOT, SG_BULK, "PIDController::configure(" << nodeName << ") [unhandled]" << endl );
247   return false;
248 }
249