X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FCockpit%2Fnavcom.cxx;h=1850539e03a6f6b30eb2e5e62ff811467c537ed7;hb=fee003e8cc17f0aa9a1e9280dd823d20e231e967;hp=4c45eff8f1cd417e7543001f726b22a9e04078f0;hpb=da5ea10d5db9675e6a5d75db5b33edfc350566b1;p=flightgear.git diff --git a/src/Cockpit/navcom.cxx b/src/Cockpit/navcom.cxx index 4c45eff8f..1850539e0 100644 --- a/src/Cockpit/navcom.cxx +++ b/src/Cockpit/navcom.cxx @@ -2,7 +2,7 @@ // // Written by Curtis Olson, started April 2000. // -// Copyright (C) 2000 - 2002 Curtis L. Olson - curt@flightgear.org +// Copyright (C) 2000 - 2002 Curtis L. Olson - http://www.flightgear.org/~curt // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License as @@ -33,7 +33,6 @@ #include #include -#include #include #include "navcom.hxx" @@ -64,6 +63,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 +211,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 ); @@ -315,7 +318,7 @@ double FGNavCom::adjustNavRange( double stationElev, double aircraftElev, // model standard ILS service volumes as per AIM 1-1-9 double FGNavCom::adjustILSRange( double stationElev, double aircraftElev, - double offsetDegrees, double distance ) + double offsetDegrees, double distance ) { // assumptions we model the standard service volume, plus @@ -338,7 +341,7 @@ double FGNavCom::adjustILSRange( double stationElev, double aircraftElev, // } else { // return 0; // } - return FG_ILS_DEFAULT_RANGE; + return FG_LOC_DEFAULT_RANGE; } @@ -375,9 +378,9 @@ FGNavCom::update(double dt) double dist = sgdClosestPointToLineDistSquared( p, p0, gs_base_vec ); nav_gs_dist = sqrt( dist ); - // cout << nav_gs_dist; + // cout << "nav_gs_dist = " << nav_gs_dist << endl; - // Point3D tmp( nav_gs_x, nav_gs_y, nav_gs_z ); + Point3D tmp( nav_gs_x, nav_gs_y, nav_gs_z ); // cout << " (" << aircraft.distance3D( tmp ) << ")" << endl; // wgs84 heading to glide slope (to determine sign of distance) @@ -419,10 +422,11 @@ FGNavCom::update(double dt) while ( offset < -180.0 ) { offset += 360.0; } while ( offset > 180.0 ) { offset -= 360.0; } // cout << "ils offset = " << offset << endl; - nav_effective_range = adjustILSRange(nav_elev, elev, offset, - nav_loc_dist * SG_METER_TO_NM ); + nav_effective_range + = adjustILSRange( nav_elev, elev, offset, + nav_loc_dist * SG_METER_TO_NM ); } else { - nav_effective_range = adjustNavRange(nav_elev, elev, nav_range); + nav_effective_range = adjustNavRange( nav_elev, elev, nav_range ); } // cout << "nav range = " << nav_effective_range // << " (" << nav_range << ")" << endl; @@ -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; - - nav_gs_rate_of_climb = -sin( des_angle * SGD_DEGREES_TO_RADIANS ) - * horiz_vel * SG_METER_TO_FEET; + 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; + } } else { nav_inrange = false; // cout << "not picking up vor. :-(" << endl; @@ -537,7 +541,7 @@ FGNavCom::update(double dt) if ( power_btn && (bus_power->getDoubleValue() > 1.0) && nav_ident_btn && audio_btn ) { - SGSimpleSound *sound; + SGSoundSample *sound; sound = globals->get_soundmgr()->find( nav_fx_name ); if ( sound != NULL ) { sound->set_volume( nav_vol_btn ); @@ -593,63 +597,84 @@ void FGNavCom::search() double lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS; double elev = alt_node->getDoubleValue() * SG_FEET_TO_METER; - FGILS *ils; - FGNav *nav; + FGNavRecord *nav = NULL; + FGNavRecord *loc = NULL; + FGNavRecord *dme = NULL; + FGNavRecord *gs = NULL; //////////////////////////////////////////////////////////////////////// // Nav. //////////////////////////////////////////////////////////////////////// - if ( (ils = current_ilslist->findByFreq(nav_freq, lon, lat, elev)) != NULL ) { - nav_id = ils->get_locident(); + nav = globals->get_navlist()->findByFreq(nav_freq, lon, lat, elev); + dme = globals->get_dmelist()->findByFreq(nav_freq, lon, lat, elev); + if ( nav == NULL ) { + loc = globals->get_loclist()->findByFreq(nav_freq, lon, lat, elev); + gs = globals->get_gslist()->findByFreq(nav_freq, lon, lat, elev); + } + + if ( loc != NULL ) { + nav_id = loc->get_ident(); + // cout << "localizer = " << nav_id << endl; nav_valid = true; if ( last_nav_id != nav_id || last_nav_vor ) { - nav_trans_ident = ils->get_trans_ident(); + nav_trans_ident = loc->get_trans_ident(); + nav_target_radial = loc->get_multiuse(); + while ( nav_target_radial < 0.0 ) { nav_target_radial += 360.0; } + while ( nav_target_radial > 360.0 ) { nav_target_radial -= 360.0; } + nav_loclon = loc->get_lon(); + nav_loclat = loc->get_lat(); + nav_x = loc->get_x(); + nav_y = loc->get_y(); + nav_z = loc->get_z(); last_nav_id = nav_id; last_nav_vor = false; nav_loc = true; - nav_has_dme = ils->get_has_dme(); - nav_has_gs = ils->get_has_gs(); - - nav_loclon = ils->get_loclon(); - nav_loclat = ils->get_loclat(); - nav_gslon = ils->get_gslon(); - nav_gslat = ils->get_gslat(); - nav_elev = ils->get_gselev(); + nav_has_dme = (dme != NULL); + nav_has_gs = (gs != NULL); + if ( nav_has_gs ) { + nav_gslon = gs->get_lon(); + nav_gslat = gs->get_lat(); + nav_elev = gs->get_elev_ft(); + int tmp = (int)(gs->get_multiuse() / 1000.0); + nav_target_gs = (double)tmp / 100.0; + nav_gs_x = gs->get_x(); + nav_gs_y = gs->get_y(); + nav_gs_z = gs->get_z(); + + // derive GS baseline (perpendicular to the runay + // along the ground) + double tlon, tlat, taz; + geo_direct_wgs_84 ( 0.0, nav_gslat, nav_gslon, + nav_target_radial + 90, + 100.0, &tlat, &tlon, &taz ); + // cout << "nav_target_radial = " << nav_target_radial << endl; + // cout << "nav_loc = " << nav_loc << endl; + // cout << nav_gslon << "," << nav_gslat << " " + // << tlon << "," << tlat << " (" << nav_elev << ")" + // << endl; + Point3D p1 = sgGeodToCart( Point3D(tlon*SGD_DEGREES_TO_RADIANS, + tlat*SGD_DEGREES_TO_RADIANS, + nav_elev*SG_FEET_TO_METER) + ); + // cout << nav_gs_x << "," << nav_gs_y << "," << nav_gs_z + // << endl; + // cout << p1 << endl; + sgdSetVec3( gs_base_vec, + p1.x()-nav_gs_x, p1.y()-nav_gs_y, p1.z()-nav_gs_z ); + // cout << gs_base_vec[0] << "," << gs_base_vec[1] << "," + // << gs_base_vec[2] << endl; + } else { + nav_elev = loc->get_elev_ft(); + } nav_twist = 0; - nav_range = FG_ILS_DEFAULT_RANGE; + nav_range = FG_LOC_DEFAULT_RANGE; nav_effective_range = nav_range; - nav_target_gs = ils->get_gsangle(); - nav_target_radial = ils->get_locheading(); - while ( nav_target_radial < 0.0 ) { nav_target_radial += 360.0; } - while ( nav_target_radial > 360.0 ) { nav_target_radial -= 360.0; } - nav_x = ils->get_x(); - nav_y = ils->get_y(); - nav_z = ils->get_z(); - nav_gs_x = ils->get_gs_x(); - nav_gs_y = ils->get_gs_y(); - nav_gs_z = ils->get_gs_z(); - - // derive GS baseline - double tlon, tlat, taz; - geo_direct_wgs_84 ( 0.0, nav_gslat, nav_gslon, nav_target_radial + 90, - 100.0, &tlat, &tlon, &taz ); - // cout << nav_gslon << "," << nav_gslat << " " - // << tlon << "," << tlat << " (" << nav_elev << ")" << endl; - Point3D p1 = sgGeodToCart( Point3D(tlon*SGD_DEGREES_TO_RADIANS, - tlat*SGD_DEGREES_TO_RADIANS, - nav_elev*SG_FEET_TO_METER) ); - // cout << nav_gs_x << "," << nav_gs_y << "," << nav_gs_z << endl; - // cout << p1 << endl; - sgdSetVec3( gs_base_vec, - p1.x()-nav_gs_x, p1.y()-nav_gs_y, p1.z()-nav_gs_z ); - // cout << gs_base_vec[0] << "," << gs_base_vec[1] << "," - // << gs_base_vec[2] << endl; if ( globals->get_soundmgr()->exists( nav_fx_name ) ) { globals->get_soundmgr()->remove( nav_fx_name ); } - SGSimpleSound *sound; + SGSoundSample *sound; sound = morse.make_ident( nav_trans_ident, LO_FREQUENCY ); sound->set_volume( 0.3 ); globals->get_soundmgr()->add( sound, nav_fx_name ); @@ -671,23 +696,24 @@ void FGNavCom::search() // << " current time = " // << globals->get_time_params()->get_cur_time() << endl; - // cout << "Found an ils station in range" << endl; - // cout << " id = " << ils->get_locident() << endl; + // cout << "Found an loc station in range" << endl; + // cout << " id = " << loc->get_locident() << endl; } - } else if ( (nav = current_navlist->findByFreq(nav_freq, lon, lat, elev)) != NULL ) { + } else if ( nav != NULL ) { nav_id = nav->get_ident(); - nav_valid = (nav->get_type() == 'V'); + // cout << "nav = " << nav_id << endl; + nav_valid = true; if ( last_nav_id != nav_id || !last_nav_vor ) { last_nav_id = nav_id; last_nav_vor = true; nav_trans_ident = nav->get_trans_ident(); nav_loc = false; - nav_has_dme = nav->get_has_dme(); + nav_has_dme = (dme != NULL); nav_has_gs = false; nav_loclon = nav->get_lon(); nav_loclat = nav->get_lat(); - nav_elev = nav->get_elev(); - nav_twist = nav->get_magvar(); + nav_elev = nav->get_elev_ft(); + nav_twist = nav->get_multiuse(); nav_range = nav->get_range(); nav_effective_range = adjustNavRange(nav_elev, elev, nav_range); nav_target_gs = 0.0; @@ -699,7 +725,7 @@ void FGNavCom::search() if ( globals->get_soundmgr()->exists( nav_fx_name ) ) { globals->get_soundmgr()->remove( nav_fx_name ); } - SGSimpleSound *sound; + SGSoundSample *sound; sound = morse.make_ident( nav_trans_ident, LO_FREQUENCY ); sound->set_volume( 0.3 ); if ( globals->get_soundmgr()->add( sound, nav_fx_name ) ) { @@ -771,6 +797,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