470a551176
This reverts commit 91ae41f9ef.
We do not need this as outputs already are written to the property
tree and most intermediate values can easily be calculated
manually.
91 lines
2.7 KiB
C++
91 lines
2.7 KiB
C++
// pisimplecontroller.cxx - implementation of a simple PI controller
|
|
//
|
|
// Written by Torsten Dreyer
|
|
// Based heavily on work created by Curtis Olson, started January 2004.
|
|
//
|
|
// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
|
|
// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
|
|
//
|
|
// This program is free software; you can redistribute it and/or
|
|
// modify it under the terms of the GNU General Public License as
|
|
// published by the Free Software Foundation; either version 2 of the
|
|
// License, or (at your option) any later version.
|
|
//
|
|
// This program is distributed in the hope that it will be useful, but
|
|
// WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
// General Public License for more details.
|
|
//
|
|
// You should have received a copy of the GNU General Public License
|
|
// along with this program; if not, write to the Free Software
|
|
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
|
|
//
|
|
|
|
#include "pisimplecontroller.hxx"
|
|
|
|
using namespace FGXMLAutopilot;
|
|
|
|
using std::endl;
|
|
using std::cout;
|
|
|
|
PISimpleController::PISimpleController() :
|
|
AnalogComponent(),
|
|
_int_sum( 0.0 )
|
|
{
|
|
}
|
|
|
|
bool PISimpleController::configure( const string& nodeName, SGPropertyNode_ptr configNode)
|
|
{
|
|
if( AnalogComponent::configure( nodeName, configNode ) )
|
|
return true;
|
|
|
|
if( nodeName == "config" ) {
|
|
Component::configure( configNode );
|
|
return true;
|
|
}
|
|
|
|
if (nodeName == "Kp") {
|
|
_Kp.push_back( new InputValue(configNode) );
|
|
return true;
|
|
}
|
|
|
|
if (nodeName == "Ki") {
|
|
_Ki.push_back( new InputValue(configNode) );
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
void PISimpleController::update( bool firstTime, double dt )
|
|
{
|
|
if ( firstTime ) {
|
|
// we have just been enabled, zero out int_sum
|
|
_int_sum = 0.0;
|
|
}
|
|
|
|
if ( _debug ) cout << "Updating " << get_name() << endl;
|
|
double y_n = _valueInput.get_value();
|
|
double r_n = _referenceInput.get_value();
|
|
|
|
double error = r_n - y_n;
|
|
if ( _debug ) cout << "input = " << y_n
|
|
<< " reference = " << r_n
|
|
<< " error = " << error
|
|
<< endl;
|
|
|
|
double prop_comp = clamp(error * _Kp.get_value());
|
|
_int_sum += error * _Ki.get_value() * dt;
|
|
|
|
|
|
double output = prop_comp + _int_sum;
|
|
double clamped_output = clamp( output );
|
|
if( output != clamped_output ) // anti-windup
|
|
_int_sum = clamped_output - prop_comp;
|
|
|
|
if ( _debug ) cout << "prop_comp = " << prop_comp
|
|
<< " int_sum = " << _int_sum << endl;
|
|
|
|
set_output_value( clamped_output );
|
|
if ( _debug ) cout << "output = " << clamped_output << endl;
|
|
}
|