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