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 #include <Aircraft/aircraft.hxx>
  12
  13 #include <simgear/route/route.hxx>
  14 #include <simgear/math/sg_random.h>
  15 #include <Airports/simple.hxx>
  16
  17 #include <Main/fg_init.hxx>
  18 #include <Main/globals.hxx>
  19 #include <Main/fg_props.hxx>
  20 #include <Main/util.hxx>
  21 #include <Navaids/fixlist.hxx>
  22 #include <Navaids/navlist.hxx>
  23
  24 #include "gps.hxx"
  25
  26 SG_USING_STD(string);
  27
  28
  29 GPS::GPS ()
  30     : _last_valid(false),
  31       _last_longitude_deg(0),
  32       _last_latitude_deg(0),
  33       _last_altitude_m(0),
  34       _last_speed_kts(0)
  35 {
  36 }
  37
  38 GPS::~GPS ()
  39 {
  40 }
  41
  42 void
  43 GPS::init ()
  44 {
  45     _longitude_node = fgGetNode("/position/longitude-deg", true);
  46     _latitude_node = fgGetNode("/position/latitude-deg", true);
  47     _altitude_node = fgGetNode("/position/altitude-ft", true);
  48     _magvar_node = fgGetNode("/environment/magnetic-variation-deg", true);
  49     _serviceable_node = fgGetNode("/instrumentation/gps/serviceable", true);
  50     _electrical_node = fgGetNode("/systems/electrical/outputs/gps", true);
  51     _wp0_longitude_node =
  52         fgGetNode("/instrumentation/gps/wp/wp[0]/longitude-deg", true);
  53     _wp0_latitude_node =
  54         fgGetNode("/instrumentation/gps/wp/wp[0]/latitude-deg", true);
  55     _wp0_altitude_node =
  56         fgGetNode("/instrumentation/gps/wp/wp[0]/altitude-ft", true);
  57     _wp0_ID_node =
  58         fgGetNode("/instrumentation/gps/wp/wp[0]/ID", true);
  59     _wp0_name_node =
  60         fgGetNode("/instrumentation/gps/wp/wp[0]/name", true);
  61     _wp0_course_node =
  62         fgGetNode("/instrumentation/gps/wp/wp[0]/desired-course-deg", true);
  63     _wp0_waypoint_type_node =
  64         fgGetNode("/instrumentation/gps/wp/wp[0]/waypoint-type", true);
  65     _wp1_longitude_node =
  66         fgGetNode("/instrumentation/gps/wp/wp[1]/longitude-deg", true);
  67     _wp1_latitude_node =
  68         fgGetNode("/instrumentation/gps/wp/wp[1]/latitude-deg", true);
  69     _wp1_altitude_node =
  70         fgGetNode("/instrumentation/gps/wp/wp[1]/altitude-ft", true);
  71     _wp1_ID_node =
  72         fgGetNode("/instrumentation/gps/wp/wp[1]/ID", true);
  73     _wp1_name_node =
  74         fgGetNode("/instrumentation/gps/wp/wp[1]/name", true);
  75     _wp1_course_node =
  76         fgGetNode("/instrumentation/gps/wp/wp[1]/desired-course-deg", true);
  77     _wp1_waypoint_type_node =
  78         fgGetNode("/instrumentation/gps/wp/wp[1]/waypoint-type", true);
  79     _get_nearest_airport_node =
  80         fgGetNode("/instrumentation/gps/wp/wp[1]/get-nearest-airport", true);
  81     _tracking_bug_node =
  82         fgGetNode("/instrumentation/gps/tracking-bug", true);
  83
  84     _raim_node = fgGetNode("/instrumentation/gps/raim", true);
  85     _indicated_longitude_node =
  86         fgGetNode("/instrumentation/gps/indicated-longitude-deg", true);
  87     _indicated_latitude_node =
  88         fgGetNode("/instrumentation/gps/indicated-latitude-deg", true);
  89     _indicated_altitude_node =
  90         fgGetNode("/instrumentation/gps/indicated-altitude-ft", true);
  91     _indicated_vertical_speed_node =
  92         fgGetNode("/instrumentation/gps/indicated-vertical-speed", true);
  93     _true_track_node =
  94         fgGetNode("/instrumentation/gps/indicated-track-true-deg", true);
  95     _magnetic_track_node =
  96         fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true);
  97     _speed_node =
  98         fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true);
  99     _wp0_distance_node =
 100         fgGetNode("/instrumentation/gps/wp/wp[0]/distance-nm", true);
 101     _wp0_ttw_node =
 102         fgGetNode("/instrumentation/gps/wp/wp[0]/TTW",true);
 103     _wp0_bearing_node =
 104         fgGetNode("/instrumentation/gps/wp/wp[0]/bearing-true-deg", true);
 105     _wp0_mag_bearing_node =
 106         fgGetNode("/instrumentation/gps/wp/wp[0]/bearing-mag-deg", true);
 107     _wp0_course_deviation_node =
 108         fgGetNode("/instrumentation/gps/wp/wp[0]/course-deviation-deg", true);
 109     _wp0_course_error_nm_node =
 110         fgGetNode("/instrumentation/gps/wp/wp[0]/course-error-nm", true);
 111     _wp0_to_flag_node =
 112         fgGetNode("/instrumentation/gps/wp/wp[0]/to-flag", true);
 113     _wp1_distance_node =
 114         fgGetNode("/instrumentation/gps/wp/wp[1]/distance-nm", true);
 115     _wp1_ttw_node =
 116         fgGetNode("/instrumentation/gps/wp/wp[1]/TTW",true);
 117     _wp1_bearing_node =
 118         fgGetNode("/instrumentation/gps/wp/wp[1]/bearing-true-deg", true);
 119     _wp1_mag_bearing_node =
 120         fgGetNode("/instrumentation/gps/wp/wp[1]/bearing-mag-deg", true);
 121     _wp1_course_deviation_node =
 122         fgGetNode("/instrumentation/gps/wp/wp[1]/course-deviation-deg", true);
 123     _wp1_course_error_nm_node =
 124         fgGetNode("/instrumentation/gps/wp/wp[1]/course-error-nm", true);
 125     _wp1_to_flag_node =
 126         fgGetNode("/instrumentation/gps/wp/wp[1]/to-flag", true);
 127     _odometer_node =
 128         fgGetNode("/instrumentation/gps/odometer", true);
 129     _trip_odometer_node =
 130         fgGetNode("/instrumentation/gps/trip-odometer", true);
 131     _true_bug_error_node =
 132         fgGetNode("/instrumentation/gps/true-bug-error-deg", true);
 133     _magnetic_bug_error_node =
 134         fgGetNode("/instrumentation/gps/magnetic-bug-error-deg", true);
 135     _true_wp0_bearing_error_node =
 136         fgGetNode("/instrumentation/gps/wp/wp[0]/true-bearing-error-deg", true);
 137     _magnetic_wp0_bearing_error_node =
 138         fgGetNode("/instrumentation/gps/wp/wp[0]/magnetic-bearing-error-deg", true);
 139     _true_wp1_bearing_error_node =
 140         fgGetNode("/instrumentation/gps/wp/wp[1]/true-bearing-error-deg", true);
 141     _magnetic_wp1_bearing_error_node =
 142         fgGetNode("/instrumentation/gps/wp/wp[1]/magnetic-bearing-error-deg", true);
 143     _leg_distance_node =
 144         fgGetNode("/instrumentation/gps/wp/leg-distance-nm", true);
 145     _leg_course_node =
 146         fgGetNode("/instrumentation/gps/wp/leg-true-course-deg", true);
 147     _leg_magnetic_course_node =
 148         fgGetNode("/instrumentation/gps/wp/leg-mag-course-deg", true);
 149     _alt_dist_ratio_node =
 150         fgGetNode("/instrumentation/gps/wp/alt-dist-ratio", true);
 151     _leg_course_deviation_node =
 152         fgGetNode("/instrumentation/gps/wp/leg-course-deviation-deg", true);
 153     _leg_course_error_nm_node =
 154         fgGetNode("/instrumentation/gps/wp/leg-course-error-nm", true);
 155     _leg_to_flag_node =
 156         fgGetNode("/instrumentation/gps/wp/leg-to-flag", true);
 157     _alt_deviation_node =
 158         fgGetNode("/instrumentation/gps/wp/alt-deviation-ft", true);
 159 }
 160
 161 void
 162 GPS::update (double delta_time_sec)
 163 {
 164                                 // If it's off, don't bother.
 165     if (!_serviceable_node->getBoolValue() ||
 166         !_electrical_node->getBoolValue()) {
 167         _last_valid = false;
 168         _last_longitude_deg = 0;
 169         _last_latitude_deg = 0;
 170         _last_altitude_m = 0;
 171         _last_speed_kts = 0;
 172         _raim_node->setDoubleValue(false);
 173         _indicated_longitude_node->setDoubleValue(0);
 174         _indicated_latitude_node->setDoubleValue(0);
 175         _indicated_altitude_node->setDoubleValue(0);
 176         _indicated_vertical_speed_node->setDoubleValue(0);
 177         _true_track_node->setDoubleValue(0);
 178         _magnetic_track_node->setDoubleValue(0);
 179         _speed_node->setDoubleValue(0);
 180         _wp1_distance_node->setDoubleValue(0);
 181         _wp1_bearing_node->setDoubleValue(0);
 182         _wp1_longitude_node->setDoubleValue(0);
 183         _wp1_latitude_node->setDoubleValue(0);
 184         _wp1_course_node->setDoubleValue(0);
 185         _odometer_node->setDoubleValue(0);
 186         _trip_odometer_node->setDoubleValue(0);
 187         _tracking_bug_node->setDoubleValue(0);
 188         _true_bug_error_node->setDoubleValue(0);
 189         _magnetic_bug_error_node->setDoubleValue(0);
 190         _true_wp1_bearing_error_node->setDoubleValue(0);
 191         _magnetic_wp1_bearing_error_node->setDoubleValue(0);
 192         return;
 193     }
 194
 195     // Get the aircraft position
 196     // TODO: Add noise and other errors.
 197     double longitude_deg = _longitude_node->getDoubleValue();
 198     double latitude_deg = _latitude_node->getDoubleValue();
 199     double altitude_m = _altitude_node->getDoubleValue() * SG_FEET_TO_METER;
 200     double magvar_deg = _magvar_node->getDoubleValue();
 201
 202 /*
 203
 204     // Bias and random error
 205     double random_factor = sg_random();
 206     double random_error = 1.4;
 207     double error_radius = 5.1;
 208     double bias_max_radius = 5.1;
 209     double random_max_radius = 1.4;
 210
 211     bias_length += (random_factor-0.5) * 1.0e-3;
 212     if (bias_length <= 0.0) bias_length = 0.0;
 213     else if (bias_length >= bias_max_radius) bias_length = bias_max_radius;
 214     bias_angle  += (random_factor-0.5) * 1.0e-3;
 215     if (bias_angle <= 0.0) bias_angle = 0.0;
 216     else if (bias_angle >= 360.0) bias_angle = 360.0;
 217
 218     double random_length = random_factor * random_max_radius;
 219     double random_angle = random_factor * 360.0;
 220
 221     double bias_x = bias_length * cos(bias_angle * SG_PI / 180.0);
 222     double bias_y = bias_length * sin(bias_angle * SG_PI / 180.0);
 223     double random_x = random_length * cos(random_angle * SG_PI / 180.0);
 224     double random_y = random_length * sin(random_angle * SG_PI / 180.0);
 225     double error_x = bias_x + random_x;
 226     double error_y = bias_y + random_y;
 227     double error_length = sqrt(error_x*error_x + error_y*error_y);
 228     double error_angle = atan(error_y / error_x) * 180.0 / SG_PI;
 229
 230     double lat2;
 231     double lon2;
 232     double az2;
 233     geo_direct_wgs_84 ( altitude_m, latitude_deg,
 234                         longitude_deg, error_angle,
 235                         error_length, &lat2, &lon2,
 236                         &az2 );
 237     //cout << lat2 << " " << lon2 << endl;
 238     printf("%f %f \n", bias_length, bias_angle);
 239     printf("%3.7f %3.7f \n", lat2, lon2);
 240     printf("%f %f \n", error_length, error_angle);
 241
 242 */
 243
 244
 245
 246     double speed_kt, vertical_speed_mpm;
 247
 248     _raim_node->setBoolValue(true);
 249     _indicated_longitude_node->setDoubleValue(longitude_deg);
 250     _indicated_latitude_node->setDoubleValue(latitude_deg);
 251     _indicated_altitude_node->setDoubleValue(altitude_m * SG_METER_TO_FEET);
 252
 253     if (_last_valid) {
 254         double track1_deg, track2_deg, distance_m, odometer, mag_track_bearing;
 255         geo_inverse_wgs_84(altitude_m,
 256                            _last_latitude_deg, _last_longitude_deg,
 257                            latitude_deg, longitude_deg,
 258                            &track1_deg, &track2_deg, &distance_m);
 259         speed_kt = ((distance_m * SG_METER_TO_NM) *
 260                     ((1 / delta_time_sec) * 3600.0));
 261         vertical_speed_mpm = ((altitude_m - _last_altitude_m) * 60 /
 262                               delta_time_sec);
 263         _indicated_vertical_speed_node->setDoubleValue
 264             (vertical_speed_mpm * SG_METER_TO_FEET);
 265         _true_track_node->setDoubleValue(track1_deg);
 266         mag_track_bearing = degrange360(track1_deg - magvar_deg);
 267         _magnetic_track_node->setDoubleValue(mag_track_bearing);
 268         speed_kt = fgGetLowPass(_last_speed_kts, speed_kt, delta_time_sec/20.0);
 269         _last_speed_kts = speed_kt;
 270         _speed_node->setDoubleValue(speed_kt);
 271
 272         odometer = _odometer_node->getDoubleValue();
 273         _odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
 274         odometer = _trip_odometer_node->getDoubleValue();
 275         _trip_odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
 276
 277         // Get waypoint 0 position
 278         double wp0_longitude_deg = _wp0_longitude_node->getDoubleValue();
 279         double wp0_latitude_deg = _wp0_latitude_node->getDoubleValue();
 280         double wp0_altitude_m = _wp0_altitude_node->getDoubleValue()
 281             * SG_FEET_TO_METER;
 282         double wp0_course_deg = _wp0_course_node->getDoubleValue();
 283         double wp0_distance, wp0_bearing_deg, wp0_course_deviation_deg,
 284             wp0_course_error_m, wp0_TTW, wp0_bearing_error_deg;
 285         string wp0_ID = _wp0_ID_node->getStringValue();
 286
 287         // Get waypoint 1 position
 288         double wp1_longitude_deg = _wp1_longitude_node->getDoubleValue();
 289         double wp1_latitude_deg = _wp1_latitude_node->getDoubleValue();
 290         double wp1_altitude_m = _wp1_altitude_node->getDoubleValue()
 291             * SG_FEET_TO_METER;
 292         double wp1_course_deg = _wp1_course_node->getDoubleValue();
 293         double wp1_distance, wp1_bearing_deg, wp1_course_deviation_deg,
 294             wp1_course_error_m, wp1_TTW, wp1_bearing_error_deg;
 295         string wp1_ID = _wp1_ID_node->getStringValue();
 296
 297         // If the get-nearest-airport-node is true.
 298         // Get the nearest airport, and set it as waypoint 1.
 299         if (_get_nearest_airport_node->getBoolValue()) {
 300             FGAirport a;
 301             //cout << "Airport found" << endl;
 302             a = globals->get_airports()->search(longitude_deg, latitude_deg, false);
 303             _wp1_ID_node->setStringValue(a.id.c_str());
 304             wp1_longitude_deg = a.longitude;
 305             wp1_latitude_deg = a.latitude;
 306             _wp1_name_node->setStringValue(a.name.c_str());
 307             _get_nearest_airport_node->setBoolValue(false);
 308             _last_wp1_ID = wp1_ID = a.id.c_str();
 309         }
 310
 311         // If the waypoint 0 ID has changed, try to find the new ID
 312         // in the airport-, fix-, nav-database.
 313         if ( !(_last_wp0_ID == wp0_ID) ) {
 314             string waypont_type =
 315                 _wp0_waypoint_type_node->getStringValue();
 316             if (waypont_type == "airport") {
 317                 FGAirport a;
 318                 a = globals->get_airports()->search( wp0_ID );
 319                 if ( a.id == wp0_ID ) {
 320                     //cout << "Airport found" << endl;
 321                     wp0_longitude_deg = a.longitude;
 322                     wp0_latitude_deg = a.latitude;
 323                     _wp0_name_node->setStringValue(a.name.c_str());
 324                 }
 325             }
 326             else if (waypont_type == "nav") {
 327                 FGNav * n;
 328                 if ( (n = current_navlist->findByIdent(wp0_ID.c_str(),
 329                                                        longitude_deg,
 330                                                        latitude_deg)) != NULL) {
 331                     //cout << "Nav found" << endl;
 332                     wp0_longitude_deg = n->get_lon();
 333                     wp0_latitude_deg = n->get_lat();
 334                     _wp0_name_node->setStringValue(n->get_name().c_str());
 335                 }
 336             }
 337             else if (waypont_type == "fix") {
 338                 FGFix f;
 339                 if ( current_fixlist->query(wp0_ID, &f) ) {
 340                     //cout << "Fix found" << endl;
 341                     wp0_longitude_deg = f.get_lon();
 342                     wp0_latitude_deg = f.get_lat();
 343                     _wp0_name_node->setStringValue(wp0_ID.c_str());
 344                 }
 345             }
 346             _last_wp0_ID = wp0_ID;
 347         }
 348
 349         // If the waypoint 1 ID has changed, try to find the new ID
 350         // in the airport-, fix-, nav-database.
 351         if ( !(_last_wp1_ID == wp1_ID) ) {
 352             string waypont_type =
 353                 _wp1_waypoint_type_node->getStringValue();
 354             if (waypont_type == "airport") {
 355                 FGAirport a;
 356                 a = globals->get_airports()->search( wp1_ID );
 357                 if ( a.id == wp1_ID ) {
 358                     //cout << "Airport found" << endl;
 359                     wp1_longitude_deg = a.longitude;
 360                     wp1_latitude_deg = a.latitude;
 361                     _wp1_name_node->setStringValue(a.name.c_str());
 362                 }
 363             }
 364             else if (waypont_type == "nav") {
 365                 FGNav * n;
 366                 if ( (n = current_navlist->findByIdent(wp1_ID.c_str(),
 367                                                        longitude_deg,
 368                                                        latitude_deg)) != NULL) {
 369                     //cout << "Nav found" << endl;
 370                     wp1_longitude_deg = n->get_lon();
 371                     wp1_latitude_deg = n->get_lat();
 372                     _wp1_name_node->setStringValue(n->get_name().c_str());
 373                 }
 374             }
 375             else if (waypont_type == "fix") {
 376                 FGFix f;
 377                 if ( current_fixlist->query(wp1_ID, &f) ) {
 378                     //cout << "Fix found" << endl;
 379                     wp1_longitude_deg = f.get_lon();
 380                     wp1_latitude_deg = f.get_lat();
 381                     _wp1_name_node->setStringValue(wp1_ID.c_str());
 382                 }
 383             }
 384             _last_wp1_ID = wp1_ID;
 385         }
 386
 387
 388
 389         // If any of the two waypoints have changed
 390         // we need to calculate a new course between them,
 391         // and values for vertical navigation.
 392         if ( wp0_longitude_deg != _wp0_longitude_deg ||
 393              wp0_latitude_deg  != _wp0_latitude_deg  ||
 394              wp0_altitude_m    != _wp0_altitude_m    ||
 395              wp1_longitude_deg != _wp1_longitude_deg ||
 396              wp1_latitude_deg  != _wp1_latitude_deg  ||
 397              wp1_altitude_m    != _wp1_altitude_m )
 398         {
 399             // Update the global variables
 400             _wp0_longitude_deg = wp0_longitude_deg;
 401             _wp0_latitude_deg  = wp0_latitude_deg;
 402             _wp0_altitude_m    = wp0_altitude_m;
 403             _wp1_longitude_deg = wp1_longitude_deg;
 404             _wp1_latitude_deg  = wp1_latitude_deg;
 405             _wp1_altitude_m    = wp1_altitude_m;
 406
 407             // Get the course and distance from wp0 to wp1
 408             SGWayPoint wp0(wp0_longitude_deg,
 409                            wp0_latitude_deg, wp0_altitude_m);
 410             SGWayPoint wp1(wp1_longitude_deg,
 411                            wp1_latitude_deg, wp1_altitude_m);
 412
 413             wp1.CourseAndDistance(wp0, &_course_deg, &_distance_m);
 414
 415             // Get the altitude / distance ratio
 416             if ( distance_m > 0.0 ) {
 417                 double alt_difference_m = wp0_altitude_m - wp1_altitude_m;
 418                 _alt_dist_ratio = alt_difference_m / _distance_m;
 419             }
 420
 421             _leg_distance_node->setDoubleValue(_distance_m);
 422             _leg_course_node->setDoubleValue(_course_deg);
 423             _alt_dist_ratio_node->setDoubleValue(_alt_dist_ratio);
 424         }
 425
 426
 427         // Find the bearing and distance to waypoint 0.
 428         SGWayPoint wp0(wp0_longitude_deg, wp0_latitude_deg, wp0_altitude_m);
 429         wp0.CourseAndDistance(longitude_deg, latitude_deg, altitude_m,
 430                               &wp0_bearing_deg, &wp0_distance);
 431         _wp0_longitude_node->setDoubleValue(wp0_longitude_deg);
 432         _wp0_latitude_node->setDoubleValue(wp0_latitude_deg);
 433         _wp0_distance_node->setDoubleValue(wp0_distance * SG_METER_TO_NM);
 434         _wp0_bearing_node->setDoubleValue(wp0_bearing_deg);
 435         double wp0_mag_bearing_deg = degrange360(wp0_bearing_deg - magvar_deg);
 436         _wp0_mag_bearing_node->setDoubleValue(wp0_mag_bearing_deg);
 437         wp0_bearing_error_deg = degrange180(track1_deg - wp0_bearing_deg);
 438         _true_wp0_bearing_error_node->setDoubleValue(wp0_bearing_error_deg);
 439
 440         // Estimate time to waypoint 0.
 441         // The estimation does not take track into consideration,
 442         // so if you are going away from the waypoint the TTW will
 443         // increase. Makes most sense when travelling directly towards
 444         // the waypoint.
 445         if (speed_kt > 0.0 && wp0_distance > 0.0) {
 446           wp0_TTW = (wp0_distance * SG_METER_TO_NM) / (speed_kt / 3600);
 447         }
 448         else {
 449           wp0_TTW = 0.0;
 450         }
 451         unsigned int wp0_TTW_seconds = (int) (wp0_TTW + 0.5);
 452         if (wp0_TTW_seconds < 356400) { // That's 99 hours
 453           unsigned int wp0_TTW_minutes = 0;
 454           unsigned int wp0_TTW_hours   = 0;
 455           char wp0_TTW_str[9];
 456           while (wp0_TTW_seconds >= 3600) {
 457             wp0_TTW_seconds -= 3600;
 458             wp0_TTW_hours++;
 459           }
 460           while (wp0_TTW_seconds >= 60) {
 461             wp0_TTW_seconds -= 60;
 462             wp0_TTW_minutes++;
 463           }
 464           snprintf(wp0_TTW_str, 8, "%02d:%02d:%02d",
 465             wp0_TTW_hours, wp0_TTW_minutes, wp0_TTW_seconds);
 466           _wp0_ttw_node->setStringValue(wp0_TTW_str);
 467         }
 468         else
 469           _wp0_ttw_node->setStringValue("--:--:--");
 470
 471         // Course deviation is the diffenrence between the bearing
 472         // and the course.
 473         wp0_course_deviation_deg = wp0_bearing_deg -
 474           wp0_course_deg;
 475         wp0_course_deviation_deg = degrange180(wp0_course_deviation_deg);
 476
 477         // If the course deviation is less than 90 degrees to either side,
 478         // our desired course is towards the waypoint.
 479         // It does not matter if we are actually moving
 480         // towards or from the waypoint.
 481         if (fabs(wp0_course_deviation_deg) < 90.0) {
 482             _wp0_to_flag_node->setBoolValue(true); }
 483         // If it's more than 90 degrees the desired
 484         // course is from the waypoint.
 485         else if (fabs(wp0_course_deviation_deg) > 90.0) {
 486           _wp0_to_flag_node->setBoolValue(false);
 487           // When the course is away from the waypoint,
 488           // it makes sense to change the sign of the deviation.
 489           wp0_course_deviation_deg *= -1.0;
 490           wp0_course_deviation_deg =
 491               degrange(wp0_course_deviation_deg, -90.0, 90.0);
 492         }
 493
 494         _wp0_course_deviation_node->setDoubleValue(wp0_course_deviation_deg);
 495
 496         // Cross track error.
 497         wp0_course_error_m = sin(wp0_course_deviation_deg * SG_PI / 180.0)
 498           * (wp0_distance);
 499         _wp0_course_error_nm_node->setDoubleValue(wp0_course_error_m
 500                                                  * SG_METER_TO_NM);
 501
 502
 503
 504         // Find the bearing and distance to waypoint 1.
 505         SGWayPoint wp1(wp1_longitude_deg, wp1_latitude_deg, wp1_altitude_m);
 506         wp1.CourseAndDistance(longitude_deg, latitude_deg, altitude_m,
 507                               &wp1_bearing_deg, &wp1_distance);
 508         _wp1_longitude_node->setDoubleValue(wp1_longitude_deg);
 509         _wp1_latitude_node->setDoubleValue(wp1_latitude_deg);
 510         _wp1_distance_node->setDoubleValue(wp1_distance * SG_METER_TO_NM);
 511         _wp1_bearing_node->setDoubleValue(wp1_bearing_deg);
 512         double wp1_mag_bearing_deg = degrange360(wp1_bearing_deg - magvar_deg);
 513         _wp1_mag_bearing_node->setDoubleValue(wp1_mag_bearing_deg);
 514         wp1_bearing_error_deg = degrange180(track1_deg - wp1_bearing_deg);
 515         _true_wp1_bearing_error_node->setDoubleValue(wp1_bearing_error_deg);
 516
 517         // Estimate time to waypoint 1.
 518         // The estimation does not take track into consideration,
 519         // so if you are going away from the waypoint the TTW will
 520         // increase. Makes most sense when travelling directly towards
 521         // the waypoint.
 522         if (speed_kt > 0.0 && wp1_distance > 0.0) {
 523           wp1_TTW = (wp1_distance * SG_METER_TO_NM) / (speed_kt / 3600);
 524         }
 525         else {
 526           wp1_TTW = 0.0;
 527         }
 528         unsigned int wp1_TTW_seconds = (int) (wp1_TTW + 0.5);
 529         if (wp1_TTW_seconds < 356400) { // That's 99 hours
 530           unsigned int wp1_TTW_minutes = 0;
 531           unsigned int wp1_TTW_hours   = 0;
 532           char wp1_TTW_str[9];
 533           while (wp1_TTW_seconds >= 3600) {
 534             wp1_TTW_seconds -= 3600;
 535             wp1_TTW_hours++;
 536           }
 537           while (wp1_TTW_seconds >= 60) {
 538             wp1_TTW_seconds -= 60;
 539             wp1_TTW_minutes++;
 540           }
 541           snprintf(wp1_TTW_str, 8, "%02d:%02d:%02d",
 542             wp1_TTW_hours, wp1_TTW_minutes, wp1_TTW_seconds);
 543           _wp1_ttw_node->setStringValue(wp1_TTW_str);
 544         }
 545         else
 546           _wp1_ttw_node->setStringValue("--:--:--");
 547
 548         // Course deviation is the diffenrence between the bearing
 549         // and the course.
 550         wp1_course_deviation_deg = wp1_bearing_deg -
 551           wp1_course_deg;
 552         wp1_course_deviation_deg = degrange180(wp1_course_deviation_deg);
 553
 554         // If the course deviation is less than 90 degrees to either side,
 555         // our desired course is towards the waypoint.
 556         // It does not matter if we are actually moving
 557         // towards or from the waypoint.
 558         if (fabs(wp1_course_deviation_deg) < 90.0) {
 559             _wp1_to_flag_node->setBoolValue(true); }
 560         // If it's more than 90 degrees the desired
 561         // course is from the waypoint.
 562         else if (fabs(wp1_course_deviation_deg) > 90.0) {
 563             _wp1_to_flag_node->setBoolValue(false);
 564             // When the course is away from the waypoint,
 565             // it makes sense to change the sign of the deviation.
 566             wp1_course_deviation_deg *= -1.0;
 567             wp1_course_deviation_deg =
 568                 degrange(wp1_course_deviation_deg, -90.0, 90.0);
 569         }
 570
 571         _wp1_course_deviation_node->setDoubleValue(wp1_course_deviation_deg);
 572
 573         // Cross track error.
 574         wp1_course_error_m = sin(wp1_course_deviation_deg * SG_PI / 180.0)
 575           * (wp1_distance);
 576         _wp1_course_error_nm_node->setDoubleValue(wp1_course_error_m
 577                                                  * SG_METER_TO_NM);
 578
 579
 580         // Leg course deviation is the diffenrence between the bearing
 581         // and the course.
 582         double course_deviation_deg = wp1_bearing_deg - _course_deg;
 583         course_deviation_deg = degrange180(course_deviation_deg);
 584
 585         // If the course deviation is less than 90 degrees to either side,
 586         // our desired course is towards the waypoint.
 587         // It does not matter if we are actually moving
 588         // towards or from the waypoint.
 589         if (fabs(course_deviation_deg) < 90.0) {
 590             _leg_to_flag_node->setBoolValue(true); }
 591         // If it's more than 90 degrees the desired
 592         // course is from the waypoint.
 593         else if (fabs(course_deviation_deg) > 90.0) {
 594             _leg_to_flag_node->setBoolValue(false);
 595             // When the course is away from the waypoint,
 596             // it makes sense to change the sign of the deviation.
 597             course_deviation_deg *= -1.0;
 598             course_deviation_deg =
 599                 degrange(course_deviation_deg, -90.0, 90.0);
 600         }
 601
 602         _leg_course_deviation_node->setDoubleValue(course_deviation_deg);
 603
 604         // Cross track error.
 605         double course_error_m = sin(course_deviation_deg * SG_PI / 180.0)
 606             * (_distance_m);
 607         _leg_course_error_nm_node->setDoubleValue(course_error_m * SG_METER_TO_NM);
 608
 609         // Altitude deviation
 610         double desired_altitude_m = wp1_altitude_m
 611             + wp1_distance * _alt_dist_ratio;
 612         double altitude_deviation_m = altitude_m - desired_altitude_m;
 613         _alt_deviation_node->setDoubleValue(altitude_deviation_m * SG_METER_TO_FEET);
 614
 615
 616
 617         // Tracking bug.
 618         double tracking_bug = _tracking_bug_node->getDoubleValue();
 619         double true_bug_error = tracking_bug - track1_deg;
 620         double magnetic_bug_error = tracking_bug - mag_track_bearing;
 621
 622         // Get the errors into the (-180,180) range.
 623         true_bug_error = degrange180(true_bug_error);
 624         magnetic_bug_error = degrange180(magnetic_bug_error);
 625
 626         _true_bug_error_node->setDoubleValue(true_bug_error);
 627         _magnetic_bug_error_node->setDoubleValue(magnetic_bug_error);
 628
 629     } else {
 630         _true_track_node->setDoubleValue(0.0);
 631         _magnetic_track_node->setDoubleValue(0.0);
 632         _speed_node->setDoubleValue(0.0);
 633     }
 634
 635     _last_valid = true;
 636     _last_longitude_deg = longitude_deg;
 637     _last_latitude_deg = latitude_deg;
 638     _last_altitude_m = altitude_m;
 639 }
 640
 641 double GPS::degrange360 (double deg)
 642 {
 643     while (deg < 0.0) {
 644         deg += 360.0; }
 645     while (deg > 360.0) {
 646         deg -= 360.0; }
 647
 648     return deg;
 649 }
 650
 651 double GPS::degrange180 (double deg)
 652 {
 653     while (deg < -180.0)
 654         deg += 360.0;
 655     while (deg >  180.0)
 656         deg -= 360.0;
 657
 658     return deg;
 659 }
 660
 661 double GPS::degrange (double deg, double min, double max)
 662 {
 663     double span = max - min;
 664
 665     while (deg < min)
 666         deg += span;
 667     while (deg >  max)
 668         deg -= span;
 669
 670     return deg;
 671 }
 672 // end of gps.cxx