Make hardcoded error values configurable.

[flightgear.git] / src / Instrumentation / navradio.cxx

diff --git a/src/Instrumentation/navradio.cxx b/src/Instrumentation/navradio.cxx
index a208a14d61b9658a76e3c00e2dc1d22c5ba03ba9..b4455be10013b15952ecf0d3823739e4c52e19a8 100644 (file)
--- a/src/Instrumentation/navradio.cxx
+++ b/src/Instrumentation/navradio.cxx
@@ -25,24 +25,22 @@
 #  include <config.h>
 #endif
 
-#include <iostream>
-#include <string>
 #include <sstream>
 
-#include <simgear/compiler.h>
 #include <simgear/sg_inlines.h>
-#include <simgear/math/sg_random.h>
+#include <simgear/timing/sg_time.hxx>
 #include <simgear/math/vector.hxx>
+#include <simgear/math/sg_random.h>
+#include <simgear/misc/sg_path.hxx>
+#include <simgear/math/sg_geodesy.hxx>
+#include <simgear/structure/exception.hxx>
 
-#include <Aircraft/aircraft.hxx>
 #include <Navaids/navlist.hxx>
-
+#include <Main/util.hxx>
 #include "navradio.hxx"
 
-#include <string>
 using std::string;
 
-
 // Constructor
 FGNavRadio::FGNavRadio(SGPropertyNode *node) :
     lon_node(fgGetNode("/position/longitude-deg", true)),
@@ -72,6 +70,7 @@ FGNavRadio::FGNavRadio(SGPropertyNode *node) :
     to_flag_node(NULL),
     from_flag_node(NULL),
     inrange_node(NULL),
+    signal_quality_norm_node(NULL),
     cdi_deflection_node(NULL),
     cdi_xtrack_error_node(NULL),
     cdi_xtrack_hdg_err_node(NULL),
@@ -180,6 +179,7 @@ FGNavRadio::init ()
     to_flag_node = node->getChild("to-flag", 0, true);
     from_flag_node = node->getChild("from-flag", 0, true);
     inrange_node = node->getChild("in-range", 0, true);
+    signal_quality_norm_node = node->getChild("signal-quality-norm", 0, true);
     cdi_deflection_node = node->getChild("heading-needle-deflection", 0, true);
     cdi_xtrack_error_node = node->getChild("crosstrack-error-m", 0, true);
     cdi_xtrack_hdg_err_node
@@ -318,6 +318,8 @@ FGNavRadio::update(double dt)
     bool has_gs = has_gs_node->getBoolValue();
     bool is_loc = loc_node->getBoolValue();
     double loc_dist = loc_dist_node->getDoubleValue();
+    double effective_range_m;
+    double signal_quality_norm = signal_quality_norm_node->getDoubleValue();
 
     double az1, az2, s;
 
@@ -422,18 +424,32 @@ FGNavRadio::update(double dt)
            effective_range
                 = adjustNavRange( nav_elev, pos.getElevationM(), range );
        }
+
+        effective_range_m = effective_range * SG_NM_TO_METER;
+
        // cout << "nav range = " << effective_range
        //      << " (" << range << ")" << endl;
 
-       if ( loc_dist < effective_range * SG_NM_TO_METER ) {
-           inrange = true;
-       } else if ( loc_dist < 2 * effective_range * SG_NM_TO_METER ) {
-           inrange = sg_random() < ( 2 * effective_range * SG_NM_TO_METER
-                                      - loc_dist )
-                                      / (effective_range * SG_NM_TO_METER);
-       } else {
-           inrange = false;
-       }
+        //////////////////////////////////////////////////////////
+        // compute signal quality
+        // 100% within effective_range
+        // decreases 1/x^2 further out
+        //////////////////////////////////////////////////////////
+        {
+            double last_signal_quality_norm = signal_quality_norm;
+
+           if ( loc_dist < effective_range_m ) {
+              signal_quality_norm = 1.0;
+            } else {
+              double range_exceed_norm = loc_dist/effective_range_m;
+              signal_quality_norm = 1/(range_exceed_norm*range_exceed_norm);
+            }
+
+            signal_quality_norm = fgGetLowPass( last_signal_quality_norm, 
+                   signal_quality_norm, dt );
+        }
+        signal_quality_norm_node->setDoubleValue( signal_quality_norm );
+        inrange = signal_quality_norm > 0.2;
         inrange_node->setBoolValue( inrange );
 
        if ( !is_loc ) {
@@ -511,6 +527,7 @@ FGNavRadio::update(double dt)
                     r *= 4.0;
                 }
                 SG_CLAMP_RANGE( r, -10.0, 10.0 );
+                r *= signal_quality_norm;
             }
         }
         cdi_deflection_node->setDoubleValue( r );
@@ -595,6 +612,7 @@ FGNavRadio::update(double dt)
                 // cout << "dist = " << x << " height = " << y << endl;
                 double angle = asin( y / x ) * SGD_RADIANS_TO_DEGREES;
                 r = (target_gs - angle) * 5.0;
+                r *= signal_quality_norm;
             }
         }
         gs_deflection_node->setDoubleValue( r );
@@ -747,10 +765,8 @@ void FGNavRadio::search()
     _time_before_search_sec = 1.0;
 
     // cache values locally for speed
-    double lon = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
-    double lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
-    double elev = alt_node->getDoubleValue() * SG_FEET_TO_METER;
-
+    SGGeod pos = SGGeod::fromDegFt(lon_node->getDoubleValue(),
+      lat_node->getDoubleValue(), alt_node->getDoubleValue());
     FGNavRecord *nav = NULL;
     FGNavRecord *loc = NULL;
     FGNavRecord *dme = NULL;
@@ -761,11 +777,11 @@ void FGNavRadio::search()
     ////////////////////////////////////////////////////////////////////////
 
     double freq = freq_node->getDoubleValue();
-    nav = globals->get_navlist()->findByFreq(freq, lon, lat, elev);
-    dme = globals->get_dmelist()->findByFreq(freq, lon, lat, elev);
+    nav = globals->get_navlist()->findByFreq(freq, pos);
+    dme = globals->get_dmelist()->findByFreq(freq, pos);
     if ( nav == NULL ) {
-        loc = globals->get_loclist()->findByFreq(freq, lon, lat, elev);
-        gs = globals->get_gslist()->findByFreq(freq, lon, lat, elev);
+        loc = globals->get_loclist()->findByFreq(freq, pos);
+        gs = globals->get_gslist()->findByFreq(freq, pos);
     }
 
     string nav_id = "";
@@ -782,7 +798,7 @@ void FGNavRadio::search()
            while ( target_radial > 360.0 ) { target_radial -= 360.0; }
            loc_lon = loc->get_lon();
            loc_lat = loc->get_lat();
-           nav_xyz = loc->get_cart();
+           nav_xyz = loc->cart();
            last_nav_id = nav_id;
            last_nav_vor = false;
            loc_node->setBoolValue( true );
@@ -794,7 +810,7 @@ void FGNavRadio::search()
                 nav_elev = gs->get_elev_ft();
                 int tmp = (int)(gs->get_multiuse() / 1000.0);
                 target_gs = (double)tmp / 100.0;
-                gs_xyz = gs->get_cart();
+                gs_xyz = gs->cart();
 
                 // derive GS baseline (perpendicular to the runay
                 // along the ground)
@@ -867,10 +883,10 @@ void FGNavRadio::search()
            nav_elev = nav->get_elev_ft();
            twist = nav->get_multiuse();
            range = nav->get_range();
-           effective_range = adjustNavRange(nav_elev, elev, range);
+           effective_range = adjustNavRange(nav_elev, pos.getElevationM(), range);
            target_gs = 0.0;
            target_radial = sel_radial_node->getDoubleValue();
-           nav_xyz = nav->get_cart();
+           nav_xyz = nav->cart();
 
            if ( globals->get_soundmgr()->exists( nav_fx_name ) ) {
                globals->get_soundmgr()->remove( nav_fx_name );