- //////////////////////////////////////////////////////////
- // compute to/from flag status
- //////////////////////////////////////////////////////////
- bool value = false;
- double offset = fabs(radial - target_radial);
- if ( tofrom_serviceable ) {
- if ( nav_slaved_to_gps_node->getBoolValue() ) {
- value = gps_to_flag_node->getBoolValue();
- } else if ( inrange ) {
- if ( is_loc ) {
- value = true;
- } else {
- value = !(offset <= 90.0 || offset >= 270.0);
- }
- }
- } else {
- value = false;
- }
- to_flag_node->setBoolValue( value );
-
- value = false;
- if ( tofrom_serviceable ) {
- if ( nav_slaved_to_gps_node->getBoolValue() ) {
- value = gps_from_flag_node->getBoolValue();
- } else if ( inrange ) {
- if ( is_loc ) {
- value = false;
- } else {
- value = !(offset > 90.0 && offset < 270.0);
- }
- }
- } else {
- value = false;
- }
- from_flag_node->setBoolValue( value );
-
- //////////////////////////////////////////////////////////
- // compute the deflection of the CDI needle, clamped to the range
- // of ( -10 , 10 )
- //////////////////////////////////////////////////////////
- double r = 0.0;
- bool loc_backside = false; // an in-code flag indicating that we are
- // on a localizer backcourse.
- if ( cdi_serviceable ) {
- if ( nav_slaved_to_gps_node->getBoolValue() ) {
- r = gps_cdi_deflection_node->getDoubleValue();
- // We want +- 5 dots deflection for the gps, so clamp
- // to -12.5/12.5
- SG_CLAMP_RANGE( r, -12.5, 12.5 );
- } else if ( inrange ) {
- r = radial - target_radial;
- // cout << "Target radial = " << target_radial
- // << " Actual radial = " << radial << 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 );
- } else {
- if ( is_loc ) {
- loc_backside = true;
- }
- }
-
- r = -r; // reverse, since radial is outbound
- if ( is_loc ) {
- // According to Robin Peel, the ILS is 4x more
- // sensitive than a vor
- r *= 4.0;
- }
- SG_CLAMP_RANGE( r, -10.0, 10.0 );
- r *= signal_quality_norm;
- }
- }
- cdi_deflection_node->setDoubleValue( r );
-
- //////////////////////////////////////////////////////////
- // compute the amount of cross track distance error in meters
- //////////////////////////////////////////////////////////
- double xtrack_error = 0.0;
- if ( inrange && nav_serviceable && cdi_serviceable ) {
- r = radial - target_radial;
- // cout << "Target radial = " << target_radial
- // << " Actual radial = " << 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
-
- xtrack_error = loc_dist * sin(r * SGD_DEGREES_TO_RADIANS);
- } else {
- xtrack_error = 0.0;
- }
- 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 );
-
- //////////////////////////////////////////////////////////
- // compute the amount of glide slope needle deflection
- // (.i.e. the number of degrees we are off the glide slope * 5.0
- //
- // CLO - 13 Mar 2006: The glide slope needle should peg at
- // +/-0.7 degrees off the ideal glideslope. I'm not sure why
- // we compute the factor the way we do (5*gs_error), but we
- // need to compensate for our 'odd' number in the glideslope
- // needle animation. This means that the needle should peg
- // when this values is +/-3.5.
- //////////////////////////////////////////////////////////
- r = 0.0;
- if ( has_gs && gs_serviceable_node->getBoolValue() ) {
- if ( nav_slaved_to_gps_node->getBoolValue() ) {
- // FIXME/FINISHME, what should be set here?
- } else if ( inrange ) {
- double x = gs_dist_node->getDoubleValue();
- double y = (fgGetDouble("/position/altitude-ft") - nav_elev)
- * SG_FEET_TO_METER;
- // cout << "dist = " << x << " height = " << y << endl;
- double angle = asin( y / x ) * SGD_RADIANS_TO_DEGREES;
- r = (target_gs - angle) * 5.0;
- r *= signal_quality_norm;
- }
- }
- gs_deflection_node->setDoubleValue( r );
-
- //////////////////////////////////////////////////////////
- // Calculate desired rate of climb for intercepting the GS
- //////////////////////////////////////////////////////////
- double x = gs_dist_node->getDoubleValue();
- double y = (alt_node->getDoubleValue() - nav_elev)
- * SG_FEET_TO_METER;
- double current_angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
-
- double target_angle = target_gs;
- double gs_diff = target_angle - current_angle;
-
- // convert desired vertical path angle into a climb rate
- double des_angle = current_angle - 10 * gs_diff;
-
- // estimate horizontal speed towards ILS in meters per minute
- double dist = last_x - x;
- last_x = x;
- if ( dt > 0.0 ) {
- // avoid nan
- double new_vel = ( dist / dt );