fly/flightgear
1
0
Fork 0
Code Activity

bugfix: don't call the update_helper() for every instance of FGXMLAutopilot. Compute the helper properties only once per FGXMLAutopilotGroup's update() method. Only one AutopilotGroup shall be instantiated, but many Autopilots may exist.

Browse source
This commit is contained in:
torsten 2009-12-29 15:34:02 +00:00 • committed by Tim Moore
parent 5cad5aa7da
commit 95e2d62d94
2 changed files with 126 additions and 132 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

223
src/Autopilot/xmlauto.cxx
View file

@ -837,10 +837,105 @@ void FGDigitalFilter::update(double dt)
}
}
FGXMLAutopilotGroup::FGXMLAutopilotGroup()
FGXMLAutopilotGroup::FGXMLAutopilotGroup() :
SGSubsystemGroup(),
average(0.0), // average/filtered prediction
v_last(0.0), // last velocity
last_static_pressure(0.0),
vel(fgGetNode( "/velocities/airspeed-kt", true )),
// Estimate speed in 5,10 seconds
lookahead5(fgGetNode( "/autopilot/internal/lookahead-5-sec-airspeed-kt", true )),
lookahead10(fgGetNode( "/autopilot/internal/lookahead-10-sec-airspeed-kt", true )),
bug(fgGetNode( "/autopilot/settings/heading-bug-deg", true )),
mag_hdg(fgGetNode( "/orientation/heading-magnetic-deg", true )),
bug_error(fgGetNode( "/autopilot/internal/heading-bug-error-deg", true )),
fdm_bug_error(fgGetNode( "/autopilot/internal/fdm-heading-bug-error-deg", true )),
target_true(fgGetNode( "/autopilot/settings/true-heading-deg", true )),
true_hdg(fgGetNode( "/orientation/heading-deg", true )),
true_error(fgGetNode( "/autopilot/internal/true-heading-error-deg", true )),
target_nav1(fgGetNode( "/instrumentation/nav[0]/radials/target-auto-hdg-deg", true )),
true_nav1(fgGetNode( "/autopilot/internal/nav1-heading-error-deg", true )),
true_track_nav1(fgGetNode( "/autopilot/internal/nav1-track-error-deg", true )),
nav1_course_error(fgGetNode( "/autopilot/internal/nav1-course-error", true )),
nav1_selected_course(fgGetNode( "/instrumentation/nav[0]/radials/selected-deg", true )),
vs_fps(fgGetNode( "/velocities/vertical-speed-fps", true )),
vs_fpm(fgGetNode( "/autopilot/internal/vert-speed-fpm", true )),
static_pressure(fgGetNode( "/systems/static[0]/pressure-inhg", true )),
pressure_rate(fgGetNode( "/autopilot/internal/pressure-rate", true )),
latNode(fgGetNode("/position/latitude-deg")),
lonNode(fgGetNode("/position/longitude-deg"))
{
}
void FGXMLAutopilotGroup::update( double dt )
{
// update all configured autopilots
SGSubsystemGroup::update( dt );
// update helper values
double v = vel->getDoubleValue();
double a = 0.0;
if ( dt > 0.0 ) {
a = (v - v_last) / dt;
if ( dt < 1.0 ) {
average = (1.0 - dt) * average + dt * a;
} else {
average = a;
}
lookahead5->setDoubleValue( v + average * 5.0 );
lookahead10->setDoubleValue( v + average * 10.0 );
v_last = v;
}
// Calculate heading bug error normalized to +/- 180.0
double diff = bug->getDoubleValue() - mag_hdg->getDoubleValue();
SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
bug_error->setDoubleValue( diff );
fdm_bug_error->setDoubleValue( diff );
// Calculate true heading error normalized to +/- 180.0
diff = target_true->getDoubleValue() - true_hdg->getDoubleValue();
SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
true_error->setDoubleValue( diff );
// Calculate nav1 target heading error normalized to +/- 180.0
diff = target_nav1->getDoubleValue() - true_hdg->getDoubleValue();
SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
true_nav1->setDoubleValue( diff );
// Calculate true groundtrack
SGGeod currentPosition(SGGeod::fromDeg(
lonNode->getDoubleValue(), latNode->getDoubleValue()));
double true_track = SGGeodesy::courseDeg(lastPosition, currentPosition);
lastPosition = currentPosition;
diff = target_nav1->getDoubleValue() - true_track;
SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
true_track_nav1->setDoubleValue( diff );
// Calculate nav1 selected course error normalized to +/- 180.0
diff = nav1_selected_course->getDoubleValue() - mag_hdg->getDoubleValue();
SG_NORMALIZE_RANGE( diff, -180.0, 180.0 );
nav1_course_error->setDoubleValue( diff );
// Calculate vertical speed in fpm
vs_fpm->setDoubleValue( vs_fps->getDoubleValue() * 60.0 );
// Calculate static port pressure rate in [inhg/s].
// Used to determine vertical speed.
if ( dt > 0.0 ) {
double current_static_pressure = static_pressure->getDoubleValue();
double current_pressure_rate =
( current_static_pressure - last_static_pressure ) / dt;
pressure_rate->setDoubleValue(current_pressure_rate);
last_static_pressure = current_static_pressure;
}
}
void FGXMLAutopilotGroup::reinit()
{
for( vector<string>::size_type i = 0; i < _autopilotNames.size(); i++ ) {
@ -928,8 +1023,6 @@ FGXMLAutopilot::~FGXMLAutopilot() {
*/
void FGXMLAutopilot::init()
{
latNode = fgGetNode("/position/latitude-deg");
lonNode = fgGetNode("/position/longitude-deg");
}
@ -973,132 +1066,12 @@ bool FGXMLAutopilot::build( SGPropertyNode_ptr config_props ) {
return true;
}
/*
* Update helper values
*/
void FGXMLAutopilot::update_helper( double dt ) {
// Estimate speed in 5,10 seconds
static SGPropertyNode_ptr vel = fgGetNode( "/velocities/airspeed-kt", true );
static SGPropertyNode_ptr lookahead5
= fgGetNode( "/autopilot/internal/lookahead-5-sec-airspeed-kt", true );
static SGPropertyNode_ptr lookahead10
= fgGetNode( "/autopilot/internal/lookahead-10-sec-airspeed-kt", true );
static double average = 0.0; // average/filtered prediction
static double v_last = 0.0; // last velocity
double v = vel->getDoubleValue();
double a = 0.0;
if ( dt > 0.0 ) {
a = (v - v_last) / dt;
if ( dt < 1.0 ) {
average = (1.0 - dt) * average + dt * a;
} else {
average = a;
}
lookahead5->setDoubleValue( v + average * 5.0 );
lookahead10->setDoubleValue( v + average * 10.0 );
v_last = v;
}
// Calculate heading bug error normalized to +/- 180.0
static SGPropertyNode_ptr bug
= fgGetNode( "/autopilot/settings/heading-bug-deg", true );
static SGPropertyNode_ptr mag_hdg
= fgGetNode( "/orientation/heading-magnetic-deg", true );
static SGPropertyNode_ptr bug_error
= fgGetNode( "/autopilot/internal/heading-bug-error-deg", true );
double diff = bug->getDoubleValue() - mag_hdg->getDoubleValue();
SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
bug_error->setDoubleValue( diff );
static SGPropertyNode_ptr fdm_bug_error
= fgGetNode( "/autopilot/internal/fdm-heading-bug-error-deg", true );
fdm_bug_error->setDoubleValue( diff );
// Calculate true heading error normalized to +/- 180.0
static SGPropertyNode_ptr target_true
= fgGetNode( "/autopilot/settings/true-heading-deg", true );
static SGPropertyNode_ptr true_hdg
= fgGetNode( "/orientation/heading-deg", true );
static SGPropertyNode_ptr true_error
= fgGetNode( "/autopilot/internal/true-heading-error-deg", true );
diff = target_true->getDoubleValue() - true_hdg->getDoubleValue();
SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
true_error->setDoubleValue( diff );
// Calculate nav1 target heading error normalized to +/- 180.0
static SGPropertyNode_ptr target_nav1
= fgGetNode( "/instrumentation/nav[0]/radials/target-auto-hdg-deg", true );
static SGPropertyNode_ptr true_nav1
= fgGetNode( "/autopilot/internal/nav1-heading-error-deg", true );
static SGPropertyNode_ptr true_track_nav1
= fgGetNode( "/autopilot/internal/nav1-track-error-deg", true );
diff = target_nav1->getDoubleValue() - true_hdg->getDoubleValue();
SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
true_nav1->setDoubleValue( diff );
// Calculate true groundtrack
SGGeod currentPosition(SGGeod::fromDeg(
lonNode->getDoubleValue(), latNode->getDoubleValue()));
double true_track = SGGeodesy::courseDeg(lastPosition, currentPosition);
lastPosition = currentPosition;
diff = target_nav1->getDoubleValue() - true_track;
SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
true_track_nav1->setDoubleValue( diff );
// Calculate nav1 selected course error normalized to +/- 180.0
static SGPropertyNode_ptr nav1_course_error
= fgGetNode( "/autopilot/internal/nav1-course-error", true );
static SGPropertyNode_ptr nav1_selected_course
= fgGetNode( "/instrumentation/nav[0]/radials/selected-deg", true );
diff = nav1_selected_course->getDoubleValue() - mag_hdg->getDoubleValue();
SG_NORMALIZE_RANGE( diff, -180.0, 180.0 );
nav1_course_error->setDoubleValue( diff );
// Calculate vertical speed in fpm
static SGPropertyNode_ptr vs_fps
= fgGetNode( "/velocities/vertical-speed-fps", true );
static SGPropertyNode_ptr vs_fpm
= fgGetNode( "/autopilot/internal/vert-speed-fpm", true );
vs_fpm->setDoubleValue( vs_fps->getDoubleValue() * 60.0 );
// Calculate static port pressure rate in [inhg/s].
// Used to determine vertical speed.
static SGPropertyNode_ptr static_pressure
= fgGetNode( "/systems/static[0]/pressure-inhg", true );
static SGPropertyNode_ptr pressure_rate
= fgGetNode( "/autopilot/internal/pressure-rate", true );
static double last_static_pressure = 0.0;
if ( dt > 0.0 ) {
double current_static_pressure = static_pressure->getDoubleValue();
double current_pressure_rate =
( current_static_pressure - last_static_pressure ) / dt;
pressure_rate->setDoubleValue(current_pressure_rate);
last_static_pressure = current_static_pressure;
}
}
/*
* Update the list of autopilot components
*/
void FGXMLAutopilot::update( double dt ) {
update_helper( dt );
void FGXMLAutopilot::update( double dt )
{
unsigned int i;
for ( i = 0; i < components.size(); ++i ) {
components[i]->update( dt );

35
src/Autopilot/xmlauto.hxx
View file

@ -378,8 +378,36 @@ public:
FGXMLAutopilotGroup();
void init();
void reinit();
void update( double dt );
private:
std::vector<std::string> _autopilotNames;
double average;
double v_last;
double last_static_pressure;
SGPropertyNode_ptr vel;
SGPropertyNode_ptr lookahead5;
SGPropertyNode_ptr lookahead10;
SGPropertyNode_ptr bug;
SGPropertyNode_ptr mag_hdg;
SGPropertyNode_ptr bug_error;
SGPropertyNode_ptr fdm_bug_error;
SGPropertyNode_ptr target_true;
SGPropertyNode_ptr true_hdg;
SGPropertyNode_ptr true_error;
SGPropertyNode_ptr target_nav1;
SGPropertyNode_ptr true_nav1;
SGPropertyNode_ptr true_track_nav1;
SGPropertyNode_ptr nav1_course_error;
SGPropertyNode_ptr nav1_selected_course;
SGPropertyNode_ptr vs_fps;
SGPropertyNode_ptr vs_fpm;
SGPropertyNode_ptr static_pressure;
SGPropertyNode_ptr pressure_rate;
SGPropertyNode_ptr latNode, lonNode;
SGGeod lastPosition;
};
class FGXMLAutopilot : public SGSubsystem
@ -402,16 +430,9 @@ protected:
typedef std::vector<FGXMLAutoComponent_ptr> comp_list;
private:
/**
* Update helper values, especially the /autopilot/internal properties
*/
void update_helper( double dt );
bool serviceable;
comp_list components;
SGPropertyNode_ptr latNode, lonNode;
SGGeod lastPosition;
};