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