Moved random ground cover object management code (userdata.[ch]xx) over

[flightgear.git] / src / Cockpit / navcom.cxx

diff --git a/src/Cockpit/navcom.cxx b/src/Cockpit/navcom.cxx
index 6391c5d120b8b974ed7dc4574d10a47c29fda839..5a87b7461b20cfc2577935e664e86e7a5d2230d5 100644 (file)
--- a/src/Cockpit/navcom.cxx
+++ b/src/Cockpit/navcom.cxx
@@ -29,6 +29,7 @@
 
 #include <simgear/compiler.h>
 #include <simgear/math/sg_random.h>
+#include <simgear/math/vector.hxx>
 
 #include <Aircraft/aircraft.hxx>
 #include <Navaids/ilslist.hxx>
@@ -192,6 +193,9 @@ FGNavCom::bind ()
     sprintf( propname, "/radios/nav[%d]/heading-needle-deflection", index );
     fgTie( propname, this, &FGNavCom::get_nav_heading_needle_deflection );
 
+    sprintf( propname, "/radios/nav[%d]/has-gs", index );
+    fgTie( propname, this, &FGNavCom::get_nav_has_gs );
+
     sprintf( propname, "/radios/nav[%d]/gs-needle-deflection", index );
     fgTie( propname, this, &FGNavCom::get_nav_gs_needle_deflection );
 
@@ -322,9 +326,20 @@ FGNavCom::update(double dt)
        nav_loc_dist = aircraft.distance3D( station );
 
        if ( nav_has_gs ) {
-           station = Point3D( nav_gs_x, nav_gs_y, nav_gs_z );
-           nav_gs_dist = aircraft.distance3D( station );
-            // wgs84 heading to glide slope
+            // find closest distance to the gs base line
+            sgdVec3 p;
+            sgdSetVec3( p, aircraft.x(), aircraft.y(), aircraft.z() );
+            sgdVec3 p0;
+            sgdSetVec3( p0, nav_gs_x, nav_gs_y, nav_gs_z );
+            double dist = sgdClosestPointToLineDistSquared( p, p0,
+                                                            gs_base_vec );
+            nav_gs_dist = sqrt( dist );
+            // cout << nav_gs_dist;
+
+            // 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)
             geo_inverse_wgs_84( elev,
                                 lat * SGD_RADIANS_TO_DEGREES,
                                 lon * SGD_RADIANS_TO_DEGREES, 
@@ -395,7 +410,7 @@ FGNavCom::update(double dt)
        if ( power_btn && (bus_power->getDoubleValue() > 1.0)
              && nav_ident_btn && audio_btn )
         {
-           FGSimpleSound *sound;
+           SGSimpleSound *sound;
            sound = globals->get_soundmgr()->find( nav_fx_name );
             if ( sound != NULL ) {
                 sound->set_volume( nav_vol_btn );
@@ -488,10 +503,26 @@ void FGNavCom::search()
            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_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 );
            }
-           FGSimpleSound *sound;
+           SGSimpleSound *sound;
            sound = morse.make_ident( nav_trans_ident, LO_FREQUENCY );
            sound->set_volume( 0.3 );
            globals->get_soundmgr()->add( sound, nav_fx_name );
@@ -541,7 +572,7 @@ void FGNavCom::search()
            if ( globals->get_soundmgr()->exists( nav_fx_name ) ) {
                globals->get_soundmgr()->remove( nav_fx_name );
            }
-           FGSimpleSound *sound;
+           SGSimpleSound *sound;
            sound = morse.make_ident( nav_trans_ident, LO_FREQUENCY );
            sound->set_volume( 0.3 );
            if ( globals->get_soundmgr()->add( sound, nav_fx_name ) ) {
@@ -622,7 +653,8 @@ double FGNavCom::get_nav_gs_needle_deflection() const {
        double x = nav_gs_dist;
        double y = (fgGetDouble("/position/altitude-ft") - nav_elev)
             * SG_FEET_TO_METER;
-       double angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
+        // cout << "dist = " << x << " height = " << y << endl;
+       double angle = asin( y / x ) * SGD_RADIANS_TO_DEGREES;
        return (nav_target_gs - angle) * 5.0;
     } else {
        return 0.0;