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