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