// pidcontroller.cxx - implementation of PID 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 "pidcontroller.hxx"
using namespace FGXMLAutopilot;
using std::endl;
using std::cout;
PIDController::PIDController():
AnalogComponent(),
alpha( 0.1 ),
beta( 1.0 ),
gamma( 0.0 ),
ep_n_1( 0.0 ),
edf_n_1( 0.0 ),
edf_n_2( 0.0 ),
u_n_1( 0.0 ),
desiredTs( 0.0 ),
elapsedTime( 0.0 )
{
}
/*
* Roy Vegard Ovesen:
*
* Ok! Here is the PID controller algorithm that I would like to see
* implemented:
*
* delta_u_n = Kp * [ (ep_n - ep_n-1) + ((Ts/Ti)*e_n)
* + (Td/Ts)*(edf_n - 2*edf_n-1 + edf_n-2) ]
*
* u_n = u_n-1 + delta_u_n
*
* where:
*
* delta_u : The incremental output
* Kp : Proportional gain
* ep : Proportional error with reference weighing
* ep = beta * r - y
* where:
* beta : Weighing factor
* r : Reference (setpoint)
* y : Process value, measured
* e : Error
* e = r - y
* Ts : Sampling interval
* Ti : Integrator time
* Td : Derivator time
* edf : Derivate error with reference weighing and filtering
* edf_n = edf_n-1 / ((Ts/Tf) + 1) + ed_n * (Ts/Tf) / ((Ts/Tf) + 1)
* where:
* Tf : Filter time
* Tf = alpha * Td , where alpha usually is set to 0.1
* ed : Unfiltered derivate error with reference weighing
* ed = gamma * r - y
* where:
* gamma : Weighing factor
*
* u : absolute output
*
* Index n means the n'th value.
*
*
* Inputs:
* enabled ,
* y_n , r_n , beta=1 , gamma=0 , alpha=0.1 ,
* Kp , Ti , Td , Ts (is the sampling time available?)
* u_min , u_max
*
* Output:
* u_n
*/
void PIDController::update( bool firstTime, double dt )
{
if( firstTime ) {
ep_n_1 = 0.0;
edf_n_2 = edf_n_1 = 0.0;
// first time being enabled, seed with current property tree value
u_n_1 = get_output_value();
}
double u_min = _minInput.get_value();
double u_max = _maxInput.get_value();
elapsedTime += dt;
if( elapsedTime <= desiredTs ) {
// do nothing if time step is not positive (i.e. no time has
// elapsed)
return;
}
double Ts = elapsedTime; // sampling interval (sec)
elapsedTime = 0.0;
if( Ts > SGLimitsd::min()) {
if( _debug ) cout << "Updating " << get_name()
<< " Ts " << Ts << endl;
double y_n = _valueInput.get_value();
double r_n = _referenceInput.get_value();
if ( _debug ) cout << " input = " << y_n << " ref = " << r_n << endl;
// Calculates proportional error:
double ep_n = beta * r_n - y_n;
if ( _debug ) cout << " ep_n = " << ep_n;
if ( _debug ) cout << " ep_n_1 = " << ep_n_1;
// Calculates error:
double e_n = r_n - y_n;
if ( _debug ) cout << " e_n = " << e_n;
double edf_n = 0.0;
double td = Td.get_value();
if ( td > 0.0 ) { // do we need to calcluate derivative error?
// Calculates derivate error:
double ed_n = gamma * r_n - y_n;
if ( _debug ) cout << " ed_n = " << ed_n;
// Calculates filter time:
double Tf = alpha * td;
if ( _debug ) cout << " Tf = " << Tf;
// Filters the derivate error:
edf_n = edf_n_1 / (Ts/Tf + 1)
+ ed_n * (Ts/Tf) / (Ts/Tf + 1);
if ( _debug ) cout << " edf_n = " << edf_n;
} else {
edf_n_2 = edf_n_1 = edf_n = 0.0;
}
// Calculates the incremental output:
double ti = Ti.get_value();
double delta_u_n = 0.0; // incremental output
if ( ti > 0.0 ) {
delta_u_n = Kp.get_value() * ( (ep_n - ep_n_1)
+ ((Ts/ti) * e_n)
+ ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
if ( _debug ) {
cout << " delta_u_n = " << delta_u_n << endl;
cout << "P:" << Kp.get_value() * (ep_n - ep_n_1)
<< " I:" << Kp.get_value() * ((Ts/ti) * e_n)
<< " D:" << Kp.get_value() * ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2))
<< endl;
}
}
// Integrator anti-windup logic:
if ( delta_u_n > (u_max - u_n_1) ) {
delta_u_n = u_max - u_n_1;
if ( _debug ) cout << " max saturation " << endl;
} else if ( delta_u_n < (u_min - u_n_1) ) {
delta_u_n = u_min - u_n_1;
if ( _debug ) cout << " min saturation " << endl;
}
// Calculates absolute output:
double u_n = u_n_1 + delta_u_n;
if ( _debug ) cout << " output = " << u_n << endl;
// Updates indexed values;
u_n_1 = u_n;
ep_n_1 = ep_n;
edf_n_2 = edf_n_1;
edf_n_1 = edf_n;
set_output_value( u_n );
}
}
//------------------------------------------------------------------------------
bool PIDController::configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root )
{
if( cfg_name == "config" ) {
Component::configure(prop_root, cfg_node);
return true;
}
if (cfg_name == "Ts") {
desiredTs = cfg_node.getDoubleValue();
return true;
}
if (cfg_name == "Kp") {
Kp.push_back( new InputValue(prop_root, cfg_node) );
return true;
}
if (cfg_name == "Ti") {
Ti.push_back( new InputValue(prop_root, cfg_node) );
return true;
}
if (cfg_name == "Td") {
Td.push_back( new InputValue(prop_root, cfg_node) );
return true;
}
if (cfg_name == "beta") {
beta = cfg_node.getDoubleValue();
return true;
}
if (cfg_name == "alpha") {
alpha = cfg_node.getDoubleValue();
return true;
}
if (cfg_name == "gamma") {
gamma = cfg_node.getDoubleValue();
return true;
}
return AnalogComponent::configure(cfg_node, cfg_name, prop_root);
}