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