Merge branches 'jmt/aircraft-name' and 'jmt/runway'

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

diff --git a/src/Instrumentation/navradio.cxx b/src/Instrumentation/navradio.cxx
index f3b95c5e3bd98a1914d861ade8b5a281dabbcb20..40a33844bd6a0f3689d762f7ae7120b80e8087c9 100644 (file)
--- a/src/Instrumentation/navradio.cxx
+++ b/src/Instrumentation/navradio.cxx
@@ -143,6 +143,7 @@ FGNavRadio::FGNavRadio(SGPropertyNode *node) :
     gps_to_flag_node(NULL),
     gps_from_flag_node(NULL),
     gps_has_gs_node(NULL),
+    gps_xtrack_error_nm_node(NULL),
     play_count(0),
     last_time(0),
     target_radial(0.0),
@@ -150,6 +151,7 @@ FGNavRadio::FGNavRadio(SGPropertyNode *node) :
     last_x(0.0),
     last_loc_dist(0.0),
     last_xtrack_error(0.0),
+    xrate_ms(0.0),
     _localizerWidth(5.0),
     _name(node->getStringValue("name", "nav")),
     _num(node->getIntValue("number", 0)),
@@ -183,8 +185,9 @@ FGNavRadio::~FGNavRadio()
 void
 FGNavRadio::init ()
 {
-    SGSoundMgr *smgr = (SGSoundMgr *)globals->get_subsystem("soundmgr");
+    SGSoundMgr *smgr = globals->get_soundmgr();
     _sgr = smgr->find("avionics", true);
+    _sgr->tie_to_listener();
 
     morse.init();
 
@@ -268,6 +271,9 @@ FGNavRadio::init ()
     gps_to_flag_node = fgGetNode("/instrumentation/gps/to-flag", true);
     gps_from_flag_node = fgGetNode("/instrumentation/gps/from-flag", true);
     gps_has_gs_node = fgGetNode("/instrumentation/gps/has-gs", true);
+    gps_course_node = fgGetNode("/instrumentation/gps/selected-course-deg", true);
+    gps_xtrack_error_nm_node = fgGetNode("/instrumentation/gps/wp/wp[1]/course-error-nm", true);
+    _magvarNode = fgGetNode("/environment/magnetic-variation-deg", true);
     
     std::ostringstream temp;
     temp << _name << "nav-ident" << _num;
@@ -574,6 +580,8 @@ void FGNavRadio::updateGlideSlope(double dt, const SGVec3d& aircraft, double sig
   if (!_gs || !inrange_node->getBoolValue()) {
     gs_dist_node->setDoubleValue( 0.0 );
     gs_inrange_node->setBoolValue(false);
+    _gsNeedleDeflection = 0.0;
+    _gsNeedleDeflectionNorm = 0.0;
     return;
   }
   
@@ -583,6 +591,8 @@ void FGNavRadio::updateGlideSlope(double dt, const SGVec3d& aircraft, double sig
   gs_inrange_node->setBoolValue(gsInRange);
         
   if (!gsInRange) {
+    _gsNeedleDeflection = 0.0;
+    _gsNeedleDeflectionNorm = 0.0;
     return;
   }
   
@@ -655,14 +665,30 @@ void FGNavRadio::updateGPSSlaved()
   _toFlag = gps_to_flag_node->getBoolValue();
   _fromFlag = gps_from_flag_node->getBoolValue();
 
-  inrange_node->setBoolValue(_toFlag | _fromFlag);
+  bool gpsValid = (_toFlag | _fromFlag);
+  inrange_node->setBoolValue(gpsValid);
+  if (!gpsValid) {
+    signal_quality_norm_node->setDoubleValue(0.0);
+    _cdiDeflection = 0.0;
+    _cdiCrossTrackErrorM = 0.0;
+    _gsNeedleDeflection = 0.0;
+    return;
+  }
+  
+  // this is unfortunate, but panel instruments use this value to decide
+  // if the navradio output is valid.
+  signal_quality_norm_node->setDoubleValue(1.0);
   
   _cdiDeflection =  gps_cdi_deflection_node->getDoubleValue();
   // clmap to some range (+/- 10 degrees) as the regular deflection
   SG_CLAMP_RANGE(_cdiDeflection, -10.0, 10.0 );
   
-  _cdiCrossTrackErrorM = 0.0; // FIXME, supply this
+  _cdiCrossTrackErrorM = gps_xtrack_error_nm_node->getDoubleValue() * SG_NM_TO_METER;
   _gsNeedleDeflection = 0.0; // FIXME, supply this
+  
+  double trtrue = gps_course_node->getDoubleValue() + _magvarNode->getDoubleValue();
+  SG_NORMALIZE_RANGE(trtrue, 0.0, 360.0);
+  target_radial_true_node->setDoubleValue( trtrue );
 }
 
 void FGNavRadio::updateCDI(double dt)
@@ -744,7 +770,8 @@ void FGNavRadio::updateCDI(double dt)
   //////////////////////////////////////////////////////////
   double t = 0.0;
   if ( inrange && cdi_serviceable ) {
-    double xrate_ms = (last_xtrack_error - _cdiCrossTrackErrorM) / dt;
+    double cur_rate = (last_xtrack_error - _cdiCrossTrackErrorM) / dt;
+    xrate_ms = 0.99 * xrate_ms + 0.01 * cur_rate;
     if ( fabs(xrate_ms) > 0.00001 ) {
         t = _cdiCrossTrackErrorM / xrate_ms;
     } else {