]> git.mxchange.org Git - flightgear.git/blob - src/Autopilot/pisimplecontroller.cxx
Kill off platformDesktopPath entirely
[flightgear.git] / src / Autopilot / pisimplecontroller.cxx
1 // pisimplecontroller.cxx - implementation of a simple PI 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 "pisimplecontroller.hxx"
25
26 using namespace FGXMLAutopilot;
27
28 using std::endl;
29 using std::cout;
30
31 PISimpleController::PISimpleController() :
32     AnalogComponent(),
33     _int_sum( 0.0 )
34 {
35 }
36
37 bool PISimpleController::configure( const string& nodeName, SGPropertyNode_ptr configNode)
38 {
39   if( AnalogComponent::configure( nodeName, configNode ) )
40     return true;
41
42   if( nodeName == "config" ) {
43     Component::configure( configNode );
44     return true;
45   }
46
47   if (nodeName == "Kp") {
48     _Kp.push_back( new InputValue(configNode) );
49     return true;
50   } 
51
52   if (nodeName == "Ki") {
53     _Ki.push_back( new InputValue(configNode) );
54     return true;
55   }
56
57   return false;
58 }
59
60 void PISimpleController::update( bool firstTime, double dt )
61 {
62     if ( firstTime ) {
63         // we have just been enabled, zero out int_sum
64         _int_sum = 0.0;
65     }
66
67     if ( _debug ) cout << "Updating " << get_name() << endl;
68     double y_n = _valueInput.get_value();
69     double r_n = _referenceInput.get_value();
70                   
71     double error = r_n - y_n;
72     if ( _debug ) cout << "input = " << y_n
73                       << " reference = " << r_n
74                       << " error = " << error
75                       << endl;
76
77     double prop_comp = clamp(error * _Kp.get_value());
78     _int_sum += error * _Ki.get_value() * dt;
79
80
81     double output = prop_comp + _int_sum;
82     double clamped_output = clamp( output );
83     if( output != clamped_output ) // anti-windup
84       _int_sum = clamped_output - prop_comp;
85
86     if ( _debug ) cout << "prop_comp = " << prop_comp
87                       << " int_sum = " << _int_sum << endl;
88
89     set_output_value( clamped_output );
90     if ( _debug ) cout << "output = " << clamped_output << endl;
91 }