- SG_CLAMP_RANGE( adjustment, -30.0, 30.0 );
-
- // determine the target heading to fly to intercept the
- // tgt_radial
- double nta_hdg = trtrue + adjustment;
- while ( nta_hdg < 0.0 ) { nta_hdg += 360.0; }
- while ( nta_hdg > 360.0 ) { nta_hdg -= 360.0; }
- target_auto_hdg_node->setDoubleValue( nta_hdg );
+ cdi_xtrack_error_node->setDoubleValue( xtrack_error );
+
+ //////////////////////////////////////////////////////////
+ // compute an approximate ground track heading error
+ //////////////////////////////////////////////////////////
+ double hdg_error = 0.0;
+ if ( inrange && cdi_serviceable ) {
+ 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);
+ 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 );
+
+ //////////////////////////////////////////////////////////
+ // compute the time to intercept selected radial (based on
+ // current and last cross track errors and dt
+ //////////////////////////////////////////////////////////
+ double t = 0.0;
+ if ( inrange && cdi_serviceable ) {
+ double xrate_ms = (last_xtrack_error - xtrack_error) / dt;
+ if ( fabs(xrate_ms) > 0.00001 ) {
+ t = xtrack_error / xrate_ms;
+ } else {
+ t = 9999.9;
+ }
+ }
+ time_to_intercept->setDoubleValue( t );