- fix unzoomed tapes (TODO: restore tick length)

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

diff --git a/src/Instrumentation/adf.cxx b/src/Instrumentation/adf.cxx
index 4cc7d269a002f75e09d01726d26431a034dc1269..efae4aa34c2699b0ac5405086047e612e6fc827e 100644 (file)
--- a/src/Instrumentation/adf.cxx
+++ b/src/Instrumentation/adf.cxx
@@ -3,10 +3,13 @@
 //
 // This file is in the Public Domain and comes with no warranty.
 
+#ifdef HAVE_CONFIG_H
+#  include <config.h>
+#endif
+
 #include <simgear/compiler.h>
 #include <simgear/math/sg_geodesy.hxx>
 #include <simgear/math/sg_random.h>
-#include <simgear/sg_inlines.h>
 
 #include <Main/fg_props.hxx>
 #include <Main/util.hxx>
@@ -58,7 +61,8 @@ ADF::ADF (SGPropertyNode *node )
     _time_before_search_sec(0),
     _last_frequency_khz(-1),
     _transmitter_valid(false),
-    _transmitter_elevation_ft(0),
+    _transmitter_pos(SGGeod::fromDeg(0, 0)),
+    _transmitter_cart(0, 0, 0),
     _transmitter_range_nm(0),
     _ident_count(0),
     _last_ident_time(0),
@@ -89,7 +93,8 @@ ADF::ADF ()
     : _time_before_search_sec(0),
       _last_frequency_khz(-1),
       _transmitter_valid(false),
-      _transmitter_elevation_ft(0),
+      _transmitter_pos(SGGeod::fromDeg(0, 0)),
+      _transmitter_cart(0, 0, 0),
       _transmitter_range_nm(0),
       _ident_count(0),
       _last_ident_time(0),
@@ -132,8 +137,6 @@ ADF::init ()
     std::ostringstream temp;
     temp << name << num;
     adf_ident = temp.str();
-
-    _serviceable_node->setBoolValue(true);
 }
 
 void
@@ -177,11 +180,11 @@ ADF::update (double delta_time_sec)
     }
 
                                 // Calculate the bearing to the transmitter
-    Point3D location =
-        sgGeodToCart(Point3D(longitude_rad, latitude_rad, altitude_m));
-
-    double distance_nm = _transmitter.distance3D(location) * SG_METER_TO_NM;
-    double range_nm = adjust_range(_transmitter_elevation_ft,
+    SGGeod geod = SGGeod::fromRadM(longitude_rad, latitude_rad, altitude_m);
+    SGVec3d location = SGVec3d::fromGeod(geod);
+    
+    double distance_nm = dist(_transmitter_cart, location) * SG_METER_TO_NM;
+    double range_nm = adjust_range(_transmitter_pos.getElevationFt(),
                                    altitude_m * SG_METER_TO_FEET,
                                    _transmitter_range_nm);
     if (distance_nm <= range_nm) {
@@ -189,16 +192,13 @@ ADF::update (double delta_time_sec)
         double bearing, az2, s;
         double heading = _heading_node->getDoubleValue();
 
-        geo_inverse_wgs_84(altitude_m,
-                           latitude_deg,
-                           longitude_deg,
-                           _transmitter_lat_deg,
-                           _transmitter_lon_deg,
+        geo_inverse_wgs_84(geod, _transmitter_pos,
                            &bearing, &az2, &s);
         _in_range_node->setBoolValue(true);
 
         bearing -= heading;
-        SG_NORMALIZE_RANGE(bearing, 0.0, 360.0);
+        if (bearing < 0)
+            bearing += 360;
         set_bearing(delta_time_sec, bearing);
 
         // adf ident sound
@@ -256,10 +256,8 @@ ADF::search (double frequency_khz, double longitude_rad,
     if ( _transmitter_valid ) {
         ident = nav->get_trans_ident();
         if ( ident != _last_ident ) {
-            _transmitter_lon_deg = nav->get_lon();
-            _transmitter_lat_deg = nav->get_lat();
-            _transmitter = Point3D(nav->get_x(), nav->get_y(), nav->get_z());
-            _transmitter_elevation_ft = nav->get_elev_ft();
+            _transmitter_pos = nav->get_pos();
+            _transmitter_cart = nav->get_cart();
             _transmitter_range_nm = nav->get_range();
         }
     }
@@ -291,7 +289,10 @@ ADF::set_bearing (double dt, double bearing_deg)
 {
     double old_bearing_deg = _bearing_node->getDoubleValue();
 
-    SG_NORMALIZE_RANGE(old_bearing_deg, 0.0, 360.0);
+    while ((bearing_deg - old_bearing_deg) >= 180)
+        old_bearing_deg += 360;
+    while ((bearing_deg - old_bearing_deg) <= -180)
+        old_bearing_deg -= 360;
     bearing_deg += _error_node->getDoubleValue();
     bearing_deg =
         fgGetLowPass(old_bearing_deg, bearing_deg, dt * RESPONSIVENESS);