X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FCockpit%2Fnavcom.cxx;h=1850539e03a6f6b30eb2e5e62ff811467c537ed7;hb=fee003e8cc17f0aa9a1e9280dd823d20e231e967;hp=a67f67336597d8527cbe3eb3f28beb2da79592c7;hpb=25fddbf476b419d8c1f8aec5a5b014b4262196ba;p=flightgear.git diff --git a/src/Cockpit/navcom.cxx b/src/Cockpit/navcom.cxx index a67f67336..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 @@ -28,11 +28,11 @@ #include // snprintf #include +#include #include #include #include -#include #include #include "navcom.hxx" @@ -61,8 +61,13 @@ FGNavCom::FGNavCom() : nav_heading(0.0), nav_radial(0.0), 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) + nav_ident_btn(true), + horiz_vel(0.0), + last_x(0.0) { SGPath path( globals->get_fg_root() ); SGPath term = path; @@ -187,6 +192,13 @@ FGNavCom::bind () snprintf(propname, 256, "/radios/nav[%d]/radials/actual-deg", index); fgTie( propname, this, &FGNavCom::get_nav_radial ); + snprintf(propname, 256, "/radios/nav[%d]/radials/target-radial-deg", index); + fgTie( propname, this, &FGNavCom::get_nav_target_radial_true ); + + snprintf(propname, 256, "/radios/nav[%d]/radials/target-auto-hdg-deg", + index); + fgTie( propname, this, &FGNavCom::get_nav_target_auto_hdg ); + snprintf(propname, 256, "/radios/nav[%d]/to-flag", index); fgTie( propname, this, &FGNavCom::get_nav_to_flag ); @@ -199,12 +211,18 @@ 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 ); snprintf(propname, 256, "/radios/nav[%d]/nav-loc", index); fgTie( propname, this, &FGNavCom::get_nav_loc ); + snprintf(propname, 256, "/radios/nav[%d]/gs-rate-of-climb", index); + fgTie( propname, this, &FGNavCom::get_nav_gs_rate_of_climb ); + snprintf(propname, 256, "/radios/nav[%d]/gs-needle-deflection", index); fgTie( propname, this, &FGNavCom::get_nav_gs_deflection ); @@ -300,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 @@ -323,7 +341,7 @@ double FGNavCom::adjustILSRange( double stationElev, double aircraftElev, // } else { // return 0; // } - return FG_ILS_DEFAULT_RANGE; + return FG_LOC_DEFAULT_RANGE; } @@ -360,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) @@ -404,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; @@ -425,6 +444,92 @@ FGNavCom::update(double dt) if ( !nav_loc ) { nav_target_radial = nav_sel_radial; } + + // Calculate some values for the nav/ils hold autopilot + + double cur_radial = get_nav_reciprocal_radial(); + if ( nav_loc ) { + // ILS localizers radials are already "true" in our + // database + } else { + cur_radial += nav_twist; + } + if ( get_nav_from_flag() ) { + cur_radial += 180.0; + while ( cur_radial >= 360.0 ) { cur_radial -= 360.0; } + } + + // AUTOPILOT HELPERS + + // determine the target radial in "true" heading + nav_target_radial_true = nav_target_radial; + if ( nav_loc ) { + // ILS localizers radials are already "true" in our + // database + } else { + // VOR radials need to have that vor's offset added in + nav_target_radial_true += nav_twist; + } + + while ( nav_target_radial_true < 0.0 ) { + nav_target_radial_true += 360.0; + } + while ( nav_target_radial_true > 360.0 ) { + nav_target_radial_true -= 360.0; + } + + // determine the heading adjustment needed. + // 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; + } + SG_CLAMP_RANGE( adjustment, -30.0, 30.0 ); + + // determine the target heading to fly to intercept the + // tgt_radial + nav_target_auto_hdg = nav_target_radial_true + adjustment; + while ( nav_target_auto_hdg < 0.0 ) { nav_target_auto_hdg += 360.0; } + while ( nav_target_auto_hdg > 360.0 ) { nav_target_auto_hdg -= 360.0; } + + // cross track error + // ???? + + // Calculate desired rate of climb for intercepting the GS + double x = nav_gs_dist; + double y = (alt_node->getDoubleValue() - nav_elev) + * SG_FEET_TO_METER; + double current_angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES; + + double target_angle = nav_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 ); + + 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; @@ -436,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 ); @@ -492,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 ); @@ -570,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; @@ -598,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 ) ) { @@ -670,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