From 94badeabcdfa669c16a316adff19a4f9f6a4b753 Mon Sep 17 00:00:00 2001 From: curt Date: Wed, 21 Jun 2006 21:23:20 +0000 Subject: [PATCH] Existing ground track estimation code depended on current_value - last_value 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. --- src/Instrumentation/navradio.cxx | 31 +++++++++++++------------------ 1 file changed, 13 insertions(+), 18 deletions(-) diff --git a/src/Instrumentation/navradio.cxx b/src/Instrumentation/navradio.cxx index 511592e76..dd8a15dad 100644 --- a/src/Instrumentation/navradio.cxx +++ b/src/Instrumentation/navradio.cxx @@ -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; -- 2.39.5