From 4a41f966317e6e536f0980c499bb836626de68eb Mon Sep 17 00:00:00 2001
From: curt <curt>
Date: Fri, 16 Apr 2004 22:12:26 +0000
Subject: [PATCH] Roy Vegard Ovesen:

I've added a tracking bug to the gps. This is of course very similar to a
heading bug for a DG. I don't know if this is the common name, but I feel
that for a gps the name tracking bug is more accurate than heading bug. A
true bug error and a magnetic bug error is calculated and shifted into the
-180 to 180 range so that they can be used by autopilots.

I've also fixed a property name that crept in when I had to change back to
indicated-***. Back then I accidentally changed the desired course name to
"indicated-course". The property that is supposed to be the input for the
desired course should naturally be named something like "desired-course", and
definitely _not_ "indicated-course". If this name change breaks anything it
should be fixed in the other end.

I've also commented out a lot of #includes that I don't think is needed. I'm
on Suse 9.0 now, and it builds fine here, but this might be a problem for
different platforms    I guess we have to cross our fingers.
---
 src/Instrumentation/gps.cxx | 76 ++++++++++++++++++++++++++-----------
 src/Instrumentation/gps.hxx | 17 +++++++--
 2 files changed, 68 insertions(+), 25 deletions(-)

diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx
index 3f65e3e94..2322fecb8 100644
--- a/src/Instrumentation/gps.cxx
+++ b/src/Instrumentation/gps.cxx
@@ -9,24 +9,10 @@
 
 #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>
@@ -70,11 +56,13 @@ GPS::init ()
     _wp_name_node =
         fgGetNode("/instrumentation/gps/wp-name", true);
     _wp_course_node = 
-        fgGetNode("/instrumentation/gps/indicated-course-deg", true);
+        fgGetNode("/instrumentation/gps/desired-course-deg", true);
     _get_nearest_airport_node =
-      fgGetNode("/instrumentation/gps/get-nearest-airport", true);
+	fgGetNode("/instrumentation/gps/get-nearest-airport", true);
     _waypoint_type_node =
-      fgGetNode("/instrumentation/gps/waypoint-type", true);
+	fgGetNode("/instrumentation/gps/waypoint-type", true);
+    _tracking_bug_node =
+	fgGetNode("/instrumentation/gps/tracking-bug", true);
 
     _raim_node = fgGetNode("/instrumentation/gps/raim", true);
     _indicated_longitude_node =
@@ -95,6 +83,8 @@ GPS::init ()
         fgGetNode("/instrumentation/gps/TTW",true);
     _wp_bearing_node =
         fgGetNode("/instrumentation/gps/wp-bearing-deg", true);
+    _wp_mag_bearing_node =
+        fgGetNode("/instrumentation/gps/wp-bearing-mag-deg", true);
     _wp_course_deviation_node =
         fgGetNode("/instrumentation/gps/course-deviation-deg", true);
     _wp_course_error_nm_node =
@@ -104,7 +94,12 @@ GPS::init ()
     _odometer_node =
         fgGetNode("/instrumentation/gps/odometer", true);
     _trip_odometer_node =
-      fgGetNode("/instrumentation/gps/trip-odometer", true);
+        fgGetNode("/instrumentation/gps/trip-odometer", true);
+    _true_bug_error_node =
+	fgGetNode("/instrumentation/gps/true-bug-error-deg", true);
+    _magnetic_bug_error_node =
+        fgGetNode("/instrumentation/gps/magnetic-bug-error-deg", true);
+	
 }
 
 void
@@ -132,10 +127,14 @@ GPS::update (double delta_time_sec)
         _wp_course_node->setDoubleValue(0);
         _odometer_node->setDoubleValue(0);
         _trip_odometer_node->setDoubleValue(0);
+        _tracking_bug_node->setDoubleValue(0);
+        _true_bug_error_node->setDoubleValue(0);
+        _magnetic_bug_error_node->setDoubleValue(0);
         return;
     }
 
-                                // Get the aircraft position
+    // Get the aircraft position
+    // TODO: Add noise and other errors.
     double longitude_deg = _longitude_node->getDoubleValue();
     double latitude_deg = _latitude_node->getDoubleValue();
     double altitude_m = _altitude_node->getDoubleValue() * SG_FEET_TO_METER;
@@ -149,7 +148,7 @@ GPS::update (double delta_time_sec)
     _indicated_altitude_node->setDoubleValue(altitude_m * SG_METER_TO_FEET);
 
     if (_last_valid) {
-        double track1_deg, track2_deg, distance_m, odometer;
+        double track1_deg, track2_deg, distance_m, odometer, mag_track_bearing;
         geo_inverse_wgs_84(altitude_m,
                            _last_latitude_deg, _last_longitude_deg,
                            latitude_deg, longitude_deg,
@@ -157,7 +156,8 @@ GPS::update (double delta_time_sec)
         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);
+	mag_track_bearing = deg360(track1_deg - magvar_deg);
+        _magnetic_track_node->setDoubleValue(mag_track_bearing);
         speed_kt = fgGetLowPass(_last_speed_kts, speed_kt, delta_time_sec/20.0);
         _last_speed_kts = speed_kt;
         _speed_node->setDoubleValue(speed_kt);
@@ -174,7 +174,6 @@ GPS::update (double delta_time_sec)
           _wp_course_node->getDoubleValue();
         double wp_distance, wp_bearing_deg, wp_course_deviation_deg,
             wp_course_error_m, wp_TTW;
-        // double wp_actual_radial_deg;
         string wp_ID = _wp_ID_node->getStringValue();
 
         // If the get-nearest-airport-node is true.
@@ -237,6 +236,7 @@ GPS::update (double delta_time_sec)
         _wp_latitude_node->setDoubleValue(wp_latitude_deg);
         _wp_distance_node->setDoubleValue(wp_distance * SG_METER_TO_NM);
         _wp_bearing_node->setDoubleValue(wp_bearing_deg);
+	_wp_mag_bearing_node->setDoubleValue(deg360(wp_bearing_deg - magvar_deg));
         
         // Estimate time to waypoint.
         // The estimation does not take track into consideration,
@@ -302,6 +302,25 @@ GPS::update (double delta_time_sec)
         _wp_course_error_nm_node->setDoubleValue(wp_course_error_m 
                                                  * SG_METER_TO_NM);
 
+        // Tracking bug.
+	double tracking_bug = _tracking_bug_node->getDoubleValue();
+	double true_bug_error = tracking_bug - track1_deg;
+	double magnetic_bug_error = tracking_bug - mag_track_bearing;
+
+	// Get the errors into the (-180,180) range.
+	while (true_bug_error < -180.0)
+            true_bug_error += 360.0;
+        while (true_bug_error >  180.0)
+            true_bug_error -= 360.0;
+
+        while (magnetic_bug_error < -180.0)
+            magnetic_bug_error += 360.0;
+        while (magnetic_bug_error >  180.0)
+            magnetic_bug_error -= 360.0;
+
+	_true_bug_error_node->setDoubleValue(true_bug_error);
+	_magnetic_bug_error_node->setDoubleValue(magnetic_bug_error);
+
     } else {
         _true_track_node->setDoubleValue(0.0);
         _magnetic_track_node->setDoubleValue(0.0);
@@ -314,4 +333,17 @@ GPS::update (double delta_time_sec)
     _last_altitude_m = altitude_m;
 }
 
+double GPS::deg360 (double deg)
+{
+    while (deg <= 0.0) {
+	deg += 360.0; }
+    while (deg > 360.0) {
+	deg -= 360.0; }
+
+    return deg;
+}
+
+    
+
+
 // end of gps.cxx
diff --git a/src/Instrumentation/gps.hxx b/src/Instrumentation/gps.hxx
index 8d19afd18..5283b916e 100644
--- a/src/Instrumentation/gps.hxx
+++ b/src/Instrumentation/gps.hxx
@@ -31,9 +31,10 @@
  * /instrumentation/gps/wp-latitude-deg
  * /instrumentation/gps/wp-ID
  * /instrumentation/gps/wp-name
- * /instrumentation/gps/course-deg
+ * /instrumentation/gps/desired-course-deg
  * /instrumentation/gps/get-nearest-airport
  * /instrumentation/gps/waypoint-type
+ * /instrumentation/gps/tracking-bug
  *
  * Output properties:
  *
@@ -44,15 +45,17 @@
  * /instrumentation/gps/indicated-track-magnetic-deg
  * /instrumentation/gps/indicated-ground-speed-kt
  *
- * /instrumentation/gps/wp-distance-m
+ * /instrumentation/gps/wp-distance-nm
  * /instrumentation/gps/wp-bearing-deg
+ * /instrumentation/gps/wp-bearing-mag-deg
  * /instrumentation/gps/TTW
- * /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
+ * /instrumentation/gps/true-bug-error-deg
+ * /instrumentation/gps/magnetic-bug-error-deg
  */
 class GPS : public SGSubsystem
 {
@@ -70,6 +73,10 @@ private:
     void search (double frequency, double longitude_rad,
                  double latitude_rad, double altitude_m);
 
+    double deg360 (double deg);
+    double deg180 (double deg);
+    double deg90 (double deg);
+
     SGPropertyNode_ptr _longitude_node;
     SGPropertyNode_ptr _latitude_node;
     SGPropertyNode_ptr _altitude_node;
@@ -83,6 +90,7 @@ private:
     SGPropertyNode_ptr _wp_course_node;
     SGPropertyNode_ptr _get_nearest_airport_node;
     SGPropertyNode_ptr _waypoint_type_node;
+    SGPropertyNode_ptr _tracking_bug_node;
 
     SGPropertyNode_ptr _raim_node;
     SGPropertyNode_ptr _indicated_longitude_node;
@@ -94,11 +102,14 @@ private:
     SGPropertyNode_ptr _wp_distance_node;
     SGPropertyNode_ptr _wp_ttw_node;
     SGPropertyNode_ptr _wp_bearing_node;
+    SGPropertyNode_ptr _wp_mag_bearing_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;
+    SGPropertyNode_ptr _true_bug_error_node;
+    SGPropertyNode_ptr _magnetic_bug_error_node;
 
     bool _last_valid;
     double _last_longitude_deg;