using namespace FGXMLAutopilot;
+using std::endl;
+using std::cout;
+
PIDController::PIDController():
AnalogComponent(),
alpha( 0.1 ),
void PIDController::update( bool firstTime, double dt )
{
- double edf_n = 0.0;
- double delta_u_n = 0.0; // incremental output
- double u_n = 0.0; // absolute output
+ 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();
double Ts = elapsedTime; // sampling interval (sec)
elapsedTime = 0.0;
- if( firstTime ) {
- // first time being enabled, seed u_n with current
- // property tree value
- ep_n_1 = 0.0;
- edf_n_2 = edf_n_1 = edf_n = 0.0;
- u_n = get_output_value();
- u_n_1 = u_n;
- }
-
if( Ts > SGLimitsd::min()) {
if( _debug ) cout << "Updating " << get_name()
<< " Ts " << Ts << endl;
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 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)
}
// Calculates absolute output:
- u_n = u_n_1 + delta_u_n;
+ double u_n = u_n_1 + delta_u_n;
if ( _debug ) cout << " output = " << u_n << endl;
// Updates indexed values;
}
}
-bool PIDController::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
+//------------------------------------------------------------------------------
+bool PIDController::configure( SGPropertyNode& cfg_node,
+ const std::string& cfg_name,
+ SGPropertyNode& prop_root )
{
- SG_LOG( SG_AUTOPILOT, SG_BULK, "PIDController::configure(" << nodeName << ")" << endl );
-
- if( AnalogComponent::configure( nodeName, configNode ) )
- return true;
-
- if( nodeName == "config" ) {
- Component::configure( configNode );
+ if( cfg_name == "config" ) {
+ Component::configure(prop_root, cfg_node);
return true;
}
- if (nodeName == "Ts") {
- desiredTs = configNode->getDoubleValue();
+ if (cfg_name == "Ts") {
+ desiredTs = cfg_node.getDoubleValue();
return true;
}
- if (nodeName == "Kp") {
- Kp.push_back( new InputValue(configNode) );
+ if (cfg_name == "Kp") {
+ Kp.push_back( new InputValue(prop_root, cfg_node) );
return true;
}
- if (nodeName == "Ti") {
- Ti.push_back( new InputValue(configNode) );
+ if (cfg_name == "Ti") {
+ Ti.push_back( new InputValue(prop_root, cfg_node) );
return true;
}
- if (nodeName == "Td") {
- Td.push_back( new InputValue(configNode) );
+ if (cfg_name == "Td") {
+ Td.push_back( new InputValue(prop_root, cfg_node) );
return true;
}
- if (nodeName == "beta") {
- beta = configNode->getDoubleValue();
+ if (cfg_name == "beta") {
+ beta = cfg_node.getDoubleValue();
return true;
}
- if (nodeName == "alpha") {
- alpha = configNode->getDoubleValue();
+ if (cfg_name == "alpha") {
+ alpha = cfg_node.getDoubleValue();
return true;
}
- if (nodeName == "gamma") {
- gamma = configNode->getDoubleValue();
+ if (cfg_name == "gamma") {
+ gamma = cfg_node.getDoubleValue();
return true;
- }
+ }
- SG_LOG( SG_AUTOPILOT, SG_BULK, "PIDController::configure(" << nodeName << ") [unhandled]" << endl );
- return false;
+ return AnalogComponent::configure(cfg_node, cfg_name, prop_root);
}
-