X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FInstrumentation%2Fgps.cxx;h=729ad0526f70460098d9127db6f3a991713e3f6e;hb=4a94071ed78efdb6c8a9c5d44102dd48a001d920;hp=e05805d831b7982fb174e13d7a4d45fb1a19a735;hpb=d35b8db13f7aceee46d63b061cd20e6d3968f92a;p=flightgear.git diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx index e05805d83..729ad0526 100644 --- a/src/Instrumentation/gps.cxx +++ b/src/Instrumentation/gps.cxx @@ -9,13 +9,17 @@ #include "gps.hxx" +#include + #include #include +#include #include "Main/fg_props.hxx" #include "Main/globals.hxx" // for get_subsystem #include "Main/util.hxx" // for fgLowPass #include "Navaids/positioned.hxx" +#include #include "Navaids/navrecord.hxx" #include "Airports/simple.hxx" #include "Airports/runways.hxx" @@ -25,9 +29,11 @@ #include #include #include +#include using std::auto_ptr; using std::string; +using namespace flightgear; /////////////////////////////////////////////////////////////////// @@ -67,6 +73,13 @@ SGGeod SGGeodProperty::get() const { double lon = _lon->getDoubleValue(), lat = _lat->getDoubleValue(); + + if (osg::isNaN(lon) || osg::isNaN(lat)) { + SG_LOG(SG_INSTR, SG_WARN, "read NaN for lon/lat:" << _lon->getPath() + << ", " << _lat->getPath()); + return SGGeod(); + } + if (_alt) { return SGGeod::fromDegFt(lon, lat, _alt->getDoubleValue()); } else { @@ -144,10 +157,10 @@ public: DeprecatedPropListener(SGPropertyNode* gps) { _parents.insert(gps); - SGPropertyNode* wp = gps->getChild("wp"); + SGPropertyNode* wp = gps->getChild("wp", 0, true); _parents.insert(wp); - _parents.insert(wp->getChild("wp", 0)); - _parents.insert(wp->getChild("wp", 1)); + _parents.insert(wp->getChild("wp", 0, true)); + _parents.insert(wp->getChild("wp", 1, true)); std::set::iterator it; for (it = _parents.begin(); it != _parents.end(); ++it) { @@ -188,84 +201,40 @@ GPS::Config::Config() : _turnRate(3.0), // degrees-per-second, so 180 degree turn takes 60 seconds _overflightArmDistance(1.0), _waypointAlertTime(30.0), - _tuneRadio1ToRefVor(false), - _minRunwayLengthFt(0.0), _requireHardSurface(true), _cdiMaxDeflectionNm(3.0), // linear mode, 3nm at the peg - _driveAutopilot(true) + _driveAutopilot(true), + _courseSelectable(false) { _enableTurnAnticipation = false; - _extCourseSource = fgGetNode("/instrumentation/nav[0]/radials/selected-deg", true); } void GPS::Config::bind(GPS* aOwner, SGPropertyNode* aCfg) { aOwner->tie(aCfg, "turn-rate-deg-sec", SGRawValuePointer(&_turnRate)); - aOwner->tie(aCfg, "turn-anticipation", SGRawValuePointer(&_enableTurnAnticipation)); aOwner->tie(aCfg, "wpt-alert-time", SGRawValuePointer(&_waypointAlertTime)); - aOwner->tie(aCfg, "tune-nav-radio-to-ref-vor", SGRawValuePointer(&_tuneRadio1ToRefVor)); - aOwner->tie(aCfg, "min-runway-length-ft", SGRawValuePointer(&_minRunwayLengthFt)); aOwner->tie(aCfg, "hard-surface-runways-only", SGRawValuePointer(&_requireHardSurface)); - - aOwner->tie(aCfg, "course-source", SGRawValueMethods - (*this, &GPS::Config::getCourseSource, &GPS::Config::setCourseSource)); - aOwner->tie(aCfg, "cdi-max-deflection-nm", SGRawValuePointer(&_cdiMaxDeflectionNm)); aOwner->tie(aCfg, "drive-autopilot", SGRawValuePointer(&_driveAutopilot)); + aOwner->tie(aCfg, "course-selectable", SGRawValuePointer(&_courseSelectable)); } -const char* -GPS::Config::getCourseSource() const -{ - if (!_extCourseSource) { - return ""; - } - - return _extCourseSource->getPath(true).c_str(); -} - -void -GPS::Config::setCourseSource(const char* aPath) -{ - SGPropertyNode* nd = fgGetNode(aPath, false); - if (!nd) { - SG_LOG(SG_INSTR, SG_WARN, "couldn't find course source at:" << aPath); - _extCourseSource = NULL; - } - - _extCourseSource = nd; -} - -double -GPS::Config::getExternalCourse() const -{ - if (!_extCourseSource) { - return 0.0; - } - - return _extCourseSource->getDoubleValue(); -} - -void -GPS::Config::setExternalCourse(double aCourseDeg) -{ - if (!_extCourseSource) { - return; - } - - _extCourseSource->setDoubleValue(aCourseDeg); -} - //////////////////////////////////////////////////////////////////////////// GPS::GPS ( SGPropertyNode *node) : _selectedCourse(0.0), + _desiredCourse(0.0), _dataValid(false), _lastPosValid(false), _mode("init"), _name(node->getStringValue("name", "gps")), _num(node->getIntValue("number", 0)), + _searchResultIndex(0), + _searchExact(true), + _searchIsRoute(false), + _searchHasNext(false), + _searchNames(false), _computeTurnData(false), _anticipateTurn(false), _inTurn(false) @@ -273,6 +242,9 @@ GPS::GPS ( SGPropertyNode *node) : string branch = "/instrumentation/" + _name; _gpsNode = fgGetNode(branch.c_str(), _num, true ); _scratchNode = _gpsNode->getChild("scratch", 0, true); + + SGPropertyNode *wp_node = _gpsNode->getChild("wp", 0, true); + _currentWayptNode = wp_node->getChild("wp", 1, true); } GPS::~GPS () @@ -297,19 +269,13 @@ GPS::init () _trip_odometer_node = _gpsNode->getChild("trip-odometer", 0, true); _true_bug_error_node = _gpsNode->getChild("true-bug-error-deg", 0, true); _magnetic_bug_error_node = _gpsNode->getChild("magnetic-bug-error-deg", 0, true); + _eastWestVelocity = _gpsNode->getChild("ew-velocity-msec", 0, true); + _northSouthVelocity = _gpsNode->getChild("ns-velocity-msec", 0, true); // waypoints - SGPropertyNode *wp_node = _gpsNode->getChild("wp", 0, true); - SGPropertyNode *wp1_node = wp_node->getChild("wp", 1, true); - // for compatability, alias selected course down to wp/wp[1]/desired-course-deg - SGPropertyNode* wp1Crs = wp1_node->getChild("desired-course-deg", 0, true); - wp1Crs->alias(_gpsNode->getChild("selected-course-deg")); - -// _true_wp1_bearing_error_node = -// wp1_node->getChild("true-bearing-error-deg", 0, true); -// _magnetic_wp1_bearing_error_node = - // wp1_node->getChild("magnetic-bearing-error-deg", 0, true); + SGPropertyNode* wp1Crs = _currentWayptNode->getChild("desired-course-deg", 0, true); + wp1Crs->alias(_gpsNode->getChild("desired-course-deg", 0, true)); _tracking_bug_node = _gpsNode->getChild("tracking-bug", 0, true); @@ -343,12 +309,13 @@ GPS::init () // navradio slaving properties SGPropertyNode* toFlag = _gpsNode->getChild("to-flag", 0, true); - toFlag->alias(wp1_node->getChild("to-flag")); + toFlag->alias(_currentWayptNode->getChild("to-flag")); SGPropertyNode* fromFlag = _gpsNode->getChild("from-flag", 0, true); - fromFlag->alias(wp1_node->getChild("from-flag")); + fromFlag->alias(_currentWayptNode->getChild("from-flag")); // autopilot drive properties + _apDrivingFlag = fgGetNode("/autopilot/settings/gps-driving-true-heading", true); _apTrueHeading = fgGetNode("/autopilot/settings/true-heading-deg",true); _apTargetAltitudeFt = fgGetNode("/autopilot/settings/target-altitude-ft", true); _apAltitudeLock = fgGetNode("/autopilot/locks/altitude", true); @@ -361,7 +328,11 @@ GPS::init () // last thing, add the deprecated prop watcher new DeprecatedPropListener(_gpsNode); - +} + +void +GPS::reinit () +{ clearOutput(); } @@ -369,11 +340,15 @@ void GPS::bind() { _config.bind(this, _gpsNode->getChild("config", 0, true)); + // basic GPS outputs tie(_gpsNode, "selected-course-deg", SGRawValueMethods - (*this, &GPS::getSelectedCourse, NULL)); - - + (*this, &GPS::getSelectedCourse, &GPS::setSelectedCourse)); + + tie(_gpsNode, "desired-course-deg", SGRawValueMethods + (*this, &GPS::getDesiredCourse, NULL)); + _desiredCourseNode = _gpsNode->getChild("desired-course-deg", 0, true); + tieSGGeodReadOnly(_gpsNode, _indicated_pos, "indicated-longitude-deg", "indicated-latitude-deg", "indicated-altitude-ft"); @@ -386,7 +361,7 @@ GPS::bind() tie(_gpsNode, "indicated-ground-speed-kt", SGRawValueMethods (*this, &GPS::getGroundspeedKts, NULL)); -// command system +// command system tie(_gpsNode, "mode", SGRawValueMethods(*this, &GPS::getMode, NULL)); tie(_gpsNode, "command", SGRawValueMethods(*this, &GPS::getCommand, &GPS::setCommand)); @@ -397,49 +372,39 @@ GPS::bind() tie(_scratchNode, "mag-bearing-deg", SGRawValueMethods(*this, &GPS::getScratchMagBearing, NULL)); tie(_scratchNode, "has-next", SGRawValueMethods(*this, &GPS::getScratchHasNext, NULL)); _scratchValid = false; + -// waypoint data (including various historical things) SGPropertyNode *wp_node = _gpsNode->getChild("wp", 0, true); - SGPropertyNode *wp0_node = wp_node->getChild("wp", 0, true); - SGPropertyNode *wp1_node = wp_node->getChild("wp", 1, true); - + SGPropertyNode* wp0_node = wp_node->getChild("wp", 0, true); + tieSGGeodReadOnly(wp0_node, _wp0_position, "longitude-deg", "latitude-deg", "altitude-ft"); - tie(wp0_node, "ID", SGRawValueMethods - (*this, &GPS::getWP0Ident, NULL)); - tie(wp0_node, "name", SGRawValueMethods - (*this, &GPS::getWP0Name, NULL)); - - tieSGGeodReadOnly(wp1_node, _wp1_position, "longitude-deg", "latitude-deg", "altitude-ft"); - tie(wp1_node, "ID", SGRawValueMethods + tie(_currentWayptNode, "ID", SGRawValueMethods (*this, &GPS::getWP1Ident, NULL)); - tie(wp1_node, "name", SGRawValueMethods - (*this, &GPS::getWP1Name, NULL)); - tie(wp1_node, "distance-nm", SGRawValueMethods + tie(_currentWayptNode, "distance-nm", SGRawValueMethods (*this, &GPS::getWP1Distance, NULL)); - tie(wp1_node, "bearing-true-deg", SGRawValueMethods + tie(_currentWayptNode, "bearing-true-deg", SGRawValueMethods (*this, &GPS::getWP1Bearing, NULL)); - tie(wp1_node, "bearing-mag-deg", SGRawValueMethods + tie(_currentWayptNode, "bearing-mag-deg", SGRawValueMethods (*this, &GPS::getWP1MagBearing, NULL)); - tie(wp1_node, "TTW-sec", SGRawValueMethods + tie(_currentWayptNode, "TTW-sec", SGRawValueMethods (*this, &GPS::getWP1TTW, NULL)); - tie(wp1_node, "TTW", SGRawValueMethods + tie(_currentWayptNode, "TTW", SGRawValueMethods (*this, &GPS::getWP1TTWString, NULL)); - tie(wp1_node, "course-deviation-deg", SGRawValueMethods + tie(_currentWayptNode, "course-deviation-deg", SGRawValueMethods (*this, &GPS::getWP1CourseDeviation, NULL)); - tie(wp1_node, "course-error-nm", SGRawValueMethods + tie(_currentWayptNode, "course-error-nm", SGRawValueMethods (*this, &GPS::getWP1CourseErrorNm, NULL)); - tie(wp1_node, "to-flag", SGRawValueMethods + tie(_currentWayptNode, "to-flag", SGRawValueMethods (*this, &GPS::getWP1ToFlag, NULL)); - tie(wp1_node, "from-flag", SGRawValueMethods + tie(_currentWayptNode, "from-flag", SGRawValueMethods (*this, &GPS::getWP1FromFlag, NULL)); // leg properties (only valid in DTO/LEG modes, not OBS) tie(wp_node, "leg-distance-nm", SGRawValueMethods(*this, &GPS::getLegDistance, NULL)); tie(wp_node, "leg-true-course-deg", SGRawValueMethods(*this, &GPS::getLegCourse, NULL)); tie(wp_node, "leg-mag-course-deg", SGRawValueMethods(*this, &GPS::getLegMagCourse, NULL)); - tie(wp_node, "alt-dist-ratio", SGRawValueMethods(*this, &GPS::getAltDistanceRatio, NULL)); // navradio slaving properties tie(_gpsNode, "cdi-deflection", SGRawValueMethods @@ -449,10 +414,7 @@ GPS::bind() void GPS::unbind() { - for (unsigned int t=0; t<_tiedNodes.size(); ++t) { - _tiedNodes[t]->untie(); - } - _tiedNodes.clear(); + _tiedProperties.Untie(); } void @@ -465,17 +427,19 @@ GPS::clearOutput() _indicated_pos = SGGeod(); _last_vertical_speed = 0.0; _last_true_track = 0.0; + _lastEWVelocity = _lastNSVelocity = 0.0; + _currentWaypt = _prevWaypt = NULL; + _legDistanceNm = -1.0; _raim_node->setDoubleValue(0.0); _indicated_pos = SGGeod(); - _wp1DistanceM = 0.0; - _wp1TrueBearing = 0.0; - _wp1_position = SGGeod(); _odometer_node->setDoubleValue(0); _trip_odometer_node->setDoubleValue(0); _tracking_bug_node->setDoubleValue(0); _true_bug_error_node->setDoubleValue(0); _magnetic_bug_error_node->setDoubleValue(0); + _northSouthVelocity->setDoubleValue(0.0); + _eastWestVelocity->setDoubleValue(0.0); } void @@ -492,72 +456,33 @@ GPS::update (double delta_time_sec) if (delta_time_sec <= 0.0) { return; // paused, don't bother } - // TODO: Add noise and other errors. -/* - - // Bias and random error - double random_factor = sg_random(); - double random_error = 1.4; - double error_radius = 5.1; - double bias_max_radius = 5.1; - double random_max_radius = 1.4; - - bias_length += (random_factor-0.5) * 1.0e-3; - if (bias_length <= 0.0) bias_length = 0.0; - else if (bias_length >= bias_max_radius) bias_length = bias_max_radius; - bias_angle += (random_factor-0.5) * 1.0e-3; - if (bias_angle <= 0.0) bias_angle = 0.0; - else if (bias_angle >= 360.0) bias_angle = 360.0; - - double random_length = random_factor * random_max_radius; - double random_angle = random_factor * 360.0; - - double bias_x = bias_length * cos(bias_angle * SG_PI / 180.0); - double bias_y = bias_length * sin(bias_angle * SG_PI / 180.0); - double random_x = random_length * cos(random_angle * SG_PI / 180.0); - double random_y = random_length * sin(random_angle * SG_PI / 180.0); - double error_x = bias_x + random_x; - double error_y = bias_y + random_y; - double error_length = sqrt(error_x*error_x + error_y*error_y); - double error_angle = atan(error_y / error_x) * 180.0 / SG_PI; - - double lat2; - double lon2; - double az2; - geo_direct_wgs_84 ( altitude_m, latitude_deg, - longitude_deg, error_angle, - error_length, &lat2, &lon2, - &az2 ); - //cout << lat2 << " " << lon2 << endl; - printf("%f %f \n", bias_length, bias_angle); - printf("%3.7f %3.7f \n", lat2, lon2); - printf("%f %f \n", error_length, error_angle); - -*/ + _raim_node->setDoubleValue(1.0); _indicated_pos = _position.get(); updateBasicData(delta_time_sec); if (_dataValid) { - if (_mode == "obs") { - _selectedCourse = _config.getExternalCourse(); - } else { + if (_wayptController.get()) { + _wayptController->update(); + SGGeod p(_wayptController->position()); + _currentWayptNode->setDoubleValue("longitude-deg", p.getLongitudeDeg()); + _currentWayptNode->setDoubleValue("latitude-deg", p.getLatitudeDeg()); + _currentWayptNode->setDoubleValue("altitude-ft", p.getElevationFt()); + + _desiredCourse = getLegMagCourse(); + updateTurn(); + updateRouteData(); } - - updateWaypoints(); + + updateTrackingBug(); updateReferenceNavaid(delta_time_sec); - updateRouteData(); driveAutopilot(); } if (_dataValid && (_mode == "init")) { - // allow a realistic delay in the future, here - SG_LOG(SG_INSTR, SG_INFO, "GPS initialisation complete"); - - _selectedCourse = _config.getExternalCourse(); - + if (_route_active_node->getBoolValue()) { // GPS init with active route SG_LOG(SG_INSTR, SG_INFO, "GPS init with active route"); @@ -573,12 +498,61 @@ GPS::update (double delta_time_sec) selectOBSMode(); } } + + if (_mode != "init") + { + // allow a realistic delay in the future, here + SG_LOG(SG_INSTR, SG_INFO, "GPS initialisation complete"); + } } // of init mode check _last_pos = _indicated_pos; - _lastPosValid = true; + _lastPosValid = !(_last_pos == SGGeod()); +} + +/////////////////////////////////////////////////////////////////////////// +// implement the RNAV interface +SGGeod GPS::position() +{ + if (!_dataValid) { + return SGGeod(); + } + + return _indicated_pos; +} + +double GPS::trackDeg() +{ + return _last_true_track; +} + +double GPS::groundSpeedKts() +{ + return _last_speed_kts; +} + +double GPS::vspeedFPM() +{ + return _last_vertical_speed; +} + +double GPS::magvarDeg() +{ + return _magvar_node->getDoubleValue(); +} + +double GPS::overflightArmDistanceM() +{ + return _config.overflightArmDistanceNm() * SG_NM_TO_METER; +} + +double GPS::selectedMagCourse() +{ + return _selectedCourse; } +/////////////////////////////////////////////////////////////////////////// + void GPS::updateBasicData(double dt) { @@ -594,9 +568,28 @@ GPS::updateBasicData(double dt) double vertical_speed_mpm = ((_indicated_pos.getElevationM() - _last_pos.getElevationM()) * 60 / dt); _last_vertical_speed = vertical_speed_mpm * SG_METER_TO_FEET; - speed_kt = fgGetLowPass(_last_speed_kts, speed_kt, dt/20.0); + speed_kt = fgGetLowPass(_last_speed_kts, speed_kt, dt/10.0); _last_speed_kts = speed_kt; + SGGeod g = _indicated_pos; + g.setLongitudeDeg(_last_pos.getLongitudeDeg()); + double northSouthM = SGGeodesy::distanceM(_last_pos, g); + northSouthM = copysign(northSouthM, _indicated_pos.getLatitudeDeg() - _last_pos.getLatitudeDeg()); + + double nsMSec = fgGetLowPass(_lastNSVelocity, northSouthM / dt, dt/2.0); + _lastNSVelocity = nsMSec; + _northSouthVelocity->setDoubleValue(nsMSec); + + + g = _indicated_pos; + g.setLatitudeDeg(_last_pos.getLatitudeDeg()); + double eastWestM = SGGeodesy::distanceM(_last_pos, g); + eastWestM = copysign(eastWestM, _indicated_pos.getLongitudeDeg() - _last_pos.getLongitudeDeg()); + + double ewMSec = fgGetLowPass(_lastEWVelocity, eastWestM / dt, dt/2.0); + _lastEWVelocity = ewMSec; + _eastWestVelocity->setDoubleValue(ewMSec); + double odometer = _odometer_node->getDoubleValue(); _odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM); odometer = _trip_odometer_node->getDoubleValue(); @@ -623,24 +616,16 @@ GPS::updateTrackingBug() _magnetic_bug_error_node->setDoubleValue(magnetic_bug_error); } -void -GPS::updateWaypoints() -{ - double az2; - SGGeodesy::inverse(_indicated_pos, _wp1_position, _wp1TrueBearing, az2,_wp1DistanceM); -} - void GPS::updateReferenceNavaid(double dt) { if (!_ref_navaid_set) { _ref_navaid_elapsed += dt; if (_ref_navaid_elapsed > 5.0) { - _ref_navaid_elapsed = 0.0; FGPositioned::TypeFilter vorFilter(FGPositioned::VOR); FGPositionedRef nav = FGPositioned::findClosest(_indicated_pos, 400.0, &vorFilter); if (!nav) { - SG_LOG(SG_INSTR, SG_INFO, "GPS couldn't find a reference navid"); + SG_LOG(SG_INSTR, SG_INFO, "GPS couldn't find a reference navaid"); _ref_navaid_id_node->setStringValue(""); _ref_navaid_name_node->setStringValue(""); _ref_navaid_bearing_node->setDoubleValue(0.0); @@ -655,12 +640,15 @@ void GPS::updateReferenceNavaid(double dt) FGNavRecord* vor = (FGNavRecord*) nav.ptr(); _ref_navaid_frequency_node->setDoubleValue(vor->get_freq() / 100.0); _listener->setGuard(false); - tuneNavRadios(); } else { // SG_LOG(SG_INSTR, SG_ALERT, "matched existing"); } _ref_navaid = nav; + // reset elapsed time (do not do that before updating the properties above, since their + // listeners may request another update (_ref_navaid_elapsed = 9999), which results in + // excessive load (FGPositioned::findClosest called in every update loop...) + _ref_navaid_elapsed = 0.0; } } @@ -689,34 +677,17 @@ void GPS::referenceNavaidSet(const std::string& aNavaid) if (_ref_navaid) { _ref_navaid_set = true; - SG_LOG(SG_INSTR, SG_INFO, "GPS code set explict ref-navaid:" << _ref_navaid->ident()); + SG_LOG(SG_INSTR, SG_INFO, "GPS code set explicit ref-navaid:" << _ref_navaid->ident()); _ref_navaid_id_node->setStringValue(_ref_navaid->ident().c_str()); _ref_navaid_name_node->setStringValue(_ref_navaid->name().c_str()); FGNavRecord* vor = (FGNavRecord*) _ref_navaid.ptr(); _ref_navaid_frequency_node->setDoubleValue(vor->get_freq() / 100.0); - tuneNavRadios(); } else { _ref_navaid_set = false; _ref_navaid_elapsed = 9999.0; // update next tick } } -void GPS::tuneNavRadios() -{ - if (!_ref_navaid || !_config.tuneNavRadioToRefVor()) { - return; - } - - SGPropertyNode_ptr navRadio1 = fgGetNode("/instrumentation/nav", false); - if (!navRadio1) { - return; - } - - FGNavRecord* vor = (FGNavRecord*) _ref_navaid.ptr(); - SGPropertyNode_ptr freqs = navRadio1->getChild("frequencies"); - freqs->setDoubleValue("selected-mhz", vor->get_freq() / 100.0); -} - void GPS::routeActivated() { if (_route_active_node->getBoolValue()) { @@ -730,7 +701,9 @@ void GPS::routeActivated() } } else if (_mode == "leg") { SG_LOG(SG_INSTR, SG_INFO, "GPS::route deactivated, switching to OBS mode"); - selectOBSMode(); + // select OBS mode, but keep current waypoint-as is + _mode = "obs"; + wp1Changed(); } } @@ -741,9 +714,11 @@ void GPS::routeManagerSequenced() return; } - int index = _routeMgr->currentWaypoint(), - count = _routeMgr->size(); + int index = _routeMgr->currentIndex(), + count = _routeMgr->numWaypts(); if ((index < 0) || (index >= count)) { + _currentWaypt=NULL; + _prevWaypt=NULL; SG_LOG(SG_INSTR, SG_ALERT, "GPS: malformed route, index=" << index); return; } @@ -751,19 +726,18 @@ void GPS::routeManagerSequenced() SG_LOG(SG_INSTR, SG_INFO, "GPS waypoint index is now " << index); if (index > 0) { - SGWayPoint wp0(_routeMgr->get_waypoint(index - 1)); - _wp0Ident = wp0.get_id(); - _wp0Name = wp0.get_name(); - _wp0_position = wp0.get_target(); - + _prevWaypt = _routeMgr->wayptAtIndex(index - 1); + if (_prevWaypt->flag(WPT_DYNAMIC)) { + _wp0_position = _indicated_pos; + } else { + _wp0_position = _prevWaypt->position(); + } } - SGWayPoint wp1(_routeMgr->get_waypoint(index)); - _wp1Ident = wp1.get_id(); - _wp1Name = wp1.get_name(); - _wp1_position = wp1.get_target(); + _currentWaypt = _routeMgr->currentWaypt(); - _selectedCourse = getLegMagCourse(); + _desiredCourse = getLegMagCourse(); + _desiredCourseNode->fireValueChanged(); wp1Changed(); } @@ -784,8 +758,8 @@ void GPS::routeFinished() } SG_LOG(SG_INSTR, SG_INFO, "GPS route finished, reverting to OBS"); + // select OBS mode, but keep current waypoint-as is _mode = "obs"; - _wp0_position = _indicated_pos; wp1Changed(); } @@ -839,7 +813,7 @@ void GPS::updateTurn() double deviationNm = (distanceM * SG_METER_TO_NM) - _turnRadius; double deviationDeg = desiredCourse - getMagTrack(); deviationNm = copysign(deviationNm, deviationDeg); - // FXIME + // FIXME //_wp1_course_deviation_node->setDoubleValue(deviationDeg); //_wp1_course_error_nm_node->setDoubleValue(deviationNm); //_cdiDeflectionNode->setDoubleValue(deviationDeg); @@ -848,26 +822,29 @@ void GPS::updateTurn() void GPS::updateOverflight() { - if ((_wp1DistanceM * SG_METER_TO_NM) > _config.overflightArmDistanceNm()) { + if (!_wayptController->isDone()) { return; } - if (getWP1ToFlag()) { - return; // still heading towards the WP - } - if (_mode == "dto") { SG_LOG(SG_INSTR, SG_INFO, "GPS DTO reached destination point"); // check for wp1 being on active route - resume leg mode if (_routeMgr->isRouteActive()) { - int index = _routeMgr->findWaypoint(_wp1_position); + int index = _routeMgr->flightPlan()->findWayptIndex(_currentWaypt->position()); if (index >= 0) { SG_LOG(SG_INSTR, SG_INFO, "GPS DTO, resuming LEG mode at wp:" << index); _mode = "leg"; _routeMgr->jumpToIndex(index); } } + + if (_mode == "dto") { + // if we didn't enter leg mode, drop back to OBS mode + // select OBS mode, but keep current waypoint-as is + _mode = "obs"; + wp1Changed(); + } } else if (_mode == "leg") { SG_LOG(SG_INSTR, SG_INFO, "GPS doing overflight sequencing"); _routeMgr->sequence(); @@ -882,7 +859,7 @@ void GPS::beginTurn() { _inTurn = true; _turnSequenced = false; - SG_LOG(SG_INSTR, SG_INFO, "begining turn"); + SG_LOG(SG_INSTR, SG_INFO, "beginning turn"); } void GPS::endTurn() @@ -906,8 +883,8 @@ void GPS::computeTurnData() return; } - int curIndex = _routeMgr->currentWaypoint(); - if ((curIndex + 1) >= _routeMgr->size()) { + WayptRef next = _routeMgr->wayptAtIndex(_routeMgr->currentIndex() + 1); + if (!next || next->flag(WPT_DYNAMIC)) { _anticipateTurn = false; return; } @@ -917,13 +894,11 @@ void GPS::computeTurnData() return; } - _turnStartBearing = _selectedCourse; + _turnStartBearing = _desiredCourse; // compute next leg course - SGWayPoint wp1(_routeMgr->get_waypoint(curIndex)), - wp2(_routeMgr->get_waypoint(curIndex + 1)); double crs, dist; - wp2.CourseAndDistance(wp1, &crs, &dist); - + boost::tie(crs, dist) = next->courseAndDistanceFrom(_currentWaypt->position()); + // compute offset bearing _turnAngle = crs - _turnStartBearing; @@ -936,9 +911,9 @@ void GPS::computeTurnData() ", out=" << crs << "; turnAngle=" << _turnAngle << ", median=" << median << ", offset=" << offsetBearing); - SG_LOG(SG_INSTR, SG_INFO, "next leg is now:" << wp1.get_id() << "->" << wp2.get_id()); + SG_LOG(SG_INSTR, SG_INFO, "next leg is now:" << _currentWaypt->ident() << "->" << next->ident()); - _turnPt = _wp1_position; + _turnPt = _currentWaypt->position(); _anticipateTurn = true; } @@ -984,10 +959,10 @@ double GPS::computeTurnRadiusNm(double aGroundSpeedKts) const void GPS::updateRouteData() { - double totalDistance = _wp1DistanceM * SG_METER_TO_NM; + double totalDistance = _wayptController->distanceToWayptM() * SG_METER_TO_NM; // walk all waypoints from wp2 to route end, and sum - for (int i=_routeMgr->currentWaypoint()+1; i<_routeMgr->size(); ++i) { - totalDistance += _routeMgr->get_waypoint(i).get_distance(); + for (int i=_routeMgr->currentIndex()+1; i<_routeMgr->numWaypts(); ++i) { + //totalDistance += _routeMgr->get_waypoint(i).get_distance(); } _routeDistanceNm->setDoubleValue(totalDistance * SG_METER_TO_NM); @@ -1000,12 +975,17 @@ void GPS::updateRouteData() void GPS::driveAutopilot() { if (!_config.driveAutopilot() || !_realismSimpleGps->getBoolValue()) { + _apDrivingFlag->setBoolValue(false); return; } // compatability feature - allow the route-manager / GPS to drive the // generic autopilot heading hold *in leg mode only* - if (_mode == "leg") { + + bool drive = _mode == "leg"; + _apDrivingFlag->setBoolValue(drive); + + if (drive) { // FIXME: we want to set desired track, not heading, here _apTrueHeading->setDoubleValue(getWP1Bearing()); } @@ -1013,38 +993,73 @@ void GPS::driveAutopilot() void GPS::wp1Changed() { - // update external HSI/CDI/NavDisplay/PFD/etc - _config.setExternalCourse(getLegMagCourse()); + if (!_currentWaypt) + return; + if (_mode == "leg") { + _wayptController.reset(WayptController::createForWaypt(this, _currentWaypt)); + } else if (_mode == "obs") { + _wayptController.reset(new OBSController(this, _currentWaypt)); + } else if (_mode == "dto") { + _wayptController.reset(new DirectToController(this, _currentWaypt, _wp0_position)); + } + _wayptController->init(); + + if (_mode == "obs") { + _legDistanceNm = -1.0; + } else { + _wayptController->update(); + _legDistanceNm = _wayptController->distanceToWayptM() * SG_METER_TO_NM; + } + if (!_config.driveAutopilot()) { return; } - double altFt = _wp1_position.getElevationFt(); - if (altFt > -9990.0) { - _apTargetAltitudeFt->setDoubleValue(altFt); + RouteRestriction ar = _currentWaypt->altitudeRestriction(); + double restrictAlt = _currentWaypt->altitudeFt(); + double alt = _indicated_pos.getElevationFt(); + if ((ar == RESTRICT_AT) || + ((ar == RESTRICT_ABOVE) && (alt < restrictAlt)) || + ((ar == RESTRICT_BELOW) && (alt > restrictAlt))) + { + SG_LOG(SG_AUTOPILOT, SG_INFO, "current waypt has altitude set, setting on AP"); + _apTargetAltitudeFt->setDoubleValue(restrictAlt); } } ///////////////////////////////////////////////////////////////////////////// // property getter/setters +void GPS::setSelectedCourse(double crs) +{ + if (_selectedCourse == crs) { + return; + } + + _selectedCourse = crs; + if ((_mode == "obs") || _config.courseSelectable()) { + _desiredCourse = _selectedCourse; + _desiredCourseNode->fireValueChanged(); + } +} + double GPS::getLegDistance() const { if (!_dataValid || (_mode == "obs")) { return -1; } - return SGGeodesy::distanceNm(_wp0_position, _wp1_position); + return _legDistanceNm; } double GPS::getLegCourse() const { - if (!_dataValid) { + if (!_dataValid || !_wayptController.get()) { return -9999.0; } - return SGGeodesy::courseDeg(_wp0_position, _wp1_position); + return _wayptController->targetTrackDeg(); } double GPS::getLegMagCourse() const @@ -1058,21 +1073,6 @@ double GPS::getLegMagCourse() const return m; } -double GPS::getAltDistanceRatio() const -{ - if (!_dataValid || (_mode == "obs")) { - return 0.0; - } - - double dist = SGGeodesy::distanceM(_wp0_position, _wp1_position); - if ( dist <= 0.0 ) { - return 0.0; - } - - double alt_difference_m = _wp0_position.getElevationM() - _wp1_position.getElevationM(); - return alt_difference_m / dist; -} - double GPS::getMagTrack() const { if (!_dataValid) { @@ -1106,136 +1106,110 @@ double GPS::getCDIDeflection() const const char* GPS::getWP0Ident() const { - if (!_dataValid || (_mode != "leg")) { + if (!_dataValid || (_mode != "leg") || (!_prevWaypt)) { return ""; } - return _wp0Ident.c_str(); + return _prevWaypt->ident().c_str(); } const char* GPS::getWP0Name() const { - if (!_dataValid || (_mode != "leg")) { - return ""; - } - - return _wp0Name.c_str(); + return ""; } const char* GPS::getWP1Ident() const { - if (!_dataValid) { + if ((!_dataValid)||(!_currentWaypt)) { return ""; } - return _wp1Ident.c_str(); + return _currentWaypt->ident().c_str(); } const char* GPS::getWP1Name() const { - if (!_dataValid) { - return ""; - } - - return _wp1Name.c_str(); + return ""; } double GPS::getWP1Distance() const { - if (!_dataValid) { + if (!_dataValid || !_wayptController.get()) { return -1.0; } - return _wp1DistanceM * SG_METER_TO_NM; + return _wayptController->distanceToWayptM() * SG_METER_TO_NM; } double GPS::getWP1TTW() const { - if (!_dataValid) { + if (!_dataValid || !_wayptController.get()) { return -1.0; } - if (_last_speed_kts < 1.0) { - return -1.0; - } - - return (getWP1Distance() / _last_speed_kts) * 3600.0; + return _wayptController->timeToWaypt(); } const char* GPS::getWP1TTWString() const { - if (!_dataValid) { + double t = getWP1TTW(); + if (t <= 0.0) { return ""; } - return makeTTWString(getWP1TTW()); + return makeTTWString(t); } double GPS::getWP1Bearing() const { - if (!_dataValid) { + if (!_dataValid || !_wayptController.get()) { return -9999.0; } - return _wp1TrueBearing; + return _wayptController->trueBearingDeg(); } double GPS::getWP1MagBearing() const { - if (!_dataValid) { + if (!_dataValid || !_wayptController.get()) { return -9999.0; } - double magBearing = _wp1TrueBearing - _magvar_node->getDoubleValue(); + double magBearing = _wayptController->trueBearingDeg() - _magvar_node->getDoubleValue(); SG_NORMALIZE_RANGE(magBearing, 0.0, 360.0); return magBearing; } double GPS::getWP1CourseDeviation() const { - if (!_dataValid) { + if (!_dataValid || !_wayptController.get()) { return 0.0; } - - double dev = getWP1MagBearing() - _selectedCourse; - SG_NORMALIZE_RANGE(dev, -180.0, 180.0); - - if (fabs(dev) > 90.0) { - // When the course is away from the waypoint, - // it makes sense to change the sign of the deviation. - dev *= -1.0; - SG_NORMALIZE_RANGE(dev, -90.0, 90.0); - } - - return dev; + + return _wayptController->courseDeviationDeg(); } double GPS::getWP1CourseErrorNm() const { - if (!_dataValid) { + if (!_dataValid || !_wayptController.get()) { return 0.0; } - double radDev = getWP1CourseDeviation() * SG_DEGREES_TO_RADIANS; - double course_error_m = sin(radDev) * _wp1DistanceM; - return course_error_m * SG_METER_TO_NM; + return _wayptController->xtrackErrorNm(); } bool GPS::getWP1ToFlag() const { - if (!_dataValid) { + if (!_dataValid || !_wayptController.get()) { return false; } - double dev = getWP1MagBearing() - _selectedCourse; - SG_NORMALIZE_RANGE(dev, -180.0, 180.0); - - return (fabs(dev) < 90.0); + return _wayptController->toFlag(); } bool GPS::getWP1FromFlag() const { - if (!_dataValid) { + if (!_dataValid || !_wayptController.get()) { return false; } @@ -1302,9 +1276,9 @@ void GPS::setCommand(const char* aCmd) defineWaypoint(); } else if (!strcmp(aCmd, "route-insert-before")) { int index = _scratchNode->getIntValue("index"); - if (index < 0 || (_routeMgr->size() == 0)) { - index = _routeMgr->size(); - } else if (index >= _routeMgr->size()) { + if (index < 0 || (_routeMgr->numWaypts() == 0)) { + index = _routeMgr->numWaypts(); + } else if (index >= _routeMgr->numWaypts()) { SG_LOG(SG_INSTR, SG_WARN, "GPS:route-insert-before, bad index:" << index); return; } @@ -1312,9 +1286,9 @@ void GPS::setCommand(const char* aCmd) insertWaypointAtIndex(index); } else if (!strcmp(aCmd, "route-insert-after")) { int index = _scratchNode->getIntValue("index"); - if (index < 0 || (_routeMgr->size() == 0)) { - index = _routeMgr->size(); - } else if (index >= _routeMgr->size()) { + if (index < 0 || (_routeMgr->numWaypts() == 0)) { + index = _routeMgr->numWaypts(); + } else if (index >= _routeMgr->numWaypts()) { SG_LOG(SG_INSTR, SG_WARN, "GPS:route-insert-after, bad index:" << index); return; } else { @@ -1325,8 +1299,8 @@ void GPS::setCommand(const char* aCmd) } else if (!strcmp(aCmd, "route-delete")) { int index = _scratchNode->getIntValue("index"); if (index < 0) { - index = _routeMgr->size(); - } else if (index >= _routeMgr->size()) { + index = _routeMgr->numWaypts(); + } else if (index >= _routeMgr->numWaypts()) { SG_LOG(SG_INSTR, SG_WARN, "GPS:route-delete, bad index:" << index); return; } @@ -1356,21 +1330,17 @@ bool GPS::isScratchPositionValid() const } void GPS::directTo() -{ +{ if (!isScratchPositionValid()) { - SG_LOG(SG_INSTR, SG_WARN, "invalid DTO lat/lon"); return; } + _prevWaypt = NULL; _wp0_position = _indicated_pos; - _wp1Ident = _scratchNode->getStringValue("ident"); - _wp1Name = _scratchNode->getStringValue("name"); - _wp1_position = _scratchPos; - + _currentWaypt = new BasicWaypt(_scratchPos, _scratchNode->getStringValue("ident"), NULL); _mode = "dto"; - _selectedCourse = getLegMagCourse(); + clearScratch(); - wp1Changed(); } @@ -1385,8 +1355,8 @@ void GPS::loadRouteWaypoint() int index = _scratchNode->getIntValue("index", -9999); clearScratch(); - if ((index < 0) || (index >= _routeMgr->size())) { // no index supplied, use current wp - index = _routeMgr->currentWaypoint(); + if ((index < 0) || (index >= _routeMgr->numWaypts())) { // no index supplied, use current wp + index = _routeMgr->currentIndex(); } _searchIsRoute = true; @@ -1396,22 +1366,17 @@ void GPS::loadRouteWaypoint() void GPS::setScratchFromRouteWaypoint(int aIndex) { assert(_searchIsRoute); - if ((aIndex < 0) || (aIndex >= _routeMgr->size())) { + if ((aIndex < 0) || (aIndex >= _routeMgr->numWaypts())) { SG_LOG(SG_INSTR, SG_WARN, "GPS:setScratchFromRouteWaypoint: route-index out of bounds"); return; } _searchResultIndex = aIndex; - SGWayPoint wp(_routeMgr->get_waypoint(aIndex)); - _scratchNode->setStringValue("name", wp.get_name()); - _scratchNode->setStringValue("ident", wp.get_id()); - _scratchPos = wp.get_target(); + WayptRef wp = _routeMgr->wayptAtIndex(aIndex); + _scratchNode->setStringValue("ident", wp->ident()); + _scratchPos = wp->position(); _scratchValid = true; - _scratchNode->setDoubleValue("course", wp.get_track()); _scratchNode->setIntValue("index", aIndex); - - int lastResult = _routeMgr->size() - 1; - _searchHasNext = (_searchResultIndex < lastResult); } void GPS::loadNearest() @@ -1436,17 +1401,14 @@ void GPS::loadNearest() _searchResults = FGPositioned::findClosestN(searchPos, limitCount, cutoffDistance, f.get()); - _searchResultsCached = true; _searchResultIndex = 0; _searchIsRoute = false; - _searchHasNext = false; if (_searchResults.empty()) { SG_LOG(SG_INSTR, SG_INFO, "GPS:loadNearest: no matches at all"); return; } - _searchHasNext = (_searchResults.size() > 1); setScratchFromCachedSearchResult(); } @@ -1479,7 +1441,7 @@ FGPositioned::Type GPS::SearchFilter::maxType() const FGPositioned::Filter* GPS::createFilter(FGPositioned::Type aTy) { if (aTy == FGPositioned::AIRPORT) { - return new FGAirport::HardSurfaceFilter(_config.minRunwayLengthFt()); + return new FGAirport::HardSurfaceFilter(); } // if we were passed INVALID, assume it means 'all types interesting to a GPS' @@ -1503,66 +1465,47 @@ void GPS::search() } _searchExact = _scratchNode->getBoolValue("exact", true); - _searchOrderByRange = _scratchNode->getBoolValue("order-by-distance", true); _searchResultIndex = 0; _searchIsRoute = false; - _searchHasNext = false; - - if (_searchExact && _searchOrderByRange) { - // immediate mode search, get all the results now and cache them - auto_ptr f(createFilter(_searchType)); - if (_searchNames) { - _searchResults = FGPositioned::findAllWithNameSortedByRange(_searchQuery, _indicated_pos, f.get()); - } else { - _searchResults = FGPositioned::findAllWithIdentSortedByRange(_searchQuery, _indicated_pos, f.get()); - } - - _searchResultsCached = true; - - if (_searchResults.empty()) { - clearScratch(); - return; - } - - _searchHasNext = (_searchResults.size() > 1); - setScratchFromCachedSearchResult(); - } else { - // iterative search, look up result zero - _searchResultsCached = false; - performSearch(); - } -} -void GPS::performSearch() -{ auto_ptr f(createFilter(_searchType)); - clearScratch(); - - FGPositionedRef r; if (_searchNames) { - if (_searchOrderByRange) { - r = FGPositioned::findClosestWithPartialName(_indicated_pos, _searchQuery, f.get(), _searchResultIndex, _searchHasNext); - } else { - r = FGPositioned::findWithPartialName(_searchQuery, f.get(), _searchResultIndex, _searchHasNext); - } + _searchResults = FGPositioned::findAllWithName(_searchQuery, f.get(), _searchExact); } else { - if (_searchOrderByRange) { - r = FGPositioned::findClosestWithPartialId(_indicated_pos, _searchQuery, f.get(), _searchResultIndex, _searchHasNext); - } else { - r = FGPositioned::findWithPartialId(_searchQuery, f.get(), _searchResultIndex, _searchHasNext); - } + _searchResults = FGPositioned::findAllWithIdent(_searchQuery, f.get(), _searchExact); + } + + bool orderByRange = _scratchNode->getBoolValue("order-by-distance", true); + if (orderByRange) { + FGPositioned::sortByRange(_searchResults, _indicated_pos); } - if (!r) { + if (_searchResults.empty()) { + clearScratch(); return; } - setScratchFromPositioned(r.get(), _searchResultIndex); + setScratchFromCachedSearchResult(); +} + +bool GPS::getScratchHasNext() const +{ + int lastResult; + if (_searchIsRoute) { + lastResult = _routeMgr->numWaypts() - 1; + } else { + lastResult = (int) _searchResults.size() - 1; + } + + if (lastResult < 0) { // search array might be empty + return false; + } + + return (_searchResultIndex < lastResult); } void GPS::setScratchFromCachedSearchResult() { - assert(_searchResultsCached); int index = _searchResultIndex; if ((index < 0) || (index >= (int) _searchResults.size())) { @@ -1571,9 +1514,6 @@ void GPS::setScratchFromCachedSearchResult() } setScratchFromPositioned(_searchResults[index], index); - - int lastResult = (int) _searchResults.size() - 1; - _searchHasNext = (_searchResultIndex < lastResult); } void GPS::setScratchFromPositioned(FGPositioned* aPos, int aIndex) @@ -1591,9 +1531,7 @@ void GPS::setScratchFromPositioned(FGPositioned* aPos, int aIndex) } _scratchValid = true; - if (_searchResultsCached) { - _scratchNode->setIntValue("result-count", _searchResults.size()); - } + _scratchNode->setIntValue("result-count", _searchResults.size()); switch (aPos->type()) { case FGPositioned::VOR: @@ -1641,16 +1579,13 @@ void GPS::addAirportToScratch(FGAirport* aAirport) void GPS::selectOBSMode() { if (!isScratchPositionValid()) { - SG_LOG(SG_INSTR, SG_WARN, "invalid OBS lat/lon"); return; } SG_LOG(SG_INSTR, SG_INFO, "GPS switching to OBS mode"); _mode = "obs"; - - _wp1Ident = _scratchNode->getStringValue("ident"); - _wp1Name = _scratchNode->getStringValue("name"); - _wp1_position = _scratchPos; + + _currentWaypt = new BasicWaypt(_scratchPos, _scratchNode->getStringValue("ident"), NULL); _wp0_position = _indicated_pos; wp1Changed(); } @@ -1680,20 +1615,17 @@ void GPS::selectLegMode() void GPS::nextResult() { - if (!_searchHasNext) { + if (!getScratchHasNext()) { return; } clearScratch(); if (_searchIsRoute) { setScratchFromRouteWaypoint(++_searchResultIndex); - } else if (_searchResultsCached) { - ++_searchResultIndex; - setScratchFromCachedSearchResult(); } else { ++_searchResultIndex; - performSearch(); - } // of iterative search case + setScratchFromCachedSearchResult(); + } } void GPS::previousResult() @@ -1707,10 +1639,8 @@ void GPS::previousResult() if (_searchIsRoute) { setScratchFromRouteWaypoint(_searchResultIndex); - } else if (_searchResultsCached) { - setScratchFromCachedSearchResult(); } else { - performSearch(); + setScratchFromCachedSearchResult(); } } @@ -1729,21 +1659,22 @@ void GPS::defineWaypoint() // check for duplicate idents FGPositioned::TypeFilter f(FGPositioned::WAYPOINT); - FGPositioned::List dups = FGPositioned::findAllWithIdentSortedByRange(ident, _indicated_pos, &f); + FGPositioned::List dups = FGPositioned::findAllWithIdent(ident, &f); if (!dups.empty()) { SG_LOG(SG_INSTR, SG_WARN, "GPS:defineWaypoint: non-unique waypoint identifier, ho-hum"); } SG_LOG(SG_INSTR, SG_INFO, "GPS:defineWaypoint: creating waypoint:" << ident); FGPositionedRef wpt = FGPositioned::createUserWaypoint(ident, _scratchPos); - _searchResultsCached = false; + _searchResults.clear(); + _searchResults.push_back(wpt); setScratchFromPositioned(wpt.get(), -1); } void GPS::insertWaypointAtIndex(int aIndex) { // note we do allow index = routeMgr->size(), that's an append - if ((aIndex < 0) || (aIndex > _routeMgr->size())) { + if ((aIndex < 0) || (aIndex > _routeMgr->numWaypts())) { throw sg_range_exception("GPS::insertWaypointAtIndex: index out of bounds"); } @@ -1753,18 +1684,18 @@ void GPS::insertWaypointAtIndex(int aIndex) } string ident = _scratchNode->getStringValue("ident"); - string name = _scratchNode->getStringValue("name"); - - _routeMgr->add_waypoint(SGWayPoint(_scratchPos, ident, name), aIndex); + + WayptRef wpt = new BasicWaypt(_scratchPos, ident, NULL); + _routeMgr->flightPlan()->insertWayptAtIndex(wpt, aIndex); } void GPS::removeWaypointAtIndex(int aIndex) { - if ((aIndex < 0) || (aIndex >= _routeMgr->size())) { + if ((aIndex < 0) || (aIndex >= _routeMgr->numWaypts())) { throw sg_range_exception("GPS::removeWaypointAtIndex: index out of bounds"); } - _routeMgr->pop_waypoint(aIndex); + _routeMgr->removeLegAtIndex(aIndex); } void GPS::tieSGGeod(SGPropertyNode* aNode, SGGeod& aRef,