From e89df85817619290067d20391817b831356c4532 Mon Sep 17 00:00:00 2001 From: James Turner Date: Wed, 27 Mar 2013 20:47:40 +0000 Subject: [PATCH] GPSs uses FlightPlans directly. --- src/Autopilot/route_mgr.cxx | 166 ++--- src/Autopilot/route_mgr.hxx | 30 +- src/Instrumentation/gps.cxx | 817 ++++------------------ src/Instrumentation/gps.hxx | 114 +-- src/Instrumentation/instrument_mgr.cxx | 2 +- src/Navaids/FlightPlan.cxx | 40 +- src/Navaids/FlightPlan.hxx | 5 +- src/Navaids/NavDataCache.cxx | 4 +- src/Navaids/NavDataCache.hxx | 2 +- src/Navaids/positioned.cxx | 26 +- src/Navaids/positioned.hxx | 7 +- src/Scripting/NasalPositioned.cxx | 51 +- src/Scripting/NasalPositioned_cppbind.cxx | 10 +- 13 files changed, 331 insertions(+), 943 deletions(-) diff --git a/src/Autopilot/route_mgr.cxx b/src/Autopilot/route_mgr.cxx index 96f76325d..3773c2e06 100644 --- a/src/Autopilot/route_mgr.cxx +++ b/src/Autopilot/route_mgr.cxx @@ -215,13 +215,17 @@ FGRouteMgr::FGRouteMgr() : input->setStringValue(""); input->addChangeListener(listener); - SGCommandMgr::instance()->addCommand("load-flightplan", commandLoadFlightPlan); - SGCommandMgr::instance()->addCommand("save-flightplan", commandSaveFlightPlan); - SGCommandMgr::instance()->addCommand("activate-flightplan", commandActivateFlightPlan); - SGCommandMgr::instance()->addCommand("clear-flightplan", commandClearFlightPlan); - SGCommandMgr::instance()->addCommand("set-active-waypt", commandSetActiveWaypt); - SGCommandMgr::instance()->addCommand("insert-waypt", commandInsertWaypt); - SGCommandMgr::instance()->addCommand("delete-waypt", commandDeleteWaypt); + SGCommandMgr* cmdMgr = SGCommandMgr::instance(); + cmdMgr->addCommand("define-user-waypoint", this, &FGRouteMgr::commandDefineUserWaypoint); + cmdMgr->addCommand("delete-user-waypoint", this, &FGRouteMgr::commandDeleteUserWaypoint); + + cmdMgr->addCommand("load-flightplan", commandLoadFlightPlan); + cmdMgr->addCommand("save-flightplan", commandSaveFlightPlan); + cmdMgr->addCommand("activate-flightplan", commandActivateFlightPlan); + cmdMgr->addCommand("clear-flightplan", commandClearFlightPlan); + cmdMgr->addCommand("set-active-waypt", commandSetActiveWaypt); + cmdMgr->addCommand("insert-waypt", commandInsertWaypt); + cmdMgr->addCommand("delete-waypt", commandDeleteWaypt); } @@ -229,6 +233,10 @@ FGRouteMgr::~FGRouteMgr() { input->removeChangeListener(listener); delete listener; + + SGCommandMgr* cmdMgr = SGCommandMgr::instance(); + //cmdMgr->removeCommand("define-user-waypoint"); + } @@ -306,14 +314,12 @@ void FGRouteMgr::init() { airborne->setBoolValue(false); _edited = fgGetNode(RM "signals/edited", true); - _finished = fgGetNode(RM "signals/finished", true); _flightplanChanged = fgGetNode(RM "signals/flightplan-changed", true); _currentWpt = fgGetNode(RM "current-wp", true); _currentWpt->tie(SGRawValueMethods (*this, &FGRouteMgr::currentIndex, &FGRouteMgr::jumpToIndex)); - // temporary distance / eta calculations, for backward-compatability wp0 = fgGetNode(RM "wp", 0, true); wp0->getChild("id", 0, true); wp0->getChild("dist", 0, true); @@ -454,10 +460,6 @@ void FGRouteMgr::update( double dt ) return; } - if (checkFinished()) { - endOfRoute(); - } - // basic course/distance information SGGeod currentPos = globals->get_aircraft_position(); @@ -590,11 +592,6 @@ void FGRouteMgr::waypointsChanged() { update_mirror(); _edited->fireValueChanged(); - -// removing waypoints, deactivate if we hit the end. - if (currentIndex() >= numLegs()) { - endOfRoute(); - } } // mirror internal route to the property system for inspection by other subsystems @@ -713,63 +710,10 @@ void FGRouteMgr::InputListener::valueChanged(SGPropertyNode *prop) r++; if (*r) mgr->flightPlan()->insertWayptAtIndex(mgr->waypointFromString(r), pos); - } else if (!strcmp(s, "@POSINIT")) { - mgr->initAtPosition(); } else mgr->flightPlan()->insertWayptAtIndex(mgr->waypointFromString(s), -1); } -void FGRouteMgr::initAtPosition() -{ - if (isRouteActive()) { - return; // don't mess with the active route - } - - if (haveUserWaypoints()) { - // user has already defined, loaded or entered a route, again - // don't interfere with it - return; - } - - if (airborne->getBoolValue()) { - SG_LOG(SG_AUTOPILOT, SG_INFO, "initAtPosition: airborne, clearing departure info"); - _plan->setDeparture((FGAirport*) NULL); - return; - } - -// on the ground - SGGeod pos = globals->get_aircraft_position(); - if (!_plan->departureAirport()) { - _plan->setDeparture(FGAirport::findClosest(pos, 20.0)); - if (!_plan->departureAirport()) { - SG_LOG(SG_AUTOPILOT, SG_INFO, "initAtPosition: couldn't find an airport within 20nm"); - return; - } - } - - std::string rwy = departure->getStringValue("runway"); - FGRunway* r = NULL; - if (!rwy.empty()) { - r = _plan->departureAirport()->getRunwayByIdent(rwy); - } else { - r = _plan->departureAirport()->findBestRunwayForPos(pos); - } - - if (!r) { - return; - } - - _plan->setDeparture(r); - SG_LOG(SG_AUTOPILOT, SG_INFO, "initAtPosition: starting at " - << _plan->departureAirport()->ident() << " on runway " << r->ident()); -} - -bool FGRouteMgr::haveUserWaypoints() const -{ - // FIXME - return false; -} - bool FGRouteMgr::activate() { if (!_plan) { @@ -798,52 +742,6 @@ void FGRouteMgr::deactivate() active->setBoolValue(false); } -void FGRouteMgr::sequence() -{ - if (!_plan || !active->getBoolValue()) { - SG_LOG(SG_AUTOPILOT, SG_ALERT, "trying to sequence waypoints with no active route"); - return; - } - - int nextIndex = _plan->currentIndex() + 1; - if (nextIndex >= _plan->numLegs()) { - SG_LOG(SG_AUTOPILOT, SG_INFO, "sequenced on final leg, deactivating route"); - endOfRoute(); - return; - } - - _plan->setCurrentIndex(nextIndex); -} - -void FGRouteMgr::endOfRoute() -{ - SG_LOG(SG_AUTOPILOT, SG_INFO, "reached end of active route"); - _finished->fireValueChanged(); - active->setBoolValue(false); -} - -bool FGRouteMgr::checkFinished() -{ - if (!_plan) { - return true; - } - - bool done = false; -// done if we're stopped on the destination runway - if (_plan->currentLeg() && - (_plan->currentLeg()->waypoint()->source() == _plan->destinationRunway())) - { - double gs = groundSpeed->getDoubleValue(); - done = weightOnWheels->getBoolValue() && (gs < 25); - } - - if (done) { - SG_LOG(SG_AUTOPILOT, SG_INFO, "checkFinished: on the ground on destination runway, we're done"); - } - - return done; -} - void FGRouteMgr::jumpToIndex(int index) { if (!_plan) { @@ -1212,3 +1110,37 @@ SGPropertyNode_ptr FGRouteMgr::wayptNodeAtIndex(int index) const return mirror->getChild("wp", index); } + +bool FGRouteMgr::commandDefineUserWaypoint(const SGPropertyNode* arg) +{ + std::string ident = arg->getStringValue("ident"); + if (ident.empty()) { + SG_LOG(SG_AUTOPILOT, SG_WARN, "missing ident defining user waypoint"); + return false; + } + + // check for duplicate idents + FGPositioned::TypeFilter f(FGPositioned::WAYPOINT); + FGPositionedList dups = FGPositioned::findAllWithIdent(ident, &f); + if (!dups.empty()) { + SG_LOG(SG_AUTOPILOT, SG_WARN, "defineUserWaypoint: non-unique waypoint identifier:" << ident); + return false; + } + + SGGeod pos(SGGeod::fromDeg(arg->getDoubleValue("longitude-deg"), + arg->getDoubleValue("latitude-deg"))); + FGPositioned::createUserWaypoint(ident, pos); + return true; +} + +bool FGRouteMgr::commandDeleteUserWaypoint(const SGPropertyNode* arg) +{ + std::string ident = arg->getStringValue("ident"); + if (ident.empty()) { + SG_LOG(SG_AUTOPILOT, SG_WARN, "missing ident deleting user waypoint"); + return false; + } + + return FGPositioned::deleteUserWaypoint(ident); +} + diff --git a/src/Autopilot/route_mgr.hxx b/src/Autopilot/route_mgr.hxx index b66a06b72..9fddc80f9 100644 --- a/src/Autopilot/route_mgr.hxx +++ b/src/Autopilot/route_mgr.hxx @@ -90,11 +90,6 @@ public: */ void deactivate(); - /** - * Step to the next waypoint on the active route - */ - void sequence(); - /** * Set the current waypoint to the specified index. */ @@ -104,13 +99,11 @@ public: bool loadRoute(const SGPath& p); flightgear::WayptRef waypointFromString(const std::string& target); - - /** - * Helper command to setup current airport/runway if necessary - */ - void initAtPosition(); private: + bool commandDefineUserWaypoint(const SGPropertyNode* arg); + bool commandDeleteUserWaypoint(const SGPropertyNode* arg); + flightgear::FlightPlan* _plan; time_t _takeoffTime; @@ -188,23 +181,6 @@ private: virtual void currentWaypointChanged(); - /** - * Check if we've reached the final waypoint. - * Returns true if we have. - */ - bool checkFinished(); - - /* - * update state when we pass the final waypoint - */ - void endOfRoute(); - - /** - * Predicate for helping the UI - test if at least one waypoint was - * entered by the user (as opposed to being generated by the route-manager) - */ - bool haveUserWaypoints() const; - // tied getters and setters const char* getDepartureICAO() const; const char* getDepartureName() const; diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx index baba6e34b..a224d066d 100644 --- a/src/Instrumentation/gps.cxx +++ b/src/Instrumentation/gps.cxx @@ -22,6 +22,7 @@ #include "Navaids/positioned.hxx" #include #include "Navaids/navrecord.hxx" +#include "Navaids/FlightPlan.hxx" #include "Airports/airport.hxx" #include "Airports/runways.hxx" #include "Autopilot/route_mgr.hxx" @@ -54,94 +55,6 @@ static const char* makeTTWString(double TTW) return TTW_str; } -///////////////////////////////////////////////////////////////////////////// - -class GPSListener : public SGPropertyChangeListener -{ -public: - GPSListener(GPS *m) : - _gps(m), - _guard(false) {} - - virtual void valueChanged (SGPropertyNode * prop) - { - if (_guard) { - return; - } - - _guard = true; - if (prop == _gps->_route_current_wp_node) { - _gps->routeManagerSequenced(); - } else if (prop == _gps->_route_active_node) { - _gps->routeActivated(); - } else if (prop == _gps->_ref_navaid_id_node) { - _gps->referenceNavaidSet(prop->getStringValue("")); - } else if (prop == _gps->_routeEditedSignal) { - _gps->routeEdited(); - } else if (prop == _gps->_routeFinishedSignal) { - _gps->routeFinished(); - } - - _guard = false; - } - - void setGuard(bool g) { - _guard = g; - } -private: - GPS* _gps; - bool _guard; // re-entrancy guard -}; - -//////////////////////////////////////////////////////////////////////////// -/** - * Helper to monitor for Nasal or other code accessing properties we haven't - * defined. For the moment we complain about all such activites, since various - * users assume all kinds of weird, wonderful and non-existent interfaces. - */ - -class DeprecatedPropListener : public SGPropertyChangeListener -{ -public: - DeprecatedPropListener(SGPropertyNode* gps) - { - _parents.insert(gps); - SGPropertyNode* wp = gps->getChild("wp", 0, true); - _parents.insert(wp); - _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) { - (*it)->addChangeListener(this); - } - } - - virtual void valueChanged (SGPropertyNode * prop) - { - } - - virtual void childAdded (SGPropertyNode * parent, SGPropertyNode * child) - { - if (isDeprecated(parent, child)) { - SG_LOG(SG_INSTR, SG_WARN, "GPS: someone accessed a deprecated property:" - << child->getPath(true)); - } - } -private: - bool isDeprecated(SGPropertyNode * parent, SGPropertyNode * child) const - { - if (_parents.count(parent) < 1) { - return false; - } - - // no child exclusions yet - return true; - } - - std::set _parents; -}; - //////////////////////////////////////////////////////////////////////////// // configuration helper object @@ -171,22 +84,22 @@ void GPS::Config::bind(GPS* aOwner, SGPropertyNode* aCfg) //////////////////////////////////////////////////////////////////////////// -GPS::GPS ( SGPropertyNode *node) : +GPS::GPS ( SGPropertyNode *node, bool defaultGPSMode) : _selectedCourse(0.0), _desiredCourse(0.0), _dataValid(false), _lastPosValid(false), + _defaultGPSMode(defaultGPSMode), _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) + _inTurn(false), + _callbackFlightPlanChanged(SGPropertyChangeCallback(this,&GPS::routeManagerFlightPlanChanged, + fgGetNode("/autopilot/route-manager/signals/flightplan-changed", true))), + _callbackRouteActivated(SGPropertyChangeCallback(this,&GPS::routeActivated, + fgGetNode("/autopilot/route-manager/active", true))) { string branch = "/instrumentation/" + _name; _gpsNode = fgGetNode(branch.c_str(), _num, true ); @@ -203,9 +116,6 @@ GPS::~GPS () void GPS::init () { - _routeMgr = (FGRouteMgr*) globals->get_subsystem("route-manager"); - assert(_routeMgr); - _magvar_node = fgGetNode("/environment/magnetic-variation-deg", true); _serviceable_node = _gpsNode->getChild("serviceable", 0, true); _serviceable_node->setBoolValue(true); @@ -226,36 +136,14 @@ GPS::init () wp1Crs->alias(_gpsNode->getChild("desired-course-deg", 0, true)); _tracking_bug_node = _gpsNode->getChild("tracking-bug", 0, true); - -// reference navid - SGPropertyNode_ptr ref_navaid = _gpsNode->getChild("ref-navaid", 0, true); - _ref_navaid_id_node = ref_navaid->getChild("id", 0, true); - _ref_navaid_name_node = ref_navaid->getChild("name", 0, true); - _ref_navaid_bearing_node = ref_navaid->getChild("bearing-deg", 0, true); - _ref_navaid_frequency_node = ref_navaid->getChild("frequency-mhz", 0, true); - _ref_navaid_distance_node = ref_navaid->getChild("distance-nm", 0, true); - _ref_navaid_mag_bearing_node = ref_navaid->getChild("mag-bearing-deg", 0, true); - _ref_navaid_elapsed = 0.0; - _ref_navaid_set = false; -// route properties +// route properties // should these move to the route manager? _routeDistanceNm = _gpsNode->getChild("route-distance-nm", 0, true); _routeETE = _gpsNode->getChild("ETE", 0, true); - _routeEditedSignal = fgGetNode("/autopilot/route-manager/signals/edited", true); - _routeFinishedSignal = fgGetNode("/autopilot/route-manager/signals/finished", true); - -// add listener to various things - _listener = new GPSListener(this); - _route_current_wp_node = fgGetNode("/autopilot/route-manager/current-wp", true); - _route_current_wp_node->addChangeListener(_listener); - _route_active_node = fgGetNode("/autopilot/route-manager/active", true); - _route_active_node->addChangeListener(_listener); - _ref_navaid_id_node->addChangeListener(_listener); - _routeEditedSignal->addChangeListener(_listener); - _routeFinishedSignal->addChangeListener(_listener); - -// navradio slaving properties + + +// navradio slaving properties SGPropertyNode* toFlag = _gpsNode->getChild("to-flag", 0, true); toFlag->alias(_currentWayptNode->getChild("to-flag")); @@ -265,19 +153,8 @@ GPS::init () // 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); - -// realism prop[s] - _realismSimpleGps = fgGetNode("/sim/realism/simple-gps", true); - if (!_realismSimpleGps->hasValue()) { - _realismSimpleGps->setBoolValue(true); - } - - clearOutput(); - // last thing, add the deprecated prop watcher - new DeprecatedPropListener(_gpsNode); + clearOutput(); } void @@ -314,20 +191,15 @@ GPS::bind() // command system tie(_gpsNode, "mode", SGRawValueMethods(*this, &GPS::getMode, NULL)); tie(_gpsNode, "command", SGRawValueMethods(*this, &GPS::getCommand, &GPS::setCommand)); - tieSGGeod(_scratchNode, _scratchPos, "longitude-deg", "latitude-deg", "altitude-ft"); - tie(_scratchNode, "valid", SGRawValueMethods(*this, &GPS::getScratchValid, NULL)); - tie(_scratchNode, "distance-nm", SGRawValueMethods(*this, &GPS::getScratchDistance, NULL)); - tie(_scratchNode, "true-bearing-deg", SGRawValueMethods(*this, &GPS::getScratchTrueBearing, NULL)); - tie(_scratchNode, "mag-bearing-deg", SGRawValueMethods(*this, &GPS::getScratchMagBearing, NULL)); - tie(_scratchNode, "has-next", SGRawValueMethods(*this, &GPS::getScratchHasNext, NULL)); - _scratchValid = false; - SGPropertyNode *wp_node = _gpsNode->getChild("wp", 0, 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(_currentWayptNode, "ID", SGRawValueMethods (*this, &GPS::getWP1Ident, NULL)); @@ -395,7 +267,7 @@ GPS::clearOutput() void GPS::update (double delta_time_sec) { - if (!_realismSimpleGps->getBoolValue()) { + if (!_defaultGPSMode) { // If it's off, don't bother. if (!_serviceable_node->getBoolValue() || !_electrical_node->getBoolValue()) { clearOutput(); @@ -427,25 +299,21 @@ GPS::update (double delta_time_sec) updateTrackingBug(); - updateReferenceNavaid(delta_time_sec); driveAutopilot(); } if (_dataValid && (_mode == "init")) { - - if (_route_active_node->getBoolValue()) { - // GPS init with active route - SG_LOG(SG_INSTR, SG_INFO, "GPS init with active route"); - selectLegMode(); - } else { + // will select LEG mode if the route is active + routeManagerFlightPlanChanged(NULL); + + FGRouteMgr* routeMgr = static_cast(globals->get_subsystem("route-manager")); + if (!routeMgr->isRouteActive()) { // initialise in OBS mode, with waypt set to the nearest airport. // keep in mind at this point, _dataValid is not set - - auto_ptr f(createFilter(FGPositioned::AIRPORT)); + auto_ptr f(new FGAirport::HardSurfaceFilter()); FGPositionedRef apt = FGPositioned::findClosest(_indicated_pos, 20.0, f.get()); if (apt) { - setScratchFromPositioned(apt, 0); - selectOBSMode(); + selectOBSMode(new flightgear::NavaidWaypoint(apt, NULL)); } } @@ -459,6 +327,44 @@ GPS::update (double delta_time_sec) _lastPosValid = !(_last_pos == SGGeod()); } +void GPS::routeManagerFlightPlanChanged(SGPropertyNode*) +{ + if (_route) { + _route->removeDelegate(this); + } + + SG_LOG(SG_INSTR, SG_INFO, "GPS saw route-manager flight-plan replaced."); + FGRouteMgr* routeMgr = static_cast(globals->get_subsystem("route-manager")); + _route = routeMgr->flightPlan(); + if (_route) { + _route->addDelegate(this); + } + + if (routeMgr->isRouteActive()) { + selectLegMode(); + } else { + selectOBSMode(_currentWaypt); // revert to OBS on current waypoint + } +} + +void GPS::routeActivated(SGPropertyNode* aNode) +{ + bool isActive = aNode->getBoolValue(); + if (isActive) { + SG_LOG(SG_INSTR, SG_INFO, "GPS::route activated, switching to LEG mode"); + selectLegMode(); + + // if we've already passed the current waypoint, sequence. + if (_dataValid && getWP1FromFlag()) { + SG_LOG(SG_INSTR, SG_INFO, "GPS::route activated, FROM wp1, sequencing"); + sequence(); + } + } else if (_mode == "leg") { + SG_LOG(SG_INSTR, SG_INFO, "GPS::route deactivated, switching to OBS mode"); + selectOBSMode(_currentWaypt); + } +} + /////////////////////////////////////////////////////////////////////////// // implement the RNAV interface SGGeod GPS::position() @@ -564,117 +470,28 @@ GPS::updateTrackingBug() _magnetic_bug_error_node->setDoubleValue(magnetic_bug_error); } -void GPS::updateReferenceNavaid(double dt) -{ - if (!_ref_navaid_set) { - _ref_navaid_elapsed += dt; - if (_ref_navaid_elapsed > 5.0) { - - FGPositioned::TypeFilter vorFilter(FGPositioned::VOR); - FGPositionedRef nav = FGPositioned::findClosest(_indicated_pos, 400.0, &vorFilter); - if (!nav) { - SG_LOG(SG_INSTR, SG_DEBUG, "GPS couldn't find a reference navaid"); - _ref_navaid_id_node->setStringValue(""); - _ref_navaid_name_node->setStringValue(""); - _ref_navaid_bearing_node->setDoubleValue(0.0); - _ref_navaid_mag_bearing_node->setDoubleValue(0.0); - _ref_navaid_distance_node->setDoubleValue(0.0); - _ref_navaid_frequency_node->setStringValue(""); - } else if (nav != _ref_navaid) { - SG_LOG(SG_INSTR, SG_DEBUG, "GPS code selected new ref-navaid:" << nav->ident()); - _listener->setGuard(true); - _ref_navaid_id_node->setStringValue(nav->ident().c_str()); - _ref_navaid_name_node->setStringValue(nav->name().c_str()); - FGNavRecord* vor = (FGNavRecord*) nav.ptr(); - _ref_navaid_frequency_node->setDoubleValue(vor->get_freq() / 100.0); - _listener->setGuard(false); - } 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; - } - } - - if (_ref_navaid) { - double trueCourse, distanceM, az2; - SGGeodesy::inverse(_indicated_pos, _ref_navaid->geod(), trueCourse, az2, distanceM); - _ref_navaid_distance_node->setDoubleValue(distanceM * SG_METER_TO_NM); - _ref_navaid_bearing_node->setDoubleValue(trueCourse); - _ref_navaid_mag_bearing_node->setDoubleValue(trueCourse - _magvar_node->getDoubleValue()); - } -} - -void GPS::referenceNavaidSet(const std::string& aNavaid) +void GPS::endOfFlightPlan() { - _ref_navaid = NULL; - // allow setting an empty string to restore normal nearest-vor selection - if (aNavaid.size() > 0) { - FGPositioned::TypeFilter vorFilter(FGPositioned::VOR); - _ref_navaid = FGPositioned::findClosestWithIdent(aNavaid, - _indicated_pos, &vorFilter); - - if (!_ref_navaid) { - SG_LOG(SG_INSTR, SG_ALERT, "GPS: unknown ref navaid:" << aNavaid); - } - } - - if (_ref_navaid) { - _ref_navaid_set = true; - 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); - } else { - _ref_navaid_set = false; - _ref_navaid_elapsed = 9999.0; // update next tick - } + selectOBSMode(_currentWaypt); } -void GPS::routeActivated() +void GPS::currentWaypointChanged() { - if (_route_active_node->getBoolValue()) { - SG_LOG(SG_INSTR, SG_INFO, "GPS::route activated, switching to LEG mode"); - selectLegMode(); - - // if we've already passed the current waypoint, sequence. - if (_dataValid && getWP1FromFlag()) { - SG_LOG(SG_INSTR, SG_INFO, "GPS::route activated, FROM wp1, sequencing"); - _routeMgr->sequence(); - } - } else if (_mode == "leg") { - SG_LOG(SG_INSTR, SG_INFO, "GPS::route deactivated, switching to OBS mode"); - // select OBS mode, but keep current waypoint-as is - _mode = "obs"; - wp1Changed(); - } -} - -void GPS::routeManagerSequenced() -{ - if (_mode != "leg") { - SG_LOG(SG_INSTR, SG_DEBUG, "GPS ignoring route sequencing, not in LEG mode"); + if (!_route) { return; } - int index = _routeMgr->currentIndex(), - count = _routeMgr->numWaypts(); + int index = _route->currentIndex(), + count = _route->numLegs(); if ((index < 0) || (index >= count)) { _currentWaypt=NULL; _prevWaypt=NULL; // no active leg on the route return; } - - SG_LOG(SG_INSTR, SG_DEBUG, "GPS waypoint index is now " << index); - + if (index > 0) { - _prevWaypt = _routeMgr->wayptAtIndex(index - 1); + _prevWaypt = _route->previousLeg()->waypoint(); if (_prevWaypt->flag(WPT_DYNAMIC)) { _wp0_position = _indicated_pos; } else { @@ -682,33 +499,48 @@ void GPS::routeManagerSequenced() } } - _currentWaypt = _routeMgr->currentWaypt(); - + _currentWaypt = _route->currentLeg()->waypoint(); _desiredCourse = getLegMagCourse(); _desiredCourseNode->fireValueChanged(); wp1Changed(); } -void GPS::routeEdited() + +void GPS::waypointsChanged() { if (_mode != "leg") { return; } SG_LOG(SG_INSTR, SG_INFO, "GPS route edited while in LEG mode, updating waypoints"); - routeManagerSequenced(); + currentWaypointChanged(); } -void GPS::routeFinished() +void GPS::cleared() { - if (_mode != "leg") { - return; - } + if (_mode != "leg") { + return; + } + + selectOBSMode(_currentWaypt); +} + +void GPS::sequence() +{ + if (!_route) { + return; + } - SG_LOG(SG_INSTR, SG_INFO, "GPS route finished, reverting to OBS"); - // select OBS mode, but keep current waypoint-as is - _mode = "obs"; - wp1Changed(); + int nextIndex = _route->currentIndex() + 1; + if (nextIndex >= _route->numLegs()) { + SG_LOG(SG_INSTR, SG_INFO, "sequenced final leg, end of route"); + _route->finish(); + selectOBSMode(_currentWaypt); + return; + } + + // will callback into currentWaypointChanged + _route->setCurrentIndex(nextIndex); } void GPS::updateTurn() @@ -747,7 +579,7 @@ void GPS::updateTurn() if (_inTurn && !_turnSequenced && (progress > 0.5)) { _turnSequenced = true; SG_LOG(SG_INSTR, SG_INFO, "turn passed midpoint, sequencing"); - _routeMgr->sequence(); + sequence(); } if (_inTurn && (progress >= 1.0)) { @@ -778,12 +610,13 @@ void GPS::updateOverflight() 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->flightPlan()->findWayptIndex(_currentWaypt->position()); + if (_route) { + int index = _route->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); + _route->setCurrentIndex(index); + sequence(); // and sequence to the next point } } @@ -795,7 +628,7 @@ void GPS::updateOverflight() } } else if (_mode == "leg") { SG_LOG(SG_INSTR, SG_DEBUG, "GPS doing overflight sequencing"); - _routeMgr->sequence(); + sequence(); } else if (_mode == "obs") { // nothing to do here, TO/FROM will update but that's fine } @@ -826,13 +659,12 @@ double GPS::computeTurnProgress(double aBearing) const void GPS::computeTurnData() { _computeTurnData = false; - int nextIndex = _routeMgr->currentIndex() + 1; - if ((_mode != "leg") || (nextIndex >= _routeMgr->numWaypts())) { + if ((_mode != "leg") || !_route->nextLeg()) { _anticipateTurn = false; return; } - WayptRef next = _routeMgr->wayptAtIndex(nextIndex); + WayptRef next = _route->nextLeg()->waypoint(); if (next->flag(WPT_DYNAMIC) || !_config.turnAnticipationEnabled()) { _anticipateTurn = false; return; @@ -905,9 +737,9 @@ void GPS::updateRouteData() { double totalDistance = _wayptController->distanceToWayptM() * SG_METER_TO_NM; // walk all waypoints from wp2 to route end, and sum - for (int i=_routeMgr->currentIndex()+1; i<_routeMgr->numWaypts(); ++i) { + // for (int i=_routeMgr->currentIndex()+1; i<_routeMgr->numWaypts(); ++i) { //totalDistance += _routeMgr->get_waypoint(i).get_distance(); - } + //} _routeDistanceNm->setDoubleValue(totalDistance * SG_METER_TO_NM); if (_last_speed_kts > 1.0) { @@ -918,7 +750,7 @@ void GPS::updateRouteData() void GPS::driveAutopilot() { - if (!_config.driveAutopilot() || !_realismSimpleGps->getBoolValue()) { + if (!_config.driveAutopilot() || !_defaultGPSMode) { _apDrivingFlag->setBoolValue(false); return; } @@ -955,21 +787,6 @@ void GPS::wp1Changed() _wayptController->update(); _legDistanceNm = _wayptController->distanceToWayptM() * SG_METER_TO_NM; } - - if (!_config.driveAutopilot()) { - return; - } - - 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); - } } ///////////////////////////////////////////////////////////////////////////// @@ -1160,37 +977,8 @@ bool GPS::getWP1FromFlag() const return !getWP1ToFlag(); } -double GPS::getScratchDistance() const -{ - if (!_scratchValid) { - return 0.0; - } - - return SGGeodesy::distanceNm(_indicated_pos, _scratchPos); -} - -double GPS::getScratchTrueBearing() const -{ - if (!_scratchValid) { - return 0.0; - } - - return SGGeodesy::courseDeg(_indicated_pos, _scratchPos); -} - -double GPS::getScratchMagBearing() const -{ - if (!_scratchValid) { - return 0.0; - } - - double crs = getScratchTrueBearing() - _magvar_node->getDoubleValue(); - SG_NORMALIZE_RANGE(crs, 0.0, 360.0); - return crs; -} - ///////////////////////////////////////////////////////////////////////////// -// command / scratch / search system +// scratch system void GPS::setCommand(const char* aCmd) { @@ -1199,57 +987,9 @@ void GPS::setCommand(const char* aCmd) if (!strcmp(aCmd, "direct")) { directTo(); } else if (!strcmp(aCmd, "obs")) { - selectOBSMode(); + selectOBSMode(NULL); } else if (!strcmp(aCmd, "leg")) { selectLegMode(); - } else if (!strcmp(aCmd, "load-route-wpt")) { - loadRouteWaypoint(); - } else if (!strcmp(aCmd, "nearest")) { - loadNearest(); - } else if (!strcmp(aCmd, "search")) { - _searchNames = false; - search(); - } else if (!strcmp(aCmd, "search-names")) { - _searchNames = true; - search(); - } else if (!strcmp(aCmd, "next")) { - nextResult(); - } else if (!strcmp(aCmd, "previous")) { - previousResult(); - } else if (!strcmp(aCmd, "define-user-wpt")) { - defineWaypoint(); - } else if (!strcmp(aCmd, "route-insert-before")) { - int index = _scratchNode->getIntValue("index"); - 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; - } - - insertWaypointAtIndex(index); - } else if (!strcmp(aCmd, "route-insert-after")) { - int index = _scratchNode->getIntValue("index"); - 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 { - ++index; - } - - insertWaypointAtIndex(index); - } else if (!strcmp(aCmd, "route-delete")) { - int index = _scratchNode->getIntValue("index"); - if (index < 0) { - index = _routeMgr->numWaypts(); - } else if (index >= _routeMgr->numWaypts()) { - SG_LOG(SG_INSTR, SG_WARN, "GPS:route-delete, bad index:" << index); - return; - } - - removeWaypointAtIndex(index); } else { SG_LOG(SG_INSTR, SG_WARN, "GPS:unrecognized command:" << aCmd); } @@ -1258,9 +998,7 @@ void GPS::setCommand(const char* aCmd) void GPS::clearScratch() { _scratchPos = SGGeod::fromDegFt(-9999.0, -9999.0, -9999.0); - _scratchValid = false; - _scratchNode->setStringValue("type", ""); - _scratchNode->setStringValue("query", ""); + _scratchNode->setBoolValue("valid", false); } bool GPS::isScratchPositionValid() const @@ -1288,245 +1026,19 @@ void GPS::directTo() wp1Changed(); } -void GPS::loadRouteWaypoint() +void GPS::selectOBSMode(flightgear::Waypt* waypt) { - _scratchValid = false; -// if (!_routeMgr->isRouteActive()) { -// SG_LOG(SG_INSTR, SG_WARN, "GPS:loadWaypoint: no active route"); -// return; -// } - - int index = _scratchNode->getIntValue("index", -9999); - clearScratch(); - - if ((index < 0) || (index >= _routeMgr->numWaypts())) { // no index supplied, use current wp - index = _routeMgr->currentIndex(); - } - - _searchIsRoute = true; - setScratchFromRouteWaypoint(index); -} - -void GPS::setScratchFromRouteWaypoint(int aIndex) -{ - assert(_searchIsRoute); - if ((aIndex < 0) || (aIndex >= _routeMgr->numWaypts())) { - SG_LOG(SG_INSTR, SG_WARN, "GPS:setScratchFromRouteWaypoint: route-index out of bounds"); - return; - } - - _searchResultIndex = aIndex; - WayptRef wp = _routeMgr->wayptAtIndex(aIndex); - _scratchNode->setStringValue("ident", wp->ident()); - _scratchPos = wp->position(); - _scratchValid = true; - _scratchNode->setIntValue("index", aIndex); -} - -void GPS::loadNearest() -{ - string sty(_scratchNode->getStringValue("type")); - FGPositioned::Type ty = FGPositioned::typeFromName(sty); - if (ty == FGPositioned::INVALID) { - SG_LOG(SG_INSTR, SG_WARN, "GPS:loadNearest: request type is invalid:" << sty); - return; - } - - auto_ptr f(createFilter(ty)); - int limitCount = _scratchNode->getIntValue("max-results", 1); - double cutoffDistance = _scratchNode->getDoubleValue("cutoff-nm", 400.0); - - SGGeod searchPos = _indicated_pos; - if (isScratchPositionValid()) { - searchPos = _scratchPos; - } - - clearScratch(); // clear now, regardless of whether we find a match or not - - _searchResults = - FGPositioned::findClosestN(searchPos, limitCount, cutoffDistance, f.get()); - _searchResultIndex = 0; - _searchIsRoute = false; - - if (_searchResults.empty()) { - return; - } - - setScratchFromCachedSearchResult(); -} - -bool GPS::SearchFilter::pass(FGPositioned* aPos) const -{ - switch (aPos->type()) { - case FGPositioned::AIRPORT: - // heliport and seaport too? - case FGPositioned::VOR: - case FGPositioned::NDB: - case FGPositioned::FIX: - case FGPositioned::TACAN: - case FGPositioned::WAYPOINT: - return true; - default: - return false; - } -} - -FGPositioned::Type GPS::SearchFilter::minType() const -{ - return FGPositioned::AIRPORT; -} - -FGPositioned::Type GPS::SearchFilter::maxType() const -{ - return FGPositioned::VOR; -} - -FGPositioned::Filter* GPS::createFilter(FGPositioned::Type aTy) -{ - if (aTy == FGPositioned::AIRPORT) { - return new FGAirport::HardSurfaceFilter(); - } - - // if we were passed INVALID, assume it means 'all types interesting to a GPS' - if (aTy == FGPositioned::INVALID) { - return new SearchFilter; - } - - return new FGPositioned::TypeFilter(aTy); -} - -void GPS::search() -{ - // parse search terms into local members, and exec the first search - string sty(_scratchNode->getStringValue("type")); - _searchType = FGPositioned::typeFromName(sty); - _searchQuery = _scratchNode->getStringValue("query"); - if (_searchQuery.empty()) { - SG_LOG(SG_INSTR, SG_WARN, "empty GPS search query"); - clearScratch(); - return; - } - - _searchExact = _scratchNode->getBoolValue("exact", true); - _searchResultIndex = 0; - _searchIsRoute = false; - - auto_ptr f(createFilter(_searchType)); - if (_searchNames) { - _searchResults = FGPositioned::findAllWithName(_searchQuery, f.get(), _searchExact); - } else { - _searchResults = FGPositioned::findAllWithIdent(_searchQuery, f.get(), _searchExact); - } - - bool orderByRange = _scratchNode->getBoolValue("order-by-distance", true); - if (orderByRange) { - FGPositioned::sortByRange(_searchResults, _indicated_pos); - } - - if (_searchResults.empty()) { - clearScratch(); - return; - } - - 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() -{ - int index = _searchResultIndex; - - if ((index < 0) || (index >= (int) _searchResults.size())) { - SG_LOG(SG_INSTR, SG_WARN, "GPS:setScratchFromCachedSearchResult: index out of bounds:" << index); - return; - } - - setScratchFromPositioned(_searchResults[index], index); -} - -void GPS::setScratchFromPositioned(FGPositioned* aPos, int aIndex) -{ - clearScratch(); - assert(aPos); - - _scratchPos = aPos->geod(); - _scratchNode->setStringValue("name", aPos->name()); - _scratchNode->setStringValue("ident", aPos->ident()); - _scratchNode->setStringValue("type", FGPositioned::nameForType(aPos->type())); - - if (aIndex >= 0) { - _scratchNode->setIntValue("index", aIndex); - } - - _scratchValid = true; - _scratchNode->setIntValue("result-count", _searchResults.size()); - - switch (aPos->type()) { - case FGPositioned::VOR: - _scratchNode->setDoubleValue("frequency-mhz", static_cast(aPos)->get_freq() / 100.0); - break; - - case FGPositioned::NDB: - _scratchNode->setDoubleValue("frequency-khz", static_cast(aPos)->get_freq() / 100.0); - break; - - case FGPositioned::AIRPORT: - addAirportToScratch((FGAirport*)aPos); - break; - - default: - // no-op - break; - } - - // look for being on the route and set? -} - -void GPS::addAirportToScratch(FGAirport* aAirport) -{ - for (unsigned int r=0; rnumRunways(); ++r) { - SGPropertyNode* rwyNd = _scratchNode->getChild("runways", r, true); - FGRunway* rwy = aAirport->getRunwayByIndex(r); - // TODO - filter out unsuitable runways in the future - // based on config again - - rwyNd->setStringValue("id", rwy->ident().c_str()); - rwyNd->setIntValue("length-ft", rwy->lengthFt()); - rwyNd->setIntValue("width-ft", rwy->widthFt()); - rwyNd->setIntValue("heading-deg", rwy->headingDeg()); - // map surface code to a string - // TODO - lighting information - - if (rwy->ILS()) { - rwyNd->setDoubleValue("ils-frequency-mhz", rwy->ILS()->get_freq() / 100.0); + if (!waypt) { + // initialise from scratch + if (!isScratchPositionValid()) { + return; } - } // of runways iteration -} - - -void GPS::selectOBSMode() -{ - if (!isScratchPositionValid()) { - return; + + waypt = new BasicWaypt(_scratchPos, _scratchNode->getStringValue("ident"), NULL); } _mode = "obs"; - _currentWaypt = new BasicWaypt(_scratchPos, _scratchNode->getStringValue("ident"), NULL); + _currentWaypt = waypt; _wp0_position = _indicated_pos; wp1Changed(); } @@ -1537,7 +1049,7 @@ void GPS::selectLegMode() return; } - if (!_routeMgr->isRouteActive()) { + if (!_route) { SG_LOG(SG_INSTR, SG_WARN, "GPS:selectLegMode: no active route"); return; } @@ -1549,92 +1061,7 @@ void GPS::selectLegMode() _wp0_position = _indicated_pos; // not really sequenced, but does all the work of updating wp0/1 - routeManagerSequenced(); -} - -void GPS::nextResult() -{ - if (!getScratchHasNext()) { - return; - } - - clearScratch(); - if (_searchIsRoute) { - setScratchFromRouteWaypoint(++_searchResultIndex); - } else { - ++_searchResultIndex; - setScratchFromCachedSearchResult(); - } -} - -void GPS::previousResult() -{ - if (_searchResultIndex <= 0) { - return; - } - - clearScratch(); - --_searchResultIndex; - - if (_searchIsRoute) { - setScratchFromRouteWaypoint(_searchResultIndex); - } else { - setScratchFromCachedSearchResult(); - } -} - -void GPS::defineWaypoint() -{ - if (!isScratchPositionValid()) { - SG_LOG(SG_INSTR, SG_WARN, "GPS:defineWaypoint: invalid lat/lon"); - return; - } - - string ident = _scratchNode->getStringValue("ident"); - if (ident.size() < 2) { - SG_LOG(SG_INSTR, SG_WARN, "GPS:defineWaypoint: waypoint identifier must be at least two characters"); - return; - } - -// check for duplicate idents - FGPositioned::TypeFilter f(FGPositioned::WAYPOINT); - FGPositionedList 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); - _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->numWaypts())) { - throw sg_range_exception("GPS::insertWaypointAtIndex: index out of bounds"); - } - - if (!isScratchPositionValid()) { - SG_LOG(SG_INSTR, SG_WARN, "GPS:insertWaypointAtIndex: invalid lat/lon"); - return; - } - - string ident = _scratchNode->getStringValue("ident"); - - WayptRef wpt = new BasicWaypt(_scratchPos, ident, NULL); - _routeMgr->flightPlan()->insertWayptAtIndex(wpt, aIndex); -} - -void GPS::removeWaypointAtIndex(int aIndex) -{ - if ((aIndex < 0) || (aIndex >= _routeMgr->numWaypts())) { - throw sg_range_exception("GPS::removeWaypointAtIndex: index out of bounds"); - } - - _routeMgr->removeLegAtIndex(aIndex); + currentWaypointChanged(); } void GPS::tieSGGeod(SGPropertyNode* aNode, SGGeod& aRef, diff --git a/src/Instrumentation/gps.hxx b/src/Instrumentation/gps.hxx index c284d87b8..55cbb1b22 100644 --- a/src/Instrumentation/gps.hxx +++ b/src/Instrumentation/gps.hxx @@ -15,12 +15,9 @@ #include #include +#include #include -// forward decls -class SGRoute; -class FGRouteMgr; -class GPSListener; /** * Model a GPS radio. @@ -57,10 +54,12 @@ class GPSListener; * /instrumentation/gps/true-bug-error-deg * /instrumentation/gps/magnetic-bug-error-deg */ -class GPS : public SGSubsystem, public flightgear::RNAV +class GPS : public SGSubsystem, + public flightgear::RNAV, + public flightgear::FlightPlan::Delegate { public: - GPS (SGPropertyNode *node); + GPS (SGPropertyNode *node, bool defaultGPSMode = false); GPS (); virtual ~GPS (); @@ -82,7 +81,6 @@ public: virtual double overflightArmDistanceM(); private: - friend class GPSListener; friend class SearchFilter; /** @@ -142,9 +140,6 @@ private: // (in seconds) double _waypointAlertTime; - // minimum runway length to require when filtering - double _minRunwayLengthFt; - // should we require a hard-surfaced runway when filtering? bool _requireHardSurface; @@ -172,15 +167,8 @@ private: void updateBasicData(double dt); void updateTrackingBug(); - void updateReferenceNavaid(double dt); - void referenceNavaidSet(const std::string& aNavaid); void updateRouteData(); void driveAutopilot(); - - void routeActivated(); - void routeManagerSequenced(); - void routeEdited(); - void routeFinished(); void updateTurn(); void updateOverflight(); @@ -195,49 +183,24 @@ private: /** Update one-shot things when WP1 / leg data change */ void wp1Changed(); -// scratch maintenance utilities - void setScratchFromPositioned(FGPositioned* aPos, int aIndex); - void setScratchFromCachedSearchResult(); - void setScratchFromRouteWaypoint(int aIndex); - - /** Add airport-specific information to a scratch result */ - void addAirportToScratch(FGAirport* aAirport); - void clearScratch(); /** Predicate, determine if the lon/lat position in the scratch is * valid or not. */ bool isScratchPositionValid() const; - FGPositioned::Filter* createFilter(FGPositioned::Type aTy); - - /** Search kernel - called each time we step through a result */ - void performSearch(); // command handlers void selectLegMode(); - void selectOBSMode(); + void selectOBSMode(flightgear::Waypt* waypt); void directTo(); - void loadRouteWaypoint(); - void loadNearest(); - void search(); - void nextResult(); - void previousResult(); - void defineWaypoint(); - void insertWaypointAtIndex(int aIndex); - void removeWaypointAtIndex(int aIndex); // tied-property getter/setters void setCommand(const char* aCmd); const char* getCommand() const { return ""; } const char* getMode() const { return _mode.c_str(); } - bool getScratchValid() const { return _scratchValid; } - double getScratchDistance() const; - double getScratchMagBearing() const; - double getScratchTrueBearing() const; - bool getScratchHasNext() const; double getSelectedCourse() const { return _selectedCourse; } void setSelectedCourse(double crs); @@ -254,9 +217,6 @@ private: double getGroundspeedKts() const { return _last_speed_kts; } double getVerticalSpeed() const { return _last_vertical_speed; } - //bool getLegMode() const { return _mode == "leg"; } - //bool getObsMode() const { return _mode == "obs"; } - const char* getWP0Ident() const; const char* getWP0Name() const; @@ -293,6 +253,16 @@ private: void tieSGGeodReadOnly(SGPropertyNode* aNode, SGGeod& aRef, const char* lonStr, const char* latStr, const char* altStr); +// FlightPlan::Delegate + virtual void currentWaypointChanged(); + virtual void waypointsChanged(); + virtual void cleared(); + virtual void endOfFlightPlan(); + + void sequence(); + void routeManagerFlightPlanChanged(SGPropertyNode*); + void routeActivated(SGPropertyNode*); + // members SGPropertyNode_ptr _gpsNode; SGPropertyNode_ptr _currentWayptNode; @@ -308,22 +278,13 @@ private: SGPropertyNode_ptr _magnetic_bug_error_node; SGPropertyNode_ptr _eastWestVelocity; SGPropertyNode_ptr _northSouthVelocity; - - SGPropertyNode_ptr _ref_navaid_id_node; - SGPropertyNode_ptr _ref_navaid_bearing_node; - SGPropertyNode_ptr _ref_navaid_distance_node; - SGPropertyNode_ptr _ref_navaid_mag_bearing_node; - SGPropertyNode_ptr _ref_navaid_frequency_node; - SGPropertyNode_ptr _ref_navaid_name_node; - - SGPropertyNode_ptr _route_active_node; + + // SGPropertyNode_ptr _route_active_node; SGPropertyNode_ptr _route_current_wp_node; SGPropertyNode_ptr _routeDistanceNm; SGPropertyNode_ptr _routeETE; - SGPropertyNode_ptr _routeEditedSignal; - SGPropertyNode_ptr _routeFinishedSignal; SGPropertyNode_ptr _desiredCourseNode; - + double _selectedCourse; double _desiredCourse; @@ -336,15 +297,17 @@ private: double _lastEWVelocity; double _lastNSVelocity; + /** + * the instrument manager creates a default instance of us, + * if no explicit GPS is specific in the aircraft's instruments.xml file. + * This allows default route-following to work with the generic autopilot. + * This flat is set in that case, to inform us we're a 'fake' installation, + * and not to worry about electrical power or similar. + */ + bool _defaultGPSMode; + std::string _mode; - GPSListener* _listener; Config _config; - FGRouteMgr* _routeMgr; - - bool _ref_navaid_set; - double _ref_navaid_elapsed; - FGPositionedRef _ref_navaid; - std::string _name; int _num; @@ -356,17 +319,7 @@ private: SGGeod _scratchPos; SGPropertyNode_ptr _scratchNode; bool _scratchValid; - -// search data - int _searchResultIndex; - std::string _searchQuery; - FGPositioned::Type _searchType; - bool _searchExact; - FGPositionedList _searchResults; - bool _searchIsRoute; ///< set if 'search' is actually the current route - bool _searchHasNext; ///< is there a result after this one? - bool _searchNames; ///< set if we're searching names instead of idents - + // turn data bool _computeTurnData; ///< do we need to update the turn data? bool _anticipateTurn; ///< are we anticipating the next turn or not? @@ -380,18 +333,19 @@ private: std::auto_ptr _wayptController; - SGPropertyNode_ptr _realismSimpleGps; ///< should the GPS be simple or realistic? flightgear::WayptRef _prevWaypt; flightgear::WayptRef _currentWaypt; // autopilot drive properties SGPropertyNode_ptr _apDrivingFlag; SGPropertyNode_ptr _apTrueHeading; - SGPropertyNode_ptr _apTargetAltitudeFt; - SGPropertyNode_ptr _apAltitudeLock; - + simgear::TiedPropertyList _tiedProperties; + SGSharedPtr _route; + + SGPropertyChangeCallback _callbackFlightPlanChanged; + SGPropertyChangeCallback _callbackRouteActivated; }; #endif // __INSTRUMENTS_GPS_HXX diff --git a/src/Instrumentation/instrument_mgr.cxx b/src/Instrumentation/instrument_mgr.cxx index 57f6a02a3..e46565dba 100644 --- a/src/Instrumentation/instrument_mgr.cxx +++ b/src/Instrumentation/instrument_mgr.cxx @@ -94,7 +94,7 @@ void FGInstrumentMgr::init() nd->setStringValue("name", "gps"); nd->setIntValue("number", 0); _instruments.push_back("gps[0]"); - set_subsystem("gps[0]", new GPS(nd)); + set_subsystem("gps[0]", new GPS(nd, true /* default GPS mode */)); } // bind() created instruments before init. diff --git a/src/Navaids/FlightPlan.cxx b/src/Navaids/FlightPlan.cxx index eda1e2db5..f19345ec3 100644 --- a/src/Navaids/FlightPlan.cxx +++ b/src/Navaids/FlightPlan.cxx @@ -265,6 +265,12 @@ int FlightPlan::clearWayptsWithFlag(WayptFlag flag) } } + // test if the current leg will be removed + bool currentIsBeingCleared = false; + if (_currentIndex >= 0) { + currentIsBeingCleared = _legs[_currentIndex]->waypoint()->flag(flag); + } + _currentIndex -= count; // now delete and remove @@ -276,11 +282,18 @@ int FlightPlan::clearWayptsWithFlag(WayptFlag flag) lockDelegate(); _waypointsChanged = true; - if (count > 0) { + if ((count > 0) || currentIsBeingCleared) { _currentWaypointChanged = true; } _legs.erase(it, _legs.end()); + + if (_legs.empty()) { // maybe all legs were deleted + if (_delegate) { + _delegate->runCleared(); + } + } + unlockDelegate(); return rf.numDeleted(); } @@ -300,6 +313,23 @@ void FlightPlan::setCurrentIndex(int index) _currentWaypointChanged = true; unlockDelegate(); } + +void FlightPlan::finish() +{ + if (_currentIndex == -1) { + return; + } + + lockDelegate(); + _currentIndex = -1; + _currentWaypointChanged = true; + + if (_delegate) { + _delegate->runFinished(); + } + + unlockDelegate(); +} int FlightPlan::findWayptIndex(const SGGeod& aPos) const { @@ -1214,5 +1244,11 @@ void FlightPlan::Delegate::runCleared() if (_inner) _inner->runCleared(); cleared(); } - + +void FlightPlan::Delegate::runFinished() +{ + if (_inner) _inner->runFinished(); + endOfFlightPlan(); +} + } // of namespace flightgear diff --git a/src/Navaids/FlightPlan.hxx b/src/Navaids/FlightPlan.hxx index fd44b38de..836730e22 100644 --- a/src/Navaids/FlightPlan.hxx +++ b/src/Navaids/FlightPlan.hxx @@ -104,7 +104,7 @@ public: virtual void waypointsChanged() { } virtual void cleared() { } virtual void currentWaypointChanged() { } - + virtual void endOfFlightPlan() { } protected: Delegate(); @@ -116,6 +116,7 @@ public: void runWaypointsChanged(); void runCurrentWaypointChanged(); void runCleared(); + void runFinished(); friend class FlightPlan; @@ -135,6 +136,8 @@ public: void setCurrentIndex(int index); + void finish(); + Leg* currentLeg() const; Leg* nextLeg() const; Leg* previousLeg() const; diff --git a/src/Navaids/NavDataCache.cxx b/src/Navaids/NavDataCache.cxx index dc8cae0cb..db6c3b6ca 100644 --- a/src/Navaids/NavDataCache.cxx +++ b/src/Navaids/NavDataCache.cxx @@ -1690,10 +1690,12 @@ PositionedID NavDataCache::createPOI(FGPositioned::Type ty, const std::string& i true /* spatial index */); } -void NavDataCache::removePOI(FGPositioned::Type ty, const std::string& aIdent) +bool NavDataCache::removePOI(FGPositioned::Type ty, const std::string& aIdent) { d->removePositionedWithIdent(ty, aIdent); // should remove from the live cache too? + + return true; } void NavDataCache::setAirportMetar(const string& icao, bool hasMetar) diff --git a/src/Navaids/NavDataCache.hxx b/src/Navaids/NavDataCache.hxx index 316167693..ed6579249 100644 --- a/src/Navaids/NavDataCache.hxx +++ b/src/Navaids/NavDataCache.hxx @@ -122,7 +122,7 @@ public: PositionedID createPOI(FGPositioned::Type ty, const std::string& ident, const SGGeod& aPos); - void removePOI(FGPositioned::Type ty, const std::string& aIdent); + bool removePOI(FGPositioned::Type ty, const std::string& aIdent); void dropGroundnetFor(PositionedID aAirport); diff --git a/src/Navaids/positioned.cxx b/src/Navaids/positioned.cxx index bdc7d17a5..51802c2ed 100644 --- a/src/Navaids/positioned.cxx +++ b/src/Navaids/positioned.cxx @@ -30,6 +30,7 @@ #include #include +#include #include #include @@ -85,10 +86,10 @@ FGPositioned::createUserWaypoint(const std::string& aIdent, const SGGeod& aPos) return cache->loadById(id); } -void FGPositioned::deleteUserWaypoint(const std::string& aIdent) +bool FGPositioned::deleteUserWaypoint(const std::string& aIdent) { NavDataCache* cache = NavDataCache::instance(); - cache->removePOI(WAYPOINT, aIdent); + return cache->removePOI(WAYPOINT, aIdent); } @@ -335,8 +336,8 @@ FGPositionedRef FGPositioned::loadByIdImpl(PositionedID id) } FGPositioned::TypeFilter::TypeFilter(Type aTy) : - mMinType(aTy), - mMaxType(aTy) + mMinType(LAST_TYPE), + mMaxType(INVALID) { addType(aTy); } @@ -352,6 +353,23 @@ void FGPositioned::TypeFilter::addType(Type aTy) mMaxType = std::max(mMaxType, aTy); } +FGPositioned::TypeFilter +FGPositioned::TypeFilter::fromString(const std::string& aFilterSpec) +{ + if (aFilterSpec.empty()) { + throw sg_format_exception("empty filter spec:", aFilterSpec); + } + + string_list parts = simgear::strutils::split(aFilterSpec, ","); + TypeFilter f; + + BOOST_FOREACH(std::string token, parts) { + f.addType(typeFromName(token)); + } + + return f; +} + bool FGPositioned::TypeFilter::pass(FGPositioned* aPos) const { diff --git a/src/Navaids/positioned.hxx b/src/Navaids/positioned.hxx index 7f852a2b8..8b4fb6a3f 100644 --- a/src/Navaids/positioned.hxx +++ b/src/Navaids/positioned.hxx @@ -163,7 +163,7 @@ public: class TypeFilter : public Filter { public: - TypeFilter(Type aTy); + TypeFilter(Type aTy = INVALID); virtual bool pass(FGPositioned* aPos) const; virtual Type minType() const @@ -173,7 +173,10 @@ public: { return mMaxType; } void addType(Type aTy); + + static TypeFilter fromString(const std::string& aFilterSpec); private: + std::vector types; Type mMinType, mMaxType; }; @@ -266,7 +269,7 @@ public: static const char* nameForType(Type aTy); static FGPositioned* createUserWaypoint(const std::string& aIdent, const SGGeod& aPos); - static void deleteUserWaypoint(const std::string& aIdent); + static bool deleteUserWaypoint(const std::string& aIdent); protected: friend class flightgear::NavDataCache; diff --git a/src/Scripting/NasalPositioned.cxx b/src/Scripting/NasalPositioned.cxx index d50ca083f..9fb00c62b 100644 --- a/src/Scripting/NasalPositioned.cxx +++ b/src/Scripting/NasalPositioned.cxx @@ -415,11 +415,11 @@ static const char* airportGhostGetMember(naContext c, void* g, naRef field, naRe static const char* waypointCommonGetMember(naContext c, Waypt* wpt, const char* fieldName, naRef* out) { - if (!strcmp(fieldName, "wp_name")) *out = stringToNasal(c, wpt->ident()); + if (!strcmp(fieldName, "wp_name") || !strcmp(fieldName, "id")) *out = stringToNasal(c, wpt->ident()); else if (!strcmp(fieldName, "wp_type")) *out = stringToNasal(c, wpt->type()); else if (!strcmp(fieldName, "wp_role")) *out = wayptFlagToNasal(c, wpt->flags()); - else if (!strcmp(fieldName, "wp_lat")) *out = naNum(wpt->position().getLatitudeDeg()); - else if (!strcmp(fieldName, "wp_lon")) *out = naNum(wpt->position().getLongitudeDeg()); + else if (!strcmp(fieldName, "wp_lat") || !strcmp(fieldName, "lat")) *out = naNum(wpt->position().getLatitudeDeg()); + else if (!strcmp(fieldName, "wp_lon") || !strcmp(fieldName, "lon")) *out = naNum(wpt->position().getLongitudeDeg()); else if (!strcmp(fieldName, "wp_parent_name")) { Procedure* proc = dynamic_cast(wpt->owner()); *out = proc ? stringToNasal(c, proc->ident()) : naNil(); @@ -497,6 +497,7 @@ static const char* legGhostGetMember(naContext c, void* g, naRef field, naRef* o if (!strcmp(fieldName, "parents")) { *out = naNewVector(c); naVec_append(*out, fpLegPrototype); + naVec_append(*out, waypointPrototype); } else if (!strcmp(fieldName, "index")) { *out = naNum(leg->index()); } else if (!strcmp(fieldName, "alt_cstr")) { @@ -589,6 +590,11 @@ static void flightplanGhostSetMember(naContext c, void* g, naRef field, naRef va return; } + if (naIsNil(value)) { + fp->setDeparture(static_cast(NULL)); + return; + } + naRuntimeError(c, "bad argument type setting departure"); } else if (!strcmp(fieldName, "destination")) { FGAirport* apt = airportGhost(value); @@ -611,7 +617,7 @@ static void flightplanGhostSetMember(naContext c, void* g, naRef field, naRef va return; } - naRuntimeError(c, "bad argument type setting departure"); + naRuntimeError(c, "bad argument type setting departure runway"); } else if (!strcmp(fieldName, "destination_runway")) { FGRunway* rwy = runwayGhost(value); if (rwy){ @@ -619,7 +625,7 @@ static void flightplanGhostSetMember(naContext c, void* g, naRef field, naRef va return; } - naRuntimeError(c, "bad argument type setting departure"); + naRuntimeError(c, "bad argument type setting destination runway"); } else if (!strcmp(fieldName, "sid")) { Procedure* proc = procedureGhost(value); if (proc && (proc->type() == PROCEDURE_SID)) { @@ -1388,6 +1394,21 @@ static naRef f_airport_getApproach(naContext c, naRef me, int argc, naRef* args) return ghostForProcedure(c, apt->findApproachWithIdent(ident)); } +static naRef f_airport_findBestRunway(naContext c, naRef me, int argc, naRef* args) +{ + FGAirport* apt = airportGhost(me); + if (!apt) { + naRuntimeError(c, "findBestRunway called on non-airport object"); + } + + SGGeod pos; + if (!geodFromArgs(args, 0, argc, pos)) { + naRuntimeError(c, "findBestRunway must be passed a position"); + } + + return ghostForRunway(c, apt->findBestRunwayForPos(pos)); +} + static naRef f_airport_toString(naContext c, naRef me, int argc, naRef* args) { FGAirport* apt = airportGhost(me); @@ -1754,13 +1775,17 @@ public: { callDelegateMethod("cleared"); } + + virtual void endOfFlightPlan() + { + callDelegateMethod("endOfFlightPlan"); + } private: void callDelegateMethod(const char* method) { naRef f; - naMember_cget(_nasal->context(), _instance, method, &f); - if (naIsNil(f)) { + if (naMember_cget(_nasal->context(), _instance, method, &f) == 0) { return; // no method on the delegate } @@ -2131,6 +2156,16 @@ static naRef f_flightplan_pathGeod(naContext c, naRef me, int argc, naRef* args) return result; } +static naRef f_flightplan_finish(naContext c, naRef me, int argc, naRef* args) +{ + FlightPlan* fp = flightplanGhost(me); + if (!fp) { + naRuntimeError(c, "flightplan.finish called on non-flightplan object"); + } + + fp->finish(); + return naNil(); +} static naRef f_leg_setSpeed(naContext c, naRef me, int argc, naRef* args) { @@ -2376,6 +2411,7 @@ naRef initNasalPositioned(naRef globals, naContext c, naRef gcSave) hashset(c, airportPrototype, "getSid", naNewFunc(c, naNewCCode(c, f_airport_getSid))); hashset(c, airportPrototype, "getStar", naNewFunc(c, naNewCCode(c, f_airport_getStar))); hashset(c, airportPrototype, "getIAP", naNewFunc(c, naNewCCode(c, f_airport_getApproach))); + hashset(c, airportPrototype, "findBestRunwayForPos", naNewFunc(c, naNewCCode(c, f_airport_findBestRunway))); hashset(c, airportPrototype, "tostring", naNewFunc(c, naNewCCode(c, f_airport_toString))); flightplanPrototype = naNewHash(c); @@ -2394,6 +2430,7 @@ naRef initNasalPositioned(naRef globals, naContext c, naRef gcSave) hashset(c, flightplanPrototype, "clearWPType", naNewFunc(c, naNewCCode(c, f_flightplan_clearWPType))); hashset(c, flightplanPrototype, "clone", naNewFunc(c, naNewCCode(c, f_flightplan_clone))); hashset(c, flightplanPrototype, "pathGeod", naNewFunc(c, naNewCCode(c, f_flightplan_pathGeod))); + hashset(c, flightplanPrototype, "finish", naNewFunc(c, naNewCCode(c, f_flightplan_finish))); waypointPrototype = naNewHash(c); hashset(c, gcSave, "wayptProto", waypointPrototype); diff --git a/src/Scripting/NasalPositioned_cppbind.cxx b/src/Scripting/NasalPositioned_cppbind.cxx index 34162e17f..d0fe0b942 100644 --- a/src/Scripting/NasalPositioned_cppbind.cxx +++ b/src/Scripting/NasalPositioned_cppbind.cxx @@ -406,8 +406,8 @@ static naRef f_findWithinRange(nasal::CallContext ctx) static naRef f_findByIdent(nasal::CallContext ctx) { std::string prefix = ctx.requireArg(0); - - FGPositioned::TypeFilter filter(FGPositioned::typeFromName(ctx.getArg(1))); + std::string typeSpec = ctx.getArg(1); + FGPositioned::TypeFilter filter(FGPositioned::TypeFilter::fromString(typeSpec)); bool exact = ctx.getArg(2, false); return ctx.to_nasal( FGPositioned::findAllWithIdent(prefix, &filter, exact) ); @@ -416,8 +416,8 @@ static naRef f_findByIdent(nasal::CallContext ctx) static naRef f_findByName(nasal::CallContext ctx) { std::string prefix = ctx.requireArg(0); - - FGPositioned::TypeFilter filter(FGPositioned::typeFromName(ctx.getArg(1))); + std::string typeSpec = ctx.getArg(1); + FGPositioned::TypeFilter filter(FGPositioned::TypeFilter::fromString(typeSpec)); return ctx.to_nasal( FGPositioned::findAllWithName(prefix, &filter, false) ); } @@ -501,7 +501,7 @@ naRef initNasalPositioned_cppbind(naRef globalsRef, naContext c, naRef gcSave) .method("getStar", &FGAirport::findSTARWithIdent) .method("getIAP", &FGAirport::findApproachWithIdent) .method("tostring", &FGAirport::toString); - + nasal::Hash globals(globalsRef, c), positioned( globals.createHash("positioned") ); -- 2.39.5