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             const FGAirport* a = globals->get_airports()->search(longitude_deg, latitude_deg, false);
 344             if(a) {
 345                 _wp1_ID_node->setStringValue(a->getId().c_str());
 346                 wp1_longitude_deg = a->getLongitude();
 347                 wp1_latitude_deg = a->getLatitude();
 348                 _wp1_name_node->setStringValue(a->getName().c_str());
 349                 _get_nearest_airport_node->setBoolValue(false);
 350                 _last_wp1_ID = wp1_ID = a->getId().c_str();
 351             }
 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                 const FGAirport* a = globals->get_airports()->search( wp0_ID );
 361                 if ( a ) {
 362                     wp0_longitude_deg = a->getLongitude();
 363                     wp0_latitude_deg = a->getLatitude();
 364                     _wp0_name_node->setStringValue(a->getName().c_str());
 365                 }
 366             }
 367             else if (waypont_type == "nav") {
 368                 FGNavRecord *n
 369                     = globals->get_navlist()->findByIdent(wp0_ID.c_str(),
 370                                                           longitude_deg,
 371                                                           latitude_deg);
 372                 if ( n != NULL ) {
 373                     //cout << "Nav found" << endl;
 374                     wp0_longitude_deg = n->get_lon();
 375                     wp0_latitude_deg = n->get_lat();
 376                     _wp0_name_node->setStringValue(n->get_name().c_str());
 377                 }
 378             }
 379             else if (waypont_type == "fix") {
 380                 FGFix f;
 381                 if ( globals->get_fixlist()->query(wp0_ID, &f) ) {
 382                     //cout << "Fix found" << endl;
 383                     wp0_longitude_deg = f.get_lon();
 384                     wp0_latitude_deg = f.get_lat();
 385                     _wp0_name_node->setStringValue(wp0_ID.c_str());
 386                 }
 387             }
 388             _last_wp0_ID = wp0_ID;
 389         }
 390
 391         // If the waypoint 1 ID has changed, try to find the new ID
 392         // in the airport-, fix-, nav-database.
 393         if ( !(_last_wp1_ID == wp1_ID) ) {
 394             string waypont_type =
 395                 _wp1_waypoint_type_node->getStringValue();
 396             if (waypont_type == "airport") {
 397                 const FGAirport* a = globals->get_airports()->search( wp1_ID );
 398                 if ( a ) {
 399                     wp1_longitude_deg = a->getLongitude();
 400                     wp1_latitude_deg = a->getLatitude();
 401                     _wp1_name_node->setStringValue(a->getName().c_str());
 402                 }
 403             }
 404             else if (waypont_type == "nav") {
 405                 FGNavRecord *n
 406                     = globals->get_navlist()->findByIdent(wp1_ID.c_str(),
 407                                                           longitude_deg,
 408                                                           latitude_deg);
 409                 if ( n != NULL ) {
 410                     //cout << "Nav found" << endl;
 411                     wp1_longitude_deg = n->get_lon();
 412                     wp1_latitude_deg = n->get_lat();
 413                     _wp1_name_node->setStringValue(n->get_name().c_str());
 414                 }
 415             }
 416             else if (waypont_type == "fix") {
 417                 FGFix f;
 418                 if ( globals->get_fixlist()->query(wp1_ID, &f) ) {
 419                     //cout << "Fix found" << endl;
 420                     wp1_longitude_deg = f.get_lon();
 421                     wp1_latitude_deg = f.get_lat();
 422                     _wp1_name_node->setStringValue(wp1_ID.c_str());
 423                 }
 424             }
 425             _last_wp1_ID = wp1_ID;
 426         }
 427
 428
 429
 430         // If any of the two waypoints have changed
 431         // we need to calculate a new course between them,
 432         // and values for vertical navigation.
 433         if ( wp0_longitude_deg != _wp0_longitude_deg ||
 434              wp0_latitude_deg  != _wp0_latitude_deg  ||
 435              wp0_altitude_m    != _wp0_altitude_m    ||
 436              wp1_longitude_deg != _wp1_longitude_deg ||
 437              wp1_latitude_deg  != _wp1_latitude_deg  ||
 438              wp1_altitude_m    != _wp1_altitude_m )
 439         {
 440             // Update the global variables
 441             _wp0_longitude_deg = wp0_longitude_deg;
 442             _wp0_latitude_deg  = wp0_latitude_deg;
 443             _wp0_altitude_m    = wp0_altitude_m;
 444             _wp1_longitude_deg = wp1_longitude_deg;
 445             _wp1_latitude_deg  = wp1_latitude_deg;
 446             _wp1_altitude_m    = wp1_altitude_m;
 447
 448             // Get the course and distance from wp0 to wp1
 449             SGWayPoint wp0(wp0_longitude_deg,
 450                            wp0_latitude_deg, wp0_altitude_m);
 451             SGWayPoint wp1(wp1_longitude_deg,
 452                            wp1_latitude_deg, wp1_altitude_m);
 453
 454             wp1.CourseAndDistance(wp0, &_course_deg, &_distance_m);
 455             double leg_mag_course = _course_deg - magvar_deg;
 456             SG_NORMALIZE_RANGE(leg_mag_course, 0.0, 360.0);
 457
 458             // Get the altitude / distance ratio
 459             if ( distance_m > 0.0 ) {
 460                 double alt_difference_m = wp0_altitude_m - wp1_altitude_m;
 461                 _alt_dist_ratio = alt_difference_m / _distance_m;
 462             }
 463
 464             _leg_distance_node->setDoubleValue(_distance_m * SG_METER_TO_NM);
 465             _leg_course_node->setDoubleValue(_course_deg);
 466             _leg_magnetic_course_node->setDoubleValue(leg_mag_course);
 467             _alt_dist_ratio_node->setDoubleValue(_alt_dist_ratio);
 468
 469             _wp0_longitude_node->setDoubleValue(wp0_longitude_deg);
 470             _wp0_latitude_node->setDoubleValue(wp0_latitude_deg);
 471             _wp1_longitude_node->setDoubleValue(wp1_longitude_deg);
 472             _wp1_latitude_node->setDoubleValue(wp1_latitude_deg);
 473         }
 474
 475
 476         // Find the bearing and distance to waypoint 0.
 477         SGWayPoint wp0(wp0_longitude_deg, wp0_latitude_deg, wp0_altitude_m);
 478         wp0.CourseAndDistance(longitude_deg, latitude_deg, altitude_m,
 479                               &wp0_bearing_deg, &wp0_distance);
 480         _wp0_distance_node->setDoubleValue(wp0_distance * SG_METER_TO_NM);
 481         _wp0_bearing_node->setDoubleValue(wp0_bearing_deg);
 482         double wp0_mag_bearing_deg = wp0_bearing_deg - magvar_deg;
 483         SG_NORMALIZE_RANGE(wp0_mag_bearing_deg, 0.0, 360.0);
 484         _wp0_mag_bearing_node->setDoubleValue(wp0_mag_bearing_deg);
 485         wp0_bearing_error_deg = track1_deg - wp0_bearing_deg;
 486         SG_NORMALIZE_RANGE(wp0_bearing_error_deg, -180.0, 180.0);
 487         _true_wp0_bearing_error_node->setDoubleValue(wp0_bearing_error_deg);
 488
 489         // Estimate time to waypoint 0.
 490         // The estimation does not take track into consideration,
 491         // so if you are going away from the waypoint the TTW will
 492         // increase. Makes most sense when travelling directly towards
 493         // the waypoint.
 494         if (speed_kt > 0.0 && wp0_distance > 0.0) {
 495           wp0_TTW = (wp0_distance * SG_METER_TO_NM) / (speed_kt / 3600);
 496         }
 497         else {
 498           wp0_TTW = 0.0;
 499         }
 500         unsigned int wp0_TTW_seconds = (int) (wp0_TTW + 0.5);
 501         if (wp0_TTW_seconds < 356400) { // That's 99 hours
 502           unsigned int wp0_TTW_minutes = 0;
 503           unsigned int wp0_TTW_hours   = 0;
 504           char wp0_TTW_str[9];
 505           while (wp0_TTW_seconds >= 3600) {
 506             wp0_TTW_seconds -= 3600;
 507             wp0_TTW_hours++;
 508           }
 509           while (wp0_TTW_seconds >= 60) {
 510             wp0_TTW_seconds -= 60;
 511             wp0_TTW_minutes++;
 512           }
 513           snprintf(wp0_TTW_str, 9, "%02d:%02d:%02d",
 514             wp0_TTW_hours, wp0_TTW_minutes, wp0_TTW_seconds);
 515           _wp0_ttw_node->setStringValue(wp0_TTW_str);
 516         }
 517         else
 518           _wp0_ttw_node->setStringValue("--:--:--");
 519
 520         // Course deviation is the diffenrence between the bearing
 521         // and the course.
 522         wp0_course_deviation_deg = wp0_bearing_deg -
 523             wp0_course_deg;
 524         SG_NORMALIZE_RANGE(wp0_course_deviation_deg, -180.0, 180.0);
 525
 526         // If the course deviation is less than 90 degrees to either side,
 527         // our desired course is towards the waypoint.
 528         // It does not matter if we are actually moving
 529         // towards or from the waypoint.
 530         if (fabs(wp0_course_deviation_deg) < 90.0) {
 531             _wp0_to_flag_node->setBoolValue(true); }
 532         // If it's more than 90 degrees the desired
 533         // course is from the waypoint.
 534         else if (fabs(wp0_course_deviation_deg) > 90.0) {
 535           _wp0_to_flag_node->setBoolValue(false);
 536           // When the course is away from the waypoint,
 537           // it makes sense to change the sign of the deviation.
 538           wp0_course_deviation_deg *= -1.0;
 539           SG_NORMALIZE_RANGE(wp0_course_deviation_deg, -90.0, 90.0);
 540         }
 541
 542         _wp0_course_deviation_node->setDoubleValue(wp0_course_deviation_deg);
 543
 544         // Cross track error.
 545         wp0_course_error_m = sin(wp0_course_deviation_deg * SG_PI / 180.0)
 546           * (wp0_distance);
 547         _wp0_course_error_nm_node->setDoubleValue(wp0_course_error_m
 548                                                  * SG_METER_TO_NM);
 549
 550
 551
 552         // Find the bearing and distance to waypoint 1.
 553         SGWayPoint wp1(wp1_longitude_deg, wp1_latitude_deg, wp1_altitude_m);
 554         wp1.CourseAndDistance(longitude_deg, latitude_deg, altitude_m,
 555                               &wp1_bearing_deg, &wp1_distance);
 556         _wp1_distance_node->setDoubleValue(wp1_distance * SG_METER_TO_NM);
 557         _wp1_bearing_node->setDoubleValue(wp1_bearing_deg);
 558         double wp1_mag_bearing_deg = wp1_bearing_deg - magvar_deg;
 559         SG_NORMALIZE_RANGE(wp1_mag_bearing_deg, 0.0, 360.0);
 560         _wp1_mag_bearing_node->setDoubleValue(wp1_mag_bearing_deg);
 561         wp1_bearing_error_deg = track1_deg - wp1_bearing_deg;
 562         SG_NORMALIZE_RANGE(wp1_bearing_error_deg, -180.0, 180.0);
 563         _true_wp1_bearing_error_node->setDoubleValue(wp1_bearing_error_deg);
 564
 565         // Estimate time to waypoint 1.
 566         // The estimation does not take track into consideration,
 567         // so if you are going away from the waypoint the TTW will
 568         // increase. Makes most sense when travelling directly towards
 569         // the waypoint.
 570         if (speed_kt > 0.0 && wp1_distance > 0.0) {
 571           wp1_TTW = (wp1_distance * SG_METER_TO_NM) / (speed_kt / 3600);
 572         }
 573         else {
 574           wp1_TTW = 0.0;
 575         }
 576         unsigned int wp1_TTW_seconds = (int) (wp1_TTW + 0.5);
 577         if (wp1_TTW_seconds < 356400) { // That's 99 hours
 578           unsigned int wp1_TTW_minutes = 0;
 579           unsigned int wp1_TTW_hours   = 0;
 580           char wp1_TTW_str[9];
 581           while (wp1_TTW_seconds >= 3600) {
 582             wp1_TTW_seconds -= 3600;
 583             wp1_TTW_hours++;
 584           }
 585           while (wp1_TTW_seconds >= 60) {
 586             wp1_TTW_seconds -= 60;
 587             wp1_TTW_minutes++;
 588           }
 589           snprintf(wp1_TTW_str, 9, "%02d:%02d:%02d",
 590             wp1_TTW_hours, wp1_TTW_minutes, wp1_TTW_seconds);
 591           _wp1_ttw_node->setStringValue(wp1_TTW_str);
 592         }
 593         else
 594           _wp1_ttw_node->setStringValue("--:--:--");
 595
 596         // Course deviation is the diffenrence between the bearing
 597         // and the course.
 598         wp1_course_deviation_deg = wp1_bearing_deg - wp1_course_deg;
 599         SG_NORMALIZE_RANGE(wp1_course_deviation_deg, -180.0, 180.0);
 600
 601         // If the course deviation is less than 90 degrees to either side,
 602         // our desired course is towards the waypoint.
 603         // It does not matter if we are actually moving
 604         // towards or from the waypoint.
 605         if (fabs(wp1_course_deviation_deg) < 90.0) {
 606             _wp1_to_flag_node->setBoolValue(true); }
 607         // If it's more than 90 degrees the desired
 608         // course is from the waypoint.
 609         else if (fabs(wp1_course_deviation_deg) > 90.0) {
 610             _wp1_to_flag_node->setBoolValue(false);
 611             // When the course is away from the waypoint,
 612             // it makes sense to change the sign of the deviation.
 613             wp1_course_deviation_deg *= -1.0;
 614             SG_NORMALIZE_RANGE(wp1_course_deviation_deg, -90.0, 90.0);
 615         }
 616
 617         _wp1_course_deviation_node->setDoubleValue(wp1_course_deviation_deg);
 618
 619         // Cross track error.
 620         wp1_course_error_m = sin(wp1_course_deviation_deg * SG_PI / 180.0)
 621           * (wp1_distance);
 622         _wp1_course_error_nm_node->setDoubleValue(wp1_course_error_m
 623                                                  * SG_METER_TO_NM);
 624
 625
 626         // Leg course deviation is the diffenrence between the bearing
 627         // and the course.
 628         double course_deviation_deg = wp1_bearing_deg - _course_deg;
 629         SG_NORMALIZE_RANGE(course_deviation_deg, -180.0, 180.0);
 630
 631         // If the course deviation is less than 90 degrees to either side,
 632         // our desired course is towards the waypoint.
 633         // It does not matter if we are actually moving
 634         // towards or from the waypoint.
 635         if (fabs(course_deviation_deg) < 90.0) {
 636             _leg_to_flag_node->setBoolValue(true); }
 637         // If it's more than 90 degrees the desired
 638         // course is from the waypoint.
 639         else if (fabs(course_deviation_deg) > 90.0) {
 640             _leg_to_flag_node->setBoolValue(false);
 641             // When the course is away from the waypoint,
 642             // it makes sense to change the sign of the deviation.
 643             course_deviation_deg *= -1.0;
 644             SG_NORMALIZE_RANGE(course_deviation_deg, -90.0, 90.0);
 645         }
 646
 647         _leg_course_deviation_node->setDoubleValue(course_deviation_deg);
 648
 649         // Cross track error.
 650         double course_error_m = sin(course_deviation_deg * SG_PI / 180.0)
 651             * (_distance_m);
 652         _leg_course_error_nm_node->setDoubleValue(course_error_m * SG_METER_TO_NM);
 653
 654         // Altitude deviation
 655         double desired_altitude_m = wp1_altitude_m
 656             + wp1_distance * _alt_dist_ratio;
 657         double altitude_deviation_m = altitude_m - desired_altitude_m;
 658         _alt_deviation_node->setDoubleValue(altitude_deviation_m * SG_METER_TO_FEET);
 659
 660
 661
 662         // Tracking bug.
 663         double tracking_bug = _tracking_bug_node->getDoubleValue();
 664         double true_bug_error = tracking_bug - track1_deg;
 665         double magnetic_bug_error = tracking_bug - mag_track_bearing;
 666
 667         // Get the errors into the (-180,180) range.
 668         SG_NORMALIZE_RANGE(true_bug_error, -180.0, 180.0);
 669         SG_NORMALIZE_RANGE(magnetic_bug_error, -180.0, 180.0);
 670
 671         _true_bug_error_node->setDoubleValue(true_bug_error);
 672         _magnetic_bug_error_node->setDoubleValue(magnetic_bug_error);
 673
 674
 675         // Add WP 1 to the route.
 676         if ( addWp->getBoolValue() )
 677         {
 678             addWp->setBoolValue(false);
 679
 680             SGWayPoint tempWp( _wp1_longitude_node->getDoubleValue(),
 681                                _wp1_latitude_node->getDoubleValue(),
 682                                _wp1_altitude_node->getDoubleValue(),
 683                                SGWayPoint::WGS84,
 684                                _wp1_ID_node->getStringValue(),
 685                                _wp1_name_node->getStringValue() );
 686
 687             route->add_waypoint(tempWp);
 688
 689             SGPropertyNode *wp =
 690                 _route->getChild("Waypoint", route->size()-1, true);
 691             SGPropertyNode *id = wp->getChild("ID", 0, true);
 692             SGPropertyNode *name = wp->getChild("Name", 0, true);
 693             SGPropertyNode *lat = wp->getChild("Latitude", 0, true);
 694             SGPropertyNode *lon = wp->getChild("Longitude", 0, true);
 695             SGPropertyNode *alt = wp->getChild("Altitude", 0, true);
 696
 697             id->setStringValue( tempWp.get_id().c_str() );
 698             name->setStringValue( tempWp.get_name().c_str() );
 699             lat->setDoubleValue( tempWp.get_target_lat() );
 700             lon->setDoubleValue( tempWp.get_target_lon() );
 701             alt->setDoubleValue( tempWp.get_target_alt() );
 702         }
 703
 704         if ( popWp->getBoolValue() )
 705         {
 706             popWp->setBoolValue(false);
 707
 708             route->delete_first();
 709             _route->removeChild("Waypoint", 0, false);
 710         }
 711
 712     } else {
 713         _true_track_node->setDoubleValue(0.0);
 714         _magnetic_track_node->setDoubleValue(0.0);
 715         _speed_node->setDoubleValue(0.0);
 716     }
 717
 718     _last_valid = true;
 719     _last_longitude_deg = longitude_deg;
 720     _last_latitude_deg = latitude_deg;
 721     _last_altitude_m = altitude_m;
 722 }
 723
 724 // end of gps.cxx