#include <stdio.h> // snprintf
#include <simgear/compiler.h>
+#include <simgear/sg_inlines.h>
#include <simgear/math/sg_random.h>
#include <simgear/math/vector.hxx>
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;
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 );
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 );
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;
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);
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
projects / flightgear.git / blobdiff