X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FCockpit%2Fnavcom.cxx;h=03fae8220bb6208705746fd68b9849b8cba3df97;hb=c0b4531d04215f76f9cf65afbdaed6b7c0b32634;hp=fd5b6d6e509e3e06cff15e8e5237b1bda5ff7e10;hpb=f2a2b21661e429bb9e29f8477d676d6efbb1b94b;p=flightgear.git

diff --git a/src/Cockpit/navcom.cxx b/src/Cockpit/navcom.cxx
index fd5b6d6e5..03fae8220 100644
--- a/src/Cockpit/navcom.cxx
+++ b/src/Cockpit/navcom.cxx
@@ -28,13 +28,13 @@
 #include <stdio.h>	// snprintf
 
 #include <simgear/compiler.h>
+#include <simgear/sg_inlines.h>
 #include <simgear/math/sg_random.h>
 #include <simgear/math/vector.hxx>
 
 #include <Aircraft/aircraft.hxx>
 #include <Navaids/ilslist.hxx>
 #include <Navaids/navlist.hxx>
-#include <Time/FGEventMgr.hxx>
 
 #include "navcom.hxx"
 
@@ -59,9 +59,16 @@ FGNavCom::FGNavCom() :
     comm_vol_btn(0.0),
     nav_freq(0.0),
     nav_alt_freq(0.0),
+    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;
@@ -102,25 +109,25 @@ FGNavCom::init ()
     fgSetDouble( propname, 60.0 );
     bus_power = fgGetNode( propname, true );
 
-    snprintf(propname, 256, "/instrumentation/comm[%d]/servicable", index);
-    com_servicable = fgGetNode( propname, true );
-    com_servicable->setBoolValue( true );
+    snprintf(propname, 256, "/instrumentation/comm[%d]/serviceable", index);
+    com_serviceable = fgGetNode( propname, true );
+    com_serviceable->setBoolValue( true );
 
-    snprintf(propname, 256, "/instrumentation/nav[%d]/servicable", index);
-    nav_servicable = fgGetNode( propname, true );
-    nav_servicable->setBoolValue( true );
+    snprintf(propname, 256, "/instrumentation/nav[%d]/serviceable", index);
+    nav_serviceable = fgGetNode( propname, true );
+    nav_serviceable->setBoolValue( true );
 
-    snprintf(propname, 256, "/instrumentation/vor[%d]/cdi/servicable", index);
-    cdi_servicable = fgGetNode( propname, true );
-    cdi_servicable->setBoolValue( true );
+    snprintf(propname, 256, "/instrumentation/vor[%d]/cdi/serviceable", index);
+    cdi_serviceable = fgGetNode( propname, true );
+    cdi_serviceable->setBoolValue( true );
 
-    snprintf(propname, 256, "/instrumentation/vor[%d]/gs/servicable", index);
-    gs_servicable = fgGetNode( propname, true );
-    gs_servicable->setBoolValue( true );
+    snprintf(propname, 256, "/instrumentation/vor[%d]/gs/serviceable", index);
+    gs_serviceable = fgGetNode( propname, true );
+    gs_serviceable->setBoolValue( true );
 
-    snprintf(propname, 256, "/instrumentation/vor[%d]/to-from/servicable", index);
-    tofrom_servicable = fgGetNode( propname, true );
-    tofrom_servicable->setBoolValue( true );
+    snprintf(propname, 256, "/instrumentation/vor[%d]/to-from/serviceable", index);
+    tofrom_serviceable = fgGetNode( propname, true );
+    tofrom_serviceable->setBoolValue( true );
 }
 
 void
@@ -180,9 +187,19 @@ FGNavCom::bind ()
            &FGNavCom::get_audio_btn, &FGNavCom::set_audio_btn );
     fgSetArchivable( propname );
 
+    snprintf(propname, 256, "/radios/nav[%d]/heading-deg", index);
+    fgTie( propname,  this, &FGNavCom::get_nav_heading );
+
     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 );
 
@@ -195,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 );
 
@@ -342,7 +365,7 @@ FGNavCom::update(double dt)
     ////////////////////////////////////////////////////////////////////////
 
     if ( nav_valid && power_btn && (bus_power->getDoubleValue() > 1.0)
-         && nav_servicable->getBoolValue() )
+         && nav_serviceable->getBoolValue() )
     {
 	station = Point3D( nav_x, nav_y, nav_z );
 	nav_loc_dist = aircraft.distance3D( station );
@@ -389,7 +412,7 @@ FGNavCom::update(double dt)
                             lat * SGD_RADIANS_TO_DEGREES,
                             lon * SGD_RADIANS_TO_DEGREES, 
 			    nav_loclat, nav_loclon,
-			    &az1, &az2, &s );
+			    &nav_heading, &az2, &s );
 	// cout << "az1 = " << az1 << " magvar = " << nav_magvar << endl;
 	nav_radial = az2 - nav_twist;
 	// cout << " heading = " << nav_heading
@@ -421,12 +444,98 @@ 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;
     }
 
-    if ( nav_valid && nav_inrange && nav_servicable->getBoolValue() ) {
+    if ( nav_valid && nav_inrange && nav_serviceable->getBoolValue() ) {
 	// play station ident via audio system if on + ident,
 	// otherwise turn it off
 	if ( power_btn && (bus_power->getDoubleValue() > 1.0)
@@ -581,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);
@@ -643,7 +752,7 @@ double FGNavCom::get_nav_cdi_deflection() const {
     double r;
 
     if ( nav_inrange
-         && nav_servicable->getBoolValue() && cdi_servicable->getBoolValue() )
+         && nav_serviceable->getBoolValue() && cdi_serviceable->getBoolValue() )
     {
         r = nav_radial - nav_target_radial;
 	// cout << "Target radial = " << nav_target_radial 
@@ -666,12 +775,39 @@ 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
 double FGNavCom::get_nav_gs_deflection() const {
     if ( nav_inrange && nav_has_gs
-         && nav_servicable->getBoolValue() && gs_servicable->getBoolValue() )
+         && nav_serviceable->getBoolValue() && gs_serviceable->getBoolValue() )
     {
 	double x = nav_gs_dist;
 	double y = (fgGetDouble("/position/altitude-ft") - nav_elev)
@@ -692,8 +828,8 @@ bool
 FGNavCom::get_nav_to_flag () const
 {
     if ( nav_inrange
-         && nav_servicable->getBoolValue()
-         && tofrom_servicable->getBoolValue() )
+         && nav_serviceable->getBoolValue()
+         && tofrom_serviceable->getBoolValue() )
     {
         double offset = fabs(nav_radial - nav_target_radial);
         if (nav_loc) {
@@ -714,8 +850,8 @@ bool
 FGNavCom::get_nav_from_flag () const
 {
     if ( nav_inrange
-         && nav_servicable->getBoolValue()
-         && tofrom_servicable->getBoolValue() ) {
+         && nav_serviceable->getBoolValue()
+         && tofrom_serviceable->getBoolValue() ) {
         double offset = fabs(nav_radial - nav_target_radial);
         if (nav_loc) {
             return false;
@@ -728,6 +864,16 @@ FGNavCom::get_nav_from_flag () const
 }
 
 
+/**
+ * Return the true heading to station
+ */
+double
+FGNavCom::get_nav_heading () const
+{
+    return nav_heading;
+}
+
+
 /**
  * Return the current radial.
  */