fly/flightgear
1
0
Fork 0
Code Activity

Cleaned up some left over stuff. Working towards infrastructure to support

Browse source 
adding additional PID type algorithms to the code.
Add support for calculating heading bug error relative to magnetic heading
for slaved DG's.
This commit is contained in:
curt 2004-02-04 17:10:32 +00:00
parent dcf8d93852
commit 8191e743f0
2 changed files with 250 additions and 243 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

432
src/Autopilot/xmlauto.cxx
View file

@ -30,152 +30,7 @@
#include "xmlauto.hxx"
FGPIDController::FGPIDController( SGPropertyNode *node, bool old ):
proportional( false ),
factor( 0.0 ),
offset_prop( NULL ),
offset_value( 0.0 ),
integral( false ),
gain( 0.0 ),
int_sum( 0.0 ),
one_eighty( false ),
clamp( false ),
debug( false ),
y_n( 0.0 ),
r_n( 0.0 ),
Kp( 0.0 ),
alpha( 0.1 ),
beta( 1.0 ),
gamma( 0.0 ),
Ti( 0.0 ),
Td( 0.0 ),
u_min( 0.0 ),
u_max( 0.0 ),
ep_n_1( 0.0 ),
edf_n_1( 0.0 ),
edf_n_2( 0.0 ),
u_n_1( 0.0 )
{
int i;
for ( i = 0; i < node->nChildren(); ++i ) {
SGPropertyNode *child = node->getChild(i);
string cname = child->getName();
string cval = child->getStringValue();
if ( cname == "name" ) {
name = cval;
} else if ( cname == "enable" ) {
// cout << "parsing enable" << endl;
SGPropertyNode *prop = child->getChild( "prop" );
if ( prop != NULL ) {
// cout << "prop = " << prop->getStringValue() << endl;
enable_prop = fgGetNode( prop->getStringValue(), true );
} else {
// cout << "no prop child" << endl;
}
SGPropertyNode *val = child->getChild( "value" );
if ( val != NULL ) {
enable_value = val->getStringValue();
}
} else if ( cname == "debug" ) {
debug = child->getBoolValue();
} else if ( cname == "input" ) {
SGPropertyNode *prop = child->getChild( "prop" );
if ( prop != NULL ) {
input_prop = fgGetNode( prop->getStringValue(), true );
}
} else if ( cname == "reference" ) {
SGPropertyNode *prop = child->getChild( "prop" );
if ( prop != NULL ) {
r_n_prop = fgGetNode( prop->getStringValue(), true );
} else {
prop = child->getChild( "value" );
if ( prop != NULL ) {
r_n_value = prop->getDoubleValue();
}
}
} else if ( cname == "output" ) {
int i = 0;
SGPropertyNode *prop;
while ( (prop = child->getChild("prop", i)) != NULL ) {
SGPropertyNode *tmp = fgGetNode( prop->getStringValue(), true );
output_list.push_back( tmp );
i++;
}
prop = child->getChild( "clamp" );
if ( prop != NULL ) {
clamp = true;
SGPropertyNode *tmp;
tmp = prop->getChild( "min" );
if ( tmp != NULL ) {
u_min = tmp->getDoubleValue();
// cout << "min = " << u_min << endl;
}
tmp = prop->getChild( "max" );
if ( tmp != NULL ) {
u_max = tmp->getDoubleValue();
// cout << "max = " << u_max << endl;
}
}
} else if ( cname == "proportional" ) {
proportional = true;
SGPropertyNode *prop;
prop = child->getChild( "pre" );
if ( prop != NULL ) {
prop = prop->getChild( "one-eighty" );
if ( prop != NULL && prop->getBoolValue() ) {
one_eighty = true;
}
}
prop = child->getChild( "factor" );
if ( prop != NULL ) {
factor = prop->getDoubleValue();
}
prop = child->getChild( "offset" );
if ( prop != NULL ) {
SGPropertyNode *sub = prop->getChild( "prop" );
if ( sub != NULL ) {
offset_prop = fgGetNode( sub->getStringValue(), true );
// cout << "offset prop = " << sub->getStringValue() << endl;
} else {
sub = prop->getChild( "value" );
if ( sub != NULL ) {
offset_value = sub->getDoubleValue();
// cout << "offset value = " << offset_value << endl;
}
}
}
} else if ( cname == "integral" ) {
integral = true;
SGPropertyNode *prop;
prop = child->getChild( "gain" );
if ( prop != NULL ) {
gain = prop->getDoubleValue();
}
} else {
SG_LOG( SG_AUTOPILOT, SG_WARN, "Error in autopilot config logic" );
}
}
}
FGPIDController::FGPIDController( SGPropertyNode *node ):
proportional( false ),
factor( 0.0 ),
offset_prop( NULL ),
offset_value( 0.0 ),
integral( false ),
gain( 0.0 ),
int_sum( 0.0 ),
one_eighty( false ),
clamp( false ),
debug( false ),
y_n( 0.0 ),
r_n( 0.0 ),
@ -289,83 +144,6 @@ FGPIDController::FGPIDController( SGPropertyNode *node ):
}
void FGPIDController::update_old( double dt ) {
if (enable_prop != NULL && enable_prop->getStringValue() == enable_value) {
if ( !enabled ) {
// we have just been enabled, zero out int_sum
int_sum = 0.0;
}
enabled = true;
} else {
enabled = false;
}
if ( enabled ) {
if ( debug ) cout << "Updating " << name << endl;
double input = 0.0;
if ( input_prop != NULL ) {
input = input_prop->getDoubleValue();
}
double r_n = 0.0;
if ( r_n_prop != NULL ) {
r_n = r_n_prop->getDoubleValue();
} else {
r_n = r_n_value;
}
double error = r_n - input;
if ( one_eighty ) {
while ( error < -180.0 ) { error += 360.0; }
while ( error > 180.0 ) { error -= 360.0; }
}
if ( debug ) cout << "input = " << input
<< " reference = " << r_n
<< " error = " << error
<< endl;
double prop_comp = 0.0;
double offset = 0.0;
if ( offset_prop != NULL ) {
offset = offset_prop->getDoubleValue();
if ( debug ) cout << "offset = " << offset << endl;
} else {
offset = offset_value;
}
if ( proportional ) {
prop_comp = error * factor + offset;
}
if ( integral ) {
int_sum += error * gain * dt;
} else {
int_sum = 0.0;
}
if ( debug ) cout << "prop_comp = " << prop_comp
<< " int_sum = " << int_sum << endl;
double output = prop_comp + int_sum;
if ( clamp ) {
if ( output < u_min ) {
output = u_min;
}
if ( output > u_max ) {
output = u_max;
}
}
if ( debug ) cout << "output = " << output << endl;
unsigned int i;
for ( i = 0; i < output_list.size(); ++i ) {
output_list[i]->setDoubleValue( output );
}
}
}
/*
* Roy Vegard Ovesen:
*
@ -518,6 +296,196 @@ void FGPIDController::update( double dt ) {
}
FGSimplePIController::FGSimplePIController( SGPropertyNode *node ):
proportional( false ),
factor( 0.0 ),
offset_prop( NULL ),
offset_value( 0.0 ),
integral( false ),
gain( 0.0 ),
int_sum( 0.0 ),
clamp( false ),
debug( false ),
y_n( 0.0 ),
r_n( 0.0 ),
u_min( 0.0 ),
u_max( 0.0 )
{
int i;
for ( i = 0; i < node->nChildren(); ++i ) {
SGPropertyNode *child = node->getChild(i);
string cname = child->getName();
string cval = child->getStringValue();
if ( cname == "name" ) {
name = cval;
} else if ( cname == "enable" ) {
// cout << "parsing enable" << endl;
SGPropertyNode *prop = child->getChild( "prop" );
if ( prop != NULL ) {
// cout << "prop = " << prop->getStringValue() << endl;
enable_prop = fgGetNode( prop->getStringValue(), true );
} else {
// cout << "no prop child" << endl;
}
SGPropertyNode *val = child->getChild( "value" );
if ( val != NULL ) {
enable_value = val->getStringValue();
}
} else if ( cname == "debug" ) {
debug = child->getBoolValue();
} else if ( cname == "input" ) {
SGPropertyNode *prop = child->getChild( "prop" );
if ( prop != NULL ) {
input_prop = fgGetNode( prop->getStringValue(), true );
}
} else if ( cname == "reference" ) {
SGPropertyNode *prop = child->getChild( "prop" );
if ( prop != NULL ) {
r_n_prop = fgGetNode( prop->getStringValue(), true );
} else {
prop = child->getChild( "value" );
if ( prop != NULL ) {
r_n_value = prop->getDoubleValue();
}
}
} else if ( cname == "output" ) {
int i = 0;
SGPropertyNode *prop;
while ( (prop = child->getChild("prop", i)) != NULL ) {
SGPropertyNode *tmp = fgGetNode( prop->getStringValue(), true );
output_list.push_back( tmp );
i++;
}
prop = child->getChild( "clamp" );
if ( prop != NULL ) {
clamp = true;
SGPropertyNode *tmp;
tmp = prop->getChild( "min" );
if ( tmp != NULL ) {
u_min = tmp->getDoubleValue();
// cout << "min = " << u_min << endl;
}
tmp = prop->getChild( "max" );
if ( tmp != NULL ) {
u_max = tmp->getDoubleValue();
// cout << "max = " << u_max << endl;
}
}
} else if ( cname == "proportional" ) {
proportional = true;
SGPropertyNode *prop;
prop = child->getChild( "factor" );
if ( prop != NULL ) {
factor = prop->getDoubleValue();
}
prop = child->getChild( "offset" );
if ( prop != NULL ) {
SGPropertyNode *sub = prop->getChild( "prop" );
if ( sub != NULL ) {
offset_prop = fgGetNode( sub->getStringValue(), true );
// cout << "offset prop = " << sub->getStringValue() << endl;
} else {
sub = prop->getChild( "value" );
if ( sub != NULL ) {
offset_value = sub->getDoubleValue();
// cout << "offset value = " << offset_value << endl;
}
}
}
} else if ( cname == "integral" ) {
integral = true;
SGPropertyNode *prop;
prop = child->getChild( "gain" );
if ( prop != NULL ) {
gain = prop->getDoubleValue();
}
} else {
SG_LOG( SG_AUTOPILOT, SG_WARN, "Error in autopilot config logic" );
}
}
}
void FGSimplePIController::update( double dt ) {
if (enable_prop != NULL && enable_prop->getStringValue() == enable_value) {
if ( !enabled ) {
// we have just been enabled, zero out int_sum
int_sum = 0.0;
}
enabled = true;
} else {
enabled = false;
}
if ( enabled ) {
if ( debug ) cout << "Updating " << name << endl;
double input = 0.0;
if ( input_prop != NULL ) {
input = input_prop->getDoubleValue();
}
double r_n = 0.0;
if ( r_n_prop != NULL ) {
r_n = r_n_prop->getDoubleValue();
} else {
r_n = r_n_value;
}
double error = r_n - input;
if ( debug ) cout << "input = " << input
<< " reference = " << r_n
<< " error = " << error
<< endl;
double prop_comp = 0.0;
double offset = 0.0;
if ( offset_prop != NULL ) {
offset = offset_prop->getDoubleValue();
if ( debug ) cout << "offset = " << offset << endl;
} else {
offset = offset_value;
}
if ( proportional ) {
prop_comp = error * factor + offset;
}
if ( integral ) {
int_sum += error * gain * dt;
} else {
int_sum = 0.0;
}
if ( debug ) cout << "prop_comp = " << prop_comp
<< " int_sum = " << int_sum << endl;
double output = prop_comp + int_sum;
if ( clamp ) {
if ( output < u_min ) {
output = u_min;
}
if ( output > u_max ) {
output = u_max;
}
}
if ( debug ) cout << "output = " << output << endl;
unsigned int i;
for ( i = 0; i < output_list.size(); ++i ) {
output_list[i]->setDoubleValue( output );
}
}
}
FGXMLAutopilot::FGXMLAutopilot() {
}
@ -627,19 +595,33 @@ static void update_helper( double dt ) {
v_last = v;
}
// Calculate heading bug error normalized to +/- 180.0
// Calculate heading bug error normalized to +/- 180.0 (based on
// DG indicated heading)
static SGPropertyNode *bug
= fgGetNode( "/autopilot/settings/heading-bug-deg", true );
static SGPropertyNode *ind_hdg
= fgGetNode( "/instrumentation/heading-indicator/indicated-heading-deg",
true );
static SGPropertyNode *bug_error
static SGPropertyNode *ind_bug_error
= fgGetNode( "/autopilot/internal/heading-bug-error-deg", true );
double diff = bug->getDoubleValue() - ind_hdg->getDoubleValue();
if ( diff < -180.0 ) { diff += 360.0; }
if ( diff > 180.0 ) { diff -= 360.0; }
bug_error->setDoubleValue( diff );
ind_bug_error->setDoubleValue( diff );
// Calculate heading bug error normalized to +/- 180.0 (based on
// actual/nodrift magnetic-heading, i.e. a DG slaved to magnetic
// compass.)
static SGPropertyNode *mag_hdg
= fgGetNode( "/orientation/heading-magnetic-deg", true );
static SGPropertyNode *fdm_bug_error
= fgGetNode( "/autopilot/internal/fdm-heading-bug-error-deg", true );
diff = bug->getDoubleValue() - mag_hdg->getDoubleValue();
if ( diff < -180.0 ) { diff += 360.0; }
if ( diff > 180.0 ) { diff -= 360.0; }
fdm_bug_error->setDoubleValue( diff );
// Calculate true heading error normalized to +/- 180.0
static SGPropertyNode *target_true

61
src/Autopilot/xmlauto.hxx
View file

@ -83,30 +83,13 @@ public:
/**
* A simple proportional controler
* Roy Ovesen's PID controller
*/
class FGPIDController : public FGXMLAutoComponent {
private:
// proportional component data
bool proportional;
double factor;
SGPropertyNode *offset_prop;
double offset_value;
// integral component data
bool integral;
double gain;
double int_sum;
// prep functions for error term
bool one_eighty;
// post functions for output
bool clamp;
// debug flag
bool debug;
@ -150,6 +133,48 @@ public:
};
/**
* A simplistic P [ + I ] PID controller
*/
class FGSimplePIController : public FGXMLAutoComponent {
private:
// proportional component data
bool proportional;
double factor;
SGPropertyNode *offset_prop;
double offset_value;
// integral component data
bool integral;
double gain;
double int_sum;
// post functions for output
bool clamp;
// debug flag
bool debug;
// Input values
double y_n; // measured process value
double r_n; // reference (set point) value
double u_min; // Minimum output clamp
double u_max; // Maximum output clamp
public:
FGSimplePIController( SGPropertyNode *node );
~FGSimplePIController() {}
void update( double dt );
};
/**
* Model an autopilot system.
*