Moved some of the low level scene graph construction code over to simgear.

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

diff --git a/src/Cockpit/navcom.cxx b/src/Cockpit/navcom.cxx
index a5f5b93578ed684f39323c8bd7583c0315727bac..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 );
 
@@ -276,23 +280,24 @@ double FGNavCom::adjustILSRange( double stationElev, double aircraftElev,
 
     // altitude difference
     // double alt = ( aircraftElev * SG_METER_TO_FEET - stationElev );
-    double offset = fabs( offsetDegrees );
-
-    if ( offset < 10 ) {
-       return FG_ILS_DEFAULT_RANGE;
-    } else if ( offset < 35 ) {
-       return 10 + (35 - offset) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
-    } else if ( offset < 45 ) {
-       return (45 - offset);
-    } else if ( offset > 170 ) {
-        return FG_ILS_DEFAULT_RANGE;
-    } else if ( offset > 145 ) {
-       return 10 + (offset - 145) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
-    } else if ( offset > 135 ) {
-        return (offset - 135);
-    } else {
-       return 0;
-    }
+//     double offset = fabs( offsetDegrees );
+
+//     if ( offset < 10 ) {
+//     return FG_ILS_DEFAULT_RANGE;
+//     } else if ( offset < 35 ) {
+//     return 10 + (35 - offset) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
+//     } else if ( offset < 45 ) {
+//     return (45 - offset);
+//     } else if ( offset > 170 ) {
+//         return FG_ILS_DEFAULT_RANGE;
+//     } else if ( offset > 145 ) {
+//     return 10 + (offset - 145) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
+//     } else if ( offset > 135 ) {
+//         return (offset - 135);
+//     } else {
+//     return 0;
+//     }
+    return FG_ILS_DEFAULT_RANGE;
 }
 
 
@@ -321,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, 
@@ -394,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 );
@@ -487,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 );
@@ -540,13 +572,13 @@ 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 ) ) {
                 // cout << "Added nav-vor-ident sound" << endl;
             } else {
-                cout << "Failed to add v1-vor-ident sound" << endl;
+                SG_LOG(SG_COCKPIT, SG_WARN, "Failed to add v1-vor-ident sound");
             }
 
            if ( globals->get_soundmgr()->exists( dme_fx_name ) ) {
@@ -575,7 +607,7 @@ void FGNavCom::search()
        nav_trans_ident = "";
        last_nav_id = "";
        if ( ! globals->get_soundmgr()->remove( nav_fx_name ) ) {
-            cout << "Failed to remove nav-vor-ident sound" << endl;
+            SG_LOG(SG_COCKPIT, SG_WARN, "Failed to remove nav-vor-ident sound");
         }
        globals->get_soundmgr()->remove( dme_fx_name );
        // cout << "not picking up vor1. :-(" << endl;
@@ -621,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;