Fix two bugs in the new autopilot code

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

diff --git a/src/Instrumentation/navradio.cxx b/src/Instrumentation/navradio.cxx
index 3e403d9cadbea9895a19b98a1bb3bcd233eb1de3..cdf61753007a5848ef5a7fce6a757bec590861ee 100644 (file)
--- a/src/Instrumentation/navradio.cxx
+++ b/src/Instrumentation/navradio.cxx
@@ -129,6 +129,14 @@ FGNavRadio::FGNavRadio(SGPropertyNode *node) :
 // Destructor
 FGNavRadio::~FGNavRadio() 
 {
+    if (gps_course_node) {
+      gps_course_node->removeChangeListener(this);
+    }
+    
+    if (nav_slaved_to_gps_node) {
+      nav_slaved_to_gps_node->removeChangeListener(this);
+    }
+    
     delete term_tbl;
     delete low_tbl;
     delete high_tbl;
@@ -208,6 +216,7 @@ FGNavRadio::init ()
     gs_deflection_deg_node = node->getChild("gs-needle-deflection-deg", 0, true);
     gs_deflection_norm_node = node->getChild("gs-needle-deflection-norm", 0, true);
     gs_rate_of_climb_node = node->getChild("gs-rate-of-climb", 0, true);
+    gs_rate_of_climb_fpm_node = node->getChild("gs-rate-of-climb-fpm", 0, true);
     gs_dist_node = node->getChild("gs-distance", 0, true);
     gs_inrange_node = node->getChild("gs-in-range", 0, true);
     
@@ -219,11 +228,15 @@ FGNavRadio::init ()
 
     // gps slaving support
     nav_slaved_to_gps_node = node->getChild("slaved-to-gps", 0, true);
+    nav_slaved_to_gps_node->addChangeListener(this);
+    
     gps_cdi_deflection_node = fgGetNode("/instrumentation/gps/cdi-deflection", true);
     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_course_node = fgGetNode("/instrumentation/gps/desired-course-deg", true);
+    gps_course_node->addChangeListener(this);
+    
     gps_xtrack_error_nm_node = fgGetNode("/instrumentation/gps/wp/wp[1]/course-error-nm", true);
     _magvarNode = fgGetNode("/environment/magnetic-variation-deg", true);
     
@@ -595,17 +608,22 @@ void FGNavRadio::updateGlideSlope(double dt, const SGVec3d& aircraft, double sig
   double gs_diff = target_gs - angle;
   // convert desired vertical path angle into a climb rate
   double des_angle = angle - 10 * gs_diff;
+  /* printf("target_gs=%.1f angle=%.1f gs_diff=%.1f des_angle=%.1f\n",
+     target_gs, angle, gs_diff, des_angle); */
 
   // estimate horizontal speed towards ILS in meters per minute
   double elapsedDistance = last_x - gsDist;
   last_x = gsDist;
       
   double new_vel = ( elapsedDistance / dt );
-  horiz_vel = 0.75 * horiz_vel + 0.25 * new_vel;
+  horiz_vel = 0.99 * horiz_vel + 0.01 * new_vel;
+  /* printf("vel=%.1f (dist=%.1f dt=%.2f)\n", horiz_vel, elapsedDistance, dt);*/
 
   gs_rate_of_climb_node
       ->setDoubleValue( -sin( des_angle * SGD_DEGREES_TO_RADIANS )
                         * horiz_vel * SG_METER_TO_FEET );
+  gs_rate_of_climb_fpm_node
+      ->setDoubleValue( gs_rate_of_climb_node->getDoubleValue() * 60 );
 }
 
 void FGNavRadio::updateDME(const SGVec3d& aircraft)
@@ -619,6 +637,26 @@ void FGNavRadio::updateDME(const SGVec3d& aircraft)
   _dmeInRange =  (dme_distance < _dme->get_range() * SG_NM_TO_METER);
 }
 
+void FGNavRadio::valueChanged (SGPropertyNode* prop)
+{
+  if (prop == gps_course_node) {
+    if (!nav_slaved_to_gps_node->getBoolValue()) {
+      return;
+    }
+  
+    // GPS desired course has changed, sync up our selected-course
+    double v = prop->getDoubleValue();
+    if (v != sel_radial_node->getDoubleValue()) {
+      sel_radial_node->setDoubleValue(v);
+    }
+  } else if (prop == nav_slaved_to_gps_node) {
+    if (prop->getBoolValue()) {
+      // slaved-to-GPS activated, sync up selected course
+      sel_radial_node->setDoubleValue(gps_course_node->getDoubleValue());
+    }
+  }
+}
+
 void FGNavRadio::updateGPSSlaved()
 {
   has_gs_node->setBoolValue(gps_has_gs_node->getBoolValue());
@@ -858,7 +896,7 @@ void FGNavRadio::search()
     } else { // ILS or LOC
       _gs = globals->get_gslist()->findByFreq(freq, pos);
       has_gs_node->setBoolValue(_gs != NULL);
-      _localizerWidth = localizerWidth(nav);
+      _localizerWidth = nav->localizerWidth();
       twist = 0.0;
            effective_range = nav->get_range();
       
@@ -908,36 +946,6 @@ void FGNavRadio::search()
   id_c4_node->setIntValue( (int)identBuffer[3] );
 }
 
-double FGNavRadio::localizerWidth(FGNavRecord* aLOC)
-{
-  FGRunway* rwy = aLOC->runway();
-  assert(rwy);
-  
-  SGVec3d thresholdCart(SGVec3d::fromGeod(rwy->threshold()));
-  double axisLength = dist(aLOC->cart(), thresholdCart);
-  double landingLength = dist(thresholdCart, SGVec3d::fromGeod(rwy->end()));
-  
-// Reference: http://dcaa.slv.dk:8000/icaodocs/
-// ICAO standard width at threshold is 210 m = 689 feet = approx 700 feet.
-// ICAO 3.1.1 half course = DDM = 0.0775
-// ICAO 3.1.3.7.1 Sensitivity 0.00145 DDM/m at threshold
-//  implies peg-to-peg of 214 m ... we will stick with 210.
-// ICAO 3.1.3.7.1 "Course sector angle shall not exceed 6 degrees."
-              
-// Very short runway:  less than 1200 m (4000 ft) landing length:
-  if (landingLength < 1200.0) {
-// ICAO fudges localizer sensitivity for very short runways.
-// This produces a non-monotonic sensitivity-versus length relation.
-    axisLength += 1050.0;
-  }
-
-// Example: very short: San Diego   KMYF (Montgomery Field) ILS RWY 28R
-// Example: short:      Tom's River KMJX (Robert J. Miller) ILS RWY 6
-// Example: very long:  Denver      KDEN (Denver)           ILS RWY 16R
-  double raw_width = 210.0 / axisLength * SGD_RADIANS_TO_DEGREES;
-  return raw_width < 6.0? raw_width : 6.0;
-}
-
 void FGNavRadio::audioNavidChanged()
 {
   if (_sgr->exists(nav_fx_name)) {