fly/flightgear
1
0
Fork 0
Code Activity

Jim Wilson:

Browse source 
Add FGPredictor class to xmlauto.  Add support for horizontal navigation based
on flight track as opposed to heading.  Add crosstrack-error support to nav.
Simplify error adjust calculation for horizontal nav (better interception).
Fixed potential divide by zero that was producing nan issues in the xmlauto
code.
This commit is contained in:
curt 2004-03-19 03:23:28 +00:00
parent a1a68b9c93
commit 2845a75753
4 changed files with 176 additions and 23 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

92
src/Autopilot/xmlauto.cxx
View file

@ -491,6 +491,86 @@ void FGPISimpleController::update( double dt ) {
}
FGPredictor::FGPredictor ( SGPropertyNode *node ):
debug( false ),
ivalue( 0.0 ),
last_value ( 999999999.9 ),
average ( 0.0 ),
seconds( 0.0 ),
filter_gain( 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 == "debug" ) {
debug = child->getBoolValue();
} else if ( cname == "input" ) {
input_prop = fgGetNode( child->getStringValue(), true );
} else if ( cname == "seconds" ) {
seconds = child->getDoubleValue();
} else if ( cname == "filter-gain" ) {
filter_gain = child->getDoubleValue();
} else if ( cname == "output" ) {
SGPropertyNode *tmp = fgGetNode( child->getStringValue(), true );
output_list.push_back( tmp );
}
}
}
void FGPredictor::update( double dt ) {
/*
Simple moving average filter converts input value to predicted value "seconds".
Smoothing as described by Curt Olson:
gain would be valid in the range of 0 - 1.0
1.0 would mean no filtering.
0.0 would mean no input.
0.5 would mean (1 part past value + 1 part current value) / 2
0.1 would mean (9 parts past value + 1 part current value) / 10
0.25 would mean (3 parts past value + 1 part current value) / 4
*/
if ( input_prop != NULL ) {
ivalue = input_prop->getDoubleValue();
// no sense if there isn't an input :-)
enabled = true;
} else {
enabled = false;
}
if ( enabled ) {
// first time initialize average
if (last_value >= 999999999.0) {
last_value = ivalue;
}
if ( dt > 0.0 ) {
double current = (ivalue - last_value)/dt; // calculate current error change (per second)
if ( dt < 1.0 ) {
average = (1.0 - dt) * average + current * dt;
} else {
average = current;
}
// calculate output with filter gain adjustment
double output = ivalue + (1.0 - filter_gain) * (average * seconds) + filter_gain * (current * seconds);
unsigned int i;
for ( i = 0; i < output_list.size(); ++i ) {
output_list[i]->setDoubleValue( output );
}
}
last_value = ivalue;
}
}
FGXMLAutopilot::FGXMLAutopilot() {
}
@ -562,6 +642,9 @@ bool FGXMLAutopilot::build() {
} else if ( name == "pi-simple-controller" ) {
FGXMLAutoComponent *c = new FGPISimpleController( node );
components.push_back( c );
} else if ( name == "predict-simple" ) {
FGXMLAutoComponent *c = new FGPredictor( node );
components.push_back( c );
} else {
SG_LOG( SG_ALL, SG_ALERT, "Unknown top level section: "
<< name );
@ -636,6 +719,8 @@ static void update_helper( double dt ) {
= fgGetNode( "/autopilot/settings/true-heading-deg", true );
static SGPropertyNode *true_hdg
= fgGetNode( "/orientation/heading-deg", true );
static SGPropertyNode *true_track
= fgGetNode( "/instrumentation/gps/indicated-track-true-deg", true );
static SGPropertyNode *true_error
= fgGetNode( "/autopilot/internal/true-heading-error-deg", true );
@ -649,12 +734,19 @@ static void update_helper( double dt ) {
= fgGetNode( "/radios/nav[0]/radials/target-auto-hdg-deg", true );
static SGPropertyNode *true_nav1
= fgGetNode( "/autopilot/internal/nav1-heading-error-deg", true );
static SGPropertyNode *true_track_nav1
= 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; }
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; }
true_track_nav1->setDoubleValue( diff );
// Calculate vertical speed in fpm
static SGPropertyNode *vs_fps
= fgGetNode( "/velocities/vertical-speed-fps", true );

29
src/Autopilot/xmlauto.hxx
View file

@ -175,6 +175,35 @@ public:
};
/**
* Predictor - calculates value in x seconds future.
*/
class FGPredictor : public FGXMLAutoComponent {
private:
// proportional component data
double last_value;
double average;
double seconds;
double filter_gain;
// debug flag
bool debug;
// Input values
double ivalue; // input value
public:
FGPredictor( SGPropertyNode *node );
~FGPredictor() {}
void update( double dt );
};
/**
* Model an autopilot system.
*

77
src/Cockpit/navcom.cxx
View file

@ -64,6 +64,7 @@ FGNavCom::FGNavCom() :
nav_target_radial(0.0),
nav_target_radial_true(0.0),
nav_target_auto_hdg(0.0),
nav_gs_rate_of_climb(0.0),
nav_vol_btn(0.0),
nav_ident_btn(true),
horiz_vel(0.0),
@ -211,6 +212,9 @@ FGNavCom::bind ()
snprintf(propname, 256, "/radios/nav[%d]/heading-needle-deflection", index);
fgTie( propname, this, &FGNavCom::get_nav_cdi_deflection );
snprintf(propname, 256, "/radios/nav[%d]/crosstrack-error-m", index);
fgTie( propname, this, &FGNavCom::get_nav_cdi_xtrack_error );
snprintf(propname, 256, "/radios/nav[%d]/has-gs", index);
fgTie( propname, this, &FGNavCom::get_nav_has_gs );
@ -475,22 +479,19 @@ FGNavCom::update(double dt)
}
// determine the heading adjustment needed.
double adjustment = get_nav_cdi_deflection()
* (nav_loc_dist * SG_METER_TO_NM);
SG_CLAMP_RANGE( adjustment, -30.0, 30.0 );
#if 0
// CLO - 01/24/2004 - This #ifdef'd out code makes no sense to
// me. Someone please justify it and explain why it should be
// here if they want this reenabled.
// clamp closer when inside cone when beyond 5km...
if ( nav_loc_dist > 5000 ) {
double clamp_angle = fabs(get_nav_cdi_deflection()) * 3;
if (clamp_angle < 30)
SG_CLAMP_RANGE( adjustment, -clamp_angle, clamp_angle);
// over 8km scale by 3.0
// (3 is chosen because max deflection is 10
// and 30 is clamped angle to radial)
// under 8km scale by 10.0
// because the overstated error helps drive it to the radial in a
// moderate cross wind.
double adjustment = 0.0;
if (nav_loc_dist > 8000) {
adjustment = get_nav_cdi_deflection() * 3.0;
} else {
adjustment = get_nav_cdi_deflection() * 10.0;
}
#endif
SG_CLAMP_RANGE( adjustment, -30.0, 30.0 );
// determine the target heading to fly to intercept the
// tgt_radial
@ -516,16 +517,19 @@ FGNavCom::update(double dt)
// estimate horizontal speed towards ILS in meters per minute
double dist = last_x - x;
last_x = x;
double new_vel = ( dist / dt );
if ( dt > 0.0 ) {
// avoid nan
double new_vel = ( dist / dt );
horiz_vel = 0.75 * horiz_vel + 0.25 * new_vel;
// double horiz_vel = cur_fdm_state->get_V_ground_speed()
// * SG_FEET_TO_METER * 60.0;
// double horiz_vel = airspeed_node->getFloatValue()
// * SG_FEET_TO_METER * 60.0;
horiz_vel = 0.75 * horiz_vel + 0.25 * new_vel;
// double horiz_vel = cur_fdm_state->get_V_ground_speed()
// * SG_FEET_TO_METER * 60.0;
// double horiz_vel = airspeed_node->getFloatValue()
// * SG_FEET_TO_METER * 60.0;
nav_gs_rate_of_climb = -sin( des_angle * SGD_DEGREES_TO_RADIANS )
* horiz_vel * SG_METER_TO_FEET;
nav_gs_rate_of_climb = -sin( des_angle * SGD_DEGREES_TO_RADIANS )
* horiz_vel * SG_METER_TO_FEET;
}
} else {
nav_inrange = false;
// cout << "not picking up vor. :-(" << endl;
@ -771,6 +775,33 @@ double FGNavCom::get_nav_cdi_deflection() const {
return r;
}
// return the amount of cross track distance error, returns a meters
double FGNavCom::get_nav_cdi_xtrack_error() const {
double r, m;
if ( nav_inrange
&& nav_serviceable->getBoolValue() && cdi_serviceable->getBoolValue() )
{
r = nav_radial - nav_target_radial;
// cout << "Target radial = " << nav_target_radial
// << " Actual radial = " << nav_radial
// << " r = " << r << endl;
while ( r > 180.0 ) { r -= 360.0;}
while ( r < -180.0 ) { r += 360.0;}
if ( fabs(r) > 90.0 )
r = ( r<0.0 ? -r-180.0 : -r+180.0 );
r = -r; // reverse, since radial is outbound
m = nav_loc_dist * sin(r * SGD_DEGREES_TO_RADIANS);
} else {
m = 0.0;
}
return m;
}
// return the amount of glide slope needle deflection (.i.e. the
// number of degrees we are off the glide slope * 5.0

1
src/Cockpit/navcom.hxx
View file

@ -237,6 +237,7 @@ public:
inline double get_nav_target_gs() const { return nav_target_gs; }
inline double get_nav_twist() const { return nav_twist; }
double get_nav_cdi_deflection() const;
double get_nav_cdi_xtrack_error() const;
double get_nav_gs_deflection() const;
inline double get_nav_vol_btn() const { return nav_vol_btn; }
inline bool get_nav_ident_btn() const { return nav_ident_btn; }