fly/flightgear
1
0
Fork 0
Code Activity

Autopilot: clean up the helpers code (which drives the various /internal/) properties. Fixes various heading modes to work in all aircraft.

Browse source
This commit is contained in:
jmt 2009-12-26 12:31:07 +00:00 committed by Tim Moore
parent 50adab32c1
commit 5cad5aa7da
2 changed files with 36 additions and 45 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

58
src/Autopilot/xmlauto.cxx
View file

@ -928,6 +928,8 @@ FGXMLAutopilot::~FGXMLAutopilot() {
*/
void FGXMLAutopilot::init()
{
latNode = fgGetNode("/position/latitude-deg");
lonNode = fgGetNode("/position/longitude-deg");
}
@ -975,7 +977,7 @@ bool FGXMLAutopilot::build( SGPropertyNode_ptr config_props ) {
/*
* Update helper values
*/
static void update_helper( double dt ) {
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
@ -1002,32 +1004,20 @@ static void update_helper( double dt ) {
v_last = v;
}
// Calculate heading bug error normalized to +/- 180.0 (based on
// DG indicated heading)
// Calculate heading bug error normalized to +/- 180.0
static SGPropertyNode_ptr bug
= fgGetNode( "/autopilot/settings/heading-bug-deg", true );
static SGPropertyNode_ptr ind_hdg
= fgGetNode( "/instrumentation/heading-indicator/indicated-heading-deg",
true );
static SGPropertyNode_ptr 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; }
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_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 );
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
@ -1035,14 +1025,11 @@ static void update_helper( double dt ) {
= fgGetNode( "/autopilot/settings/true-heading-deg", true );
static SGPropertyNode_ptr true_hdg
= fgGetNode( "/orientation/heading-deg", true );
static SGPropertyNode_ptr true_track
= fgGetNode( "/instrumentation/gps/indicated-track-true-deg", true );
static SGPropertyNode_ptr true_error
= fgGetNode( "/autopilot/internal/true-heading-error-deg", true );
diff = target_true->getDoubleValue() - true_hdg->getDoubleValue();
if ( diff < -180.0 ) { diff += 360.0; }
if ( diff > 180.0 ) { diff -= 360.0; }
SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
true_error->setDoubleValue( diff );
// Calculate nav1 target heading error normalized to +/- 180.0
@ -1054,25 +1041,25 @@ static void update_helper( double dt ) {
= fgGetNode( "/autopilot/internal/nav1-track-error-deg", true );
diff = target_nav1->getDoubleValue() - true_hdg->getDoubleValue();
if ( diff < -180.0 ) { diff += 360.0; }
if ( diff > 180.0 ) { diff -= 360.0; }
SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
true_nav1->setDoubleValue( diff );
diff = target_nav1->getDoubleValue() - true_track->getDoubleValue();
if ( diff < -180.0 ) { diff += 360.0; }
if ( diff > 180.0 ) { diff -= 360.0; }
// 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
// (based on DG indicated heading)
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() - ind_hdg->getDoubleValue();
// if ( diff < -180.0 ) { diff += 360.0; }
// if ( diff > 180.0 ) { diff -= 360.0; }
diff = nav1_selected_course->getDoubleValue() - mag_hdg->getDoubleValue();
SG_NORMALIZE_RANGE( diff, -180.0, 180.0 );
nav1_course_error->setDoubleValue( diff );
@ -1096,15 +1083,12 @@ static void update_helper( double dt ) {
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;
}
}

7
src/Autopilot/xmlauto.hxx
View file

@ -402,9 +402,16 @@ 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;
};