fly/flightgear
1
0
Fork 0
Code Activity

Existing ground track estimation code depended on current_value - last_value

Browse source 
each frame.  However, often these values didn't change leading to bogus data
getting introduced into the computational pipeline.

This patch switches to a much more sane method for ground track computation.
This commit is contained in:
curt 2006-06-21 21:23:20 +00:00
parent 229d499f7b
commit 94badeabcd
1 changed files with 13 additions and 18 deletions

31
src/Instrumentation/navradio.cxx
View file

@ -357,7 +357,7 @@ FGNavRadio::update(double dt)
SGVec3d aircraft = SGVec3d::fromGeod(pos);
loc_dist = dist(aircraft, nav_xyz);
loc_dist_node->setDoubleValue( loc_dist );
// cout << "station = " << station << " dist = " << loc_dist << endl;
// cout << "dt = " << dt << " dist = " << loc_dist << endl;
if ( has_gs ) {
// find closest distance to the gs base line
@ -551,24 +551,17 @@ FGNavRadio::update(double dt)
//////////////////////////////////////////////////////////
double hdg_error = 0.0;
if ( inrange && cdi_serviceable ) {
double ddist = last_loc_dist - loc_dist;
double dxtrack = last_xtrack_error - xtrack_error;
double a = atan2( dxtrack, ddist ) * SGD_RADIANS_TO_DEGREES;
if ( from_flag_node->getBoolValue() ) {
a = 180.0 - a;
if ( a > 180.0 ) { a -= 360.0; }
if ( a < -180.0 ) { a += 360.0; }
}
double vn = fgGetDouble( "/velocities/speed-north-fps" );
double ve = fgGetDouble( "/velocities/speed-east-fps" );
double gnd_trk_true = atan2( ve, vn ) * SGD_RADIANS_TO_DEGREES;
if ( gnd_trk_true < 0.0 ) { gnd_trk_true += 360.0; }
SGPropertyNode *true_hdg
= fgGetNode("/orientation/heading-deg", true);
// cout << "true heading = " << true_hdg->getDoubleValue()
// << " selrad = " << sel_radial_node->getDoubleValue()
// << " artr = " << a
// << endl;
double est_hdg = trtrue + a;
if ( est_hdg < 0.0 ) { est_hdg += 360.0; }
if ( est_hdg >= 360.0 ) { est_hdg -= 360.0; }
hdg_error = est_hdg - true_hdg->getDoubleValue();
hdg_error = gnd_trk_true - true_hdg->getDoubleValue();
// cout << "ground track = " << gnd_trk_true
// << " orientation = " << true_hdg->getDoubleValue() << endl;
}
cdi_xtrack_hdg_err_node->setDoubleValue( hdg_error );
@ -672,8 +665,10 @@ FGNavRadio::update(double dt)
// tgt_radial = target radial (true) + cdi offset adjustmest -
// xtrack heading error adjustment
double nta_hdg = trtrue + adjustment - hdg_error;
// cout << "trtrue = " << trtrue << " adj = " << adjustment
// << " hdg_error = " << hdg_error << endl;
while ( nta_hdg < 0.0 ) { nta_hdg += 360.0; }
while ( nta_hdg > 360.0 ) { nta_hdg -= 360.0; }
while ( nta_hdg >= 360.0 ) { nta_hdg -= 360.0; }
target_auto_hdg_node->setDoubleValue( nta_hdg );
last_xtrack_error = xtrack_error;