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