src/Instrumentation/gps.cxx

   1 // gps.cxx - distance-measuring equipment.
   2 // Written by David Megginson, started 2003.
   3 //
   4 // This file is in the Public Domain and comes with no warranty.
   5
   6 #ifdef HAVE_CONFIG_H
   7 #  include <config.h>
   8 #endif
   9
  10 #include <simgear/compiler.h>
  11
  12 #include <string.h>
  13
  14 #include STL_STRING
  15
  16 #include <Aircraft/aircraft.hxx>
  17 #include <FDM/flight.hxx>
  18 #include <Controls/controls.hxx>
  19 #include <Scenery/scenery.hxx>
  20
  21 #include <simgear/constants.h>
  22 #include <simgear/sg_inlines.h>
  23 #include <simgear/debug/logstream.hxx>
  24 #include <simgear/math/sg_geodesy.hxx>
  25 #include <simgear/misc/sg_path.hxx>
  26 #include <simgear/route/route.hxx>
  27
  28 #include <Airports/simple.hxx>
  29 #include <GUI/gui.h>
  30 #include <Main/fg_init.hxx>
  31 #include <Main/globals.hxx>
  32 #include <Main/fg_props.hxx>
  33 #include <Main/util.hxx>
  34 #include <Navaids/fixlist.hxx>
  35 #include <Navaids/navlist.hxx>
  36
  37 #include "gps.hxx"
  38
  39 SG_USING_STD(string);
  40
  41
  42 GPS::GPS ()
  43     : _last_valid(false),
  44       _last_longitude_deg(0),
  45       _last_latitude_deg(0),
  46       _last_altitude_m(0),
  47       _last_speed_kts(0)
  48 {
  49 }
  50
  51 GPS::~GPS ()
  52 {
  53 }
  54
  55 void
  56 GPS::init ()
  57 {
  58     _longitude_node = fgGetNode("/position/longitude-deg", true);
  59     _latitude_node = fgGetNode("/position/latitude-deg", true);
  60     _altitude_node = fgGetNode("/position/altitude-ft", true);
  61     _magvar_node = fgGetNode("/environment/magnetic-variation-deg", true);
  62     _serviceable_node = fgGetNode("/instrumentation/gps/serviceable", true);
  63     _electrical_node = fgGetNode("/systems/electrical/outputs/gps", true);
  64     _wp_longitude_node =
  65         fgGetNode("/instrumentation/gps/wp-longitude-deg", true);
  66     _wp_latitude_node =
  67         fgGetNode("/instrumentation/gps/wp-latitude-deg", true);
  68     _wp_ID_node =
  69         fgGetNode("/instrumentation/gps/wp-ID", true);
  70     _wp_name_node =
  71         fgGetNode("/instrumentation/gps/wp-name", true);
  72     _wp_course_node =
  73         fgGetNode("/instrumentation/gps/course-deg", true);
  74     _get_nearest_airport_node =
  75       fgGetNode("/instrumentation/gps/get-nearest-airport", true);
  76     _waypoint_type_node =
  77       fgGetNode("/instrumentation/gps/waypoint-type", true);
  78
  79     _raim_node = fgGetNode("/instrumentation/gps/raim", true);
  80     _indicated_longitude_node =
  81         fgGetNode("/instrumentation/gps/longitude-deg", true);
  82     _indicated_latitude_node =
  83         fgGetNode("/instrumentation/gps/latitude-deg", true);
  84     _indicated_altitude_node =
  85         fgGetNode("/instrumentation/gps/altitude-ft", true);
  86     _true_track_node =
  87         fgGetNode("/instrumentation/gps/track-true-deg", true);
  88     _magnetic_track_node =
  89         fgGetNode("/instrumentation/gps/track-magnetic-deg", true);
  90     _speed_node =
  91         fgGetNode("/instrumentation/gps/ground-speed-kt", true);
  92     _wp_distance_node =
  93         fgGetNode("/instrumentation/gps/wp-distance-nm", true);
  94     _wp_ttw_node =
  95         fgGetNode("/instrumentation/gps/TTW",true);
  96     _wp_bearing_node =
  97         fgGetNode("/instrumentation/gps/wp-bearing-deg", true);
  98     _wp_actual_radial_node =
  99         fgGetNode("/instrumentation/gps/radials/actual-deg", true);
 100     _wp_course_deviation_node =
 101         fgGetNode("/instrumentation/gps/course-deviation-deg", true);
 102     _wp_course_error_nm_node =
 103         fgGetNode("/instrumentation/gps/course-error-nm", true);
 104     _wp_to_flag_node =
 105         fgGetNode("/instrumentation/gps/to-flag", true);
 106     _odometer_node =
 107         fgGetNode("/instrumentation/gps/odometer", true);
 108     _trip_odometer_node =
 109       fgGetNode("/instrumentation/gps/trip-odometer", true);
 110 }
 111
 112 void
 113 GPS::update (double delta_time_sec)
 114 {
 115                                 // If it's off, don't bother.
 116     if (!_serviceable_node->getBoolValue() ||
 117         !_electrical_node->getBoolValue()) {
 118         _last_valid = false;
 119         _last_longitude_deg = 0;
 120         _last_latitude_deg = 0;
 121         _last_altitude_m = 0;
 122         _last_speed_kts = 0;
 123         _raim_node->setDoubleValue(false);
 124         _indicated_longitude_node->setDoubleValue(0);
 125         _indicated_latitude_node->setDoubleValue(0);
 126         _indicated_altitude_node->setDoubleValue(0);
 127         _true_track_node->setDoubleValue(0);
 128         _magnetic_track_node->setDoubleValue(0);
 129         _speed_node->setDoubleValue(0);
 130         _wp_distance_node->setDoubleValue(0);
 131         _wp_bearing_node->setDoubleValue(0);
 132         _wp_actual_radial_node->setDoubleValue(0);
 133         _wp_longitude_node->setDoubleValue(0);
 134         _wp_latitude_node->setDoubleValue(0);
 135         _wp_course_node->setDoubleValue(0);
 136         _odometer_node->setDoubleValue(0);
 137         _trip_odometer_node->setDoubleValue(0);
 138         return;
 139     }
 140
 141                                 // Get the aircraft position
 142     double longitude_deg = _longitude_node->getDoubleValue();
 143     double latitude_deg = _latitude_node->getDoubleValue();
 144     double altitude_m = _altitude_node->getDoubleValue() * SG_FEET_TO_METER;
 145     double magvar_deg = _magvar_node->getDoubleValue();
 146
 147     double speed_kt;
 148
 149     _raim_node->setBoolValue(true);
 150     _indicated_longitude_node->setDoubleValue(longitude_deg);
 151     _indicated_latitude_node->setDoubleValue(latitude_deg);
 152     _indicated_altitude_node->setDoubleValue(altitude_m * SG_METER_TO_FEET);
 153
 154     if (_last_valid) {
 155         double track1_deg, track2_deg, distance_m, odometer;
 156         geo_inverse_wgs_84(altitude_m,
 157                            _last_latitude_deg, _last_longitude_deg,
 158                            latitude_deg, longitude_deg,
 159                            &track1_deg, &track2_deg, &distance_m);
 160         speed_kt = ((distance_m * SG_METER_TO_NM) *
 161                     ((1 / delta_time_sec) * 3600.0));
 162         _true_track_node->setDoubleValue(track1_deg);
 163         _magnetic_track_node->setDoubleValue(track1_deg - magvar_deg);
 164         speed_kt = fgGetLowPass(_last_speed_kts, speed_kt, delta_time_sec/20.0);
 165         _last_speed_kts = speed_kt;
 166         _speed_node->setDoubleValue(speed_kt);
 167
 168         odometer = _odometer_node->getDoubleValue();
 169         _odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
 170         odometer = _trip_odometer_node->getDoubleValue();
 171         _trip_odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
 172
 173         // Get waypoint position
 174         double wp_longitude_deg = _wp_longitude_node->getDoubleValue();
 175         double wp_latitude_deg = _wp_latitude_node->getDoubleValue();
 176         double wp_course_deg =
 177           _wp_course_node->getDoubleValue();
 178         double wp_distance, wp_bearing_deg, wp_actual_radial_deg,
 179           wp_course_deviation_deg, wp_course_error_m, wp_TTW;
 180         string wp_ID = _wp_ID_node->getStringValue();
 181
 182         // If the get-nearest-airport-node is true.
 183         // Get the nearest airport, and set it as waypoint.
 184         if (_get_nearest_airport_node->getBoolValue()) {
 185           FGAirport a;
 186           cout << "Airport found" << endl;
 187           a = globals->get_airports()->search(longitude_deg, latitude_deg, false);
 188           _wp_ID_node->setStringValue(a.id.c_str());
 189           wp_longitude_deg = a.longitude;
 190           wp_latitude_deg = a.latitude;
 191           _wp_name_node->setStringValue(a.name.c_str());
 192           _get_nearest_airport_node->setBoolValue(false);
 193           _last_wp_ID = wp_ID = a.id.c_str();
 194         }
 195
 196         // If the waypoint ID has changed, try to find the new ID
 197         // in the airport-, fix-, nav-database.
 198         if ( !(_last_wp_ID == wp_ID) ) {
 199           string waypont_type =
 200             _waypoint_type_node->getStringValue();
 201           if (waypont_type == "airport") {
 202             FGAirport a;
 203             a = globals->get_airports()->search( wp_ID );
 204             if ( a.id == wp_ID ) {
 205               cout << "Airport found" << endl;
 206               wp_longitude_deg = a.longitude;
 207               wp_latitude_deg = a.latitude;
 208               _wp_name_node->setStringValue(a.name.c_str());
 209             }
 210           }
 211           else if (waypont_type == "nav") {
 212             FGNav * n;
 213             if ( (n = current_navlist->findByIdent(wp_ID.c_str(),
 214                                                       longitude_deg,
 215                                                       latitude_deg)) != NULL) {
 216               cout << "Nav found" << endl;
 217               wp_longitude_deg = n->get_lon();
 218               wp_latitude_deg = n->get_lat();
 219               _wp_name_node->setStringValue(n->get_name().c_str());
 220             }
 221           }
 222           else if (waypont_type == "fix") {
 223             FGFix f;
 224             if ( current_fixlist->query(wp_ID, &f) ) {
 225               cout << "Fix found" << endl;
 226               wp_longitude_deg = f.get_lon();
 227               wp_latitude_deg = f.get_lat();
 228               _wp_name_node->setStringValue(wp_ID.c_str());
 229             }
 230           }
 231           _last_wp_ID = wp_ID;
 232         }
 233
 234         // Find the bearing and distance to the waypoint.
 235         SGWayPoint wp(wp_longitude_deg, wp_latitude_deg, altitude_m);
 236         wp.CourseAndDistance(longitude_deg, latitude_deg, altitude_m,
 237                             &wp_bearing_deg, &wp_distance);
 238         _wp_longitude_node->setDoubleValue(wp_longitude_deg);
 239         _wp_latitude_node->setDoubleValue(wp_latitude_deg);
 240         _wp_distance_node->setDoubleValue(wp_distance * SG_METER_TO_NM);
 241         _wp_bearing_node->setDoubleValue(wp_bearing_deg);
 242
 243         // Estimate time to waypoint.
 244         // The estimation does not take track into consideration,
 245         // so if you are going away from the waypoint the TTW will
 246         // increase. Makes most sense when travelling directly towards
 247         // the waypoint.
 248         if (speed_kt > 0.0 && wp_distance > 0.0) {
 249           wp_TTW = (wp_distance * SG_METER_TO_NM) / (speed_kt / 3600);
 250         }
 251         else {
 252           wp_TTW = 0.0;
 253         }
 254         unsigned int wp_TTW_seconds = (int) (wp_TTW + 0.5);
 255         if (wp_TTW_seconds < 356400) { // That's 99 hours
 256           unsigned int wp_TTW_minutes = 0;
 257           unsigned int wp_TTW_hours   = 0;
 258           char wp_TTW_str[8];
 259           while (wp_TTW_seconds >= 3600) {
 260             wp_TTW_seconds -= 3600;
 261             wp_TTW_hours++;
 262           }
 263           while (wp_TTW_seconds >= 60) {
 264             wp_TTW_seconds -= 60;
 265             wp_TTW_minutes++;
 266           }
 267           sprintf(wp_TTW_str, "%02d:%02d:%02d",
 268             wp_TTW_hours, wp_TTW_minutes, wp_TTW_seconds);
 269           _wp_ttw_node->setStringValue(wp_TTW_str);
 270         }
 271         else
 272           _wp_ttw_node->setStringValue("--:--:--");
 273
 274         /*
 275         // We are on the radial 180 degrees from the bearing to the waypoint.
 276         wp_actual_radial_deg = wp_bearing - 180.0;
 277         while (wp_actual_radial_deg < 0.0) {
 278             wp_actual_radial_deg += 360.0; }
 279         while (wp_actual_radial_deg >= 360.0) {
 280             wp_actual_radial_deg -= 360.0; }
 281         _wp_actual_radial_node->setDoubleValue(wp_actual_radial_deg);
 282
 283         // We want to be on the radial 180 degrees from the desired course.
 284         wp_course_deg -= 180.0;
 285         while (wp_course_deg < 0.0) {
 286             wp_course_deg += 360.0; }
 287         while (wp_course_deg >= 360.0) {
 288             wp_course_deg -= 360.0;
 289         }
 290         */
 291
 292         // Course deviation is the diffenrence between the bearing
 293         // and the course.
 294         wp_course_deviation_deg = wp_bearing_deg -
 295           wp_course_deg;
 296         while (wp_course_deviation_deg < -180.0) {
 297           wp_course_deviation_deg += 360.0; }
 298         while (wp_course_deviation_deg > 180.0) {
 299           wp_course_deviation_deg -= 360.0; }
 300
 301         // If the course deviation is less than 90 degrees to either side,
 302         // our desired course is towards the waypoint.
 303         // It does not matter if we are actually moving towards or from the waypoint.
 304         if (fabs(wp_course_deviation_deg) < 90.0) {
 305             _wp_to_flag_node->setBoolValue(true); }
 306         // If it's more than 90 degrees the desired course is from the waypoint.
 307         else if (fabs(wp_course_deviation_deg) > 90.0) {
 308           _wp_to_flag_node->setBoolValue(false);
 309           // When the course is away from the waypoint,
 310           // it makes sense to change the sign of the deviation.
 311           wp_course_deviation_deg *= -1.0;
 312           while (wp_course_deviation_deg < -90.0)
 313             wp_course_deviation_deg += 180.0;
 314           while (wp_course_deviation_deg >  90.0)
 315             wp_course_deviation_deg -= 180.0; }
 316
 317         _wp_course_deviation_node->setDoubleValue(wp_course_deviation_deg);
 318
 319         // Cross track error.
 320         wp_course_error_m = sin(wp_course_deviation_deg * SG_PI / 180.0)
 321           * (wp_distance);
 322         _wp_course_error_nm_node->setDoubleValue(wp_course_error_m
 323                                                  * SG_METER_TO_NM);
 324
 325     } else {
 326         _true_track_node->setDoubleValue(0.0);
 327         _magnetic_track_node->setDoubleValue(0.0);
 328         _speed_node->setDoubleValue(0.0);
 329     }
 330
 331     _last_valid = true;
 332     _last_longitude_deg = longitude_deg;
 333     _last_latitude_deg = latitude_deg;
 334     _last_altitude_m = altitude_m;
 335 }
 336
 337 // end of gps.cxx