X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FCockpit%2Fnavcom.cxx;h=03fae8220bb6208705746fd68b9849b8cba3df97;hb=924e95fec2db4a1539f97ab3edbff8fb3b87c044;hp=a67f67336597d8527cbe3eb3f28beb2da79592c7;hpb=25fddbf476b419d8c1f8aec5a5b014b4262196ba;p=flightgear.git diff --git a/src/Cockpit/navcom.cxx b/src/Cockpit/navcom.cxx index a67f67336..03fae8220 100644 --- a/src/Cockpit/navcom.cxx +++ b/src/Cockpit/navcom.cxx @@ -28,6 +28,7 @@ #include // snprintf #include +#include #include #include @@ -61,8 +62,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 +193,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 +212,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 ); @@ -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; @@ -585,7 +690,7 @@ void FGNavCom::search() nav_has_gs = false; nav_loclon = nav->get_lon(); nav_loclat = nav->get_lat(); - nav_elev = nav->get_elev(); + nav_elev = nav->get_elev_ft(); nav_twist = nav->get_magvar(); nav_range = nav->get_range(); nav_effective_range = adjustNavRange(nav_elev, elev, nav_range); @@ -670,6 +775,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