src/Instrumentation/gps.cxx

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