From 00002357b3c25a3d2e0f30a5f6472356e9c783a9 Mon Sep 17 00:00:00 2001
From: curt <curt>
Date: Mon, 15 Mar 2004 19:23:39 +0000
Subject: [PATCH] Roy Vegard Ovesen:

I've done som more work on the gps instrument.

- You can now input airport-, nav- or fix-ID to select a waypoint.
- You have to specify either "airport", "nav" or "fix" in the waypoint-type
  property (some fixes and navs have identical IDs).
- Formatted the time to waypoint output.
- Cleaned up and changed some propery names (wp-heading -> wp-bearing).

- I've also added a name member to the FGNav class so that the gps instrument
  can get the name of the nav.
- Changed the airport name parsing in simple.cxx.
---
 src/Airports/simple.cxx     |   9 +-
 src/Instrumentation/gps.cxx | 254 ++++++++++++++++++++++++++++++++++--
 src/Instrumentation/gps.hxx |  52 +++++++-
 src/Navaids/nav.hxx         |  11 +-
 4 files changed, 302 insertions(+), 24 deletions(-)

diff --git a/src/Airports/simple.cxx b/src/Airports/simple.cxx
index a0fd2aecf..ca3fd3995 100644
--- a/src/Airports/simple.cxx
+++ b/src/Airports/simple.cxx
@@ -50,9 +50,12 @@ operator >> ( istream& in, FGAirport& a )
     in >> junk >> a.id >> a.latitude >> a.longitude >> a.elevation
        >> a.code;
 
-    char name[256];             // should never be longer than this, right? :-)
-    in.getline( name, 256 );
-    a.name = name;
+    getline( in,a.name );
+
+    // Remove the space before the name
+    if ( a.name.substr(0,1) == " " ) {
+        a.name = a.name.erase(0,1);
+    }
 
     a.has_metar = false;
 
diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx
index e5c040bb3..08183fde9 100644
--- a/src/Instrumentation/gps.cxx
+++ b/src/Instrumentation/gps.cxx
@@ -4,19 +4,43 @@
 // This file is in the Public Domain and comes with no warranty.
 
 #include <simgear/compiler.h>
+
+#include <string.h>
+
+#include STL_STRING
+
+#include <Aircraft/aircraft.hxx>
+#include <FDM/flight.hxx>
+#include <Controls/controls.hxx>
+#include <Scenery/scenery.hxx>
+
 #include <simgear/constants.h>
+#include <simgear/sg_inlines.h>
+#include <simgear/debug/logstream.hxx>
 #include <simgear/math/sg_geodesy.hxx>
+#include <simgear/misc/sg_path.hxx>
+#include <simgear/route/route.hxx>
 
+#include <Airports/simple.hxx>
+#include <GUI/gui.h>
+#include <Main/fg_init.hxx>
+#include <Main/globals.hxx>
 #include <Main/fg_props.hxx>
+#include <Main/util.hxx>
+#include <Navaids/fixlist.hxx>
+#include <Navaids/navlist.hxx>
 
 #include "gps.hxx"
 
+SG_USING_STD(string);
+
 
 GPS::GPS ()
     : _last_valid(false),
       _last_longitude_deg(0),
       _last_latitude_deg(0),
-      _last_altitude_m(0)
+      _last_altitude_m(0),
+      _last_speed_kts(0)
 {
 }
 
@@ -33,20 +57,52 @@ GPS::init ()
     _magvar_node = fgGetNode("/environment/magnetic-variation-deg", true);
     _serviceable_node = fgGetNode("/instrumentation/gps/serviceable", true);
     _electrical_node = fgGetNode("/systems/electrical/outputs/gps", true);
+    _wp_longitude_node = 
+        fgGetNode("/instrumentation/gps/wp-longitude-deg", true);
+    _wp_latitude_node =
+        fgGetNode("/instrumentation/gps/wp-latitude-deg", true);
+    _wp_ID_node =
+        fgGetNode("/instrumentation/gps/wp-ID", true);
+    _wp_name_node =
+        fgGetNode("/instrumentation/gps/wp-name", true);
+    _wp_course_node = 
+        fgGetNode("/instrumentation/gps/course-deg", true);
+    _get_nearest_airport_node =
+      fgGetNode("/instrumentation/gps/get-nearest-airport", true);
+    _waypoint_type_node =
+      fgGetNode("/instrumentation/gps/waypoint-type", true);
 
     _raim_node = fgGetNode("/instrumentation/gps/raim", true);
     _indicated_longitude_node =
-        fgGetNode("/instrumentation/gps/indicated-longitude-deg", true);
+        fgGetNode("/instrumentation/gps/longitude-deg", true);
     _indicated_latitude_node =
-        fgGetNode("/instrumentation/gps/indicated-latitude-deg", true);
+        fgGetNode("/instrumentation/gps/latitude-deg", true);
     _indicated_altitude_node =
-        fgGetNode("/instrumentation/gps/indicated-altitude-ft", true);
+        fgGetNode("/instrumentation/gps/altitude-ft", true);
     _true_track_node =
-        fgGetNode("/instrumentation/gps/indicated-track-true-deg", true);
+        fgGetNode("/instrumentation/gps/track-true-deg", true);
     _magnetic_track_node =
-        fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true);
+        fgGetNode("/instrumentation/gps/track-magnetic-deg", true);
     _speed_node =
-        fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true);
+        fgGetNode("/instrumentation/gps/ground-speed-kt", true);
+    _wp_distance_node =
+        fgGetNode("/instrumentation/gps/wp-distance-nm", true);
+    _wp_ttw_node =
+        fgGetNode("/instrumentation/gps/TTW",true);
+    _wp_bearing_node =
+        fgGetNode("/instrumentation/gps/wp-bearing-deg", true);
+    _wp_actual_radial_node =
+        fgGetNode("/instrumentation/gps/radials/actual-deg", true);
+    _wp_course_deviation_node =
+        fgGetNode("/instrumentation/gps/course-deviation-deg", true);
+    _wp_course_error_nm_node =
+        fgGetNode("/instrumentation/gps/course-error-nm", true);
+    _wp_to_flag_node =
+        fgGetNode("/instrumentation/gps/to-flag", true);
+    _odometer_node =
+        fgGetNode("/instrumentation/gps/odometer", true);
+    _trip_odometer_node =
+      fgGetNode("/instrumentation/gps/trip-odometer", true);
 }
 
 void
@@ -59,6 +115,7 @@ GPS::update (double delta_time_sec)
         _last_longitude_deg = 0;
         _last_latitude_deg = 0;
         _last_altitude_m = 0;
+        _last_speed_kts = 0;
         _raim_node->setDoubleValue(false);
         _indicated_longitude_node->setDoubleValue(0);
         _indicated_latitude_node->setDoubleValue(0);
@@ -66,6 +123,14 @@ GPS::update (double delta_time_sec)
         _true_track_node->setDoubleValue(0);
         _magnetic_track_node->setDoubleValue(0);
         _speed_node->setDoubleValue(0);
+        _wp_distance_node->setDoubleValue(0);
+        _wp_bearing_node->setDoubleValue(0);
+        _wp_actual_radial_node->setDoubleValue(0);
+        _wp_longitude_node->setDoubleValue(0);
+        _wp_latitude_node->setDoubleValue(0);
+        _wp_course_node->setDoubleValue(0);
+        _odometer_node->setDoubleValue(0);
+        _trip_odometer_node->setDoubleValue(0);
         return;
     }
 
@@ -75,27 +140,188 @@ GPS::update (double delta_time_sec)
     double altitude_m = _altitude_node->getDoubleValue() * SG_FEET_TO_METER;
     double magvar_deg = _magvar_node->getDoubleValue();
 
+    double speed_kt;
+
     _raim_node->setBoolValue(true);
     _indicated_longitude_node->setDoubleValue(longitude_deg);
     _indicated_latitude_node->setDoubleValue(latitude_deg);
     _indicated_altitude_node->setDoubleValue(altitude_m * SG_METER_TO_FEET);
 
     if (_last_valid) {
-        double track1_deg, track2_deg, distance_m;
+        double track1_deg, track2_deg, distance_m, odometer;
         geo_inverse_wgs_84(altitude_m,
                            _last_latitude_deg, _last_longitude_deg,
                            latitude_deg, longitude_deg,
                            &track1_deg, &track2_deg, &distance_m);
-        double distance_nm = distance_m * SG_METER_TO_NM;
-        double speed_kt = ((distance_m * SG_METER_TO_NM) *
-                           ((1 / delta_time_sec) * 3600.0));
+        speed_kt = ((distance_m * SG_METER_TO_NM) *
+                    ((1 / delta_time_sec) * 3600.0));
         _true_track_node->setDoubleValue(track1_deg);
         _magnetic_track_node->setDoubleValue(track1_deg - magvar_deg);
+        speed_kt = fgGetLowPass(_last_speed_kts, speed_kt, delta_time_sec/20.0);
+        _last_speed_kts = speed_kt;
         _speed_node->setDoubleValue(speed_kt);
+
+        odometer = _odometer_node->getDoubleValue();
+        _odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
+        odometer = _trip_odometer_node->getDoubleValue();
+        _trip_odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
+
+        // Get waypoint position
+        double wp_longitude_deg = _wp_longitude_node->getDoubleValue();
+        double wp_latitude_deg = _wp_latitude_node->getDoubleValue();
+        double wp_course_deg = 
+          _wp_course_node->getDoubleValue();
+        double wp_distance, wp_bearing_deg, wp_actual_radial_deg,
+          wp_course_deviation_deg, wp_course_error_m, wp_TTW;
+        string wp_ID = _wp_ID_node->getStringValue();
+
+        // If the get-nearest-airport-node is true.
+        // Get the nearest airport, and set it as waypoint.
+        if (_get_nearest_airport_node->getBoolValue()) {
+          FGAirport a;
+          cout << "Airport found" << endl;
+          a = globals->get_airports()->search(longitude_deg, latitude_deg, false);
+          _wp_ID_node->setStringValue(a.id.c_str());
+          wp_longitude_deg = a.longitude;
+          wp_latitude_deg = a.latitude;
+          _wp_name_node->setStringValue(a.name.c_str());
+          _get_nearest_airport_node->setBoolValue(false);
+          _last_wp_ID = wp_ID = a.id.c_str();
+        }
+
+        // If the waypoint ID has changed, try to find the new ID
+        // in the airport-, fix-, nav-database.
+        if ( !(_last_wp_ID == wp_ID) ) {
+          string waypont_type =
+            _waypoint_type_node->getStringValue();
+          if (waypont_type == "airport") {
+            FGAirport a;
+            a = globals->get_airports()->search( wp_ID );
+            if ( a.id == wp_ID ) {
+              cout << "Airport found" << endl;
+              wp_longitude_deg = a.longitude;
+              wp_latitude_deg = a.latitude;
+              _wp_name_node->setStringValue(a.name.c_str());
+            }
+          }
+          else if (waypont_type == "nav") {
+            FGNav * n;
+            if ( (n = current_navlist->findByIdent(wp_ID.c_str(), 
+                                                      longitude_deg, 
+                                                      latitude_deg)) != NULL) {
+              cout << "Nav found" << endl;
+              wp_longitude_deg = n->get_lon();
+              wp_latitude_deg = n->get_lat();
+              _wp_name_node->setStringValue(n->get_name().c_str());
+            }
+          }
+          else if (waypont_type == "fix") {
+            FGFix f;
+            if ( current_fixlist->query(wp_ID, &f) ) {
+              cout << "Fix found" << endl;
+              wp_longitude_deg = f.get_lon();
+              wp_latitude_deg = f.get_lat();
+              _wp_name_node->setStringValue(wp_ID.c_str());
+            }
+          }
+          _last_wp_ID = wp_ID;
+        }
+
+        // Find the bearing and distance to the waypoint.
+        SGWayPoint wp(wp_longitude_deg, wp_latitude_deg, altitude_m);
+        wp.CourseAndDistance(longitude_deg, latitude_deg, altitude_m,
+                            &wp_bearing_deg, &wp_distance);
+        _wp_longitude_node->setDoubleValue(wp_longitude_deg);
+        _wp_latitude_node->setDoubleValue(wp_latitude_deg);
+        _wp_distance_node->setDoubleValue(wp_distance * SG_METER_TO_NM);
+        _wp_bearing_node->setDoubleValue(wp_bearing_deg);
+        
+        // Estimate time to waypoint.
+        // The estimation does not take track into consideration,
+        // so if you are going away from the waypoint the TTW will
+        // increase. Makes most sense when travelling directly towards
+        // the waypoint.
+        if (speed_kt > 0.0 && wp_distance > 0.0) {
+          wp_TTW = (wp_distance * SG_METER_TO_NM) / (speed_kt / 3600);
+        }
+        else {
+          wp_TTW = 0.0;
+        }
+        unsigned int wp_TTW_seconds = (int) (wp_TTW + 0.5);
+        if (wp_TTW_seconds < 356400) { // That's 99 hours
+          unsigned int wp_TTW_minutes = 0;
+          unsigned int wp_TTW_hours   = 0;
+          char wp_TTW_str[8];
+          while (wp_TTW_seconds >= 3600) {
+            wp_TTW_seconds -= 3600;
+            wp_TTW_hours++;
+          }
+          while (wp_TTW_seconds >= 60) {
+            wp_TTW_seconds -= 60;
+            wp_TTW_minutes++;
+          }
+          sprintf(wp_TTW_str, "%02d:%02d:%02d",
+            wp_TTW_hours, wp_TTW_minutes, wp_TTW_seconds);
+          _wp_ttw_node->setStringValue(wp_TTW_str);
+        }
+        else
+          _wp_ttw_node->setStringValue("--:--:--");
+
+        /*
+        // We are on the radial 180 degrees from the bearing to the waypoint.
+        wp_actual_radial_deg = wp_bearing - 180.0;
+        while (wp_actual_radial_deg < 0.0) {
+            wp_actual_radial_deg += 360.0; }
+        while (wp_actual_radial_deg >= 360.0) {
+            wp_actual_radial_deg -= 360.0; }
+        _wp_actual_radial_node->setDoubleValue(wp_actual_radial_deg);
+
+        // We want to be on the radial 180 degrees from the desired course.
+        wp_course_deg -= 180.0;
+        while (wp_course_deg < 0.0) {
+            wp_course_deg += 360.0; }
+        while (wp_course_deg >= 360.0) {
+            wp_course_deg -= 360.0;
+        }
+        */
+
+        // Course deviation is the diffenrence between the bearing
+        // and the course.
+        wp_course_deviation_deg = wp_bearing_deg -
+          wp_course_deg;
+        while (wp_course_deviation_deg < -180.0) {
+          wp_course_deviation_deg += 360.0; }
+        while (wp_course_deviation_deg > 180.0) {
+          wp_course_deviation_deg -= 360.0; }
+
+        // If the course deviation is less than 90 degrees to either side,
+        // our desired course is towards the waypoint.
+        // It does not matter if we are actually moving towards or from the waypoint.
+        if (fabs(wp_course_deviation_deg) < 90.0) {
+            _wp_to_flag_node->setBoolValue(true); }
+        // If it's more than 90 degrees the desired course is from the waypoint.
+        else if (fabs(wp_course_deviation_deg) > 90.0) {
+          _wp_to_flag_node->setBoolValue(false);
+          // When the course is away from the waypoint, 
+          // it makes sense to change the sign of the deviation.
+          wp_course_deviation_deg *= -1.0;
+          while (wp_course_deviation_deg < -90.0)
+            wp_course_deviation_deg += 180.0;
+          while (wp_course_deviation_deg >  90.0)
+            wp_course_deviation_deg -= 180.0; }
+
+        _wp_course_deviation_node->setDoubleValue(wp_course_deviation_deg);
+
+        // Cross track error.
+        wp_course_error_m = sin(wp_course_deviation_deg * SG_PI / 180.0) 
+          * (wp_distance);
+        _wp_course_error_nm_node->setDoubleValue(wp_course_error_m 
+                                                 * SG_METER_TO_NM);
+
     } else {
-        _true_track_node->setDoubleValue(0);
-        _magnetic_track_node->setDoubleValue(0);
-        _speed_node->setDoubleValue(0);
+        _true_track_node->setDoubleValue(0.0);
+        _magnetic_track_node->setDoubleValue(0.0);
+        _speed_node->setDoubleValue(0.0);
     }
 
     _last_valid = true;
diff --git a/src/Instrumentation/gps.hxx b/src/Instrumentation/gps.hxx
index 72578c61c..e7bb25268 100644
--- a/src/Instrumentation/gps.hxx
+++ b/src/Instrumentation/gps.hxx
@@ -26,15 +26,34 @@
  * /environment/magnetic-variation-deg
  * /systems/electrical/outputs/gps
  * /instrumentation/gps/serviceable
+ * 
+ * /instrumentation/gps/wp-longitude-deg
+ * /instrumentation/gps/wp-latitude-deg
+ * /instrumentation/gps/wp-ID
+ * /instrumentation/gps/wp-name
+ * /instrumentation/gps/course-deg
+ * /instrumentation/gps/get-nearest-airport
+ * /instrumentation/gps/waypoint-type
  *
  * Output properties:
  *
- * /instrumentation/gps/indicated-longitude-deg
- * /instrumentation/gps/indicated-latitude-deg
- * /instrumentation/gps/indicated-altitude-ft
- * /instrumentation/gps/indicated-track-true-deg
- * /instrumentation/gps/indicated-track-magnetic-deg
- * /instrumentation/gps/indicated-ground-speed-kt
+ * /instrumentation/gps/longitude-deg
+ * /instrumentation/gps/latitude-deg
+ * /instrumentation/gps/altitude-ft
+ * /instrumentation/gps/track-true-deg
+ * /instrumentation/gps/track-magnetic-deg
+ * /instrumentation/gps/ground-speed-kt
+ *
+ * /instrumentation/gps/wp-distance-m
+ * /instrumentation/gps/wp-bearing-deg
+ * /instrumentation/gps/TTW
+ * /instrumentation/gps/wp-bearing-deg
+ * /instrumentation/gps/radials/actual-deg
+ * /instrumentation/gps/course-deviation-deg
+ * /instrumentation/gps/course-error-nm
+ * /instrumentation/gps/to-flag
+ * /instrumentation/gps/odometer
+ * /instrumentation/gps/trip-odometer
  */
 class GPS : public SGSubsystem
 {
@@ -58,6 +77,13 @@ private:
     SGPropertyNode_ptr _magvar_node;
     SGPropertyNode_ptr _serviceable_node;
     SGPropertyNode_ptr _electrical_node;
+    SGPropertyNode_ptr _wp_longitude_node;
+    SGPropertyNode_ptr _wp_latitude_node;
+    SGPropertyNode_ptr _wp_ID_node;
+    SGPropertyNode_ptr _wp_name_node;
+    SGPropertyNode_ptr _wp_course_node;
+    SGPropertyNode_ptr _get_nearest_airport_node;
+    SGPropertyNode_ptr _waypoint_type_node;
 
     SGPropertyNode_ptr _raim_node;
     SGPropertyNode_ptr _indicated_longitude_node;
@@ -66,11 +92,25 @@ private:
     SGPropertyNode_ptr _true_track_node;
     SGPropertyNode_ptr _magnetic_track_node;
     SGPropertyNode_ptr _speed_node;
+    SGPropertyNode_ptr _wp_distance_node;
+    SGPropertyNode_ptr _wp_ttw_node;
+    SGPropertyNode_ptr _wp_bearing_node;
+    SGPropertyNode_ptr _wp_actual_radial_node;
+    SGPropertyNode_ptr _wp_course_deviation_node;
+    SGPropertyNode_ptr _wp_course_error_nm_node;
+    SGPropertyNode_ptr _wp_to_flag_node;
+    SGPropertyNode_ptr _odometer_node;
+    SGPropertyNode_ptr _trip_odometer_node;
 
     bool _last_valid;
     double _last_longitude_deg;
     double _last_latitude_deg;
     double _last_altitude_m;
+    double _last_speed_kts;
+
+    double _wp_latitude;
+    double _wp_longitude;
+    string _last_wp_ID;
 
 };
 
diff --git a/src/Navaids/nav.hxx b/src/Navaids/nav.hxx
index 4f27e6216..074670651 100644
--- a/src/Navaids/nav.hxx
+++ b/src/Navaids/nav.hxx
@@ -54,6 +54,7 @@ class FGNav {
     bool has_dme;
     string ident;		// to avoid a core dump with corrupt data
     double magvar;		// magvar from true north (negative = W)
+    string name;
 
     // for failure modeling
     string trans_ident;		// transmitted ident
@@ -78,6 +79,7 @@ public:
     inline const char *get_ident() { return ident.c_str(); }
     inline string get_trans_ident() { return trans_ident; }
     inline double get_magvar () const { return magvar; }
+    inline string get_name () { return name; }
 
     friend istream& operator>> ( istream&, FGNav& );
 };
@@ -94,6 +96,7 @@ FGNav::FGNav(void) :
     has_dme(false),
     ident(""),
     magvar(0.0),
+    name(""),
     trans_ident(""),
     nav_failed(false),
     dme_failed(false)
@@ -124,6 +127,12 @@ operator >> ( istream& in, FGNav& n )
     in >> n.lat >> n.lon >> n.elev_ft >> f >> n.range 
        >> c >> n.ident >> magvar_s;
 
+    getline(in,n.name);
+    // Remove the space before the name
+    if ( n.name.substr(0,1) == " " ) {
+        n.name = n.name.erase(0,1);
+    }
+
     n.freq = (int)(f*100.0 + 0.5);
     if ( c == 'Y' ) {
 	n.has_dme = true;
@@ -176,7 +185,7 @@ operator >> ( istream& in, FGNav& n )
     n.trans_ident = n.ident;
     n.nav_failed = n.dme_failed = false;
 
-    return in >> skipeol;
+    return in;
 }
 
 
-- 
2.39.5