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