From 0d7769ae724986397205fbe9bf188f4bb5d49386 Mon Sep 17 00:00:00 2001 From: James Turner Date: Wed, 20 Oct 2010 00:07:54 +0100 Subject: [PATCH 1/6] Search aircraft dirs when loading AI models. Thanks to Thorsten Brehm for the catch. --- src/AIModel/AIBase.cxx | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/src/AIModel/AIBase.cxx b/src/AIModel/AIBase.cxx index 8a073eb6d..514c6abb0 100644 --- a/src/AIModel/AIBase.cxx +++ b/src/AIModel/AIBase.cxx @@ -194,18 +194,27 @@ void FGAIBase::Transform() { } bool FGAIBase::init(bool search_in_AI_path) { - osg::ref_ptr opt= - new osgDB::ReaderWriter::Options(*osgDB::Registry::instance()->getOptions()); - + + string f; if(search_in_AI_path) { - SGPath ai_path(globals->get_fg_root()); - ai_path.append("AI"); - opt->getDatabasePathList().push_front(ai_path.str()); + // setup a modified Options strucutre, with only the $fg-root/AI defined; + // we'll check that first, then give the normal search logic a chance. + // this ensures that models in AI/ are preferred to normal models, where + // both exist. + osg::ref_ptr + opt(osg::clone(osgDB::Registry::instance()->getOptions(), osg::CopyOp::SHALLOW_COPY)); + + SGPath ai_path(globals->get_fg_root(), "AI"); + opt->setDatabasePath(ai_path.str()); + + f = osgDB::findDataFile(model_path, opt.get()); } - string f = osgDB::findDataFile(model_path, opt.get()); - + if (f.empty()) { + f = simgear::SGModelLib::findDataFile(model_path); + } + if(f.empty()) f = fgGetString("/sim/multiplay/default-model", default_model); From dd2eec7bd885e4219d326eca27138ad2d9043da3 Mon Sep 17 00:00:00 2001 From: James Turner Date: Sun, 11 Oct 2009 12:37:13 +0100 Subject: [PATCH 2/6] Airways/procedures code - add new data structures to store waypoints and procedures, and routing algorithms, and modify the GPS, route manager and WaypointList to use the new objects. --- src/Airports/runways.cxx | 30 + src/Airports/runways.hxx | 15 + src/Airports/simple.cxx | 243 +++ src/Airports/simple.hxx | 65 + src/Autopilot/route_mgr.cxx | 1333 +++++++++++------ src/Autopilot/route_mgr.hxx | 171 ++- src/GUI/MapWidget.cxx | 81 +- src/GUI/WaypointList.cxx | 218 ++- src/GUI/WaypointList.hxx | 16 +- src/GUI/dialog.cxx | 33 + src/GUI/dialog.hxx | 10 +- src/Instrumentation/Makefile.am | 3 +- src/Instrumentation/gps.cxx | 434 +++--- src/Instrumentation/gps.hxx | 29 +- src/Instrumentation/rnav_waypt_controller.cxx | 697 +++++++++ src/Instrumentation/rnav_waypt_controller.hxx | 193 +++ src/Instrumentation/testgps.cxx | 130 +- src/Main/fg_init.cxx | 8 +- src/Navaids/Makefile.am | 8 +- src/Navaids/airways.cxx | 454 ++++++ src/Navaids/airways.hxx | 134 ++ src/Navaids/positioned.cxx | 27 +- src/Navaids/positioned.hxx | 6 +- src/Navaids/procedure.cxx | 333 ++++ src/Navaids/procedure.hxx | 212 +++ src/Navaids/route.cxx | 679 +++++++++ src/Navaids/route.hxx | 208 +++ src/Navaids/routePath.cxx | 346 +++++ src/Navaids/routePath.hxx | 69 + src/Navaids/testnavs.cxx | 18 + src/Navaids/waypoint.cxx | 471 ++++++ src/Navaids/waypoint.hxx | 314 ++++ src/Scripting/NasalSys.cxx | 23 + 33 files changed, 6136 insertions(+), 875 deletions(-) create mode 100644 src/Instrumentation/rnav_waypt_controller.cxx create mode 100644 src/Instrumentation/rnav_waypt_controller.hxx create mode 100644 src/Navaids/airways.cxx create mode 100644 src/Navaids/airways.hxx create mode 100644 src/Navaids/procedure.cxx create mode 100644 src/Navaids/procedure.hxx create mode 100644 src/Navaids/route.cxx create mode 100644 src/Navaids/route.hxx create mode 100644 src/Navaids/routePath.cxx create mode 100644 src/Navaids/routePath.hxx create mode 100644 src/Navaids/waypoint.cxx create mode 100644 src/Navaids/waypoint.hxx diff --git a/src/Airports/runways.cxx b/src/Airports/runways.cxx index 27caa4f6d..c3e48ecbb 100644 --- a/src/Airports/runways.cxx +++ b/src/Airports/runways.cxx @@ -37,6 +37,9 @@ #include "runways.hxx" +#include +#include + using std::string; static std::string cleanRunwayNo(const std::string& aRwyNo) @@ -168,3 +171,30 @@ void FGRunway::setReciprocalRunway(FGRunway* other) _reciprocal = other; } +std::vector FGRunway::getSIDs() +{ + std::vector result; + for (unsigned int i=0; i<_airport->numSIDs(); ++i) { + flightgear::SID* s = _airport->getSIDByIndex(i); + if (s->isForRunway(this)) { + result.push_back(s); + } + } // of SIDs at the airport iteration + + return result; +} + +std::vector FGRunway::getSTARs() +{ + std::vector result; + for (unsigned int i=0; i<_airport->numSTARs(); ++i) { + flightgear::STAR* s = _airport->getSTARByIndex(i); + if (s->isForRunway(this)) { + result.push_back(s); + } + } // of STARs at the airport iteration + + return result; +} + + diff --git a/src/Airports/runways.hxx b/src/Airports/runways.hxx index e6f97ea95..1c5ab85b1 100644 --- a/src/Airports/runways.hxx +++ b/src/Airports/runways.hxx @@ -33,6 +33,11 @@ class FGAirport; class FGNavRecord; class SGPropertyNode; +namespace flightgear { + class SID; + class STAR; +} + class FGRunway : public FGRunwayBase { FGAirport* _airport; @@ -115,6 +120,16 @@ public: * Helper to process property data loaded from an ICAO.threshold.xml file */ void processThreshold(SGPropertyNode* aThreshold); + + /** + * Get SIDs (DPs) associated with this runway + */ + std::vector getSIDs(); + + /** + * Get STARs associared with this runway + */ + std::vector getSTARs(); }; #endif // _FG_RUNWAYS_HXX diff --git a/src/Airports/simple.cxx b/src/Airports/simple.cxx index a418bd414..f563cf1d0 100644 --- a/src/Airports/simple.cxx +++ b/src/Airports/simple.cxx @@ -45,6 +45,11 @@ #include #include #include +#include +#include + +using std::vector; +using namespace flightgear; // magic import of a helper which uses FGPositioned internals extern char** searchAirportNamesAndIdents(const std::string& aFilter); @@ -187,6 +192,30 @@ FGRunway* FGAirport::findBestRunwayForHeading(double aHeading) const return result; } +FGRunway* FGAirport::findBestRunwayForPos(const SGGeod& aPos) const +{ + loadRunways(); + + Runway_iterator it = mRunways.begin(); + FGRunway* result = NULL; + double currentLowestDev = 180.0; + + for (; it != mRunways.end(); ++it) { + double inboundCourse = SGGeodesy::courseDeg(aPos, (*it)->end()); + double dev = inboundCourse - (*it)->headingDeg(); + SG_NORMALIZE_RANGE(dev, -180.0, 180.0); + + dev = fabs(dev); + if (dev < currentLowestDev) { // new best match + currentLowestDev = dev; + result = *it; + } + } // of runway iteration + + return result; + +} + bool FGAirport::hasHardRunwayOfLengthFt(double aLengthFt) const { loadRunways(); @@ -350,6 +379,23 @@ void FGAirport::loadTaxiways() const } } +void FGAirport::loadProcedures() const +{ + if (mProceduresLoaded) { + return; + } + + mProceduresLoaded = true; + SGPath path; + if (!XMLLoader::findAirportData(ident(), "procedures", path)) { + SG_LOG(SG_GENERAL, SG_INFO, "no procedures data available for " << ident()); + return; + } + + SG_LOG(SG_GENERAL, SG_INFO, ident() << ": loading procedures from " << path.str()); + Route::loadAirportProcedures(path, const_cast(this)); +} + void FGAirport::loadSceneryDefintions() const { // allow users to disable the scenery data in the short-term @@ -412,6 +458,203 @@ void FGAirport::readTowerData(SGPropertyNode* aRoot) _tower_location = SGGeod::fromDegM(lon, lat, elevM); } +bool FGAirport::buildApproach(Waypt* aEnroute, STAR* aSTAR, FGRunway* aRwy, WayptVec& aRoute) +{ + loadProcedures(); + + if ((aRwy && (aRwy->airport() != this))) { + throw sg_exception("invalid parameters", "FGAirport::buildApproach"); + } + + if (aSTAR) { + bool ok = aSTAR->route(aRwy, aEnroute, aRoute); + if (!ok) { + SG_LOG(SG_GENERAL, SG_WARN, ident() << ": build approach, STAR " << aSTAR->ident() + << " failed to route from transition " << aEnroute->ident()); + return false; + } + } else if (aEnroute) { + // no a STAR specified, just use enroute point directly + aRoute.push_back(aEnroute); + } + + if (!aRwy) { + // no runway selected yet, but we loaded the STAR, so that's fine, we're done + return true; + } + +// build the approach (possibly including transition), and including the missed segment + vector aps; + for (unsigned int j=0; jrunway() == aRwy) { + aps.push_back(mApproaches[j]); + } + } // of approach filter by runway + + if (aps.empty()) { + SG_LOG(SG_GENERAL, SG_INFO, ident() << "; no approaches defined for runway " << aRwy->ident()); + // could build a fallback approach here + return false; + } + + for (unsigned int k=0; kroute(aRoute.back(), aRoute)) { + return true; + } + } // of initial approach iteration + + SG_LOG(SG_GENERAL, SG_INFO, ident() << ": unable to find transition to runway " + << aRwy->ident() << ", assume vectors"); + + WayptRef v(new ATCVectors(NULL, this)); + aRoute.push_back(v); + return aps.front()->routeFromVectors(aRoute); +} + +pair +FGAirport::selectSID(const SGGeod& aDest, FGRunway* aRwy) +{ + loadProcedures(); + + WayptRef enroute; + SID* sid = NULL; + double d = 1e9; + + for (unsigned int i=0; iisForRunway(aRwy)) { + continue; + } + + WayptRef e = mSIDs[i]->findBestTransition(aDest); + if (!e) { + continue; // strange, but let's not worry about it + } + + // assert(e->isFixedPosition()); + double ed = SGGeodesy::distanceM(aDest, e->position()); + if (ed < d) { // new best match + enroute = e; + d = ed; + sid = mSIDs[i]; + } + } // of SID iteration + + if (!mSIDs.empty() && !sid) { + SG_LOG(SG_GENERAL, SG_INFO, ident() << "selectSID, no SID found (runway=" + << (aRwy ? aRwy->ident() : "no runway preference")); + } + + return make_pair(sid, enroute); +} + +pair +FGAirport::selectSTAR(const SGGeod& aOrigin, FGRunway* aRwy) +{ + loadProcedures(); + + WayptRef enroute; + STAR* star = NULL; + double d = 1e9; + + for (unsigned int i=0; iisForRunway(aRwy)) { + continue; + } + + SG_LOG(SG_GENERAL, SG_INFO, "STAR " << mSTARs[i]->ident() << " is valid for runway"); + WayptRef e = mSTARs[i]->findBestTransition(aOrigin); + if (!e) { + continue; // strange, but let's not worry about it + } + + // assert(e->isFixedPosition()); + double ed = SGGeodesy::distanceM(aOrigin, e->position()); + if (ed < d) { // new best match + enroute = e; + d = ed; + star = mSTARs[i]; + } + } // of STAR iteration + + return make_pair(star, enroute); +} + + +void FGAirport::addSID(SID* aSid) +{ + mSIDs.push_back(aSid); +} + +void FGAirport::addSTAR(STAR* aStar) +{ + mSTARs.push_back(aStar); +} + +void FGAirport::addApproach(Approach* aApp) +{ + mApproaches.push_back(aApp); +} + +unsigned int FGAirport::numSIDs() const +{ + loadProcedures(); + return mSIDs.size(); +} + +SID* FGAirport::getSIDByIndex(unsigned int aIndex) const +{ + loadProcedures(); + return mSIDs[aIndex]; +} + +SID* FGAirport::findSIDWithIdent(const std::string& aIdent) const +{ + loadProcedures(); + for (unsigned int i=0; iident() == aIdent) { + return mSIDs[i]; + } + } + + return NULL; +} + +unsigned int FGAirport::numSTARs() const +{ + loadProcedures(); + return mSTARs.size(); +} + +STAR* FGAirport::getSTARByIndex(unsigned int aIndex) const +{ + loadProcedures(); + return mSTARs[aIndex]; +} + +STAR* FGAirport::findSTARWithIdent(const std::string& aIdent) const +{ + loadProcedures(); + for (unsigned int i=0; iident() == aIdent) { + return mSTARs[i]; + } + } + + return NULL; +} + +unsigned int FGAirport::numApproaches() const +{ + loadProcedures(); + return mApproaches.size(); +} + +Approach* FGAirport::getApproachByIndex(unsigned int aIndex) const +{ + loadProcedures(); + return mApproaches[aIndex]; +} + // get airport elevation double fgGetAirportElev( const string& id ) { diff --git a/src/Airports/simple.hxx b/src/Airports/simple.hxx index 521c48c9a..1eea8d8ec 100644 --- a/src/Airports/simple.hxx +++ b/src/Airports/simple.hxx @@ -45,6 +45,19 @@ typedef SGSharedPtr FGRunwayPtr; typedef SGSharedPtr FGTaxiwayPtr; typedef SGSharedPtr FGPavementPtr; +namespace flightgear { + class SID; + class STAR; + class Approach; + class Waypt; + + + typedef SGSharedPtr WayptRef; + typedef std::vector WayptVec; +} + + + /*************************************************************************************** * **************************************************************************************/ @@ -85,6 +98,15 @@ public: FGRunway* getRunwayByIdent(const std::string& aIdent) const; FGRunway* findBestRunwayForHeading(double aHeading) const; + /** + * return the most likely target runway based on a position. + * Specifically, return the runway for which the course from aPos + * to the runway end, mostly closely matches the runway heading. + * This is a good approximation of which runway the position is on or + * aiming towards. + */ + FGRunway* findBestRunwayForPos(const SGGeod& aPos) const; + /** * Useful predicate for FMS/GPS/NAV displays and similar - check if this * aiport has a hard-surfaced runway of at least the specified length. @@ -143,6 +165,26 @@ public: double mMinLengthFt; }; + + void setProcedures(const std::vector& aSids, + const std::vector& aStars, + const std::vector& aApproaches); + + void addSID(flightgear::SID* aSid); + void addSTAR(flightgear::STAR* aStar); + void addApproach(flightgear::Approach* aApp); + + unsigned int numSIDs() const; + flightgear::SID* getSIDByIndex(unsigned int aIndex) const; + flightgear::SID* findSIDWithIdent(const std::string& aIdent) const; + + unsigned int numSTARs() const; + flightgear::STAR* getSTARByIndex(unsigned int aIndex) const; + flightgear::STAR* findSTARWithIdent(const std::string& aIdent) const; + + unsigned int numApproaches() const; + flightgear::Approach* getApproachByIndex(unsigned int aIndex) const; + /** * Syntactic wrapper around FGPositioned::findClosest - find the closest * match for filter, and return it cast to FGAirport. The default filter @@ -169,6 +211,23 @@ public: * matches in a format suitable for use by a puaList. */ static char** searchNamesAndIdents(const std::string& aFilter); + + bool buildApproach(flightgear::Waypt* aEnroute, flightgear::STAR* aSTAR, + FGRunway* aRwy, flightgear::WayptVec& aRoute); + + /** + * Given a destiation point, select the best SID and transition waypt from + * this airport. Returns (NULL,NULL) is no SIDs are defined, otherwise the + * best SID/transition is that which is closest to the destination point. + */ + std::pair selectSID(const SGGeod& aDest, FGRunway* aRwy); + + /** + * Select a STAR and enroute transition waypt, given an origin (departure) position. + * returns (NULL, NULL) is no suitable STAR is exists + */ + std::pair selectSTAR(const SGGeod& aOrigin, FGRunway* aRwy); + private: typedef std::vector::const_iterator Runway_iterator; /** @@ -203,13 +262,19 @@ private: void loadRunways() const; void loadTaxiways() const; + void loadProcedures() const; mutable bool mRunwaysLoaded; mutable bool mTaxiwaysLoaded; + mutable bool mProceduresLoaded; std::vector mRunways; std::vector mTaxiways; std::vector mPavements; + + std::vector mSIDs; + std::vector mSTARs; + std::vector mApproaches; }; // find basic airport location info from airport database diff --git a/src/Autopilot/route_mgr.cxx b/src/Autopilot/route_mgr.cxx index 07059cc16..1896946fc 100644 --- a/src/Autopilot/route_mgr.cxx +++ b/src/Autopilot/route_mgr.cxx @@ -36,6 +36,7 @@ #include "route_mgr.hxx" #include +#include #include #include @@ -48,27 +49,81 @@ #include "Main/fg_props.hxx" #include "Navaids/positioned.hxx" +#include +#include +#include #include "Airports/simple.hxx" #include "Airports/runways.hxx" #define RM "/autopilot/route-manager/" -FGRouteMgr::FGRouteMgr() : - _route( new SGRoute ), - input(fgGetNode( RM "input", true )), - mirror(fgGetNode( RM "route", true )) +#include +#include + +using namespace flightgear; + +class PropertyWatcher : public SGPropertyChangeListener { - listener = new InputListener(this); - input->setStringValue(""); - input->addChangeListener(listener); +public: + void watch(SGPropertyNode* p) + { + p->addChangeListener(this, false); + } + + virtual void valueChanged(SGPropertyNode*) + { + fire(); + } +protected: + virtual void fire() = 0; +}; + +/** + * Template adapter, created by convenience helper below + */ +template +class MethodPropertyWatcher : public PropertyWatcher +{ +public: + typedef void (T::*fire_method)(); + + MethodPropertyWatcher(T* obj, fire_method m) : + _object(obj), + _method(m) + { ; } + +protected: + virtual void fire() + { // dispatch to the object method we're helping + (_object->*_method)(); + } + +private: + T* _object; + fire_method _method; +}; + +template +PropertyWatcher* createWatcher(T* obj, void (T::*m)()) +{ + return new MethodPropertyWatcher(obj, m); +} + +FGRouteMgr::FGRouteMgr() : + _currentIndex(0), + input(fgGetNode( RM "input", true )), + mirror(fgGetNode( RM "route", true )) +{ + listener = new InputListener(this); + input->setStringValue(""); + input->addChangeListener(listener); } -FGRouteMgr::~FGRouteMgr() { - input->removeChangeListener(listener); - - delete listener; - delete _route; +FGRouteMgr::~FGRouteMgr() +{ + input->removeChangeListener(listener); + delete listener; } @@ -85,18 +140,13 @@ void FGRouteMgr::init() { &FGRouteMgr::getDepartureICAO, &FGRouteMgr::setDepartureICAO)); departure->tie("name", SGRawValueMethods(*this, &FGRouteMgr::getDepartureName, NULL)); - -// init departure information from current location - SGGeod pos = SGGeod::fromDegFt(lon->getDoubleValue(), lat->getDoubleValue(), alt->getDoubleValue()); - _departure = FGAirport::findClosest(pos, 20.0); - if (_departure) { - FGRunway* active = _departure->getActiveRunwayForUsage(); - departure->setStringValue("runway", active->ident().c_str()); - } else { - departure->setStringValue("runway", ""); - } + departure->setStringValue("runway", ""); + + _departureWatcher = createWatcher(this, &FGRouteMgr::departureChanged); + _departureWatcher->watch(departure->getChild("runway")); departure->getChild("etd", 0, true); + _departureWatcher->watch(departure->getChild("sid", 0, true)); departure->getChild("takeoff-time", 0, true); destination = fgGetNode(RM "destination", true); @@ -106,11 +156,15 @@ void FGRouteMgr::init() { &FGRouteMgr::getDestinationICAO, &FGRouteMgr::setDestinationICAO)); destination->tie("name", SGRawValueMethods(*this, &FGRouteMgr::getDestinationName, NULL)); - - destination->getChild("runway", 0, true); - destination->getChild("eta", 0, true); - destination->getChild("touchdown-time", 0, true); + _arrivalWatcher = createWatcher(this, &FGRouteMgr::arrivalChanged); + _arrivalWatcher->watch(destination->getChild("runway", 0, true)); + + destination->getChild("eta", 0, true); + _arrivalWatcher->watch(destination->getChild("star", 0, true)); + _arrivalWatcher->watch(destination->getChild("transition", 0, true)); + destination->getChild("touchdown-time", 0, true); + alternate = fgGetNode(RM "alternate", true); alternate->getChild("airport", 0, true); alternate->getChild("runway", 0, true); @@ -122,6 +176,9 @@ void FGRouteMgr::init() { cruise->getChild("speed-kts", 0, true); cruise->setDoubleValue("speed-kts", 160.0); + _routingType = cruise->getChild("routing", 0, true); + _routingType->setIntValue(ROUTE_HIGH_AIRWAYS); + totalDistance = fgGetNode(RM "total-distance", true); totalDistance->setDoubleValue(0.0); @@ -142,7 +199,7 @@ void FGRouteMgr::init() { _currentWpt = fgGetNode(RM "current-wp", true); _currentWpt->tie(SGRawValueMethods - (*this, &FGRouteMgr::currentWaypoint, &FGRouteMgr::jumpToIndex)); + (*this, &FGRouteMgr::currentIndex, &FGRouteMgr::jumpToIndex)); // temporary distance / eta calculations, for backward-compatability wp0 = fgGetNode(RM "wp", 0, true); @@ -160,27 +217,29 @@ void FGRouteMgr::init() { wpn->getChild("dist", 0, true); wpn->getChild("eta", 0, true); - _route->clear(); - _route->set_current(0); update_mirror(); - _pathNode = fgGetNode(RM "file-path", 0, true); } -void FGRouteMgr::postinit() { - string_list *waypoints = globals->get_initial_waypoints(); - if (waypoints) { - vector::iterator it; - for (it = waypoints->begin(); it != waypoints->end(); ++it) - new_waypoint(*it); +void FGRouteMgr::postinit() +{ + string_list *waypoints = globals->get_initial_waypoints(); + if (waypoints) { + string_list::iterator it; + for (it = waypoints->begin(); it != waypoints->end(); ++it) { + WayptRef w = waypointFromString(*it); + if (w) { + _route.push_back(w); + } } - - weightOnWheels = fgGetNode("/gear/gear[0]/wow", false); - // check airbone flag agrees with presets -} + SG_LOG(SG_AUTOPILOT, SG_INFO, "loaded initial waypoints:" << _route.size()); + } + weightOnWheels = fgGetNode("/gear/gear[0]/wow", false); + // check airbone flag agrees with presets +} void FGRouteMgr::bind() { } void FGRouteMgr::unbind() { } @@ -190,260 +249,604 @@ bool FGRouteMgr::isRouteActive() const return active->getBoolValue(); } -void FGRouteMgr::update( double dt ) { - if (dt <= 0.0) { - // paused, nothing to do here - return; - } - - if (!active->getBoolValue()) { - return; - } - - double groundSpeed = fgGetDouble("/velocities/groundspeed-kt", 0.0); - if (airborne->getBoolValue()) { - time_t now = time(NULL); - elapsedFlightTime->setDoubleValue(difftime(now, _takeoffTime)); - } else { // not airborne - if (weightOnWheels->getBoolValue() || (groundSpeed < 40)) { - return; - } - - airborne->setBoolValue(true); - _takeoffTime = time(NULL); // start the clock - departure->setIntValue("takeoff-time", _takeoffTime); - } - - // basic course/distance information - double wp_course, wp_distance; - SGWayPoint wp = _route->get_current(); - wp.CourseAndDistance( lon->getDoubleValue(), lat->getDoubleValue(), - alt->getDoubleValue(), &wp_course, &wp_distance ); - - // update wp0 / wp1 / wp-last for legacy users - wp0->setDoubleValue("dist", wp_distance * SG_METER_TO_NM); - wp_course -= magvar->getDoubleValue(); // expose magnetic bearing - wp0->setDoubleValue("bearing-deg", wp_course); - setETAPropertyFromDistance(wp0->getChild("eta"), wp_distance); - - if ((_route->current_index() + 1) < _route->size()) { - wp = _route->get_waypoint(_route->current_index() + 1); - double wp1_course, wp1_distance; - wp.CourseAndDistance(lon->getDoubleValue(), lat->getDoubleValue(), - alt->getDoubleValue(), &wp1_course, &wp1_distance); - - wp1->setDoubleValue("dist", wp1_distance * SG_METER_TO_NM); - setETAPropertyFromDistance(wp1->getChild("eta"), wp1_distance); - } - - double totalDistanceRemaining = wp_distance; // distance to current waypoint - for (int i=_route->current_index() + 1; i<_route->size(); ++i) { - totalDistanceRemaining += _route->get_waypoint(i).get_distance(); - } - - wpn->setDoubleValue("dist", totalDistanceRemaining * SG_METER_TO_NM); - ete->setDoubleValue(totalDistanceRemaining * SG_METER_TO_NM / groundSpeed * 3600.0); - setETAPropertyFromDistance(wpn->getChild("eta"), totalDistanceRemaining); - - // get time now at destination tz as tm struct - // add ete seconds - // convert to string ... and stash in property - //destination->setDoubleValue("eta", eta); -} - - -void FGRouteMgr::setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDistance) { - double speed =fgGetDouble("/velocities/groundspeed-kt", 0.0); - if (speed < 1.0) { - aProp->setStringValue("--:--"); - return; - } - - char eta_str[64]; - double eta = aDistance * SG_METER_TO_NM / speed; - if ( eta >= 100.0 ) { - eta = 99.999; // clamp - } - - if ( eta < (1.0/6.0) ) { - eta *= 60.0; // within 10 minutes, bump up to min/secs - } - - int major = (int)eta, - minor = (int)((eta - (int)eta) * 60.0); - snprintf( eta_str, 64, "%d:%02d", major, minor ); - aProp->setStringValue( eta_str ); -} - -void FGRouteMgr::add_waypoint( const SGWayPoint& wp, int n ) +void FGRouteMgr::update( double dt ) { - _route->add_waypoint( wp, n ); - - if ((n >= 0) && (_route->current_index() > n)) { - _route->set_current(_route->current_index() + 1); + if (dt <= 0.0) { + return; // paused, nothing to do here } - waypointsChanged(); + double groundSpeed = fgGetDouble("/velocities/groundspeed-kt", 0.0); + if (airborne->getBoolValue()) { + time_t now = time(NULL); + elapsedFlightTime->setDoubleValue(difftime(now, _takeoffTime)); + } else { // not airborne + if (weightOnWheels->getBoolValue() || (groundSpeed < 40)) { + return; + } + + airborne->setBoolValue(true); + _takeoffTime = time(NULL); // start the clock + departure->setIntValue("takeoff-time", _takeoffTime); + } + + if (!active->getBoolValue()) { + return; + } + +// basic course/distance information + SGGeod currentPos = SGGeod::fromDegFt(lon->getDoubleValue(), + lat->getDoubleValue(),alt->getDoubleValue()); + + Waypt* curWpt = currentWaypt(); + if (!curWpt) { + return; + } + + double courseDeg; + double distanceM; + boost::tie(courseDeg, distanceM) = curWpt->courseAndDistanceFrom(currentPos); + +// update wp0 / wp1 / wp-last for legacy users + wp0->setDoubleValue("dist", distanceM * SG_METER_TO_NM); + courseDeg -= magvar->getDoubleValue(); // expose magnetic bearing + wp0->setDoubleValue("bearing-deg", courseDeg); + setETAPropertyFromDistance(wp0->getChild("eta"), distanceM); + + double totalDistanceRemaining = distanceM; // distance to current waypoint + + Waypt* nextWpt = nextWaypt(); + if (nextWpt) { + boost::tie(courseDeg, distanceM) = nextWpt->courseAndDistanceFrom(currentPos); + + wp1->setDoubleValue("dist", distanceM * SG_METER_TO_NM); + courseDeg -= magvar->getDoubleValue(); // expose magnetic bearing + wp1->setDoubleValue("bearing-deg", courseDeg); + setETAPropertyFromDistance(wp1->getChild("eta"), distanceM); + } + + Waypt* prev = curWpt; + for (unsigned int i=_currentIndex + 1; i<_route.size(); ++i) { + Waypt* w = _route[i]; + if (w->flag(WPT_DYNAMIC)) continue; + totalDistanceRemaining += SGGeodesy::distanceM(prev->position(), w->position()); + prev = w; + } + + wpn->setDoubleValue("dist", totalDistanceRemaining * SG_METER_TO_NM); + ete->setDoubleValue(totalDistanceRemaining * SG_METER_TO_NM / groundSpeed * 3600.0); + setETAPropertyFromDistance(wpn->getChild("eta"), totalDistanceRemaining); } -void FGRouteMgr::waypointsChanged() +void FGRouteMgr::setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDistance) { - double routeDistanceNm = _route->total_distance() * SG_METER_TO_NM; - totalDistance->setDoubleValue(routeDistanceNm); - double cruiseSpeedKts = cruise->getDoubleValue("speed", 0.0); - if (cruiseSpeedKts > 1.0) { - // very very crude approximation, doesn't allow for climb / descent - // performance or anything else at all - ete->setDoubleValue(routeDistanceNm / cruiseSpeedKts * (60.0 * 60.0)); + double speed = fgGetDouble("/velocities/groundspeed-kt", 0.0); + if (speed < 1.0) { + aProp->setStringValue("--:--"); + return; + } + + char eta_str[64]; + double eta = aDistance * SG_METER_TO_NM / speed; + if ( eta >= 100.0 ) { + eta = 99.999; // clamp + } + + if ( eta < (1.0/6.0) ) { + eta *= 60.0; // within 10 minutes, bump up to min/secs + } + + int major = (int)eta, + minor = (int)((eta - (int)eta) * 60.0); + snprintf( eta_str, 64, "%d:%02d", major, minor ); + aProp->setStringValue( eta_str ); +} + +flightgear::WayptRef FGRouteMgr::removeWayptAtIndex(int aIndex) +{ + int index = aIndex; + if (aIndex < 0) { // negative indices count the the end + index = _route.size() - index; + } + + if ((index < 0) || (index >= numWaypts())) { + SG_LOG(SG_AUTOPILOT, SG_WARN, "removeWayptAtIndex with invalid index:" << aIndex); + return NULL; + } + + if (_currentIndex > index) { + --_currentIndex; // shift current index down if necessary + } + + WayptVec::iterator it = _route.begin(); + it += index; + + WayptRef w = *it; // hold a ref now, in case _route is the only other owner + _route.erase(it); + + update_mirror(); + _edited->fireValueChanged(); + checkFinished(); + + return w; +} + +void FGRouteMgr::clearRoute() +{ + _route.clear(); + _currentIndex = -1; + + update_mirror(); + active->setBoolValue(false); + _edited->fireValueChanged(); +} + +/** + * route between index-1 and index, using airways. + */ +bool FGRouteMgr::routeToIndex(int index, RouteType aRouteType) +{ + WayptRef wp1; + WayptRef wp2; + + if (index == -1) { + index = _route.size(); // can still be zero, of course + } + + if (index == 0) { + if (!_departure) { + SG_LOG(SG_AUTOPILOT, SG_WARN, "routeToIndex: no departure set"); + return false; + } + + wp1 = new NavaidWaypoint(_departure.get(), NULL); + } else { + wp1 = wayptAtIndex(index - 1); + } + + if (index >= numWaypts()) { + if (!_destination) { + SG_LOG(SG_AUTOPILOT, SG_WARN, "routeToIndex: no destination set"); + return false; + } + + wp2 = new NavaidWaypoint(_destination.get(), NULL); + } else { + wp2 = wayptAtIndex(index); + } + + double distNm = SGGeodesy::distanceNm(wp1->position(), wp2->position()); + if (distNm < 100.0) { + SG_LOG(SG_AUTOPILOT, SG_INFO, "routeToIndex: existing waypoints are nearby, direct route"); + return true; + } + + WayptVec r; + switch (aRouteType) { + case ROUTE_HIGH_AIRWAYS: + Airway::highLevel()->route(wp1, wp2, r); + break; + + case ROUTE_LOW_AIRWAYS: + Airway::lowLevel()->route(wp1, wp2, r); + break; + + case ROUTE_VOR: + throw sg_exception("VOR routing not supported yet"); + } + + if (r.empty()) { + SG_LOG(SG_AUTOPILOT, SG_INFO, "routeToIndex: no route found"); + return false; + } + + WayptVec::iterator it = _route.begin(); + it += index; + _route.insert(it, r.begin(), r.end()); + + update_mirror(); + _edited->fireValueChanged(); + return true; +} + +void FGRouteMgr::autoRoute() +{ + if (!_departure || !_destination) { + return; + } + + string runwayId(departure->getStringValue("runway")); + FGRunway* runway = NULL; + if (_departure->hasRunwayWithIdent(runwayId)) { + runway = _departure->getRunwayByIdent(runwayId); + } + + FGRunway* dstRunway = NULL; + runwayId = destination->getStringValue("runway"); + if (_destination->hasRunwayWithIdent(runwayId)) { + dstRunway = _destination->getRunwayByIdent(runwayId); + } + + _route.clear(); // clear out the existing, first +// SID + SID* sid; + WayptRef sidTrans; + + boost::tie(sid, sidTrans) = _departure->selectSID(_destination->geod(), runway); + if (sid) { + SG_LOG(SG_AUTOPILOT, SG_INFO, "selected SID " << sid->ident()); + if (sidTrans) { + SG_LOG(SG_AUTOPILOT, SG_INFO, "\tvia " << sidTrans->ident() << " transition"); + } + + sid->route(runway, sidTrans, _route); + departure->setStringValue("sid", sid->ident()); + } else { + // use airport location for airway search + sidTrans = new NavaidWaypoint(_departure.get(), NULL); + departure->setStringValue("sid", ""); + } + +// STAR + destination->setStringValue("transition", ""); + destination->setStringValue("star", ""); + + STAR* star; + WayptRef starTrans; + boost::tie(star, starTrans) = _destination->selectSTAR(_departure->geod(), dstRunway); + if (star) { + SG_LOG(SG_AUTOPILOT, SG_INFO, "selected STAR " << star->ident()); + if (starTrans) { + SG_LOG(SG_AUTOPILOT, SG_INFO, "\tvia " << starTrans->ident() << " transition"); + destination->setStringValue("transition", starTrans->ident()); + } + destination->setStringValue("star", star->ident()); + } else { + // use airport location for search + starTrans = new NavaidWaypoint(_destination.get(), NULL); + } + +// route between them + WayptVec airwayRoute; + if (Airway::highLevel()->route(sidTrans, starTrans, airwayRoute)) { + _route.insert(_route.end(), airwayRoute.begin(), airwayRoute.end()); + } + +// add the STAR if we have one + if (star) { + _destination->buildApproach(starTrans, star, dstRunway, _route); } update_mirror(); _edited->fireValueChanged(); - checkFinished(); } -SGWayPoint FGRouteMgr::pop_waypoint( int n ) { - if ( _route->size() <= 0 ) { - return SGWayPoint(); +void FGRouteMgr::departureChanged() +{ +// remove existing departure waypoints + WayptVec::iterator it = _route.begin(); + for (; it != _route.end(); ++it) { + if (!(*it)->flag(WPT_DEPARTURE)) { + break; + } } - if ( n < 0 ) { - n = _route->size() - 1; - } - - if (_route->current_index() > n) { - _route->set_current(_route->current_index() - 1); + // erase() invalidates iterators, so grab now + WayptRef enroute; + if (it == _route.end()) { + if (_destination) { + enroute = new NavaidWaypoint(_destination.get(), NULL); + } + } else { + enroute = *it; } - SGWayPoint wp = _route->get_waypoint(n); - _route->delete_waypoint(n); - + _route.erase(_route.begin(), it); + if (!_departure) { + waypointsChanged(); + return; + } + + WayptVec wps; + buildDeparture(enroute, wps); + for (it = wps.begin(); it != wps.end(); ++it) { + (*it)->setFlag(WPT_DEPARTURE); + (*it)->setFlag(WPT_GENERATED); + } + _route.insert(_route.begin(), wps.begin(), wps.end()); + + update_mirror(); waypointsChanged(); - return wp; } +void FGRouteMgr::buildDeparture(WayptRef enroute, WayptVec& wps) +{ + string runwayId(departure->getStringValue("runway")); + if (!_departure->hasRunwayWithIdent(runwayId)) { +// valid airport, but no runway selected, so just the airport noide itself + wps.push_back(new NavaidWaypoint(_departure.get(), NULL)); + return; + } + + FGRunway* r = _departure->getRunwayByIdent(runwayId); + string sidId = departure->getStringValue("sid"); + SID* sid = _departure->findSIDWithIdent(sidId); + if (!sid) { +// valid runway, but no SID selected/found, so just the runway node for now + if (!sidId.empty() && (sidId != "(none)")) { + SG_LOG(SG_AUTOPILOT, SG_INFO, "SID not found:" << sidId); + } + + wps.push_back(new RunwayWaypt(r, NULL)); + return; + } + +// we have a valid SID, awesome + string trans(departure->getStringValue("transition")); + WayptRef t = sid->findTransitionByName(trans); + if (!t && enroute) { + t = sid->findBestTransition(enroute->position()); + } -bool FGRouteMgr::build() { - return true; + sid->route(r, t, wps); + if (!wps.empty() && wps.front()->flag(WPT_DYNAMIC)) { + // ensure first waypoint is static, to simplify other computations + wps.insert(wps.begin(), new RunwayWaypt(r, NULL)); + } } - -void FGRouteMgr::new_waypoint( const string& target, int n ) { - SGWayPoint* wp = make_waypoint( target ); - if (!wp) { - return; +void FGRouteMgr::arrivalChanged() +{ + // remove existing arrival waypoints + WayptVec::reverse_iterator rit = _route.rbegin(); + for (; rit != _route.rend(); ++rit) { + if (!(*rit)->flag(WPT_ARRIVAL)) { + break; } - - add_waypoint( *wp, n ); - delete wp; + } + + // erase() invalidates iterators, so grab now + WayptRef enroute; + WayptVec::iterator it; + + if (rit != _route.rend()) { + enroute = *rit; + it = rit.base(); // convert to fwd iterator + } else { + it = _route.begin(); + } + + _route.erase(it, _route.end()); + + WayptVec wps; + buildArrival(enroute, wps); + for (it = wps.begin(); it != wps.end(); ++it) { + (*it)->setFlag(WPT_ARRIVAL); + (*it)->setFlag(WPT_GENERATED); + } + _route.insert(_route.end(), wps.begin(), wps.end()); + + update_mirror(); + waypointsChanged(); } +void FGRouteMgr::buildArrival(WayptRef enroute, WayptVec& wps) +{ + if (!_destination) { + return; + } + + string runwayId(destination->getStringValue("runway")); + if (!_destination->hasRunwayWithIdent(runwayId)) { +// valid airport, but no runway selected, so just the airport node itself + wps.push_back(new NavaidWaypoint(_destination.get(), NULL)); + return; + } + + FGRunway* r = _destination->getRunwayByIdent(runwayId); + string starId = destination->getStringValue("star"); + STAR* star = _destination->findSTARWithIdent(starId); + if (!star) { +// valid runway, but no STAR selected/found, so just the runway node for now + wps.push_back(new RunwayWaypt(r, NULL)); + return; + } + +// we have a valid STAR + string trans(destination->getStringValue("transition")); + WayptRef t = star->findTransitionByName(trans); + if (!t && enroute) { + t = star->findBestTransition(enroute->position()); + } + + _destination->buildApproach(t, star, r, wps); +} -SGWayPoint* FGRouteMgr::make_waypoint(const string& tgt ) { - string target(boost::to_upper_copy(tgt)); +void FGRouteMgr::waypointsChanged() +{ + +} + +void FGRouteMgr::insertWayptAtIndex(Waypt* aWpt, int aIndex) +{ + if (!aWpt) { + return; + } + + int index = aIndex; + if ((aIndex == -1) || (aIndex > _route.size())) { + index = _route.size(); + } + + WayptVec::iterator it = _route.begin(); + it += index; + + if (_currentIndex > index) { + ++_currentIndex; + } + + _route.insert(it, aWpt); + + update_mirror(); + _edited->fireValueChanged(); +} + +WayptRef FGRouteMgr::waypointFromString(const string& tgt ) +{ + string target(boost::to_upper_copy(tgt)); + WayptRef wpt; + +// extract altitude + double altFt = cruise->getDoubleValue("altitude-ft"); + RouteRestriction altSetting = RESTRICT_NONE; + size_t pos = target.find( '@' ); + if ( pos != string::npos ) { + altFt = atof( target.c_str() + pos + 1 ); + target = target.substr( 0, pos ); + if ( !strcmp(fgGetString("/sim/startup/units"), "meter") ) + altFt *= SG_METER_TO_FEET; + altSetting = RESTRICT_AT; + } + +// check for lon,lat + pos = target.find( ',' ); + if ( pos != string::npos ) { + double lon = atof( target.substr(0, pos).c_str()); + double lat = atof( target.c_str() + pos + 1); + char buf[32]; + char ew = (lon < 0.0) ? 'W' : 'E'; + char ns = (lat < 0.0) ? 'S' : 'N'; + snprintf(buf, 32, "%c%03d%c%03d", ew, (int) fabs(lon), ns, (int)fabs(lat)); - double alt = -9999.0; - // extract altitude - size_t pos = target.find( '@' ); - if ( pos != string::npos ) { - alt = atof( target.c_str() + pos + 1 ); - target = target.substr( 0, pos ); - if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) - alt *= SG_FEET_TO_METER; + wpt = new BasicWaypt(SGGeod::fromDeg(lon, lat), buf, NULL); + if (altSetting != RESTRICT_NONE) { + wpt->setAltitude(altFt, altSetting); } + return wpt; + } - // check for lon,lat - pos = target.find( ',' ); - if ( pos != string::npos ) { - double lon = atof( target.substr(0, pos).c_str()); - double lat = atof( target.c_str() + pos + 1); - char buf[32]; - char ew = (lon < 0.0) ? 'W' : 'E'; - char ns = (lat < 0.0) ? 'S' : 'N'; - snprintf(buf, 32, "%c%03d%c%03d", ew, (int) fabs(lon), ns, (int)fabs(lat)); - return new SGWayPoint( lon, lat, alt, SGWayPoint::WGS84, buf); - } - - SGGeod basePosition; - if (_route->size() > 0) { - SGWayPoint wp = get_waypoint(_route->size()-1); - basePosition = wp.get_target(); - } else { - // route is empty, use current position - basePosition = SGGeod::fromDeg( - fgGetNode("/position/longitude-deg")->getDoubleValue(), - fgGetNode("/position/latitude-deg")->getDoubleValue()); - } + SGGeod basePosition; + if (_route.empty()) { + // route is empty, use current position + basePosition = SGGeod::fromDeg(lon->getDoubleValue(), lat->getDoubleValue()); + } else { + basePosition = _route.back()->position(); + } - vector pieces(simgear::strutils::split(target, "/")); + string_list pieces(simgear::strutils::split(target, "/")); + FGPositionedRef p = FGPositioned::findClosestWithIdent(pieces.front(), basePosition); + if (!p) { + SG_LOG( SG_AUTOPILOT, SG_INFO, "Unable to find FGPositioned with ident:" << pieces.front()); + return NULL; + } - - FGPositionedRef p = FGPositioned::findClosestWithIdent(pieces.front(), basePosition); - if (!p) { - SG_LOG( SG_AUTOPILOT, SG_INFO, "Unable to find FGPositioned with ident:" << pieces.front()); + if (pieces.size() == 1) { + wpt = new NavaidWaypoint(p, NULL); + } else if (pieces.size() == 3) { + // navaid/radial/distance-nm notation + double radial = atof(pieces[1].c_str()), + distanceNm = atof(pieces[2].c_str()); + radial += magvar->getDoubleValue(); // convert to true bearing + wpt = new OffsetNavaidWaypoint(p, NULL, radial, distanceNm); + } else if (pieces.size() == 2) { + FGAirport* apt = dynamic_cast(p.ptr()); + if (!apt) { + SG_LOG(SG_AUTOPILOT, SG_INFO, "Waypoint is not an airport:" << pieces.front()); return NULL; } - SGGeod geod = SGGeod::fromGeodM(p->geod(), alt); - if (pieces.size() == 1) { - // simple case - return new SGWayPoint(geod, target, p->name()); + if (!apt->hasRunwayWithIdent(pieces[1])) { + SG_LOG(SG_AUTOPILOT, SG_INFO, "No runway: " << pieces[1] << " at " << pieces[0]); + return NULL; } - - if (pieces.size() == 3) { - // navaid/radial/distance-nm notation - double radial = atof(pieces[1].c_str()), - distanceNm = atof(pieces[2].c_str()), - az2; - radial += magvar->getDoubleValue(); // convert to true bearing - SGGeod offsetPos; - SGGeodesy::direct(geod, radial, distanceNm * SG_NM_TO_METER, offsetPos, az2); - offsetPos.setElevationM(alt); - SG_LOG(SG_AUTOPILOT, SG_INFO, "final offset radial is " << radial); - return new SGWayPoint(offsetPos, p->ident() + pieces[2], target); - } - - if (pieces.size() == 2) { - FGAirport* apt = dynamic_cast(p.ptr()); - if (!apt) { - SG_LOG(SG_AUTOPILOT, SG_INFO, "Waypoint is not an airport:" << pieces.front()); - return NULL; - } - - if (!apt->hasRunwayWithIdent(pieces[1])) { - SG_LOG(SG_AUTOPILOT, SG_INFO, "No runway: " << pieces[1] << " at " << pieces[0]); - return NULL; - } - - FGRunway* runway = apt->getRunwayByIdent(pieces[1]); - SGGeod t = runway->threshold(); - return new SGWayPoint(t.getLongitudeDeg(), t.getLatitudeDeg(), alt, SGWayPoint::WGS84, pieces[1]); + FGRunway* runway = apt->getRunwayByIdent(pieces[1]); + wpt = new NavaidWaypoint(runway, NULL); + } else if (pieces.size() == 4) { + // navid/radial/navid/radial notation + FGPositionedRef p2 = FGPositioned::findClosestWithIdent(pieces[2], basePosition); + if (!p2) { + SG_LOG( SG_AUTOPILOT, SG_INFO, "Unable to find FGPositioned with ident:" << pieces[2]); + return NULL; + } + + double r1 = atof(pieces[1].c_str()), + r2 = atof(pieces[3].c_str()); + r1 += magvar->getDoubleValue(); + r2 += magvar->getDoubleValue(); + + SGGeod intersection; + bool ok = SGGeodesy::radialIntersection(p->geod(), r1, p2->geod(), r2, intersection); + if (!ok) { + SG_LOG(SG_AUTOPILOT, SG_INFO, "no valid intersection for:" << target); + return NULL; } + std::string name = p->ident() + "-" + p2->ident(); + wpt = new BasicWaypt(intersection, name, NULL); + } + + if (!wpt) { SG_LOG(SG_AUTOPILOT, SG_INFO, "Unable to parse waypoint:" << target); return NULL; + } + + if (altSetting != RESTRICT_NONE) { + wpt->setAltitude(altFt, altSetting); + } + return wpt; } - // mirror internal route to the property system for inspection by other subsystems -void FGRouteMgr::update_mirror() { - mirror->removeChildren("wp"); - for (int i = 0; i < _route->size(); i++) { - SGWayPoint wp = _route->get_waypoint(i); - SGPropertyNode *prop = mirror->getChild("wp", i, 1); +void FGRouteMgr::update_mirror() +{ + mirror->removeChildren("wp"); + for (int i = 0; i < numWaypts(); i++) { + Waypt* wp = _route[i]; + SGPropertyNode *prop = mirror->getChild("wp", i, 1); - const SGGeod& pos(wp.get_target()); - prop->setStringValue("id", wp.get_id().c_str()); - prop->setStringValue("name", wp.get_name().c_str()); - prop->setDoubleValue("longitude-deg", pos.getLongitudeDeg()); - prop->setDoubleValue("latitude-deg",pos.getLatitudeDeg()); - prop->setDoubleValue("altitude-m", pos.getElevationM()); - prop->setDoubleValue("altitude-ft", pos.getElevationFt()); + const SGGeod& pos(wp->position()); + prop->setStringValue("id", wp->ident().c_str()); + //prop->setStringValue("name", wp.get_name().c_str()); + prop->setDoubleValue("longitude-deg", pos.getLongitudeDeg()); + prop->setDoubleValue("latitude-deg",pos.getLatitudeDeg()); + + if (wp->altitudeRestriction() != RESTRICT_NONE) { + double ft = wp->altitudeFt(); + prop->setDoubleValue("altitude-m", ft * SG_FEET_TO_METER); + prop->setDoubleValue("altitude-ft", ft); + } else { + prop->setDoubleValue("altitude-m", -9999.9); + prop->setDoubleValue("altitude-ft", -9999.9); } - // set number as listener attachment point - mirror->setIntValue("num", _route->size()); + + if (wp->speedRestriction() != RESTRICT_NONE) { + prop->setDoubleValue("speed-kts", wp->speedKts()); + } + + if (wp->flag(WPT_ARRIVAL)) { + prop->setBoolValue("arrival", true); + } + + if (wp->flag(WPT_DEPARTURE)) { + prop->setBoolValue("departure", true); + } + + if (wp->flag(WPT_MISS)) { + prop->setBoolValue("missed-approach", true); + } + + prop->setBoolValue("generated", wp->flag(WPT_GENERATED)); + } // of waypoint iteration + + // set number as listener attachment point + mirror->setIntValue("num", _route.size()); + + NewGUI * gui = (NewGUI *)globals->get_subsystem("gui"); + FGDialog* rmDlg = gui->getDialog("route-manager"); + if (rmDlg) { + rmDlg->updateValues(); + } } // command interface /autopilot/route-manager/input: @@ -462,7 +865,7 @@ void FGRouteMgr::InputListener::valueChanged(SGPropertyNode *prop) } if (!strcmp(s, "@CLEAR")) - mgr->init(); + mgr->clearRoute(); else if (!strcmp(s, "@ACTIVATE")) mgr->activate(); else if (!strcmp(s, "@LOAD")) { @@ -472,13 +875,13 @@ void FGRouteMgr::InputListener::valueChanged(SGPropertyNode *prop) } else if (!strcmp(s, "@POP")) { SG_LOG(SG_AUTOPILOT, SG_WARN, "route-manager @POP command is deprecated"); } else if (!strcmp(s, "@NEXT")) { - mgr->jumpToIndex(mgr->currentWaypoint() + 1); + mgr->jumpToIndex(mgr->_currentIndex + 1); } else if (!strcmp(s, "@PREVIOUS")) { - mgr->jumpToIndex(mgr->currentWaypoint() - 1); + mgr->jumpToIndex(mgr->_currentIndex - 1); } else if (!strncmp(s, "@JUMP", 5)) { mgr->jumpToIndex(atoi(s + 5)); } else if (!strncmp(s, "@DELETE", 7)) - mgr->pop_waypoint(atoi(s + 7)); + mgr->removeWayptAtIndex(atoi(s + 7)); else if (!strncmp(s, "@INSERT", 7)) { char *r; int pos = strtol(s + 7, &r, 10); @@ -487,14 +890,70 @@ void FGRouteMgr::InputListener::valueChanged(SGPropertyNode *prop) while (isspace(*r)) r++; if (*r) - mgr->new_waypoint(r, pos); + mgr->insertWayptAtIndex(mgr->waypointFromString(r), pos); + } else if (!strncmp(s, "@ROUTE", 6)) { + char* r; + int endIndex = strtol(s + 6, &r, 10); + RouteType rt = (RouteType) mgr->_routingType->getIntValue(); + mgr->routeToIndex(endIndex, rt); + } else if (!strcmp(s, "@AUTOROUTE")) { + mgr->autoRoute(); + } else if (!strcmp(s, "@POSINIT")) { + mgr->initAtPosition(); } else - mgr->new_waypoint(s); + mgr->insertWayptAtIndex(mgr->waypointFromString(s), -1); } -// SGWayPoint( const double lon = 0.0, const double lat = 0.0, -// const double alt = 0.0, const modetype m = WGS84, -// const string& s = "", const string& n = "" ); +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"); + _departure = NULL; + departure->setStringValue("runway", ""); + return; + } + +// on the ground + SGGeod pos = SGGeod::fromDegFt(lon->getDoubleValue(), lat->getDoubleValue(), alt->getDoubleValue()); + _departure = FGAirport::findClosest(pos, 20.0); + if (!_departure) { + SG_LOG(SG_AUTOPILOT, SG_INFO, "initAtPosition: couldn't find an airport within 20nm"); + departure->setStringValue("runway", ""); + return; + } + + FGRunway* r = _departure->findBestRunwayForPos(pos); + if (!r) { + return; + } + + departure->setStringValue("runway", r->ident().c_str()); + SG_LOG(SG_AUTOPILOT, SG_INFO, "initAtPosition: starting at " + << _departure->ident() << " on runway " << r->ident()); +} + +bool FGRouteMgr::haveUserWaypoints() const +{ + for (int i = 0; i < numWaypts(); i++) { + if (!_route[i]->flag(WPT_GENERATED)) { + // have a non-generated waypoint, we're done + return true; + } + } + + // all waypoints are generated + return false; +} bool FGRouteMgr::activate() { @@ -502,41 +961,19 @@ bool FGRouteMgr::activate() SG_LOG(SG_AUTOPILOT, SG_WARN, "duplicate route-activation, no-op"); return false; } - - // only add departure waypoint if we're not airborne, so that - // in-air route activation doesn't confuse matters. - if (weightOnWheels->getBoolValue() && _departure) { - string runwayId(departure->getStringValue("runway")); - FGRunway* runway = NULL; - if (_departure->hasRunwayWithIdent(runwayId)) { - runway = _departure->getRunwayByIdent(runwayId); - } else { - SG_LOG(SG_AUTOPILOT, SG_INFO, - "route-manager, departure runway not found:" << runwayId); - runway = _departure->getActiveRunwayForUsage(); - } - - SGWayPoint swp(runway->threshold(), - _departure->ident() + "-" + runway->ident(), runway->name()); - add_waypoint(swp, 0); - } + + _currentIndex = 0; + currentWaypointChanged(); - if (_destination) { - string runwayId = (destination->getStringValue("runway")); - if (_destination->hasRunwayWithIdent(runwayId)) { - FGRunway* runway = _destination->getRunwayByIdent(runwayId); - SGWayPoint swp(runway->end(), - _destination->ident() + "-" + runway->ident(), runway->name()); - add_waypoint(swp); - } else { - // quite likely, since destination runway may not be known until enroute - // probably want a listener on the 'destination' node to allow an enroute - // update - add_waypoint(SGWayPoint(_destination->geod(), _destination->ident(), _destination->name())); - } + /* double routeDistanceNm = _route->total_distance() * SG_METER_TO_NM; + totalDistance->setDoubleValue(routeDistanceNm); + double cruiseSpeedKts = cruise->getDoubleValue("speed", 0.0); + if (cruiseSpeedKts > 1.0) { + // very very crude approximation, doesn't allow for climb / descent + // performance or anything else at all + ete->setDoubleValue(routeDistanceNm / cruiseSpeedKts * (60.0 * 60.0)); } - - _route->set_current(0); + */ active->setBoolValue(true); SG_LOG(SG_AUTOPILOT, SG_INFO, "route-manager, activate route ok"); return true; @@ -554,15 +991,13 @@ void FGRouteMgr::sequence() return; } - _route->increment_current(); + _currentIndex++; currentWaypointChanged(); - _currentWpt->fireValueChanged(); } bool FGRouteMgr::checkFinished() { - int lastWayptIndex = _route->size() - 1; - if (_route->current_index() < lastWayptIndex) { + if (_currentIndex < (int) _route.size()) { return false; } @@ -574,41 +1009,38 @@ bool FGRouteMgr::checkFinished() void FGRouteMgr::jumpToIndex(int index) { - if ((index < 0) || (index >= _route->size())) { + if ((index < 0) || (index >= (int) _route.size())) { SG_LOG(SG_AUTOPILOT, SG_ALERT, "passed invalid index (" << index << ") to FGRouteMgr::jumpToIndex"); return; } - if (_route->current_index() == index) { + if (_currentIndex == index) { return; // no-op } - _route->set_current(index); +// all the checks out the way, go ahead and update state + _currentIndex = index; currentWaypointChanged(); _currentWpt->fireValueChanged(); } void FGRouteMgr::currentWaypointChanged() { - SGWayPoint previous = _route->get_previous(); - SGWayPoint cur = _route->get_current(); + Waypt* cur = currentWaypt(); + Waypt* next = nextWaypt(); + + wp0->getChild("id")->setStringValue(cur ? cur->ident() : ""); + wp1->getChild("id")->setStringValue(next ? next->ident() : ""); - wp0->getChild("id")->setStringValue(cur.get_id()); - if ((_route->current_index() + 1) < _route->size()) { - wp1->getChild("id")->setStringValue(_route->get_next().get_id()); - } else { - wp1->getChild("id")->setStringValue(""); - } - - SG_LOG(SG_AUTOPILOT, SG_INFO, "route manager, current-wp is now " << _route->current_index()); + _currentWpt->fireValueChanged(); + SG_LOG(SG_AUTOPILOT, SG_INFO, "route manager, current-wp is now " << _currentIndex); } -int FGRouteMgr::findWaypoint(const SGGeod& aPos) const +int FGRouteMgr::findWayptIndex(const SGGeod& aPos) const { - for (int i=0; i<_route->size(); ++i) { - double d = SGGeodesy::distanceM(aPos, _route->get_waypoint(i).get_target()); - if (d < 200.0) { // 200 metres seems close enough + for (int i=0; imatches(aPos)) { return i; } } @@ -616,29 +1048,36 @@ int FGRouteMgr::findWaypoint(const SGGeod& aPos) const return -1; } -SGWayPoint FGRouteMgr::get_waypoint( int i ) const +Waypt* FGRouteMgr::currentWaypt() const { - return _route->get_waypoint(i); + return wayptAtIndex(_currentIndex); } -int FGRouteMgr::size() const +Waypt* FGRouteMgr::previousWaypt() const { - return _route->size(); + if (_currentIndex == 0) { + return NULL; + } + + return wayptAtIndex(_currentIndex - 1); } -int FGRouteMgr::currentWaypoint() const +Waypt* FGRouteMgr::nextWaypt() const { - return _route->current_index(); + if ((_currentIndex + 1) >= numWaypts()) { + return NULL; + } + + return wayptAtIndex(_currentIndex + 1); } -void FGRouteMgr::setWaypointTargetAltitudeFt(unsigned int index, int altFt) +Waypt* FGRouteMgr::wayptAtIndex(int index) const { - SGWayPoint wp = _route->get_waypoint(index); - wp.setTargetAltFt(altFt); - // simplest way to update a waypoint is to remove and re-add it - _route->delete_waypoint(index); - _route->add_waypoint(wp, index); - waypointsChanged(); + if ((index < 0) || (index >= numWaypts())) { + throw sg_range_exception("waypt index out of range", "FGRouteMgr::wayptAtIndex"); + } + + return _route[index]; } void FGRouteMgr::saveRoute() @@ -648,7 +1087,7 @@ void FGRouteMgr::saveRoute() try { SGPropertyNode_ptr d(new SGPropertyNode); SGPath path(_pathNode->getStringValue()); - d->setIntValue("version", 1); + d->setIntValue("version", 2); if (_departure) { d->setStringValue("departure/airport", _departure->ident()); @@ -662,28 +1101,16 @@ void FGRouteMgr::saveRoute() d->setStringValue("destination/transition", destination->getStringValue("transition")); d->setStringValue("destination/runway", destination->getStringValue("runway")); } - - // route nodes - SGPropertyNode* routeNode = d->getChild("route", 0, true); - for (int i=0; i<_route->size(); ++i) { - SGPropertyNode* wpNode = routeNode->getChild("wp",i, true); - SGWayPoint wp(_route->get_waypoint(i)); - - wpNode->setStringValue("ident", wp.get_id()); - wpNode->setStringValue("name", wp.get_name()); - SGGeod geod(wp.get_target()); - - wpNode->setDoubleValue("longitude-deg", geod.getLongitudeDeg()); - wpNode->setDoubleValue("latitude-deg", geod.getLatitudeDeg()); - - if (geod.getElevationFt() > -9990.0) { - wpNode->setDoubleValue("altitude-ft", geod.getElevationFt()); - } - } // of waypoint iteration + // route nodes + SGPropertyNode* routeNode = d->getChild("route", 0, true); + for (unsigned int i=0; i<_route.size(); ++i) { + Waypt* wpt = _route[i]; + wpt->saveAsNode(routeNode->getChild("wp", i, true)); + } // of waypoint iteration writeProperties(path.str(), d, true /* write-all */); - } catch (const sg_exception &e) { - SG_LOG(SG_IO, SG_WARN, "Error saving route:" << e.getMessage()); + } catch (sg_exception& e) { + SG_LOG(SG_IO, SG_WARN, "failed to save flight-plan:" << e.getMessage()); } } @@ -706,87 +1133,112 @@ void FGRouteMgr::loadRoute() } try { - // departure nodes - SGPropertyNode* dep = routeData->getChild("departure"); - if (!dep) { - throw sg_io_exception("malformed route file, no departure node"); + int version = routeData->getIntValue("version", 1); + if (version == 1) { + loadVersion1XMLRoute(routeData); + } else if (version == 2) { + loadVersion2XMLRoute(routeData); + } else { + throw sg_io_exception("unsupported XML route version"); } - - string depIdent = dep->getStringValue("airport"); - _departure = (FGAirport*) fgFindAirportID(depIdent); - - - // destination - SGPropertyNode* dst = routeData->getChild("destination"); - if (!dst) { - throw sg_io_exception("malformed route file, no destination node"); - } - - _destination = (FGAirport*) fgFindAirportID(dst->getStringValue("airport")); - destination->setStringValue("runway", dst->getStringValue("runway")); - - // alternate - SGPropertyNode* alt = routeData->getChild("alternate"); - if (alt) { - alternate->setStringValue(alt->getStringValue("airport")); - } // of cruise data loading - - // cruise - SGPropertyNode* crs = routeData->getChild("cruise"); - if (crs) { - cruise->setDoubleValue("speed-kts", crs->getDoubleValue("speed-kts")); - cruise->setDoubleValue("mach", crs->getDoubleValue("mach")); - cruise->setDoubleValue("altitude-ft", crs->getDoubleValue("altitude-ft")); - } // of cruise data loading - - // route nodes - _route->clear(); - SGPropertyNode_ptr _route = routeData->getChild("route", 0); - SGGeod lastPos = (_departure ? _departure->geod() : SGGeod()); - - for (int i=0; i<_route->nChildren(); ++i) { - SGPropertyNode_ptr wp = _route->getChild("wp", i); - parseRouteWaypoint(wp); - } // of route iteration } catch (sg_exception& e) { SG_LOG(SG_IO, SG_WARN, "failed to load flight-plan (from '" << e.getOrigin() << "'):" << e.getMessage()); } } -void FGRouteMgr::parseRouteWaypoint(SGPropertyNode* aWP) +void FGRouteMgr::loadXMLRouteHeader(SGPropertyNode_ptr routeData) { - SGGeod lastPos; - if (_route->size() > 0) { - lastPos = get_waypoint(_route->size()-1).get_target(); - } else { - // route is empty, use departure airport position - const FGAirport* apt = fgFindAirportID(departure->getStringValue("airport")); - assert(apt); // shouldn't have got this far with an invalid airport - lastPos = apt->geod(); + // departure nodes + SGPropertyNode* dep = routeData->getChild("departure"); + if (dep) { + string depIdent = dep->getStringValue("airport"); + _departure = (FGAirport*) fgFindAirportID(depIdent); + departure->setStringValue("runway", dep->getStringValue("runway")); + departure->setStringValue("sid", dep->getStringValue("sid")); + departure->setStringValue("transition", dep->getStringValue("transition")); + } + +// destination + SGPropertyNode* dst = routeData->getChild("destination"); + if (dst) { + _destination = (FGAirport*) fgFindAirportID(dst->getStringValue("airport")); + destination->setStringValue("runway", dst->getStringValue("runway")); + destination->setStringValue("star", dst->getStringValue("star")); + destination->setStringValue("transition", dst->getStringValue("transition")); } - SGPropertyNode_ptr altProp = aWP->getChild("altitude-ft"); - double altM = -9999.0; - if (altProp) { - altM = altProp->getDoubleValue() * SG_FEET_TO_METER; +// alternate + SGPropertyNode* alt = routeData->getChild("alternate"); + if (alt) { + alternate->setStringValue(alt->getStringValue("airport")); + } // of cruise data loading + +// cruise + SGPropertyNode* crs = routeData->getChild("cruise"); + if (crs) { + cruise->setDoubleValue("speed-kts", crs->getDoubleValue("speed-kts")); + cruise->setDoubleValue("mach", crs->getDoubleValue("mach")); + cruise->setDoubleValue("altitude-ft", crs->getDoubleValue("altitude-ft")); + } // of cruise data loading + +} + +void FGRouteMgr::loadVersion2XMLRoute(SGPropertyNode_ptr routeData) +{ + loadXMLRouteHeader(routeData); + +// route nodes + WayptVec wpts; + SGPropertyNode_ptr routeNode = routeData->getChild("route", 0); + for (int i=0; inChildren(); ++i) { + SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i); + WayptRef wpt = Waypt::createFromProperties(NULL, wpNode); + wpts.push_back(wpt); + } // of route iteration + + _route = wpts; +} + +void FGRouteMgr::loadVersion1XMLRoute(SGPropertyNode_ptr routeData) +{ + loadXMLRouteHeader(routeData); + +// route nodes + WayptVec wpts; + SGPropertyNode_ptr routeNode = routeData->getChild("route", 0); + for (int i=0; inChildren(); ++i) { + SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i); + WayptRef wpt = parseVersion1XMLWaypt(wpNode); + wpts.push_back(wpt); + } // of route iteration + + _route = wpts; +} + +WayptRef FGRouteMgr::parseVersion1XMLWaypt(SGPropertyNode* aWP) +{ + SGGeod lastPos; + if (!_route.empty()) { + lastPos = _route.back()->position(); + } else if (_departure) { + lastPos = _departure->geod(); } - + + WayptRef w; string ident(aWP->getStringValue("ident")); if (aWP->hasChild("longitude-deg")) { // explicit longitude/latitude - SGWayPoint swp(aWP->getDoubleValue("longitude-deg"), - aWP->getDoubleValue("latitude-deg"), altM, - SGWayPoint::WGS84, ident, aWP->getStringValue("name")); - add_waypoint(swp); - } else if (aWP->hasChild("navid")) { - // lookup by navid (possibly with offset) - string nid(aWP->getStringValue("navid")); + w = new BasicWaypt(SGGeod::fromDeg(aWP->getDoubleValue("longitude-deg"), + aWP->getDoubleValue("latitude-deg")), ident, NULL); + + } else { + string nid = aWP->getStringValue("navid", ident.c_str()); FGPositionedRef p = FGPositioned::findClosestWithIdent(nid, lastPos); if (!p) { throw sg_io_exception("bad route file, unknown navid:" + nid); } - + SGGeod pos(p->geod()); if (aWP->hasChild("offset-nm") && aWP->hasChild("offset-radial")) { double radialDeg = aWP->getDoubleValue("offset-radial"); @@ -796,21 +1248,16 @@ void FGRouteMgr::parseRouteWaypoint(SGPropertyNode* aWP) double az2; SGGeodesy::direct(p->geod(), radialDeg, offsetNm * SG_NM_TO_METER, pos, az2); } - - SGWayPoint swp(pos.getLongitudeDeg(), pos.getLatitudeDeg(), altM, - SGWayPoint::WGS84, ident, ""); - add_waypoint(swp); - } else { - // lookup by ident (symbolic waypoint) - FGPositionedRef p = FGPositioned::findClosestWithIdent(ident, lastPos); - if (!p) { - throw sg_io_exception("bad route file, unknown waypoint:" + ident); - } - - SGWayPoint swp(p->longitude(), p->latitude(), altM, - SGWayPoint::WGS84, p->ident(), p->name()); - add_waypoint(swp); + + w = new BasicWaypt(pos, ident, NULL); } + + double altFt = aWP->getDoubleValue("altitude-ft", -9999.9); + if (altFt > -9990.0) { + w->setAltitude(altFt, RESTRICT_AT); + } + + return w; } void FGRouteMgr::loadPlainTextRoute(const SGPath& path) @@ -820,17 +1267,33 @@ void FGRouteMgr::loadPlainTextRoute(const SGPath& path) return; } - _route->clear(); - while (!in.eof()) { - string line; - getline(in, line, '\n'); - // trim CR from end of line, if found - if (line[line.size() - 1] == '\r') { - line.erase(line.size() - 1, 1); - } - - new_waypoint(line, -1); - } // of line iteration + try { + WayptVec wpts; + while (!in.eof()) { + string line; + getline(in, line, '\n'); + // trim CR from end of line, if found + if (line[line.size() - 1] == '\r') { + line.erase(line.size() - 1, 1); + } + + line = simgear::strutils::strip(line); + if (line.empty() || (line[0] == '#')) { + continue; // ignore empty/comment lines + } + + WayptRef w = waypointFromString(line); + if (!w) { + throw sg_io_exception("failed to create waypoint from line:" + line); + } + + wpts.push_back(w); + } // of line iteration + + _route = wpts; + } catch (sg_exception& e) { + SG_LOG(SG_IO, SG_WARN, "failed to load route from:" << path.str() << ":" << e.getMessage()); + } } const char* FGRouteMgr::getDepartureICAO() const @@ -858,6 +1321,8 @@ void FGRouteMgr::setDepartureICAO(const char* aIdent) } else { _departure = FGAirport::findByIdent(aIdent); } + + departureChanged(); } const char* FGRouteMgr::getDestinationICAO() const @@ -885,5 +1350,7 @@ void FGRouteMgr::setDestinationICAO(const char* aIdent) } else { _destination = FGAirport::findByIdent(aIdent); } + + arrivalChanged(); } diff --git a/src/Autopilot/route_mgr.hxx b/src/Autopilot/route_mgr.hxx index f3720d09e..16b4adc4b 100644 --- a/src/Autopilot/route_mgr.hxx +++ b/src/Autopilot/route_mgr.hxx @@ -28,9 +28,11 @@ #include #include +#include + // forward decls -class SGRoute; class SGPath; +class PropertyWatcher; class FGAirport; typedef SGSharedPtr FGAirportRef; @@ -42,9 +44,91 @@ typedef SGSharedPtr FGAirportRef; class FGRouteMgr : public SGSubsystem { +public: + FGRouteMgr(); + ~FGRouteMgr(); + void init (); + void postinit (); + void bind (); + void unbind (); + void update (double dt); + + void insertWayptAtIndex(flightgear::Waypt* aWpt, int aIndex); + flightgear::WayptRef removeWayptAtIndex(int index); + + void clearRoute(); + + typedef enum { + ROUTE_HIGH_AIRWAYS, ///< high-level airways routing + ROUTE_LOW_AIRWAYS, ///< low-level airways routing + ROUTE_VOR ///< VOR-VOR routing + } RouteType; + + /** + * Insert waypoints from index-1 to index. In practice this means you can + * 'fill in the gaps' between defined waypoints. If index=0, the departure + * airport is used as index-1; if index is -1, the destination airport is + * used as the final waypoint. + */ + bool routeToIndex(int index, RouteType aRouteType); + + void autoRoute(); + + bool isRouteActive() const; + + int currentIndex() const + { return _currentIndex; } + + flightgear::Waypt* currentWaypt() const; + flightgear::Waypt* nextWaypt() const; + flightgear::Waypt* previousWaypt() const; + + const flightgear::WayptVec& waypts() const + { return _route; } + + int numWaypts() const + { return _route.size(); } + + flightgear::Waypt* wayptAtIndex(int index) const; + + /** + * Find a waypoint in the route, by position, and return its index, or + * -1 if no matching waypoint was found in the route. + */ + int findWayptIndex(const SGGeod& aPos) const; + + /** + * Activate a built route. This checks for various mandatory pieces of + * data, such as departure and destination airports, and creates waypoints + * for them on the route structure. + * + * returns true if the route was activated successfully, or false if the + * route could not be activated for some reason + */ + bool activate(); + + /** + * Step to the next waypoint on the active route + */ + void sequence(); + + /** + * Set the current waypoint to the specified index. + */ + void jumpToIndex(int index); + + void saveRoute(); + void loadRoute(); + + /** + * Helper command to setup current airport/runway if necessary + */ + void initAtPosition(); private: - SGRoute* _route; + flightgear::WayptVec _route; + int _currentIndex; + time_t _takeoffTime; time_t _touchdownTime; FGAirportRef _departure; @@ -77,6 +161,8 @@ private: SGPropertyNode_ptr _pathNode; SGPropertyNode_ptr _currentWpt; + /// integer property corresponding to the RouteType enum + SGPropertyNode_ptr _routingType; /** * Signal property to notify people that the route was edited @@ -111,10 +197,17 @@ private: * - airport-id/runway-id * - navaid/radial-deg/offset-nm */ - SGWayPoint* make_waypoint(const string& target); + flightgear::WayptRef waypointFromString(const std::string& target); + + + void departureChanged(); + void buildDeparture(flightgear::WayptRef enroute, flightgear::WayptVec& wps); + + void arrivalChanged(); + void buildArrival(flightgear::WayptRef enroute, flightgear::WayptVec& wps); /** - * Helper to keep various pieces of state in sync when the SGRoute is + * Helper to keep various pieces of state in sync when the route is * modified (waypoints added, inserted, removed). Notably, this fires the * 'edited' signal. */ @@ -138,6 +231,17 @@ private: void loadPlainTextRoute(const SGPath& path); + void loadVersion1XMLRoute(SGPropertyNode_ptr routeData); + void loadVersion2XMLRoute(SGPropertyNode_ptr routeData); + void loadXMLRouteHeader(SGPropertyNode_ptr routeData); + flightgear::WayptRef parseVersion1XMLWaypt(SGPropertyNode* aWP); + + /** + * 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; @@ -146,64 +250,9 @@ private: const char* getDestinationICAO() const; const char* getDestinationName() const; void setDestinationICAO(const char* aIdent); - -public: - FGRouteMgr(); - ~FGRouteMgr(); - - void init (); - void postinit (); - void bind (); - void unbind (); - void update (double dt); - - bool build (); - - void new_waypoint( const string& tgt_alt, int n = -1 ); - void add_waypoint( const SGWayPoint& wp, int n = -1 ); - SGWayPoint pop_waypoint( int i = 0 ); - - SGWayPoint get_waypoint( int i ) const; - int size() const; - - bool isRouteActive() const; - - int currentWaypoint() const; - - /** - * Find a waypoint in the route, by position, and return its index, or - * -1 if no matching waypoint was found in the route. - */ - int findWaypoint(const SGGeod& aPos) const; - - /** - * Activate a built route. This checks for various mandatory pieces of - * data, such as departure and destination airports, and creates waypoints - * for them on the route structure. - * - * returns true if the route was activated successfully, or false if the - * route could not be activated for some reason - */ - bool activate(); - - /** - * Step to the next waypoint on the active route - */ - void sequence(); - - /** - * - */ - void jumpToIndex(int index); - - /** - * - */ - void setWaypointTargetAltitudeFt(unsigned int index, int altFt); - - void saveRoute(); - void loadRoute(); + PropertyWatcher* _departureWatcher; + PropertyWatcher* _arrivalWatcher; }; diff --git a/src/GUI/MapWidget.cxx b/src/GUI/MapWidget.cxx index 44b1f7601..bdd73a11e 100644 --- a/src/GUI/MapWidget.cxx +++ b/src/GUI/MapWidget.cxx @@ -25,6 +25,7 @@ #include #include #include
// fgGetKeyModifiers() +#include const char* RULER_LEGEND_KEY = "ruler-legend"; @@ -657,77 +658,73 @@ void MapWidget::paintAircraftLocation(const SGGeod& aircraftPos) void MapWidget::paintRoute() { - if (_route->size() < 2) { + if (_route->numWaypts() < 2) { return; } -// first pass, draw the actual line + RoutePath path(_route->waypts()); + +// first pass, draw the actual lines glLineWidth(2.0); - glBegin(GL_LINE_STRIP); - SGVec2d prev = project(_route->get_waypoint(0).get_target()); - glVertex2d(prev.x(), prev.y()); - - for (int w=1; w < _route->size(); ++w) { + for (int w=0; w<_route->numWaypts(); ++w) { + SGGeodVec gv(path.pathForIndex(w)); + if (gv.empty()) { + continue; + } - SGVec2d p = project(_route->get_waypoint(w).get_target()); - - if (w < _route->currentWaypoint()) { + if (w < _route->currentIndex()) { glColor4f(0.5, 0.5, 0.5, 0.7); } else { glColor4f(1.0, 0.0, 1.0, 1.0); } - glVertex2d(p.x(), p.y()); + flightgear::WayptRef wpt(_route->wayptAtIndex(w)); + if (wpt->flag(flightgear::WPT_MISS)) { + glEnable(GL_LINE_STIPPLE); + glLineStipple(1, 0x00FF); + } + glBegin(GL_LINE_STRIP); + for (unsigned int i=0; isize(); ++w) { - const SGWayPoint& wpt(_route->get_waypoint(w)); - SGVec2d p = project(wpt.get_target()); + for (int w=0; w < _route->numWaypts(); ++w) { + flightgear::WayptRef wpt(_route->wayptAtIndex(w)); + SGGeod g = path.positionForIndex(w); + if (g == SGGeod()) { + continue; // Vectors or similar + } + + SGVec2d p = project(g); glColor4f(1.0, 0.0, 1.0, 1.0); circleAtAlt(p, 8, 12, 5); std::ostringstream legend; - legend << wpt.get_id(); - if (wpt.get_target_alt() > -9990.0) { - legend << '\n' << SGMiscd::roundToInt(wpt.get_target_alt()) << '\''; + legend << wpt->ident(); + if (wpt->altitudeRestriction() != flightgear::RESTRICT_NONE) { + legend << '\n' << SGMiscd::roundToInt(wpt->altitudeFt()) << '\''; } - if (wpt.get_speed() > 0.0) { - legend << '\n' << SGMiscd::roundToInt(wpt.get_speed()) << "Kts"; + if (wpt->speedRestriction() != flightgear::RESTRICT_NONE) { + legend << '\n' << SGMiscd::roundToInt(wpt->speedKts()) << "Kts"; } MapData* d = getOrCreateDataForKey(reinterpret_cast(w * 2)); d->setText(legend.str()); - d->setLabel(wpt.get_id()); + d->setLabel(wpt->ident()); d->setAnchor(p); d->setOffset(MapData::VALIGN_TOP | MapData::HALIGN_CENTER, 15); - d->setPriority(w < _route->currentWaypoint() ? 9000 : 12000); + d->setPriority(w < _route->currentIndex() ? 9000 : 12000); - if (w > 0) { - SGVec2d legMid = (prev + p) * 0.5; - std::ostringstream legLegend; - - double track = wpt.get_track(); - if (_magneticHeadings) { - track -= _magVar->get_magvar(); // show magnetic track for leg - } - - legLegend << SGMiscd::roundToInt(track) << " " - << SGMiscd::roundToInt(wpt.get_distance() * SG_METER_TO_NM) << "Nm"; - - MapData* ld = getOrCreateDataForKey(reinterpret_cast(w * 2 + 1)); - ld->setText(legLegend.str()); - ld->setAnchor(legMid); - ld->setOffset(MapData::VALIGN_TOP | MapData::HALIGN_CENTER, 15); - ld->setPriority(w < _route->currentWaypoint() ? 8000 : 11000); - } // of draw leg data - - prev = p; } // of second waypoint iteration } diff --git a/src/GUI/WaypointList.cxx b/src/GUI/WaypointList.cxx index 5498076e5..1c1014e03 100644 --- a/src/GUI/WaypointList.cxx +++ b/src/GUI/WaypointList.cxx @@ -7,6 +7,8 @@ #include "WaypointList.hxx" #include +#include + #include #include @@ -16,14 +18,18 @@ #include
#include
+#include #include +using namespace flightgear; + enum { SCROLL_NO = 0, SCROLL_UP, SCROLL_DOWN }; +static const double BLINK_TIME = 0.3; static const int DRAG_START_DISTANCE_PX = 5; class RouteManagerWaypointModel : @@ -48,37 +54,38 @@ public: // implement WaypointList::Model virtual unsigned int numWaypoints() const { - return _rm->size(); + return _rm->numWaypts(); } virtual int currentWaypoint() const { - return _rm->currentWaypoint(); + return _rm->currentIndex(); } - virtual SGWayPoint waypointAt(unsigned int index) const + virtual flightgear::Waypt* waypointAt(unsigned int index) const { - return _rm->get_waypoint(index); + if (index >= numWaypoints()) { + return NULL; + } + + return _rm->wayptAtIndex(index); } virtual void deleteAt(unsigned int index) { - _rm->pop_waypoint(index); - } - - virtual void setWaypointTargetAltitudeFt(unsigned int index, int altFt) - { - _rm->setWaypointTargetAltitudeFt(index, altFt); + _rm->removeWayptAtIndex(index); } virtual void moveWaypointToIndex(unsigned int srcIndex, unsigned int destIndex) { + SG_LOG(SG_GENERAL, SG_INFO, "moveWaypoint: from " << srcIndex << " to " << destIndex); if (destIndex > srcIndex) { --destIndex; } - SGWayPoint wp = _rm->pop_waypoint(srcIndex); - _rm->add_waypoint(wp, destIndex); + WayptRef w(_rm->removeWayptAtIndex(srcIndex)); + SG_LOG(SG_GENERAL, SG_INFO, "wpt:" << w->ident()); + _rm->insertWayptAtIndex(w, destIndex); } virtual void setUpdateCallback(SGCallback* cb) @@ -129,13 +136,16 @@ WaypointList::WaypointList(int x, int y, int width, int height) : _showLatLon(false), _model(NULL), _updateCallback(NULL), - _scrollCallback(NULL) + _scrollCallback(NULL), + _blink(false) { // pretend to be a list, so fgPopup doesn't mess with our mouse events type |= PUCLASS_LIST; setModel(new RouteManagerWaypointModel()); setSize(width, height); setValue(-1); + + _blinkTimer.stamp(); } WaypointList::~WaypointList() @@ -230,6 +240,11 @@ void WaypointList::handleDrag(int x, int y) } _dragSourceRow = rowForY(y - abox.min[1]); + Waypt* wp = _model->waypointAt(_dragSourceRow); + if (!wp || wp->flag(WPT_GENERATED)) { + return; // don't allow generated points to be dragged + } + _dragging = true; _dragScroll = SCROLL_NO; } @@ -255,20 +270,26 @@ void WaypointList::doDrop(int x, int y) _dragging = false; puDeactivateWidget(); + SG_LOG(SG_GENERAL, SG_INFO, "doDrop"); + if ((y < abox.min[1]) || (y >= abox.max[1])) { + SG_LOG(SG_GENERAL, SG_INFO, "y out of bounds:" << y); return; } - if (_dragSourceRow != _dragTargetRow) { - _model->moveWaypointToIndex(_dragSourceRow, _dragTargetRow); - - // keep row indexes linged up when moving an item down the list - if (_dragSourceRow < _dragTargetRow) { - --_dragTargetRow; - } - - setSelected(_dragTargetRow); + if (_dragSourceRow == _dragTargetRow) { + SG_LOG(SG_GENERAL, SG_INFO, "source and target row match"); + return; } + + _model->moveWaypointToIndex(_dragSourceRow, _dragTargetRow); + + // keep row indexes linged up when moving an item down the list + if (_dragSourceRow < _dragTargetRow) { + --_dragTargetRow; + } + + setSelected(_dragTargetRow); } void WaypointList::invokeDownCallback(void) @@ -302,6 +323,12 @@ void WaypointList::draw( int dx, int dy ) doDragScroll(); } + double dt = (SGTimeStamp::now() - _blinkTimer).toSecs(); + if (dt > BLINK_TIME) { + _blinkTimer.stamp(); + _blink = !_blink; + } + glEnable(GL_SCISSOR_TEST); GLint sx = (int) abox.min[0], sy = abox.min[1]; @@ -321,6 +348,7 @@ void WaypointList::draw( int dx, int dy ) y -= (_scrollPx % rowHeight); // partially draw the first row + _arrowWidth = legendFont.getStringWidth(">"); for ( ; row <= final; ++row, y += rowHeight) { drawRow(dx, dy, row, y); } // of row drawing iteration @@ -343,6 +371,8 @@ void WaypointList::draw( int dx, int dy ) void WaypointList::drawRow(int dx, int dy, int rowIndex, int y) { + flightgear::Waypt* wp(_model->waypointAt(rowIndex)); + bool isSelected = (rowIndex == getSelected()); bool isCurrent = (rowIndex == _model->currentWaypoint()); bool isDragSource = (_dragging && (rowIndex == _dragSourceRow)); @@ -351,62 +381,100 @@ void WaypointList::drawRow(int dx, int dy, int rowIndex, int y) bkgBox.min[1] = abox.max[1] - y; bkgBox.max[1] = bkgBox.min[1] + rowHeightPx(); - puColour currentColor; - puSetColor(currentColor, 1.0, 1.0, 0.0, 0.5); + puColour col; + puFont* f = &legendFont; + bool drawBox = false; + + if (wp->flag(WPT_MISS)) { + drawBox = true; + puSetColor(col, 1.0, 0.0, 0.0, 0.3); // red + } else if (wp->flag(WPT_ARRIVAL)) { + drawBox = true; + puSetColor(col, 0.0, 0.0, 0.0, 0.3); + } else if (wp->flag(WPT_DEPARTURE)) { + drawBox = true; + puSetColor(col, 0.0, 0.0, 0.0, 0.3); + } if (isDragSource) { // draw later, on *top* of text string - } else if (isCurrent) { - bkgBox.draw(dx, dy, PUSTYLE_PLAIN, ¤tColor, false, 0); } else if (isSelected) { // -PLAIN means selected, apparently bkgBox.draw(dx, dy, -PUSTYLE_PLAIN, colour, false, 0); + } else if (drawBox) { + bkgBox.draw(dx, dy, PUSTYLE_PLAIN, &col, false, 0); + } + + if (isCurrent) { + glColor4f (1.0, 0.5, 0.0, 1.0) ; + } else { + glColor4fv ( colour [ PUCOL_LEGEND ] ) ; } int xx = dx + abox.min[0] + PUSTR_LGAP; int yy = dy + abox.max[1] - y ; yy += 4; // center text in row height - // row textual data - const SGWayPoint wp(_model->waypointAt(rowIndex)); - char buffer[128]; - int count = ::snprintf(buffer, 128, "%03d %-5s", rowIndex, wp.get_id().c_str()); - - if (wp.get_name().size() > 0 && (wp.get_name() != wp.get_id())) { - // append name if present, and different to id - ::snprintf(buffer + count, 128 - count, " (%s)", wp.get_name().c_str()); + if (isCurrent) { + f->drawString(">", xx, yy); } - glColor4fv ( colour [ PUCOL_LEGEND ] ) ; - drawClippedString(legendFont, buffer, xx, yy, 300); + int x = xx; + x += _arrowWidth + PUSTR_LGAP; - if (_showLatLon) { - char ns = (wp.get_target_lat() > 0.0) ? 'N' : 'S'; - char ew = (wp.get_target_lon() > 0.0) ? 'E' : 'W'; - - ::snprintf(buffer, 128 - count, "%4.2f%c %4.2f%c", - fabs(wp.get_target_lon()), ew, fabs(wp.get_target_lat()), ns); - } else { - ::snprintf(buffer, 128 - count, "%03.0f %5.1fnm", - wp.get_track(), wp.get_distance() * SG_METER_TO_NM); + // row textual data + + char buffer[128]; + int count = ::snprintf(buffer, 128, "%03d %-5s", rowIndex, wp->ident().c_str()); + + FGPositioned* src = wp->source(); + if (src && !src->name().empty() && (src->name() != wp->ident())) { + // append name if present, and different to id + ::snprintf(buffer + count, 128 - count, " (%s)", src->name().c_str()); } - legendFont.drawString(buffer, xx + 300 + PUSTR_LGAP, yy); + drawClippedString(legendFont, buffer, x, yy, 300); + x += 300 + PUSTR_LGAP; - int altFt = (int) wp.get_target_alt() * SG_METER_TO_FEET; - if (altFt > -9990) { - int altHundredFt = (altFt + 50) / 100; // round to nearest 100ft + if (_showLatLon) { + SGGeod p(wp->position()); + char ns = (p.getLatitudeDeg() > 0.0) ? 'N' : 'S'; + char ew = (p.getLongitudeDeg() > 0.0) ? 'E' : 'W'; + + ::snprintf(buffer, 128 - count, "%4.2f%c %4.2f%c", + fabs(p.getLongitudeDeg()), ew, fabs(p.getLatitudeDeg()), ns); + } else if (rowIndex > 0) { + double courseDeg; + double distanceM; + Waypt* prev = _model->waypointAt(rowIndex - 1); + boost::tie(courseDeg, distanceM) = wp->courseAndDistanceFrom(prev->position()); + + ::snprintf(buffer, 128 - count, "%03.0f %5.1fnm", + courseDeg, distanceM * SG_METER_TO_NM); + } + + f->drawString(buffer, x, yy); + x += 100 + PUSTR_LGAP; + + if (wp->altitudeRestriction() != RESTRICT_NONE) { + int altHundredFt = (wp->altitudeFt() + 50) / 100; // round to nearest 100ft if (altHundredFt < 100) { count = ::snprintf(buffer, 128, "%d'", altHundredFt * 100); } else { // display as a flight-level count = ::snprintf(buffer, 128, "FL%d", altHundredFt); } - legendFont.drawString(buffer, xx + 400 + PUSTR_LGAP, yy); + f->drawString(buffer, x, yy); } // of valid wp altitude + x += 60 + PUSTR_LGAP; + + if (wp->speedRestriction() != RESTRICT_NONE) { + count = ::snprintf(buffer, 126, "%dKts", (int) wp->speedKts()); + f->drawString(buffer, x, yy); + } if (isDragSource) { - puSetColor(currentColor, 1.0, 0.5, 0.0, 0.5); - bkgBox.draw(dx, dy, PUSTYLE_PLAIN, ¤tColor, false, 0); + puSetColor(col, 1.0, 0.5, 0.0, 0.5); + bkgBox.draw(dx, dy, PUSTYLE_PLAIN, &col, false, 0); } } @@ -613,22 +681,34 @@ int WaypointList::checkKey (int key, int updown ) case '-': if (getSelected() >= 0) { - int newAlt = wayptAltFtHundreds(getSelected()) - 10; - if (newAlt < 0) { - _model->setWaypointTargetAltitudeFt(getSelected(), -9999); - } else { - _model->setWaypointTargetAltitudeFt(getSelected(), newAlt * 100); + Waypt* wp = _model->waypointAt(getSelected()); + if (wp->flag(WPT_GENERATED)) { + break; + } + + if (wp->altitudeRestriction() != RESTRICT_NONE) { + int curAlt = (static_cast(wp->altitudeFt()) + 50) / 100; + if (curAlt <= 0) { + wp->setAltitude(0, RESTRICT_NONE); + } else { + wp->setAltitude((curAlt - 10) * 100, wp->altitudeRestriction()); + } } } break; case '=': if (getSelected() >= 0) { - int newAlt = wayptAltFtHundreds(getSelected()) + 10; - if (newAlt < 0) { - _model->setWaypointTargetAltitudeFt(getSelected(), 0); + flightgear::Waypt* wp = _model->waypointAt(getSelected()); + if (wp->flag(WPT_GENERATED)) { + break; + } + + if (wp->altitudeRestriction() == RESTRICT_NONE) { + wp->setAltitude(1000, RESTRICT_AT); } else { - _model->setWaypointTargetAltitudeFt(getSelected(), newAlt * 100); + int curAlt = (static_cast(wp->altitudeFt()) + 50) / 100; + wp->setAltitude((curAlt + 10) * 100, wp->altitudeRestriction()); } } break; @@ -636,6 +716,11 @@ int WaypointList::checkKey (int key, int updown ) case 0x7f: // delete if (getSelected() >= 0) { int index = getSelected(); + Waypt* wp = _model->waypointAt(getSelected()); + if (wp->flag(WPT_GENERATED)) { + break; + } + _model->deleteAt(index); setSelected(index - 1); } @@ -648,17 +733,6 @@ int WaypointList::checkKey (int key, int updown ) return TRUE ; } -int WaypointList::wayptAltFtHundreds(int index) const -{ - double alt = _model->waypointAt(index).get_target_alt(); - if (alt < -9990.0) { - return -9999; - } - - int altFt = (int) alt * SG_METER_TO_FEET; - return (altFt + 50) / 100; // round to nearest 100ft -} - void WaypointList::modelUpdateCallback() { // local stuff diff --git a/src/GUI/WaypointList.hxx b/src/GUI/WaypointList.hxx index 165a1dad3..46a386b59 100644 --- a/src/GUI/WaypointList.hxx +++ b/src/GUI/WaypointList.hxx @@ -14,9 +14,12 @@ // forward decls class puaScrollBar; -class SGWayPoint; class SGCallback; +namespace flightgear { + class Waypt; +} + class WaypointList : public puFrame, public GUI_ID { public: @@ -71,14 +74,13 @@ public: virtual unsigned int numWaypoints() const = 0; virtual int currentWaypoint() const = 0; - virtual SGWayPoint waypointAt(unsigned int index) const = 0; + virtual flightgear::Waypt* waypointAt(unsigned int index) const = 0; // update notifications virtual void setUpdateCallback(SGCallback* cb) = 0; // editing operations virtual void deleteAt(unsigned int index) = 0; - virtual void setWaypointTargetAltitudeFt(unsigned int index, int altFt) = 0; virtual void moveWaypointToIndex(unsigned int srcIndex, unsigned int dstIndex) = 0; }; @@ -121,9 +123,7 @@ private: int numFullyVisibleRows() const; int firstFullyVisibleRow() const; int lastFullyVisibleRow() const; - - int wayptAltFtHundreds(int index) const; - + void modelUpdateCallback(); int _scrollPx; // scroll ammount (in pixels) @@ -141,6 +141,10 @@ private: Model* _model; SGCallback* _updateCallback; SGCallback* _scrollCallback; + + SGTimeStamp _blinkTimer; + bool _blink; + int _arrowWidth; }; class ScrolledWaypointList : public puGroup diff --git a/src/GUI/dialog.cxx b/src/GUI/dialog.cxx index fbf758b29..71852fbc7 100644 --- a/src/GUI/dialog.cxx +++ b/src/GUI/dialog.cxx @@ -1339,8 +1339,41 @@ fgList::update() void fgComboBox::update() { + if (_inHit) { + return; + } + + std::string curValue(getStringValue()); fgValueList::update(); newList(_list); + int currentItem = puaComboBox::getCurrentItem(); + + +// look for the previous value, in the new list + for (int i = 0; _list[i] != 0; i++) { + if (_list[i] == curValue) { + // don't set the current item again; this is important to avoid + // recursion here if the combo callback ultimately causes another dialog-update + if (i != currentItem) { + puaComboBox::setCurrentItem(i); + } + + return; + } + } // of list values iteration + +// cound't find current item, default to first + if (currentItem != 0) { + puaComboBox::setCurrentItem(0); + } +} + +int fgComboBox::checkHit(int b, int up, int x, int y) +{ + _inHit = true; + int r = puaComboBox::checkHit(b, up, x, y); + _inHit = false; + return r; } // end of dialog.cxx diff --git a/src/GUI/dialog.hxx b/src/GUI/dialog.hxx index 090ef2bbc..10739280d 100644 --- a/src/GUI/dialog.hxx +++ b/src/GUI/dialog.hxx @@ -259,9 +259,17 @@ public: class fgComboBox : public fgValueList, public puaComboBox { public: fgComboBox(int x1, int y1, int x2, int y2, SGPropertyNode *p, bool editable) : - fgValueList(p), puaComboBox(x1, y1, x2, y2, _list, editable) {} + fgValueList(p), + puaComboBox(x1, y1, x2, y2, _list, editable), + _inHit(false) + {} void update(); + + virtual int checkHit(int b, int up, int x, int y); + +private: + bool _inHit; }; class fgSelectBox : public fgValueList, public puaSelectBox { diff --git a/src/Instrumentation/Makefile.am b/src/Instrumentation/Makefile.am index fa7f85ea3..136929dc7 100644 --- a/src/Instrumentation/Makefile.am +++ b/src/Instrumentation/Makefile.am @@ -32,6 +32,7 @@ libInstrumentation_a_SOURCES = \ dclgps.cxx dclgps.hxx \ render_area_2d.cxx render_area_2d.hxx \ groundradar.cxx groundradar.hxx \ - agradar.cxx agradar.hxx rad_alt.cxx rad_alt.hxx + agradar.cxx agradar.hxx rad_alt.cxx rad_alt.hxx \ + rnav_waypt_controller.cxx rnav_waypt_controller.hxx \ INCLUDES = -I$(top_srcdir) -I$(top_srcdir)/src -I$(top_builddir)/src diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx index 71e3becbd..1add1b97f 100644 --- a/src/Instrumentation/gps.cxx +++ b/src/Instrumentation/gps.cxx @@ -9,6 +9,8 @@ #include "gps.hxx" +#include + #include #include #include @@ -17,6 +19,7 @@ #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" @@ -29,6 +32,7 @@ using std::auto_ptr; using std::string; +using namespace flightgear; /////////////////////////////////////////////////////////////////// @@ -227,6 +231,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 () @@ -255,17 +262,9 @@ GPS::init () _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); + SGPropertyNode* wp1Crs = _currentWayptNode->getChild("desired-course-deg", 0, true); wp1Crs->alias(_gpsNode->getChild("desired-course-deg", 0, true)); - -// _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); _tracking_bug_node = _gpsNode->getChild("tracking-bug", 0, true); @@ -299,10 +298,10 @@ 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); @@ -357,49 +356,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); - 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 - (*this, &GPS::getWP1Ident, NULL)); - tie(wp1_node, "name", SGRawValueMethods - (*this, &GPS::getWP1Name, NULL)); - tie(wp1_node, "distance-nm", SGRawValueMethods + 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(_currentWayptNode, "ID", SGRawValueMethods + (*this, &GPS::getWP1Ident, NULL)); + + 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 @@ -426,12 +415,11 @@ GPS::clearOutput() _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); @@ -455,61 +443,28 @@ 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") { - updateTurn(); - } + 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()); - updateWaypoints(); + _desiredCourse = getLegMagCourse(); + + updateTurn(); + updateRouteData(); + } + + updateTrackingBug(); updateReferenceNavaid(delta_time_sec); - updateRouteData(); driveAutopilot(); } @@ -538,6 +493,49 @@ GPS::update (double delta_time_sec) _lastPosValid = true; } +/////////////////////////////////////////////////////////////////////////// +// 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) { @@ -601,13 +599,6 @@ 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) { @@ -690,7 +681,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(); } } @@ -701,8 +694,8 @@ void GPS::routeManagerSequenced() return; } - int index = _routeMgr->currentWaypoint(), - count = _routeMgr->size(); + int index = _routeMgr->currentIndex(), + count = _routeMgr->numWaypts(); if ((index < 0) || (index >= count)) { SG_LOG(SG_INSTR, SG_ALERT, "GPS: malformed route, index=" << index); return; @@ -711,17 +704,15 @@ 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->previousWaypt(); + 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(); _desiredCourse = getLegMagCourse(); _desiredCourseNode->fireValueChanged(); @@ -745,8 +736,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(); } @@ -800,7 +791,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); @@ -809,26 +800,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->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(); @@ -867,8 +861,8 @@ void GPS::computeTurnData() return; } - int curIndex = _routeMgr->currentWaypoint(); - if ((curIndex + 1) >= _routeMgr->size()) { + WayptRef next = _routeMgr->nextWaypt(); + if (!next || next->flag(WPT_DYNAMIC)) { _anticipateTurn = false; return; } @@ -880,11 +874,9 @@ void GPS::computeTurnData() _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; @@ -897,9 +889,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; } @@ -945,10 +937,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); @@ -979,13 +971,36 @@ void GPS::driveAutopilot() void GPS::wp1Changed() { + 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); } } @@ -1011,16 +1026,16 @@ double GPS::getLegDistance() const 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 @@ -1034,21 +1049,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) { @@ -1086,16 +1086,12 @@ const char* GPS::getWP0Ident() const 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 @@ -1104,56 +1100,49 @@ const char* GPS::getWP1Ident() const 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 @@ -1162,56 +1151,41 @@ double GPS::getWP1MagBearing() const 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() - _desiredCourse; - 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() - _desiredCourse; - 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; } @@ -1278,9 +1252,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; } @@ -1288,9 +1262,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 { @@ -1301,8 +1275,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; } @@ -1332,18 +1306,16 @@ bool GPS::isScratchPositionValid() const } void GPS::directTo() -{ - _wp0_position = _indicated_pos; - - if (isScratchPositionValid()) { - _wp1Ident = _scratchNode->getStringValue("ident"); - _wp1Name = _scratchNode->getStringValue("name"); - _wp1_position = _scratchPos; +{ + if (!isScratchPositionValid()) { + return; } + _prevWaypt = NULL; + _wp0_position = _indicated_pos; + _currentWaypt = new BasicWaypt(_scratchPos, _scratchNode->getStringValue("ident"), NULL); _mode = "dto"; - _desiredCourse = getLegMagCourse(); - _desiredCourseNode->fireValueChanged(); + clearScratch(); wp1Changed(); } @@ -1359,8 +1331,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; @@ -1370,18 +1342,16 @@ 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); } @@ -1476,9 +1446,9 @@ void GPS::search() auto_ptr f(createFilter(_searchType)); if (_searchNames) { - _searchResults = FGPositioned::findAllWithName(_searchQuery, f.get()); + _searchResults = FGPositioned::findAllWithName(_searchQuery, f.get(), _searchExact); } else { - _searchResults = FGPositioned::findAllWithIdent(_searchQuery, f.get()); + _searchResults = FGPositioned::findAllWithIdent(_searchQuery, f.get(), _searchExact); } bool orderByRange = _scratchNode->getBoolValue("order-by-distance", true); @@ -1498,7 +1468,7 @@ bool GPS::getScratchHasNext() const { int lastResult; if (_searchIsRoute) { - lastResult = _routeMgr->size() - 1; + lastResult = _routeMgr->numWaypts() - 1; } else { lastResult = (int) _searchResults.size() - 1; } @@ -1584,14 +1554,14 @@ void GPS::addAirportToScratch(FGAirport* aAirport) void GPS::selectOBSMode() { - if (isScratchPositionValid()) { - _wp1Ident = _scratchNode->getStringValue("ident"); - _wp1Name = _scratchNode->getStringValue("name"); - _wp1_position = _scratchPos; + if (!isScratchPositionValid()) { + return; } SG_LOG(SG_INSTR, SG_INFO, "GPS switching to OBS mode"); _mode = "obs"; + + _currentWaypt = new BasicWaypt(_scratchPos, _scratchNode->getStringValue("ident"), NULL); _wp0_position = _indicated_pos; wp1Changed(); } @@ -1680,7 +1650,7 @@ void GPS::defineWaypoint() 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"); } @@ -1690,18 +1660,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->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->removeWayptAtIndex(aIndex); } void GPS::tieSGGeod(SGPropertyNode* aNode, SGGeod& aRef, diff --git a/src/Instrumentation/gps.hxx b/src/Instrumentation/gps.hxx index d6c3a7069..3005997c0 100644 --- a/src/Instrumentation/gps.hxx +++ b/src/Instrumentation/gps.hxx @@ -13,7 +13,8 @@ #include #include -#include "Navaids/positioned.hxx" +#include +#include // forward decls class SGRoute; @@ -73,20 +74,29 @@ private: * /instrumentation/gps/magnetic-bug-error-deg */ -class GPS : public SGSubsystem +class GPS : public SGSubsystem, public flightgear::RNAV { - public: - GPS (SGPropertyNode *node); GPS (); virtual ~GPS (); + // SGSubsystem interface virtual void init (); virtual void update (double delta_time_sec); virtual void bind(); virtual void unbind(); + + // RNAV interface + virtual SGGeod position(); + virtual double trackDeg(); + virtual double groundSpeedKts(); + virtual double vspeedFPM(); + virtual double magvarDeg(); + virtual double selectedMagCourse(); + virtual double overflightArmDistanceM(); + private: friend class GPSListener; friend class SearchFilter; @@ -188,7 +198,6 @@ private: void clearOutput(); void updateBasicData(double dt); - void updateWaypoints(); void updateTrackingBug(); void updateReferenceNavaid(double dt); @@ -275,7 +284,6 @@ private: double getLegDistance() const; double getLegCourse() const; double getLegMagCourse() const; - double getAltDistanceRatio() const; double getTrueTrack() const { return _last_true_track; } double getMagTrack() const; @@ -325,6 +333,7 @@ private: // members SGPropertyNode_ptr _gpsNode; + SGPropertyNode_ptr _currentWayptNode; SGPropertyNode_ptr _magvar_node; SGPropertyNode_ptr _serviceable_node; SGPropertyNode_ptr _electrical_node; @@ -379,10 +388,8 @@ private: SGGeodProperty _position; SGGeod _wp0_position; - SGGeod _wp1_position; SGGeod _indicated_pos; - std::string _wp0Ident, _wp0Name, _wp1Ident, _wp1Name; - double _wp1DistanceM, _wp1TrueBearing; + double _legDistanceNm; // scratch data SGGeod _scratchPos; @@ -410,7 +417,11 @@ private: SGGeod _turnPt; SGGeod _turnCentre; + 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; diff --git a/src/Instrumentation/rnav_waypt_controller.cxx b/src/Instrumentation/rnav_waypt_controller.cxx new file mode 100644 index 000000000..fbdb5eeb1 --- /dev/null +++ b/src/Instrumentation/rnav_waypt_controller.cxx @@ -0,0 +1,697 @@ +// rnav_waypt_controller.cxx - Waypoint-specific behaviours for RNAV systems +// Written by James Turner, started 2009. +// +// Copyright (C) 2009 Curtis L. Olson +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +#include "rnav_waypt_controller.hxx" + +#include +#include + +#include + +namespace flightgear +{ + +const double KNOTS_TO_METRES_PER_SECOND = SG_NM_TO_METER / 3600.0; + +double pmod(double x, double y) +{ + if (x < 0.0) { + return -fmod(x, y); + } else { + return fmod(x,y); + } +} + +// implementation of +// http://williams.best.vwh.net/avform.htm#Intersection +bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result) +{ + double crs13 = r1 * SG_DEGREES_TO_RADIANS; + double crs23 = r2 * SG_DEGREES_TO_RADIANS; + double dst12 = SGGeodesy::distanceRad(a, b); + + //IF sin(lon2-lon1)<0 + // crs12=acos((sin(lat2)-sin(lat1)*cos(dst12))/(sin(dst12)*cos(lat1))) + // crs21=2.*pi-acos((sin(lat1)-sin(lat2)*cos(dst12))/(sin(dst12)*cos(lat2))) + // ELSE + // crs12=2.*pi-acos((sin(lat2)-sin(lat1)*cos(dst12))/(sin(dst12)*cos(lat1))) + // crs21=acos((sin(lat1)-sin(lat2)*cos(dst12))/(sin(dst12)*cos(lat2))) + // ENDIF + + + // double diffLon = b.getLongitudeRad() - a.getLongitudeRad(); + + double sinLat1 = sin(a.getLatitudeRad()); + double cosLat1 = cos(a.getLatitudeRad()); + // double sinLat2 = sin(b.getLatitudeRad()); + //double cosLat2 = cos(b.getLatitudeRad()); + double sinDst12 = sin(dst12); + double cosDst12 = cos(dst12); + + double crs12 = SGGeodesy::courseRad(a, b), + crs21 = SGGeodesy::courseRad(b, a); + + double degCrs12 = crs12 * SG_RADIANS_TO_DEGREES; + double degCrs21 = crs21 * SG_RADIANS_TO_DEGREES; + + /* + if (sin(diffLon) < 0.0) { + crs12 = acos((sinLat2 - sinLat1 * cosDst12) / (sinDst12 * cosLat1)); + crs21 = SGMiscd::twopi() - acos((sinLat1 - sinLat2*cosDst12)/(sinDst12*cosLat2)); + } else { + crs12 = SGMiscd::twopi() - acos((sinLat2 - sinLat1 * cosDst12)/(sinDst12 * cosLat1)); + crs21 = acos((sinLat1 - sinLat2 * cosDst12)/(sinDst12 * cosLat2)); + } + */ + + double ang1 = SGMiscd::normalizeAngle2(crs13-crs12); + double ang2 = SGMiscd::normalizeAngle2(crs21-crs23); + + if ((sin(ang1) == 0.0) && (sin(ang2) == 0.0)) { + SG_LOG(SG_GENERAL, SG_WARN, "geocRadialIntersection: infinity of intersections"); + return false; + } + + if ((sin(ang1)*sin(ang2))<0.0) { + SG_LOG(SG_GENERAL, SG_WARN, "geocRadialIntersection: intersection ambiguous"); + return false; + } + + ang1 = fabs(ang1); + ang2 = fabs(ang2); + + //ang3=acos(-cos(ang1)*cos(ang2)+sin(ang1)*sin(ang2)*cos(dst12)) + //dst13=atan2(sin(dst12)*sin(ang1)*sin(ang2),cos(ang2)+cos(ang1)*cos(ang3)) + //lat3=asin(sin(lat1)*cos(dst13)+cos(lat1)*sin(dst13)*cos(crs13)) + + //lon3=mod(lon1-dlon+pi,2*pi)-pi + + double ang3 = acos(-cos(ang1) * cos(ang2) + sin(ang1) * sin(ang2) * cosDst12); + double dst13 = atan2(sinDst12 * sin(ang1) * sin(ang2), cos(ang2) + cos(ang1)*cos(ang3)); + + SGGeoc pt3; + SGGeodesy::advanceRadM(a, crs13, dst13 * SG_RAD_TO_NM * SG_NM_TO_METER, pt3); + + double lat3 = asin(sinLat1 * cos(dst13) + cosLat1 * sin(dst13) * cos(crs13)); + + //dlon=atan2(sin(crs13)*sin(dst13)*cos(lat1),cos(dst13)-sin(lat1)*sin(lat3)) + double dlon = atan2(sin(crs13)*sin(dst13)*cosLat1, cos(dst13)- (sinLat1 * sin(lat3))); + double lon3 = SGMiscd::normalizeAngle(-a.getLongitudeRad()-dlon); + + result = SGGeoc::fromRadM(-lon3, lat3, a.getRadiusM()); + //result = pt3; + return true; +} + +//////////////////////////////////////////////////////////////////////////// + +WayptController::~WayptController() +{ +} + +void WayptController::init() +{ +} + +void WayptController::setDone() +{ + if (_isDone) { + SG_LOG(SG_AUTOPILOT, SG_WARN, "already done @ WayptController::setDone"); + } + + _isDone = true; +} + +double WayptController::timeToWaypt() const +{ + double gs = _rnav->groundSpeedKts(); + if (gs < 1.0) { + return -1.0; // stationary + } + + gs*= KNOTS_TO_METRES_PER_SECOND; + return (distanceToWayptM() / gs); +} + +////////////// + +class BasicWayptCtl : public WayptController +{ +public: + BasicWayptCtl(RNAV* aRNAV, const WayptRef& aWpt) : + WayptController(aRNAV, aWpt) + { + if (aWpt->flag(WPT_DYNAMIC)) { + throw sg_exception("BasicWayptCtrl doesn't work with dynamic waypoints"); + } + } + + virtual void init() + { + _targetTrack = SGGeodesy::courseDeg(_rnav->position(), _waypt->position()); + } + + virtual void update() + { + double brg, az2; + SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM); + _courseDev = brg - _targetTrack; + SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0); + + if ((fabs(_courseDev) > 90.0) && (_distanceM < _rnav->overflightArmDistanceM())) { + setDone(); + } + } + + virtual double distanceToWayptM() const + { + return _distanceM; + } + + virtual double xtrackErrorNm() const + { + double x = sin(courseDeviationDeg() * SG_DEGREES_TO_RADIANS) * _distanceM; + return x * SG_METER_TO_NM; + } + + virtual bool toFlag() const + { + return (fabs(_courseDev) < 90.0); + } + + virtual double courseDeviationDeg() const + { + return _courseDev; + } + + virtual double trueBearingDeg() const + { + return SGGeodesy::courseDeg(_rnav->position(), _waypt->position()); + } + + virtual SGGeod position() const + { + return _waypt->position(); + } + +private: + double _distanceM; + double _courseDev; +}; + +/** + * Special controller for runways. For runways, we want very narrow deviation + * contraints, and to understand that any point along the paved area is + * equivalent to being 'at' the runway. + */ +class RunwayCtl : public WayptController +{ +public: + RunwayCtl(RNAV* aRNAV, const WayptRef& aWpt) : + WayptController(aRNAV, aWpt) + { + } + + virtual void init() + { + _runway = static_cast(_waypt.get())->runway(); + _targetTrack = _runway->headingDeg(); + } + + virtual void update() + { + double brg, az2; + // use the far end of the runway for course deviation calculations. + // this should do the correct thing both for takeoffs (including entering + // the runway at a taxiway after the threshold) and also landings. + // seperately compute the distance to the threshold for timeToWaypt calc + SGGeodesy::inverse(_rnav->position(), _runway->end(), brg, az2, _distanceM); + double _courseDev = brg - _targetTrack; + SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0); + + if (fabs(_courseDev) > 90.0) { + setDone(); + } + } + + virtual double distanceToWayptM() const + { + return SGGeodesy::distanceM(_rnav->position(), _runway->threshold()); + } + + virtual double xtrackErrorNm() const + { + double x = sin(_courseDev * SG_RADIANS_TO_DEGREES) * _distanceM; + return x * SG_METER_TO_NM; + } + + virtual double courseDeviationDeg() const + { + return _courseDev; + } + + virtual double trueBearingDeg() const + { + // as in update(), use runway->end here, so the value remains + // sensible whether taking off or landing. + return SGGeodesy::courseDeg(_rnav->position(), _runway->end()); + } + + virtual SGGeod position() const + { + return _runway->threshold(); + } +private: + FGRunway* _runway; + double _distanceM; + double _courseDev; +}; + +class ConstHdgToAltCtl : public WayptController +{ +public: + ConstHdgToAltCtl(RNAV* aRNAV, const WayptRef& aWpt) : + WayptController(aRNAV, aWpt) + + { + if (_waypt->type() != "hdgToAlt") { + throw sg_exception("invalid waypoint type", "ConstHdgToAltCtl ctor"); + } + + if (_waypt->altitudeRestriction() == RESTRICT_NONE) { + throw sg_exception("invalid waypoint alt restriction", "ConstHdgToAltCtl ctor"); + } + } + + virtual void init() + { + HeadingToAltitude* w = (HeadingToAltitude*) _waypt.get(); + _targetTrack = w->headingDegMagnetic() + _rnav->magvarDeg(); + } + + virtual void update() + { + double curAlt = _rnav->position().getElevationFt(); + + switch (_waypt->altitudeRestriction()) { + case RESTRICT_AT: { + double d = curAlt - _waypt->altitudeFt(); + if (fabs(d) < 50.0) { + SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, reached target altitude " << _waypt->altitudeFt()); + setDone(); + } + } break; + + case RESTRICT_ABOVE: + if (curAlt >= _waypt->altitudeFt()) { + SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, above target altitude " << _waypt->altitudeFt()); + setDone(); + } + break; + + case RESTRICT_BELOW: + if (curAlt <= _waypt->altitudeFt()) { + SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, below target altitude " << _waypt->altitudeFt()); + setDone(); + } + break; + + case RESTRICT_NONE: + assert(false); + break; + } + } + + virtual double timeToWaypt() const + { + double d = fabs(_rnav->position().getElevationFt() - _waypt->altitudeFt()); + return (d / _rnav->vspeedFPM()) * 60.0; // low pass filter here, probably + } + + virtual double distanceToWayptM() const + { + double gsMsec = _rnav->groundSpeedKts() * KNOTS_TO_METRES_PER_SECOND; + return timeToWaypt() * gsMsec; + } + + virtual SGGeod position() const + { + SGGeod p; + double az2; + SGGeodesy::direct(_rnav->position(), _targetTrack, distanceToWayptM(), p, az2); + return p; + } +}; + +class InterceptCtl : public WayptController +{ +public: + InterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) : + WayptController(aRNAV, aWpt) + + { + if (_waypt->type() != "radialIntercept") { + throw sg_exception("invalid waypoint type", "InterceptCtl ctor"); + } + } + + virtual void init() + { + RadialIntercept* w = (RadialIntercept*) _waypt.get(); + _trueRadial = w->radialDegMagnetic() + _rnav->magvarDeg(); + _targetTrack = w->courseDegMagnetic() + _rnav->magvarDeg(); + } + + virtual void update() + { + // note we want the outbound radial from the waypt, hence the ordering + // of arguments to courseDeg + double r = SGGeodesy::courseDeg(_waypt->position(), _rnav->position()); + SG_LOG(SG_AUTOPILOT, SG_INFO, "current radial=" << r); + if (fabs(r - _trueRadial) < 0.5) { + SG_LOG(SG_GENERAL, SG_INFO, "InterceptCtl, intercepted radial " << _trueRadial); + setDone(); + } + } + + virtual double distanceToWayptM() const + { + return SGGeodesy::distanceM(_rnav->position(), position()); + } + + virtual SGGeod position() const + { + SGGeoc c; + geocRadialIntersection(SGGeoc::fromGeod(_rnav->position()), _rnav->trackDeg(), + SGGeoc::fromGeod(_waypt->position()), _trueRadial, c); + return SGGeod::fromGeoc(c); + } +private: + double _trueRadial; +}; + +class DMEInterceptCtl : public WayptController +{ +public: + DMEInterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) : + WayptController(aRNAV, aWpt) + + { + if (_waypt->type() != "dmeIntercept") { + throw sg_exception("invalid waypoint type", "DMEInterceptCtl ctor"); + } + } + + virtual void init() + { + _dme = (DMEIntercept*) _waypt.get(); + _targetTrack = _dme->courseDegMagnetic() + _rnav->magvarDeg(); + } + + virtual void update() + { + _distanceNm = SGGeodesy::distanceNm(_rnav->position(), _dme->position()); + double d = fabs(_distanceNm - _dme->dmeDistanceNm()); + if (d < 0.1) { + SG_LOG(SG_GENERAL, SG_INFO, "DMEInterceptCtl, intercepted DME " << _dme->dmeDistanceNm()); + setDone(); + } + } + + virtual double distanceToWayptM() const + { + return fabs(_distanceNm - _dme->dmeDistanceNm()) * SG_NM_TO_METER; + } + + virtual SGGeod position() const + { + SGGeod p; + double az2; + SGGeodesy::direct(_rnav->position(), _targetTrack, distanceToWayptM(), p, az2); + return p; + } + +private: + DMEIntercept* _dme; + double _distanceNm; +}; + +class HoldCtl : public WayptController +{ +public: + HoldCtl(RNAV* aRNAV, const WayptRef& aWpt) : + WayptController(aRNAV, aWpt) + + { + + } + + virtual void init() + { + } + + virtual void update() + { + // fly inbound / outbound sides, or execute the turn + #if 0 + if (inTurn) { + + targetTrack += dt * turnRateSec * turnDirection; + if (inbound) { + if .. targetTrack has passed inbound radial, doen with this turn + } else { + if target track has passed reciprocal radial done with turn + } + } else { + check time / distance elapsed + + if (sideDone) { + inTurn = true; + inbound = !inbound; + nextHeading = inbound; + if (!inbound) { + nextHeading += 180.0; + SG_NORMALIZE_RANGE(nextHeading, 0.0, 360.0); + } + } + + } + + #endif + setDone(); + } + + virtual double distanceToWayptM() const + { + return -1.0; + } + + virtual SGGeod position() const + { + return _waypt->position(); + } +}; + +class VectorsCtl : public WayptController +{ +public: + VectorsCtl(RNAV* aRNAV, const WayptRef& aWpt) : + WayptController(aRNAV, aWpt) + + { + } + + virtual void init() + { + + } + + virtual void update() + { + setDone(); + } + + virtual double distanceToWayptM() const + { + return -1.0; + } + + virtual SGGeod position() const + { + return _waypt->position(); + } + +private: +}; + +/////////////////////////////////////////////////////////////////////////////// + +DirectToController::DirectToController(RNAV* aRNAV, const WayptRef& aWpt, const SGGeod& aOrigin) : + WayptController(aRNAV, aWpt), + _origin(aOrigin) +{ +} + +void DirectToController::init() +{ + if (_waypt->flag(WPT_DYNAMIC)) { + throw sg_exception("can't direct-to a dynamic waypoint"); + } + + _targetTrack = SGGeodesy::courseDeg(_origin, _waypt->position()); +} + +void DirectToController::update() +{ + double brg, az2; + SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM); + _courseDev = brg - _targetTrack; + SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0); + + if ((fabs(_courseDev) > 90.0) && (_distanceM < _rnav->overflightArmDistanceM())) { + setDone(); + } +} + +double DirectToController::distanceToWayptM() const +{ + return _distanceM; +} + +double DirectToController::xtrackErrorNm() const +{ + double x = sin(courseDeviationDeg() * SG_DEGREES_TO_RADIANS) * _distanceM; + return x * SG_METER_TO_NM; +} + +double DirectToController::courseDeviationDeg() const +{ + return _courseDev; +} + +double DirectToController::trueBearingDeg() const +{ + return SGGeodesy::courseDeg(_rnav->position(), _waypt->position()); +} + +SGGeod DirectToController::position() const +{ + return _waypt->position(); +} + +/////////////////////////////////////////////////////////////////////////////// + +OBSController::OBSController(RNAV* aRNAV, const WayptRef& aWpt) : + WayptController(aRNAV, aWpt) +{ +} + +void OBSController::init() +{ + if (_waypt->flag(WPT_DYNAMIC)) { + throw sg_exception("can't use a dynamic waypoint for OBS mode"); + } + + _targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg(); +} + +void OBSController::update() +{ + _targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg(); + double brg, az2; + SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM); + _courseDev = brg - _targetTrack; + SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0); +} + +bool OBSController::toFlag() const +{ + return (fabs(_courseDev) < 90.0); +} + +double OBSController::distanceToWayptM() const +{ + return _distanceM; +} + +double OBSController::xtrackErrorNm() const +{ + double x = sin(_courseDev * SG_DEGREES_TO_RADIANS) * _distanceM; + return x * SG_METER_TO_NM; +} + +double OBSController::courseDeviationDeg() const +{ +// if (fabs(_courseDev) > 90.0) { + // double d = -_courseDev; + // SG_NORMALIZE_RANGE(d, -90.0, 90.0); + // return d; + //} + + return _courseDev; +} + +double OBSController::trueBearingDeg() const +{ + return SGGeodesy::courseDeg(_rnav->position(), _waypt->position()); +} + +SGGeod OBSController::position() const +{ + return _waypt->position(); +} + +/////////////////////////////////////////////////////////////////////////////// + +WayptController* WayptController::createForWaypt(RNAV* aRNAV, const WayptRef& aWpt) +{ + if (!aWpt) { + throw sg_exception("Passed null waypt", "WayptController::createForWaypt"); + } + + const std::string& wty(aWpt->type()); + if (wty == "runway") { + return new RunwayCtl(aRNAV, aWpt); + } + + if (wty == "radialIntercept") { + return new InterceptCtl(aRNAV, aWpt); + } + + if (wty == "dmeIntercept") { + return new DMEInterceptCtl(aRNAV, aWpt); + } + + if (wty == "hdgToAlt") { + return new ConstHdgToAltCtl(aRNAV, aWpt); + } + + if (wty == "vectors") { + return new VectorsCtl(aRNAV, aWpt); + } + + if (wty == "hold") { + return new HoldCtl(aRNAV, aWpt); + } + + return new BasicWayptCtl(aRNAV, aWpt); +} + +} // of namespace flightgear + diff --git a/src/Instrumentation/rnav_waypt_controller.hxx b/src/Instrumentation/rnav_waypt_controller.hxx new file mode 100644 index 000000000..4d99c8af7 --- /dev/null +++ b/src/Instrumentation/rnav_waypt_controller.hxx @@ -0,0 +1,193 @@ +// rnav_waypt_controller.hxx - Waypoint-specific behaviours for RNAV systems +// Written by James Turner, started 2009. +// +// Copyright (C) 2009 Curtis L. Olson +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +#ifndef FG_WAYPT_CONTROLLER_HXX +#define FG_WAYPT_CONTROLLER_HXX + +#include + +namespace flightgear +{ + +/** + * Abstract RNAV interface, for devices which implement an RNAV + * system - INS / GPS / FMS + */ +class RNAV +{ +public: + virtual SGGeod position() = 0; + + /** + * True track in degrees + */ + virtual double trackDeg() = 0; + + /** + * Ground speed (along the track) in knots + */ + virtual double groundSpeedKts() = 0; + + /** + * Vertical speed in ft/minute + */ + virtual double vspeedFPM()= 0; + + /** + * Magnetic variation at current position + */ + virtual double magvarDeg() = 0; + + /** + * device selected course (eg, from autopilot / MCP / OBS) in degrees + */ + virtual double selectedMagCourse() = 0; + + /** + * minimum distance to a waypoint for overflight sequencing. + */ + virtual double overflightArmDistanceM() = 0; +}; + +class WayptController +{ +public: + virtual ~WayptController(); + + virtual void init(); + + virtual void update() = 0; + + /** + * Compute time until the waypoint is done + */ + virtual double timeToWaypt() const; + + /** + * Compute distance until the waypoint is done + */ + virtual double distanceToWayptM() const = 0; + + /** + * Bearing to the waypoint, if this value is meaningful. + * Default implementation returns the target track + */ + virtual double trueBearingDeg() const + { return _targetTrack; } + + virtual double targetTrackDeg() const + { return _targetTrack; } + + virtual double xtrackErrorNm() const + { return 0.0; } + + virtual double courseDeviationDeg() const + { return 0.0; } + + /** + * Position associated with the waypt. For static waypoints, this is + * simply the waypoint position itself; for dynamic points, it's the + * estimated location at which the controller will be done. + */ + virtual SGGeod position() const = 0; + + /** + * Is this controller finished? + */ + bool isDone() const + { return _isDone; } + + /** + * to/from flag - true = to, false = from. Defaults to 'true' because + * nearly all waypoint controllers become done as soon as this value would + * become false. + */ + virtual bool toFlag() const + { return true; } + + /** + * Static factory method, given a waypoint, return a controller bound + * to it, of the appropriate type + */ + static WayptController* createForWaypt(RNAV* rnav, const WayptRef& aWpt); +protected: + WayptController(RNAV* aRNAV, const WayptRef& aWpt) : + _waypt(aWpt), + _rnav(aRNAV), + _isDone(false) + { } + + WayptRef _waypt; + double _targetTrack; + RNAV* _rnav; + + void setDone(); +private: + bool _isDone; +}; + +/** + * Controller supports 'directTo' (DTO) navigation to a waypoint. This + * creates a course from a starting point, to the waypoint, and reports + * deviation from that course. + * + * The controller is done when the waypoint is reached (to/from goes to 'from') + */ +class DirectToController : public WayptController +{ +public: + DirectToController(RNAV* aRNAV, const WayptRef& aWpt, const SGGeod& aOrigin); + + virtual void init(); + virtual void update(); + virtual double distanceToWayptM() const; + virtual double xtrackErrorNm() const; + virtual double courseDeviationDeg() const; + virtual double trueBearingDeg() const; + virtual SGGeod position() const; +private: + SGGeod _origin; + double _distanceM; + double _courseDev; +}; + +/** + * + */ +class OBSController : public WayptController +{ +public: + OBSController(RNAV* aRNAV, const WayptRef& aWpt); + + virtual void init(); + virtual void update(); + virtual double distanceToWayptM() const; + virtual double xtrackErrorNm() const; + virtual double courseDeviationDeg() const; + virtual double trueBearingDeg() const; + virtual bool toFlag() const; + virtual SGGeod position() const; +private: + double _distanceM; + double _courseDev; +}; + +} // of namespace flightgear + +#endif diff --git a/src/Instrumentation/testgps.cxx b/src/Instrumentation/testgps.cxx index cc73bded0..a35012f4b 100644 --- a/src/Instrumentation/testgps.cxx +++ b/src/Instrumentation/testgps.cxx @@ -1,4 +1,9 @@ +#include + +#include +#include + #include
#include
#include
@@ -6,8 +11,12 @@ #include #include #include +#include +#include +#include using std::string; +using namespace flightgear; char *homedir = ::getenv( "HOME" ); char *hostname = ::getenv( "HOSTNAME" ); @@ -45,6 +54,16 @@ void printScratch(SGPropertyNode* scratch) } } +void printRoute(const WayptVec& aRoute) +{ + SG_LOG(SG_GENERAL, SG_INFO, "route size=" << aRoute.size()); + for (unsigned int r=0; rident() << " " + << w->owner()->ident()); + } +} + void createDummyRoute(FGRouteMgr* rm) { SGPropertyNode* rmInput = fgGetNode("/autopilot/route-manager/input", true); @@ -59,6 +78,8 @@ void createDummyRoute(FGRouteMgr* rm) int main(int argc, char* argv[]) { + +try{ globals = new FGGlobals; fgInitFGRoot(argc, argv); @@ -69,6 +90,9 @@ int main(int argc, char* argv[]) fgInitNav(); + fgSetDouble("/environment/magnetic-variation-deg", 0.0); + + Airway::load(); SG_LOG(SG_GENERAL, SG_ALERT, "hello world!"); @@ -94,12 +118,15 @@ int main(int argc, char* argv[]) // globals->add_subsystem("environment", envMgr); // envMgr->init(); + fgSetBool("/sim/realism/simple-gps", true); + + // _realismSimpleGps SGPropertyNode* nd = fgGetNode("/instrumentation/gps", true); GPS* gps = new GPS(nd); globals->add_subsystem("gps", gps); - - + + const FGAirport* egph = fgFindAirportID("EGPH"); testSetPosition(egph->geod()); // startup the route manager @@ -116,6 +143,7 @@ int main(int argc, char* argv[]) // update a few times gps->update(0.05); gps->update(0.05); + gps->update(0.05); scratch->setStringValue("query", "TL"); scratch->setStringValue("type", "Vor"); @@ -221,7 +249,105 @@ int main(int argc, char* argv[]) nd->setStringValue("command", "define-user-wpt"); printScratch(scratch); +// airways + FGPositioned::TypeFilter vorFilt(FGPositioned::VOR); + FGPositionedRef tla = FGPositioned::findClosestWithIdent("TLA", pos, &vorFilt); + FGPositionedRef big = FGPositioned::findClosestWithIdent("BIG", pos, &vorFilt); + FGPositionedRef pol = FGPositioned::findClosestWithIdent("POL", pos, &vorFilt); + + const FGAirport* eddm = fgFindAirportID("EDDM"); + FGPositionedRef mun = FGPositioned::findClosestWithIdent("MUN", + eddm->geod(), &vorFilt); + + const FGAirport* ksfo = fgFindAirportID("KSFO"); + FGPositionedRef sfo = FGPositioned::findClosestWithIdent("SFO", + ksfo->geod(), &vorFilt); + + + WayptRef awy1 = new NavaidWaypoint(tla, NULL); + WayptRef awy2 = new NavaidWaypoint(big, NULL); + WayptRef awy3 = new NavaidWaypoint(pol, NULL); + WayptRef awy4 = new NavaidWaypoint(mun, NULL); + WayptRef awy5 = new NavaidWaypoint(sfo, NULL); + + WayptRef awy6 = new NavaidWaypoint( + (FGPositioned*) fgFindAirportID("KJFK"), NULL); + + SGPath p("/Users/jmt/Desktop/airways.kml"); + std::fstream f; + f.open(p.str().c_str(), fstream::out | fstream::trunc); + +// pre-amble + f << "\n" + "\n" + "\n"; + + WayptVec route; + Airway::highLevel()->route(awy1, awy3, route); + Route::dumpRouteToLineString("egph-egcc", route, f); + + Airway::lowLevel()->route(awy1, awy2, route); + Route::dumpRouteToLineString("egph-big", route, f); + + Airway::lowLevel()->route(awy2, awy4, route); + Route::dumpRouteToLineString("big-mun", route, f); + + Airway::highLevel()->route(awy4, awy5, route); + Route::dumpRouteToLineString("mun-sfo", route, f); + + Airway::lowLevel()->route(awy5, awy6, route); + Route::dumpRouteToLineString("sfo-jfk", route, f); + + // post-amble + f << "\n" + "" << endl; + f.close(); + +// procedures + SGPath op("/Users/jmt/Desktop/procedures.kml"); + f.open(op.str().c_str(), fstream::out | fstream::trunc); + + FGAirport* eham = (FGAirport*) fgFindAirportID("EHAM"); + FGPositioned::TypeFilter fixFilt(FGPositioned::FIX); + + WayptVec approach; + FGPositionedRef redfa = FGPositioned::findClosestWithIdent("REDFA", + eham->geod(), &fixFilt); + bool ok = eham->buildApproach(new NavaidWaypoint(redfa, NULL), + eham->getRunwayByIdent("18R"), approach); + if (!ok ) { + SG_LOG(SG_GENERAL, SG_INFO, "failed to build approach"); + } + FGAirport* egll = (FGAirport*) fgFindAirportID("EGLL"); + WayptVec approach2; + ok = egll->buildApproach(new NavaidWaypoint(big, NULL), + egll->getRunwayByIdent("27R"), approach2); + if (!ok ) { + SG_LOG(SG_GENERAL, SG_INFO, "failed to build approach"); + } + +// pre-amble + f << "\n" + "\n" + "\n"; + + Route::dumpRouteToLineString("REDFA 18R", approach, f); + Route::dumpRouteToLineString("EGLL 27R", approach2, f); + + // post-amble + f << "\n" + "" << endl; + f.close(); + + return EXIT_SUCCESS; + + + +} catch (sg_exception& ex) { + SG_LOG(SG_GENERAL, SG_ALERT, "exception:" << ex.getFormattedMessage()); +} + return EXIT_FAILURE; } diff --git a/src/Main/fg_init.cxx b/src/Main/fg_init.cxx index 1ebff97b1..184e5623a 100644 --- a/src/Main/fg_init.cxx +++ b/src/Main/fg_init.cxx @@ -97,6 +97,7 @@ #include #include #include +#include #include #include #include @@ -1071,7 +1072,8 @@ fgInitNav () fixlist.init( p_fix ); // adds fixes to the DB in positioned.cxx SG_LOG(SG_GENERAL, SG_INFO, " Airways"); - SGPath p_awy( globals->get_fg_root() ); + #if 0 + SGPath p_awy( globals->get_fg_root() ); p_awy.append( "Navaids/awy.dat" ); FGAirwayNetwork *awyNet = new FGAirwayNetwork; //cerr << "Loading Airways" << endl; @@ -1079,7 +1081,9 @@ fgInitNav () awyNet->init(); //cerr << "initializing airways" << endl; globals->set_airwaynet( awyNet ); - +#endif + flightgear::Airway::load(); + return true; } diff --git a/src/Navaids/Makefile.am b/src/Navaids/Makefile.am index a1f38ed14..66c650045 100644 --- a/src/Navaids/Makefile.am +++ b/src/Navaids/Makefile.am @@ -5,11 +5,15 @@ noinst_LIBRARIES = libNavaids.a libNavaids_a_SOURCES = \ navdb.hxx navdb.cxx \ fix.hxx fixlist.hxx fixlist.cxx \ - awynet.hxx awynet.cxx \ navrecord.hxx navrecord.cxx \ navlist.hxx navlist.cxx \ positioned.hxx positioned.cxx \ - markerbeacon.hxx markerbeacon.cxx + markerbeacon.hxx markerbeacon.cxx \ + routePath.hxx routePath.cxx \ + airways.hxx airways.cxx \ + route.hxx route.cxx \ + waypoint.hxx waypoint.cxx \ + procedure.hxx procedure.cxx \ # # testnavs_SOURCES = testnavs.cxx diff --git a/src/Navaids/airways.cxx b/src/Navaids/airways.cxx new file mode 100644 index 000000000..5bdbb2bee --- /dev/null +++ b/src/Navaids/airways.cxx @@ -0,0 +1,454 @@ +// airways.cxx - storage of airways network, and routing between nodes +// Written by James Turner, started 2009. +// +// Copyright (C) 2009 Curtis L. Olson +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +#ifndef HAVE_CONFIG_H +# include "config.h" +#endif + +#include "airways.hxx" + +#include +#include + +#include +#include +#include +#include + +#include + +#include
+#include +#include + +using std::make_pair; +using std::string; +using std::set; +using std::vector; + +#define DEBUG_AWY_SEARCH 1 + +namespace flightgear +{ + +Airway::Network* Airway::static_lowLevel = NULL; +Airway::Network* Airway::static_highLevel = NULL; + +////////////////////////////////////////////////////////////////////////////// + +/** + * information about an edge in the network. + * Some of this information is computed lazily + */ +class AdjacentWaypoint +{ +public: + AdjacentWaypoint(const FGPositionedRef& aOrigin, + const FGPositionedRef& aDest, Airway* aWay); + + double distanceM() const; + + const FGPositionedRef& other(const FGPositionedRef& aEnd) const; + + const FGPositionedRef origin; + const FGPositionedRef destination; + const Airway* airway; + +private: + void validate() const; + mutable double _distanceM; +}; + +class AStarOpenNode : public SGReferenced +{ +public: + AStarOpenNode(FGPositionedRef aNode, double aLegDist, + Airway* aAirway, + FGPositionedRef aDest, AStarOpenNode* aPrev) : + node(aNode), + airway(aAirway), + previous(aPrev) + { + distanceFromStart = aLegDist; + if (previous) { + distanceFromStart += previous->distanceFromStart; + } + + directDistanceToDestination = SGGeodesy::distanceM(node->geod(), aDest->geod()); + } + + virtual ~AStarOpenNode() + { + } + + FGPositionedRef node; + Airway* airway; + SGSharedPtr previous; + double distanceFromStart; // aka 'g(x)' + double directDistanceToDestination; // aka 'h(x)' + + /** + * aka 'f(x)' + */ + double totalCost() const { + return distanceFromStart + directDistanceToDestination; + } +}; + +typedef SGSharedPtr AStarOpenNodeRef; + +//////////////////////////////////////////////////////////////////////////// + +Airway::Network* Airway::lowLevel() +{ + return static_lowLevel; +} + +Airway::Network* Airway::highLevel() +{ + return static_highLevel; +} + +Airway::Airway(const std::string& aIdent, double aTop, double aBottom) : + _ident(aIdent), + _topAltitudeFt(aTop), + _bottomAltitudeFt(aBottom) +{ +} + +void Airway::load() +{ + static_lowLevel = new Network; + static_highLevel = new Network; + + SGPath path( globals->get_fg_root() ); + path.append( "Navaids/awy.dat" ); + + std::string identStart, identEnd, name; + double latStart, lonStart, latEnd, lonEnd; + int type, base, top; + //int airwayIndex = 0; + //FGNode *n; + + sg_gzifstream in( path.str() ); + if ( !in.is_open() ) { + SG_LOG( SG_GENERAL, SG_ALERT, "Cannot open file: " << path.str() ); + throw sg_io_exception("Could not open airways data", sg_location(path.str())); + } +// toss the first two lines of the file + in >> skipeol; + in >> skipeol; + +// read in each remaining line of the file + while (!in.eof()) { + in >> identStart; + + if (identStart == "99") { + break; + } + + in >> latStart >> lonStart >> identEnd >> latEnd >> lonEnd >> type >> base >> top >> name; + in >> skipeol; + + // type = 1; low-altitude + // type = 2; high-altitude + Network* net = (type == 1) ? static_lowLevel : static_highLevel; + + SGGeod startPos(SGGeod::fromDeg(lonStart, latStart)), + endPos(SGGeod::fromDeg(lonEnd, latEnd)); + + Airway* awy = net->findAirway(name, top, base); + net->addEdge(awy, startPos, identStart, endPos, identEnd); + } // of file line iteration +} + +Airway* Airway::Network::findAirway(const std::string& aName, double aTop, double aBase) +{ + AirwayDict::iterator it = _airways.find(aName); + if (it == _airways.end()) { + Airway* awy = new Airway(aName, aTop, aBase); + it = _airways.insert(it, make_pair(aName, awy)); + } + + return it->second; +} + +void Airway::Network::addEdge(Airway* aWay, const SGGeod& aStartPos, + const std::string& aStartIdent, + const SGGeod& aEndPos, const std::string& aEndIdent) +{ + FGPositionedRef start = FGPositioned::findClosestWithIdent(aStartIdent, aStartPos); + FGPositionedRef end = FGPositioned::findClosestWithIdent(aEndIdent, aEndPos); + + if (!start) { + SG_LOG(SG_GENERAL, SG_INFO, "unknown airways start pt: '" << aStartIdent << "'"); + start = FGPositioned::createUserWaypoint(aStartIdent, aStartPos); + return; + } + + if (!end) { + SG_LOG(SG_GENERAL, SG_INFO, "unknown airways end pt: '" << aEndIdent << "'"); + end = FGPositioned::createUserWaypoint(aEndIdent, aEndPos); + return; + } + + AdjacentWaypoint* edge = new AdjacentWaypoint(start, end, aWay); + _graph.insert(make_pair(start, edge)); + _graph.insert(make_pair(end, edge)); +} + +////////////////////////////////////////////////////////////////////////////// + +bool Airway::Network::inNetwork(const FGPositioned* aPos) const +{ + FGPositioned* pos = const_cast(aPos); + return (_graph.find(pos) != _graph.end()); +} + +bool Airway::Network::route(WayptRef aFrom, WayptRef aTo, + WayptVec& aPath) +{ + if (!aFrom || !aTo) { + throw sg_exception("invalid waypoints to route between"); + } + +// find closest nodes on the graph to from/to +// if argument waypoints are directly on the graph (which is frequently the +// case), note this so we don't duplicate them in the output. + + FGPositionedRef from, to; + bool exactTo, exactFrom; + boost::tie(from, exactFrom) = findClosestNode(aFrom); + boost::tie(to, exactTo) = findClosestNode(aTo); + +#ifdef DEBUG_AWY_SEARCH + SG_LOG(SG_GENERAL, SG_INFO, "from:" << from->ident() << "/" << from->name()); + SG_LOG(SG_GENERAL, SG_INFO, "to:" << to->ident() << "/" << to->name()); +#endif + + bool ok = search2(from, to, aPath); + if (!ok) { + return false; + } + + if (exactTo) { + aPath.pop_back(); + } + + if (exactFrom) { + // edge case - if from and to are equal, which can happen, don't + // crash here. This happens routing EGPH -> EGCC; 'DCS' is common + // to the EGPH departure and EGCC STAR. + if (!aPath.empty()) { + aPath.erase(aPath.begin()); + } + } + + return true; +} + +std::pair +Airway::Network::findClosestNode(WayptRef aRef) +{ + return findClosestNode(aRef->position()); +} + +class InAirwayFilter : public FGPositioned::Filter +{ +public: + InAirwayFilter(Airway::Network* aNet) : + _net(aNet) + { ; } + + virtual bool pass(FGPositioned* aPos) const + { + return _net->inNetwork(aPos); + } + + virtual FGPositioned::Type minType() const + { return FGPositioned::WAYPOINT; } + + virtual FGPositioned::Type maxType() const + { return FGPositioned::NDB; } + +private: + Airway::Network* _net; +}; + +std::pair +Airway::Network::findClosestNode(const SGGeod& aGeod) +{ + InAirwayFilter f(this); + FGPositionedRef r = FGPositioned::findClosest(aGeod, 800.0, &f); + bool exact = false; + + if (r && (SGGeodesy::distanceM(aGeod, r->geod()) < 100.0)) { + exact = true; // within 100 metres, let's call that exact + } + + return make_pair(r, exact); +} + +///////////////////////////////////////////////////////////////////////////// + +typedef vector OpenNodeHeap; + +static void buildWaypoints(AStarOpenNodeRef aNode, WayptVec& aRoute) +{ +// count the route length, and hence pre-size aRoute + int count = 0; + AStarOpenNodeRef n = aNode; + for (; n != NULL; ++count, n = n->previous) {;} + aRoute.resize(count); + +// run over the route, creating waypoints + for (n = aNode; n; n=n->previous) { + aRoute[--count] = new NavaidWaypoint(n->node, n->airway); + } +} + +/** + * Inefficent (linear) helper to find an open node in the heap + */ +static AStarOpenNodeRef +findInOpen(const OpenNodeHeap& aHeap, FGPositioned* aPos) +{ + for (unsigned int i=0; inode == aPos) { + return aHeap[i]; + } + } + + return NULL; +} + +class HeapOrder +{ +public: + bool operator()(AStarOpenNode* a, AStarOpenNode* b) + { + return a->totalCost() > b->totalCost(); + } +}; + +bool Airway::Network::search2(FGPositionedRef aStart, FGPositionedRef aDest, + WayptVec& aRoute) +{ + + typedef set ClosedNodeSet; + typedef std::pair AdjacencyMapRange; + + OpenNodeHeap openNodes; + ClosedNodeSet closedNodes; + HeapOrder ordering; + + openNodes.push_back(new AStarOpenNode(aStart, 0.0, NULL, aDest, NULL)); + +// A* open node iteration + while (!openNodes.empty()) { + std::pop_heap(openNodes.begin(), openNodes.end(), ordering); + AStarOpenNodeRef x = openNodes.back(); + FGPositioned* xp = x->node; + openNodes.pop_back(); + closedNodes.insert(xp); + + // SG_LOG(SG_GENERAL, SG_INFO, "x:" << xp->ident() << ", f(x)=" << x->totalCost()); + + // check if xp is the goal; if so we're done, since there cannot be an open + // node with lower f(x) value. + if (xp == aDest) { + buildWaypoints(x, aRoute); + return true; + } + + // adjacent (neighbour) iteration + AdjacencyMapRange r(_graph.equal_range(xp)); + for (; r.first != r.second; ++r.first) { + AdjacentWaypoint* adj(r.first->second); + FGPositioned* yp = adj->other(xp); + if (closedNodes.count(yp)) { + continue; // closed, ignore + } + + AStarOpenNodeRef y = findInOpen(openNodes, yp); + if (y) { // already open + double g = x->distanceFromStart + adj->distanceM(); + if (g > y->distanceFromStart) { + // worse path, ignore + //SG_LOG(SG_GENERAL, SG_INFO, "\tabandoning " << yp->ident() << + // " path is worse: g(y)" << y->distanceFromStart << ", g'=" << g); + continue; + } + + // we need to update y. Unfortunately this means rebuilding the heap, + // since y's score can change arbitrarily + //SG_LOG(SG_GENERAL, SG_INFO, "\tfixing up previous for new path to " << yp->ident() << ", d =" << g); + y->previous = x; + y->distanceFromStart = g; + y->airway = (Airway*) adj->airway; + std::make_heap(openNodes.begin(), openNodes.end(), ordering); + } else { // not open, insert a new node for y into the heap + y = new AStarOpenNode(yp, adj->distanceM(), + (Airway*) adj->airway, aDest, x); + //SG_LOG(SG_GENERAL, SG_INFO, "\ty=" << yp->ident() << ", f(y)=" << y->totalCost()); + openNodes.push_back(y); + std::push_heap(openNodes.begin(), openNodes.end(), ordering); + } + } // of neighbour iteration + } // of open node iteration + + SG_LOG(SG_GENERAL, SG_INFO, "A* failed to find route"); + return false; +} + +////////////////////////////////////////////////////////////////////////////// +// AdjacentWaypoint definitions + +AdjacentWaypoint::AdjacentWaypoint( + const FGPositionedRef& aOrigin, const FGPositionedRef& aDest, Airway* aWay) : + origin(aOrigin), + destination(aDest), + airway(aWay), + _distanceM(-1.0) +{ + +} + +const FGPositionedRef& +AdjacentWaypoint::other(const FGPositionedRef& aEnd) const +{ + return (aEnd == origin) ? destination : origin; +} + +double AdjacentWaypoint::distanceM() const +{ + validate(); + return _distanceM; +} + +void AdjacentWaypoint::validate() const +{ + if (_distanceM > 0.0) { + return; // already validated + } + + _distanceM = SGGeodesy::distanceM(origin->geod(), destination->geod()); +} + +} // of namespace flightgear diff --git a/src/Navaids/airways.hxx b/src/Navaids/airways.hxx new file mode 100644 index 000000000..f29ecbb20 --- /dev/null +++ b/src/Navaids/airways.hxx @@ -0,0 +1,134 @@ +// airways.hxx - storage of airways network, and routing between nodes +// Written by James Turner, started 2009. +// +// Copyright (C) 2009 Curtis L. Olson +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +#ifndef FG_AIRWAYS_HXX +#define FG_AIRWAYS_HXX + +#include +#include + +#include + +class FGPositioned; +typedef SGSharedPtr FGPositionedRef; + +namespace flightgear { + +// forward declare some helpers +struct SearchContext; +class AdjacentWaypoint; +class InAirwayFilter; + +class Airway : public Route +{ +public: + virtual std::string ident() const + { return _ident; } + + static void load(); + + /** + * Track a network of airways + * + */ + class Network + { + public: + friend class Airway; + friend class InAirwayFilter; + + + + /** + * Principal routing algorithm. Attempts to find the best route beween + * two points. If either point is part of the airway network (e.g, a SID + * or STAR transition), it will not be duplicated in the result + * path. + * + * Returns true if a route could be found, or false otherwise. + */ + bool route(WayptRef aFrom, WayptRef aTo, WayptVec& aPath); + private: + void addEdge(Airway* aWay, const SGGeod& aStartPos, + const std::string& aStartIdent, + const SGGeod& aEndPos, const std::string& aEndIdent); + + Airway* findAirway(const std::string& aName, double aTop, double aBase); + + typedef std::multimap AdjacencyMap; + AdjacencyMap _graph; + + typedef std::vector AdjacentWaypointVec; + + typedef std::map AirwayDict; + AirwayDict _airways; + + bool search2(FGPositionedRef aStart, FGPositionedRef aDest, WayptVec& aRoute); + + /** + * Test if a positioned item is part of this airway network or not. + */ + bool inNetwork(const FGPositioned* aRef) const; + + /** + * Find the closest node on the network, to the specified waypoint + * + * May return NULL,false if no match could be found; the search is + * internally limited to avoid very poor performance; for example, + * in the middle of an ocean. + * + * The second return value indicates if the returned value is + * equal (true) or distinct (false) to the input waypoint. + * Equality here means being physically within a close tolerance, + * on the order of a hundred metres. + */ + std::pair findClosestNode(WayptRef aRef); + + /** + * Overloaded version working with a raw SGGeod + */ + std::pair findClosestNode(const SGGeod& aGeod); + }; + + + static Network* highLevel(); + static Network* lowLevel(); + +private: + Airway(const std::string& aIdent, double aTop, double aBottom); + + friend class Network; + + static Network* static_highLevel; + static Network* static_lowLevel; + + std::string _ident; + double _topAltitudeFt; + double _bottomAltitudeFt; + + // high-level vs low-level flag + // ... ? + + WayptVec _elements; +}; + +} // of namespace flightgear + + +#endif //of FG_AIRWAYS_HXX diff --git a/src/Navaids/positioned.cxx b/src/Navaids/positioned.cxx index 917156877..fcf5ec848 100644 --- a/src/Navaids/positioned.cxx +++ b/src/Navaids/positioned.cxx @@ -518,7 +518,9 @@ FGPositioned::Filter::passType(Type aTy) const static FGPositioned::List findAll(const NamedPositionedIndex& aIndex, - const std::string& aName, FGPositioned::Filter* aFilter) + const std::string& aName, + FGPositioned::Filter* aFilter, + bool aExact) { FGPositioned::List result; if (aName.empty()) { @@ -526,9 +528,16 @@ findAll(const NamedPositionedIndex& aIndex, } std::string name = boost::to_upper_copy(aName); - std::string upperBoundId = name; - upperBoundId[upperBoundId.size()-1]++; - NamedPositionedIndex::const_iterator upperBound = aIndex.lower_bound(upperBoundId); + NamedPositionedIndex::const_iterator upperBound; + + if (aExact) { + upperBound = aIndex.upper_bound(name); + } else { + std::string upperBoundId = name; + upperBoundId[upperBoundId.size()-1]++; + upperBound = aIndex.lower_bound(upperBoundId); + } + NamedPositionedIndex::const_iterator it = aIndex.lower_bound(name); for (; it != upperBound; ++it) { @@ -663,7 +672,7 @@ const char* FGPositioned::nameForType(Type aTy) FGPositionedRef FGPositioned::findClosestWithIdent(const std::string& aIdent, const SGGeod& aPos, Filter* aFilter) { - FGPositioned::List r(findAll(global_identIndex, aIdent, aFilter)); + FGPositioned::List r(findAll(global_identIndex, aIdent, aFilter, true)); if (r.empty()) { return FGPositionedRef(); } @@ -682,15 +691,15 @@ FGPositioned::findWithinRange(const SGGeod& aPos, double aRangeNm, Filter* aFilt } FGPositioned::List -FGPositioned::findAllWithIdent(const std::string& aIdent, Filter* aFilter) +FGPositioned::findAllWithIdent(const std::string& aIdent, Filter* aFilter, bool aExact) { - return findAll(global_identIndex, aIdent, aFilter); + return findAll(global_identIndex, aIdent, aFilter, aExact); } FGPositioned::List -FGPositioned::findAllWithName(const std::string& aName, Filter* aFilter) +FGPositioned::findAllWithName(const std::string& aName, Filter* aFilter, bool aExact) { - return findAll(global_nameIndex, aName, aFilter); + return findAll(global_nameIndex, aName, aFilter, aExact); } FGPositionedRef diff --git a/src/Navaids/positioned.hxx b/src/Navaids/positioned.hxx index 27d807a72..f6f61229a 100644 --- a/src/Navaids/positioned.hxx +++ b/src/Navaids/positioned.hxx @@ -43,6 +43,7 @@ public: TAXIWAY, PAVEMENT, PARK_STAND, + WAYPOINT, FIX, VOR, NDB, @@ -55,7 +56,6 @@ public: DME, TACAN, OBSTACLE, - WAYPOINT, // user-defined waypoint FREQ_GND, FREQ_TWR, FREQ_ATIS, @@ -160,12 +160,12 @@ public: * Find all items with the specified ident * @param aFilter - optional filter on items */ - static List findAllWithIdent(const std::string& aIdent, Filter* aFilter = NULL); + static List findAllWithIdent(const std::string& aIdent, Filter* aFilter = NULL, bool aExact = true); /** * As above, but searches names instead of idents */ - static List findAllWithName(const std::string& aName, Filter* aFilter = NULL); + static List findAllWithName(const std::string& aName, Filter* aFilter = NULL, bool aExact = true); /** * Sort an FGPositionedList by distance from a position diff --git a/src/Navaids/procedure.cxx b/src/Navaids/procedure.cxx new file mode 100644 index 000000000..4080f0a75 --- /dev/null +++ b/src/Navaids/procedure.cxx @@ -0,0 +1,333 @@ +// procedure.cxx - define route storing an approach, arrival or departure procedure +// Written by James Turner, started 2009. +// +// Copyright (C) 2009 Curtis L. Olson +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +#include "procedure.hxx" + +#include + +#include + +using std::string; + +namespace flightgear +{ + +Procedure::Procedure(const string& aIdent) : + _ident(aIdent) +{ +} + +Approach::Approach(const string& aIdent) : + Procedure(aIdent) +{ + +} + +void Approach::setRunway(FGRunwayRef aRwy) +{ + _runway = aRwy; +} + +void Approach::setPrimaryAndMissed(const WayptVec& aPrimary, const WayptVec& aMissed) +{ + _primary = aPrimary; + _primary[0]->setFlag(WPT_IAF, true); + _primary[_primary.size()-1]->setFlag(WPT_FAF, true); + + _missed = aMissed; + + if (!_missed.empty()) { + // mark the first point as the published missed-approach point + _missed[0]->setFlag(WPT_MAP, true); + + // mark all the points as being on the missed approach route + for (unsigned int i=0; i<_missed.size(); ++i) { + _missed[i]->setFlag(WPT_MISS, true); + } + } +} + +void Approach::addTransition(Transition* aTrans) +{ + WayptRef entry = aTrans->enroute(); + _transitions[entry] = aTrans; +} + +bool Approach::route(WayptRef aIAF, WayptVec& aWps) +{ + WptTransitionMap::iterator it; + bool haveTrans = false; + for (it = _transitions.begin(); it != _transitions.end(); ++it) { + Transition* t= it->second; + if (t->route(aIAF, aWps)) { + haveTrans = true; + break; + } + } // of transitions iteration + + if (!haveTrans) { + SG_LOG(SG_GENERAL, SG_INFO, "approach " << ident() << " has no transition " << + "for IAF: " << aIAF->ident()); + return false; + } + + aWps.insert(aWps.end(), _primary.begin(), _primary.end()); + aWps.push_back(new RunwayWaypt(_runway, NULL)); + aWps.insert(aWps.end(), _missed.begin(), _missed.end()); + return true; +} + +bool Approach::routeFromVectors(WayptVec& aWps) +{ + aWps.insert(aWps.end(), _primary.begin(), _primary.end()); + aWps.push_back(new RunwayWaypt(_runway, NULL)); + aWps.insert(aWps.end(), _missed.begin(), _missed.end()); + return true; +} + +////////////////////////////////////////////////////////////////////////////// + +ArrivalDeparture::ArrivalDeparture(const string& aIdent) : + Procedure(aIdent) +{ +} + +void ArrivalDeparture::addRunway(FGRunwayRef aWay) +{ + _runways[aWay] = NULL; +} + +bool ArrivalDeparture::isForRunway(FGRunwayRef aWay) const +{ + // null runway always passes + if (!aWay) { + return true; + } + + return (_runways.count(aWay)); +} + +void ArrivalDeparture::addTransition(Transition* aTrans) +{ + WayptRef entry = aTrans->enroute(); + _enrouteTransitions[entry] = aTrans; +} + +void ArrivalDeparture::addRunwayTransition(FGRunwayRef aWay, Transition* aTrans) +{ + assert(aWay->ident() == aTrans->ident()); + if (!isForRunway(aWay)) { + throw sg_io_exception("adding transition for unspecified runway:" + aWay->ident(), ident()); + } + + _runways[aWay] = aTrans; +} + +void ArrivalDeparture::setCommon(const WayptVec& aWps) +{ + _common = aWps; +} + +bool ArrivalDeparture::commonRoute(Waypt* aEnroute, WayptVec& aPath, FGRunwayRef aRwy) +{ + // assume we're routing from enroute, to the runway. + // for departures, we'll flip the result points + + Transition* t = findTransitionByEnroute(aEnroute); + WayptVec::iterator firstCommon = _common.begin(); + if (t) { + t->route(aEnroute, aPath); + + Waypt* transEnd = t->procedureEnd(); + for (; firstCommon != _common.end(); ++firstCommon) { + if ((*firstCommon)->matches(transEnd)) { + // found transition->common point, stop search + break; + } + } // of common points + + // if we hit this point, the transition doesn't end (start, for a SID) on + // a common point. We assume this means we should just append the entire + // common section after the transition. + firstCommon = _common.begin(); + } else { + if (aEnroute && !(*firstCommon)->matches(aEnroute)) { + return false; + } + } // of not using a transition + + // append (some) common points + aPath.insert(aPath.end(), firstCommon, _common.end()); + + if (!aRwy) { + // no runway specified, we're done + return true; + } + + RunwayTransitionMap::iterator r = _runways.find(aRwy); + assert(r != _runways.end()); + if (!r->second) { + // no transitions specified. Not great, but not + // much we can do about it. Calling code will insert VECTORS to the approach + // if required, or maybe there's an approach transition defined. + return true; + } + + SG_LOG(SG_GENERAL, SG_INFO, ident() << " using runway transition for " << r->first->ident()); + r->second->route(NULL, aPath); + return true; +} + +Transition* ArrivalDeparture::findTransitionByEnroute(Waypt* aEnroute) const +{ + if (!aEnroute) { + return NULL; + } + + WptTransitionMap::const_iterator eit; + for (eit = _enrouteTransitions.begin(); eit != _enrouteTransitions.end(); ++eit) { + if (eit->second->enroute()->matches(aEnroute)) { + SG_LOG(SG_GENERAL, SG_INFO, ident() << " using enroute transition " << eit->second->ident()); + return eit->second; + } + } // of enroute transition iteration + + return NULL; +} + +WayptRef ArrivalDeparture::findBestTransition(const SGGeod& aPos) const +{ + // no transitions, that's easy + if (_enrouteTransitions.empty()) { + SG_LOG(SG_GENERAL, SG_INFO, "no enroute transitions for " << ident()); + return _common.front(); + } + + double d = 1e9; + WayptRef w; + WptTransitionMap::const_iterator eit; + for (eit = _enrouteTransitions.begin(); eit != _enrouteTransitions.end(); ++eit) { + WayptRef c = eit->second->enroute(); + SG_LOG(SG_GENERAL, SG_INFO, "findBestTransition for " << ident() << ", looking at " << c->ident()); + // assert(c->hasFixedPosition()); + double cd = SGGeodesy::distanceM(aPos, c->position()); + + if (cd < d) { // distance to 'c' is less, new best match + d = cd; + w = c; + } + } // of transitions iteration + + assert(w); + return w; +} + +WayptRef ArrivalDeparture::findTransitionByName(const string& aIdent) const +{ + WptTransitionMap::const_iterator eit; + for (eit = _enrouteTransitions.begin(); eit != _enrouteTransitions.end(); ++eit) { + WayptRef c = eit->second->enroute(); + if (c->ident() == aIdent) { + return c; + } + } + + return NULL; +} + +//////////////////////////////////////////////////////////////////////////// + +SID::SID(const string& aIdent) : + ArrivalDeparture(aIdent) +{ +} + +bool SID::route(FGRunwayRef aWay, Waypt* aEnroute, WayptVec& aPath) +{ + if (!isForRunway(aWay)) { + SG_LOG(SG_GENERAL, SG_WARN, "SID " << ident() << " not for runway " << aWay->ident()); + return false; + } + + WayptVec path; + if (!commonRoute(aEnroute, path, aWay)) { + return false; + } + + // SID waypoints (including transitions) are stored reversed, so we can + // re-use the routing code. This is where we fix the ordering for client code + std::back_insert_iterator bi(aPath); + std::reverse_copy(path.begin(), path.end(), bi); + + return true; +} + +//////////////////////////////////////////////////////////////////////////// + +STAR::STAR(const string& aIdent) : + ArrivalDeparture(aIdent) +{ +} + +bool STAR::route(FGRunwayRef aWay, Waypt* aEnroute, WayptVec& aPath) +{ + if (aWay && !isForRunway(aWay)) { + return false; + } + + return commonRoute(aEnroute, aPath, aWay); +} + +///////////////////////////////////////////////////////////////////////////// + +Transition::Transition(const std::string& aIdent, Procedure* aPr, + const WayptVec& aWps) : + _ident(aIdent), + _parent(aPr), + _primary(aWps) +{ + assert(aPr); + assert(!_primary.empty()); + + _primary[0]->setFlag(WPT_TRANSITION, true); +} + +WayptRef Transition::enroute() const +{ + assert(!_primary.empty()); + return _primary[0]; +} + +WayptRef Transition::procedureEnd() const +{ + assert(!_primary.empty()); + return _primary[_primary.size() - 1]; +} + +bool Transition::route(Waypt* aEnroute, WayptVec& aPath) +{ + if (aEnroute && !enroute()->matches(aEnroute)) { + return false; + } + + aPath.insert(aPath.end(), _primary.begin(), _primary.end()); + return true; +} + +} // of namespace diff --git a/src/Navaids/procedure.hxx b/src/Navaids/procedure.hxx new file mode 100644 index 000000000..13df69036 --- /dev/null +++ b/src/Navaids/procedure.hxx @@ -0,0 +1,212 @@ +/// procedure.hxx - define route storing an approach, arrival or departure procedure +// Written by James Turner, started 2009. +// +// Copyright (C) 2009 Curtis L. Olson +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +#ifndef FG_NAVAID_PROCEDURE_HXX +#define FG_NAVAID_PROCEDURE_HXX + +#include + +#include +#include + +typedef SGSharedPtr FGRunwayRef; + +namespace flightgear { + +// forward decls +class NavdataVisitor; + +class Procedure : public Route +{ +public: + + virtual std::string ident() const + { return _ident; } +protected: + Procedure(const std::string& aIdent); + + std::string _ident; +}; + +/** + * Encapsulate a transition segment + */ +class Transition : public Route +{ +public: + bool route(Waypt* aEnroute, WayptVec& aPath); + + Procedure* parent() const + { return _parent; } + + /** + * Return the enroute end of the transition + */ + WayptRef enroute() const; + + /** + * Return the procedure end of the transition + */ + WayptRef procedureEnd() const; + + virtual std::string ident() const + { return _ident; } +private: + friend class NavdataVisitor; + + Transition(const std::string& aIdent, Procedure* aPr, const WayptVec& aWps); + + std::string _ident; + Procedure* _parent; + WayptVec _primary; +}; + +/** + * Describe an approach procedure, including the missed approach + * segment + */ +class Approach : public Procedure +{ +public: + FGRunwayRef runway() + { return _runway; } + + /** + * Build a route from a valid IAF to the runway, including the missed + * segment. Return false if no valid transition from the specified IAF + * could be found + */ + bool route(WayptRef aIAF, WayptVec& aWps); + + /** + * Build route as above, but ignore transitions, and assume radar + * vectoring to the start of main approach + */ + bool routeFromVectors(WayptVec& aWps); + + const WayptVec& primary() const + { return _primary; } + + const WayptVec& missed() const + { return _missed; } + +private: + friend class NavdataVisitor; + + Approach(const std::string& aIdent); + + void setRunway(FGRunwayRef aRwy); + void setPrimaryAndMissed(const WayptVec& aPrimary, const WayptVec& aMissed); + void addTransition(Transition* aTrans); + + FGRunwayRef _runway; + + typedef std::map WptTransitionMap; + WptTransitionMap _transitions; + + WayptVec _primary; // unify these? + WayptVec _missed; +}; + +class ArrivalDeparture : public Procedure +{ +public: + /** + * Predicate, test if this procedure applies to the requested runway + */ + virtual bool isForRunway(FGRunwayRef aWay) const; + + /** + * Find a path between the runway and enroute structure. Waypoints + * corresponding to the appropriate transitions and segments will be created. + */ + virtual bool route(FGRunwayRef aWay, Waypt* aEnroute, WayptVec& aPath) = 0; + + const WayptVec& common() const + { return _common; } + + /** + * Given an enroute location, find the best enroute transition point for + * this arrival/departure. Best is currently determined as 'closest to the + * enroute location'. + */ + WayptRef findBestTransition(const SGGeod& aPos) const; + + /** + * Find an enroute transition waypoint by identifier. This is necessary + * for the route-manager and similar code that that needs to talk about + * transitions in a human-meaningful way (including persistence). + */ + WayptRef findTransitionByName(const std::string& aIdent) const; + + Transition* findTransitionByEnroute(Waypt* aEnroute) const; +protected: + + bool commonRoute(Waypt* aEnroute, WayptVec& aPath, FGRunwayRef aRwy); + + ArrivalDeparture(const std::string& aIdent); + + + void addRunway(FGRunwayRef aRwy); + + typedef std::map RunwayTransitionMap; + RunwayTransitionMap _runways; + +private: + friend class NavdataVisitor; + + void addTransition(Transition* aTrans); + + void setCommon(const WayptVec& aWps); + + void addRunwayTransition(FGRunwayRef aRwy, Transition* aTrans); + + WayptVec _common; + + typedef std::map WptTransitionMap; + WptTransitionMap _enrouteTransitions; + + +}; + +class SID : public ArrivalDeparture +{ +public: + virtual bool route(FGRunwayRef aWay, Waypt* aEnroute, WayptVec& aPath); + +private: + friend class NavdataVisitor; + + SID(const std::string& aIdent); +}; + +class STAR : public ArrivalDeparture +{ +public: + virtual bool route(FGRunwayRef aWay, Waypt* aEnroute, WayptVec& aPath); + +private: + friend class NavdataVisitor; + + STAR(const std::string& aIdent); +}; + +} // of namespace + +#endif diff --git a/src/Navaids/route.cxx b/src/Navaids/route.cxx new file mode 100644 index 000000000..613e5b3c4 --- /dev/null +++ b/src/Navaids/route.cxx @@ -0,0 +1,679 @@ +// route.cxx - classes supporting waypoints and route structures + +// Written by James Turner, started 2009. +// +// Copyright (C) 2009 Curtis L. Olson +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +#ifdef HAVE_CONFIG_H +# include "config.h" +#endif + +#include "route.hxx" + +// std +#include +#include + +// Boost +#include +#include + +// SimGear +#include +#include +#include + +// FlightGear +#include +#include +#include + +using std::string; +using std::vector; +using std::endl; +using std::fstream; + +namespace flightgear { + +Waypt::Waypt(Route* aOwner) : + _altitudeFt(0.0), + _speedKts(0.0), + _altRestrict(RESTRICT_NONE), + _speedRestrict(RESTRICT_NONE), + _owner(aOwner), + _flags(0) +{ +} + +std::string Waypt::ident() const +{ + return ""; +} + +bool Waypt::flag(WayptFlag aFlag) const +{ + return (_flags & aFlag); +} + +void Waypt::setFlag(WayptFlag aFlag, bool aV) +{ + _flags = (_flags & ~aFlag); + if (aV) _flags |= aFlag; +} + +bool Waypt::matches(Waypt* aOther) const +{ + assert(aOther); + if (ident() != aOther->ident()) { // cheap check first + return false; + } + + return matches(aOther->position()); +} + + +bool Waypt::matches(const SGGeod& aPos) const +{ + double d = SGGeodesy::distanceM(position(), aPos); + return (d < 100.0); // 100 metres seems plenty +} + +void Waypt::setAltitude(double aAlt, RouteRestriction aRestrict) +{ + _altitudeFt = aAlt; + _altRestrict = aRestrict; +} + +void Waypt::setSpeed(double aSpeed, RouteRestriction aRestrict) +{ + _speedKts = aSpeed; + _speedRestrict = aRestrict; +} + +std::pair +Waypt::courseAndDistanceFrom(const SGGeod& aPos) const +{ + if (flag(WPT_DYNAMIC)) { + return std::make_pair(0.0, 0.0); + } + + double course, az2, distance; + SGGeodesy::inverse(aPos, position(), course, az2, distance); + return std::make_pair(course, distance); +} + +/////////////////////////////////////////////////////////////////////////// +// persistence + +static RouteRestriction restrictionFromString(const char* aStr) +{ + std::string l = boost::to_lower_copy(std::string(aStr)); + + if (l == "at") return RESTRICT_AT; + if (l == "above") return RESTRICT_ABOVE; + if (l == "below") return RESTRICT_BELOW; + if (l == "none") return RESTRICT_NONE; + + if (l.empty()) return RESTRICT_NONE; + throw sg_io_exception("unknown restriction specification:" + l, + "Route restrictFromString"); +} + +static const char* restrictionToString(RouteRestriction aRestrict) +{ + switch (aRestrict) { + case RESTRICT_AT: return "at"; + case RESTRICT_BELOW: return "below"; + case RESTRICT_ABOVE: return "above"; + case RESTRICT_NONE: return "none"; + default: + throw sg_exception("invalid route restriction", + "Route restrictToString"); + } +} + +Waypt* Waypt::createInstance(Route* aOwner, const std::string& aTypeName) +{ + Waypt* r = NULL; + if (aTypeName == "basic") { + r = new BasicWaypt(aOwner); + } else if (aTypeName == "navaid") { + r = new NavaidWaypoint(aOwner); + } else if (aTypeName == "offset-navaid") { + r = new OffsetNavaidWaypoint(aOwner); + } else if (aTypeName == "hold") { + r = new Hold(aOwner); + } else if (aTypeName == "runway") { + r = new RunwayWaypt(aOwner); + } else if (aTypeName == "hdgToAlt") { + r = new HeadingToAltitude(aOwner); + } else if (aTypeName == "dmeIntercept") { + r = new DMEIntercept(aOwner); + } else if (aTypeName == "radialIntercept") { + r = new RadialIntercept(aOwner); + } else if (aTypeName == "vectors") { + r = new ATCVectors(aOwner); + } + + if (!r || (r->type() != aTypeName)) { + throw sg_exception("broken factory method for type:" + aTypeName, + "Waypt::createInstance"); + } + + return r; +} + +WayptRef Waypt::createFromProperties(Route* aOwner, SGPropertyNode_ptr aProp) +{ + if (!aProp->hasChild("type")) { + throw sg_io_exception("bad props node, no type provided", + "Waypt::createFromProperties"); + } + + WayptRef nd(createInstance(aOwner, aProp->getStringValue("type"))); + nd->initFromProperties(aProp); + return nd; +} + +void Waypt::saveAsNode(SGPropertyNode* n) const +{ + n->setStringValue("type", type()); + writeToProperties(n); +} + +void Waypt::initFromProperties(SGPropertyNode_ptr aProp) +{ + if (aProp->hasChild("generated")) { + setFlag(WPT_GENERATED, aProp->getBoolValue("generated")); + } + + if (aProp->hasChild("overflight")) { + setFlag(WPT_OVERFLIGHT, aProp->getBoolValue("overflight")); + } + + if (aProp->hasChild("arrival")) { + setFlag(WPT_ARRIVAL, aProp->getBoolValue("arrival")); + } + + if (aProp->hasChild("departure")) { + setFlag(WPT_DEPARTURE, aProp->getBoolValue("departure")); + } + + if (aProp->hasChild("miss")) { + setFlag(WPT_MISS, aProp->getBoolValue("miss")); + } + + if (aProp->hasChild("alt-restrict")) { + _altRestrict = restrictionFromString(aProp->getStringValue("alt-restrict")); + _altitudeFt = aProp->getDoubleValue("altitude-ft"); + } + + if (aProp->hasChild("speed-restrict")) { + _speedRestrict = restrictionFromString(aProp->getStringValue("speed-restrict")); + _speedKts = aProp->getDoubleValue("speed-kts"); + } + + +} + +void Waypt::writeToProperties(SGPropertyNode_ptr aProp) const +{ + if (flag(WPT_OVERFLIGHT)) { + aProp->setBoolValue("overflight", true); + } + + if (flag(WPT_DEPARTURE)) { + aProp->setBoolValue("departure", true); + } + + if (flag(WPT_ARRIVAL)) { + aProp->setBoolValue("arrival", true); + } + + if (flag(WPT_MISS)) { + aProp->setBoolValue("miss", true); + } + + if (flag(WPT_GENERATED)) { + aProp->setBoolValue("generated", true); + } + + if (_altRestrict != RESTRICT_NONE) { + aProp->setStringValue("alt-restrict", restrictionToString(_altRestrict)); + aProp->setDoubleValue("altitude-ft", _altitudeFt); + } + + if (_speedRestrict != RESTRICT_NONE) { + aProp->setStringValue("speed-restrict", restrictionToString(_speedRestrict)); + aProp->setDoubleValue("speed-kts", _speedKts); + } +} + +void Route::dumpRouteToFile(const WayptVec& aRoute, const std::string& aName) +{ + SGPath p = "/Users/jmt/Desktop/" + aName + ".kml"; + std::fstream f; + f.open(p.str().c_str(), fstream::out | fstream::app); + if (!f.is_open()) { + SG_LOG(SG_GENERAL, SG_WARN, "unable to open:" << p.str()); + return; + } + +// pre-amble + f << "\n" + "\n" + "\n"; + + dumpRouteToLineString(aName, aRoute, f); + +// post-amble + f << "\n" + "" << endl; + f.close(); +} + +void Route::dumpRouteToLineString(const std::string& aIdent, + const WayptVec& aRoute, std::ostream& aStream) +{ + // preamble + aStream << "\n"; + aStream << "" << aIdent << "\n"; + aStream << "\n"; + aStream << "1\n"; + aStream << "\n"; + + // waypoints + for (unsigned int i=0; iposition(); + aStream << pos.getLongitudeDeg() << "," << pos.getLatitudeDeg() << " " << endl; + } + + // postable + aStream << "\n" + "\n" + "\n" << endl; +} + +/////////////////////////////////////////////////////////////////////////// + +class NavdataVisitor : public XMLVisitor { +public: + NavdataVisitor(FGAirport* aApt, const SGPath& aPath); + +protected: + virtual void startXML (); + virtual void endXML (); + virtual void startElement (const char * name, const XMLAttributes &atts); + virtual void endElement (const char * name); + virtual void data (const char * s, int len); + virtual void pi (const char * target, const char * data); + virtual void warning (const char * message, int line, int column); + virtual void error (const char * message, int line, int column); + +private: + Waypt* buildWaypoint(); + void processRunways(ArrivalDeparture* aProc, const XMLAttributes &atts); + + void finishApproach(); + void finishSid(); + void finishStar(); + + FGAirport* _airport; + SGPath _path; + string _text; ///< last element text value + + SID* _sid; + STAR* _star; + Approach* _approach; + + WayptVec _waypoints; ///< waypoint list for current approach/sid/star + WayptVec _transWaypts; ///< waypoint list for current transition + + string _wayptName; + string _wayptType; + string _ident; // id of segment under construction + string _transIdent; + double _longitude, _latitude, _altitude, _speed; + RouteRestriction _altRestrict; + + double _holdRadial; // inbound hold radial, or -1 if radial is 'inbound' + double _holdTD; ///< hold time (seconds) or distance (nm), based on flag below + bool _holdRighthanded; + bool _holdDistance; // true, TD is distance in nm; false, TD is time in seconds + + double _course, _radial, _dmeDistance; +}; + +void Route::loadAirportProcedures(const SGPath& aPath, FGAirport* aApt) +{ + assert(aApt); + try { + NavdataVisitor visitor(aApt, aPath); + readXML(aPath.str(), visitor); + } catch (sg_io_exception& ex) { + SG_LOG(SG_GENERAL, SG_WARN, "failured parsing procedures: " << aPath.str() << + "\n\t" << ex.getMessage() << "\n\tat:" << ex.getLocation().asString()); + } catch (sg_exception& ex) { + SG_LOG(SG_GENERAL, SG_WARN, "failured parsing procedures: " << aPath.str() << + "\n\t" << ex.getMessage()); + } +} + +NavdataVisitor::NavdataVisitor(FGAirport* aApt, const SGPath& aPath): + _airport(aApt), + _path(aPath), + _sid(NULL), + _star(NULL), + _approach(NULL) +{ +} + +void NavdataVisitor::startXML() +{ +} + +void NavdataVisitor::endXML() +{ +} + +void NavdataVisitor::startElement(const char* name, const XMLAttributes &atts) +{ + _text.clear(); + string tag(name); + if (tag == "Airport") { + string icao(atts.getValue("ICAOcode")); + if (_airport->ident() != icao) { + throw sg_format_exception("Airport and ICAO mismatch", icao, _path.str()); + } + } else if (tag == "Sid") { + string ident(atts.getValue("Name")); + _sid = new SID(ident); + _waypoints.clear(); + processRunways(_sid, atts); + } else if (tag == "Star") { + string ident(atts.getValue("Name")); + _star = new STAR(ident); + _waypoints.clear(); + processRunways(_star, atts); + } else if ((tag == "Sid_Waypoint") || + (tag == "App_Waypoint") || + (tag == "Star_Waypoint") || + (tag == "AppTr_Waypoint") || + (tag == "SidTr_Waypoint") || + (tag == "RwyTr_Waypoint")) + { + // reset waypoint data + _speed = 0.0; + _altRestrict = RESTRICT_NONE; + _altitude = 0.0; + } else if (tag == "Approach") { + _ident = atts.getValue("Name"); + _waypoints.clear(); + _approach = new Approach(_ident); + } else if ((tag == "Sid_Transition") || + (tag == "App_Transition") || + (tag == "Star_Transition")) { + _transIdent = atts.getValue("Name"); + _transWaypts.clear(); + } else if (tag == "RunwayTransition") { + _transIdent = atts.getValue("Runway"); + _transWaypts.clear(); + } else { + + } +} + +void NavdataVisitor::processRunways(ArrivalDeparture* aProc, const XMLAttributes &atts) +{ + string v("All"); + if (atts.hasAttribute("Runways")) { + v = atts.getValue("Runways"); + } + + if (v == "All") { + for (unsigned int r=0; r<_airport->numRunways(); ++r) { + aProc->addRunway(_airport->getRunwayByIndex(r)); + } + return; + } + + vector rwys; + boost::split(rwys, v, boost::is_any_of(" ,")); + for (unsigned int r=0; rgetRunwayByIdent(rwys[r]); + aProc->addRunway(rwy); + } +} + +void NavdataVisitor::endElement(const char* name) +{ + string tag(name); + if ((tag == "Sid_Waypoint") || + (tag == "App_Waypoint") || + (tag == "Star_Waypoint")) + { + _waypoints.push_back(buildWaypoint()); + } else if ((tag == "AppTr_Waypoint") || + (tag == "SidTr_Waypoint") || + (tag == "RwyTr_Waypoint") || + (tag == "StarTr_Waypoint")) + { + _transWaypts.push_back(buildWaypoint()); + } else if (tag == "Sid_Transition") { + assert(_sid); + // SID waypoints are stored backwards, to share code with STARs + std::reverse(_transWaypts.begin(), _transWaypts.end()); + Transition* t = new Transition(_transIdent, _sid, _transWaypts); + _sid->addTransition(t); + } else if (tag == "Star_Transition") { + assert(_star); + Transition* t = new Transition(_transIdent, _star, _transWaypts); + _star->addTransition(t); + } else if (tag == "App_Transition") { + assert(_approach); + Transition* t = new Transition(_transIdent, _approach, _transWaypts); + _approach->addTransition(t); + } else if (tag == "RunwayTransition") { + ArrivalDeparture* ad; + if (_sid) { + // SID waypoints are stored backwards, to share code with STARs + std::reverse(_transWaypts.begin(), _transWaypts.end()); + ad = _sid; + } else { + ad = _star; + } + + Transition* t = new Transition(_transIdent, ad, _transWaypts); + FGRunwayRef rwy = _airport->getRunwayByIdent(_transIdent); + ad->addRunwayTransition(rwy, t); + } else if (tag == "Approach") { + finishApproach(); + } else if (tag == "Sid") { + finishSid(); + } else if (tag == "Star") { + finishStar(); + } else if (tag == "Longitude") { + _longitude = atof(_text.c_str()); + } else if (tag == "Latitude") { + _latitude = atof(_text.c_str()); + } else if (tag == "Name") { + _wayptName = _text; + } else if (tag == "Type") { + _wayptType = _text; + } else if (tag == "Speed") { + _speed = atoi(_text.c_str()); + } else if (tag == "Altitude") { + _altitude = atof(_text.c_str()); + } else if (tag == "AltitudeRestriction") { + if (_text == "at") { + _altRestrict = RESTRICT_AT; + } else if (_text == "above") { + _altRestrict = RESTRICT_ABOVE; + } else if (_text == "below") { + _altRestrict = RESTRICT_BELOW; + } else { + throw sg_format_exception("Unrecognized altitude restriction", _text); + } + } else if (tag == "Hld_Rad_or_Inbd") { + if (_text == "Inbd") { + _holdRadial = -1.0; + } + } else if (tag == "Hld_Time_or_Dist") { + _holdDistance = (_text == "Dist"); + } else if (tag == "Hld_Rad_value") { + _holdRadial = atof(_text.c_str()); + } else if (tag == "Hld_Turn") { + _holdRighthanded = (_text == "Right"); + } else if (tag == "Hld_td_value") { + _holdTD = atof(_text.c_str()); + } else if (tag == "Hdg_Crs_value") { + _course = atof(_text.c_str()); + } else if (tag == "DMEtoIntercept") { + _dmeDistance = atof(_text.c_str()); + } else if (tag == "RadialtoIntercept") { + _radial = atof(_text.c_str()); + } else { + + } +} + +Waypt* NavdataVisitor::buildWaypoint() +{ + Waypt* wp = NULL; + if (_wayptType == "Normal") { + // new LatLonWaypoint + SGGeod pos(SGGeod::fromDeg(_longitude, _latitude)); + wp = new BasicWaypt(pos, _wayptName, NULL); + } else if (_wayptType == "Runway") { + string ident = _wayptName.substr(2); + FGRunwayRef rwy = _airport->getRunwayByIdent(ident); + wp = new RunwayWaypt(rwy, NULL); + } else if (_wayptType == "Hold") { + SGGeod pos(SGGeod::fromDeg(_longitude, _latitude)); + Hold* h = new Hold(pos, _wayptName, NULL); + wp = h; + if (_holdRighthanded) { + h->setRightHanded(); + } else { + h->setLeftHanded(); + } + + if (_holdDistance) { + h->setHoldDistance(_holdTD); + } else { + h->setHoldTime(_holdTD * 60.0); + } + + if (_holdRadial >= 0.0) { + h->setHoldRadial(_holdRadial); + } + } else if (_wayptType == "Vectors") { + wp = new ATCVectors(NULL, _airport); + } else if ((_wayptType == "Intc") || (_wayptType == "VorRadialIntc")) { + SGGeod pos(SGGeod::fromDeg(_longitude, _latitude)); + wp = new RadialIntercept(NULL, _wayptName, pos, _course, _radial); + } else if (_wayptType == "DmeIntc") { + SGGeod pos(SGGeod::fromDeg(_longitude, _latitude)); + wp = new DMEIntercept(NULL, _wayptName, pos, _course, _dmeDistance); + } else if (_wayptType == "ConstHdgtoAlt") { + wp = new HeadingToAltitude(NULL, _wayptName, _course); + } else { + SG_LOG(SG_GENERAL, SG_ALERT, "implement waypoint type:" << _wayptType); + throw sg_format_exception("Unrecognized waypt type", _wayptType); + } + + assert(wp); + if ((_altitude > 0.0) && (_altRestrict != RESTRICT_NONE)) { + wp->setAltitude(_altitude,_altRestrict); + } + + if (_speed > 0.0) { + wp->setSpeed(_speed, RESTRICT_AT); // or _BELOW? + } + + return wp; +} + +void NavdataVisitor::finishApproach() +{ + WayptVec::iterator it; + FGRunwayRef rwy; + +// find the runway node + for (it = _waypoints.begin(); it != _waypoints.end(); ++it) { + FGPositionedRef navid = (*it)->source(); + if (!navid) { + continue; + } + + if (navid->type() == FGPositioned::RUNWAY) { + rwy = (FGRunway*) navid.get(); + break; + } + } + + if (!rwy) { + throw sg_format_exception("Malformed approach, no runway waypt", _ident); + } + + WayptVec primary(_waypoints.begin(), it); + // erase all points up to and including the runway, to leave only the + // missed segments + _waypoints.erase(_waypoints.begin(), ++it); + + _approach->setRunway(rwy); + _approach->setPrimaryAndMissed(primary, _waypoints); + _airport->addApproach(_approach); + _approach = NULL; +} + +void NavdataVisitor::finishSid() +{ + // reverse order, because that's how we deal with commonality between + // STARs and SIDs. SID::route undoes this + std::reverse(_waypoints.begin(), _waypoints.end()); + _sid->setCommon(_waypoints); + _airport->addSID(_sid); + _sid = NULL; +} + +void NavdataVisitor::finishStar() +{ + _star->setCommon(_waypoints); + _airport->addSTAR(_star); + _star = NULL; +} + +void NavdataVisitor::data (const char * s, int len) +{ + _text += string(s, len); +} + + +void NavdataVisitor::pi (const char * target, const char * data) { + //cout << "Processing instruction " << target << ' ' << data << endl; +} + +void NavdataVisitor::warning (const char * message, int line, int column) { + SG_LOG(SG_IO, SG_WARN, "Warning: " << message << " (" << line << ',' << column << ')'); +} + +void NavdataVisitor::error (const char * message, int line, int column) { + SG_LOG(SG_IO, SG_ALERT, "Error: " << message << " (" << line << ',' << column << ')'); +} + +} // of namespace flightgear diff --git a/src/Navaids/route.hxx b/src/Navaids/route.hxx new file mode 100644 index 000000000..a23d728a7 --- /dev/null +++ b/src/Navaids/route.hxx @@ -0,0 +1,208 @@ +/** + * route.hxx - defines basic route and route-element classes. Route elements + * are specialised into waypoints and related things. Routes are any class tha + * owns a collection (list, tree, graph) of route elements - such as airways, + * procedures or a flight plan. + */ + +// Written by James Turner, started 2009. +// +// Copyright (C) 2009 Curtis L. Olson +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +#ifndef FG_ROUTE_HXX +#define FG_ROUTE_HXX + +// std +#include +#include +#include + +// Simgear +#include +#include +#include +#include + +// forward decls +class FGPositioned; +class SGPath; +class FGAirport; + +namespace flightgear +{ + +// forward decls +class Route; +class Waypt; +class NavdataVisitor; + +typedef SGSharedPtr WayptRef; + +typedef enum { + WPT_MAP = 1 << 0, ///< missed approach point + WPT_IAF = 1 << 1, ///< initial approach fix + WPT_FAF = 1 << 2, ///< final approach fix + WPT_OVERFLIGHT = 1 << 3, ///< must overfly the point directly + WPT_TRANSITION = 1 << 4, ///< transition to/from enroute structure + WPT_MISS = 1 << 5, ///< segment is part of missed approach + /// waypoint position is dynamic, i.e moves based on other criteria, + /// such as altitude, inbound course, or so on. + WPT_DYNAMIC = 1 << 6, + /// waypoint was created automatically (not manually entered/loaded) + /// for example waypoints from airway routing or a procedure + WPT_GENERATED = 1 << 7, + + WPT_DEPARTURE = 1 << 8, + WPT_ARRIVAL = 1 << 9 +} WayptFlag; + +typedef enum { + RESTRICT_NONE, + RESTRICT_AT, + RESTRICT_ABOVE, + RESTRICT_BELOW +} RouteRestriction; + +/** + * Abstract base class for waypoints (and things that are treated similarly + * by navigation systems) + */ +class Waypt : public SGReferenced +{ +public: + Route* owner() const + { return _owner; } + + /** + * Return true course (in degrees) and distance (in metres) from the provided + * position to this waypoint + */ + virtual std::pair courseAndDistanceFrom(const SGGeod& aPos) const; + + virtual SGGeod position() const = 0; + + /** + * The Positioned associated with this element, if one exists + */ + virtual FGPositioned* source() const + { return NULL; } + + virtual double altitudeFt() const + { return _altitudeFt; } + + virtual double speedKts() const + { return _speedKts; } + + virtual RouteRestriction altitudeRestriction() const + { return _altRestrict; } + + virtual RouteRestriction speedRestriction() const + { return _speedRestrict; } + + void setAltitude(double aAlt, RouteRestriction aRestrict); + void setSpeed(double aSpeed, RouteRestriction aRestrict); + + /** + * Identifier assoicated with the waypoint. Human-readable, but + * possibly quite terse, and definitiely not unique. + */ + virtual std::string ident() const; + + /** + * Test if the specified flag is set for this element + */ + virtual bool flag(WayptFlag aFlag) const; + + void setFlag(WayptFlag aFlag, bool aV = true); + + /** + * Factory method + */ + static WayptRef createFromProperties(Route* aOwner, SGPropertyNode_ptr aProp); + + void saveAsNode(SGPropertyNode* node) const; + + /** + * Test if this element and another are 'the same', i.e matching + * ident and lat/lon are approximately equal + */ + bool matches(Waypt* aOther) const; + + /** + * Test if this element and a position 'the same' + * this can be defined by either position, ident or both + */ + bool matches(const SGGeod& aPos) const; + + virtual std::string type() const = 0; +protected: + friend class NavdataVisitor; + + Waypt(Route* aOwner); + + /** + * Persistence helper - read node properties from a file + */ + virtual void initFromProperties(SGPropertyNode_ptr aProp); + + /** + * Persistence helper - save this element to a node + */ + virtual void writeToProperties(SGPropertyNode_ptr aProp) const; + + typedef Waypt* (FactoryFunction)(Route* aOwner) ; + static void registerFactory(const std::string aNodeType, FactoryFunction* aFactory); + + double _altitudeFt; + double _speedKts; + RouteRestriction _altRestrict; + RouteRestriction _speedRestrict; +private: + + /** + * Create an instance of a concrete subclass, or throw an exception + */ + static Waypt* createInstance(Route* aOwner, const std::string& aTypeName); + + Route* _owner; + unsigned short _flags; + +}; + +typedef std::vector WayptVec; + +class Route +{ +public: + /** + * + */ + virtual std::string ident() const = 0; + + static void loadAirportProcedures(const SGPath& aPath, FGAirport* aApt); + + static void dumpRouteToFile(const WayptVec& aRoute, const std::string& aName); + + static void dumpRouteToLineString(const std::string& aIdent, + const WayptVec& aRoute, std::ostream& aStream); +private: + +}; + +} // of namespace flightgear + +#endif // of FG_ROUTE_HXX diff --git a/src/Navaids/routePath.cxx b/src/Navaids/routePath.cxx new file mode 100644 index 000000000..b9ac48c1e --- /dev/null +++ b/src/Navaids/routePath.cxx @@ -0,0 +1,346 @@ +#include + +#include +#include +#include + +#include
+#include +#include +#include + +namespace flightgear { + bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result); +} + +using namespace flightgear; + +// implement Point(s) known distance from a great circle + +static double sqr(const double x) +{ + return x * x; +} + +double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const SGGeoc& d, double dist) +{ + double A = SGGeodesy::courseRad(a, d) - SGGeodesy::courseRad(a, b); + double bDist = SGGeodesy::distanceRad(a, d); + + // r=(cos(b)^2+sin(b)^2*cos(A)^2)^(1/2) + double r = pow(sqr(cos(bDist)) + sqr(sin(bDist)) * sqr(cos(A)), 0.5); + + double p = atan2(sin(bDist)*cos(A), cos(bDist)); + + if (sqr(cos(dist)) > sqr(r)) { + SG_LOG(SG_GENERAL, SG_INFO, "pointsKnownDistanceFromGC, no points exist"); + return -1.0; + } + + double dp1 = p + acos(cos(dist)/r); + double dp2 = p - acos(cos(dist)/r); + + double dp1Nm = fabs(dp1 * SG_RAD_TO_NM); + double dp2Nm = fabs(dp2 * SG_RAD_TO_NM); + + return SGMiscd::min(dp1Nm, dp2Nm); +} + +RoutePath::RoutePath(const flightgear::WayptVec& wpts) : + _waypts(wpts) +{ + _pathClimbFPM = 1200; + _pathDescentFPM = 800; + _pathIAS = 190; + _pathTurnRate = 3.0; // 3 deg/sec = 180def/min = standard rate turn +} + +SGGeodVec RoutePath::pathForIndex(int index) const +{ + if (index == 0) { + return SGGeodVec(); // no path for first waypoint + } + + if (_waypts[index]->type() == "vectors") { + return SGGeodVec(); // empty + } + + if (_waypts[index]->type() == "hold") { + return pathForHold((Hold*) _waypts[index].get()); + } + + SGGeodVec r; + SGGeod pos; + if (!computedPositionForIndex(index-1, pos)) { + return SGGeodVec(); + } + + r.push_back(pos); + if (!computedPositionForIndex(index, pos)) { + return SGGeodVec(); + } + + r.push_back(pos); + + if (_waypts[index]->type() == "runway") { + // runways get an extra point, at the end. this is particularly + // important so missed approach segments draw correctly + FGRunway* rwy = static_cast(_waypts[index].get())->runway(); + r.push_back(rwy->end()); + } + + return r; +} + +SGGeod RoutePath::positionForIndex(int index) const +{ + SGGeod r; + bool ok = computedPositionForIndex(index, r); + if (!ok) { + return SGGeod(); + } + + return r; +} + +SGGeodVec RoutePath::pathForHold(Hold* hold) const +{ + int turnSteps = 16; + double hdg = hold->inboundRadial(); + double turnDelta = 180.0 / turnSteps; + + SGGeodVec r; + double az2; + double stepTime = turnDelta / _pathTurnRate; // in seconds + double stepDist = _pathIAS * (stepTime / 3600.0) * SG_NM_TO_METER; + double legDist = hold->isDistance() ? + hold->timeOrDistance() + : _pathIAS * (hold->timeOrDistance() / 3600.0); + legDist *= SG_NM_TO_METER; + + if (hold->isLeftHanded()) { + turnDelta = -turnDelta; + } + SGGeod pos = hold->position(); + r.push_back(pos); + + // turn+leg sides are a mirror + for (int j=0; j < 2; ++j) { + // turn + for (int i=0;i= (int) _waypts.size())) { + throw sg_range_exception("waypt index out of range", + "RoutePath::computedPositionForIndex"); + } + + WayptRef w = _waypts[index]; + if (!w->flag(WPT_DYNAMIC)) { + r = w->position(); + return true; + } + + if (w->type() == "radialIntercept") { + // radial intersection along track + SGGeod prev; + if (!computedPositionForIndex(index - 1, prev)) { + return false; + } + + SGGeoc prevGc = SGGeoc::fromGeod(prev); + SGGeoc navid = SGGeoc::fromGeod(w->position()); + SGGeoc rGc; + double magVar = magVarFor(prev); + + RadialIntercept* i = (RadialIntercept*) w.get(); + double radial = i->radialDegMagnetic() + magVar; + double track = i->courseDegMagnetic() + magVar; + bool ok = geocRadialIntersection(prevGc, track, navid, radial, rGc); + if (!ok) { + return false; + } + + r = SGGeod::fromGeoc(rGc); + return true; + } else if (w->type() == "dmeIntercept") { + // find the point along the DME track, from prev, that is the correct distance + // from the DME + SGGeod prev; + if (!computedPositionForIndex(index - 1, prev)) { + return false; + } + + DMEIntercept* di = (DMEIntercept*) w.get(); + + SGGeoc prevGc = SGGeoc::fromGeod(prev); + SGGeoc navid = SGGeoc::fromGeod(w->position()); + double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD; + SGGeoc rGc; + + SGGeoc bPt; + double crs = di->courseDegMagnetic() + magVarFor(prev); + SGGeodesy::advanceRadM(prevGc, crs, 100 * SG_NM_TO_RAD, bPt); + + double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad); + if (dNm < 0.0) { + return false; + } + + double az2; + SGGeodesy::direct(prev, crs, dNm * SG_NM_TO_METER, r, az2); + return true; + } else if (w->type() == "hdgToAlt") { + HeadingToAltitude* h = (HeadingToAltitude*) w.get(); + double climb = h->altitudeFt() - computeAltitudeForIndex(index - 1); + double d = distanceForClimb(climb); + + SGGeod prevPos; + if (!computedPositionForIndex(index - 1, prevPos)) { + return false; + } + + double hdg = h->headingDegMagnetic() + magVarFor(prevPos); + + double az2; + SGGeodesy::direct(prevPos, hdg, d * SG_NM_TO_METER, r, az2); + return true; + } else if (w->type() == "vectors"){ + return false; + } else if (w->type() == "hold") { + r = w->position(); + return true; + } + + SG_LOG(SG_GENERAL, SG_INFO, "RoutePath::computedPositionForIndex: unhandled type:" << w->type()); + return false; +} + +double RoutePath::computeAltitudeForIndex(int index) const +{ + if ((index < 0) || (index >= (int) _waypts.size())) { + throw sg_range_exception("waypt index out of range", + "RoutePath::computeAltitudeForIndex"); + } + + WayptRef w = _waypts[index]; + if (w->altitudeRestriction() != RESTRICT_NONE) { + return w->altitudeFt(); // easy! + } + + if (w->type() == "runway") { + FGRunway* rwy = static_cast(w.get())->runway(); + return rwy->threshold().getElevationFt(); + } else if ((w->type() == "hold") || (w->type() == "vectors")) { + // pretend we don't change altitude in holds/vectoring + return computeAltitudeForIndex(index - 1); + } + + double prevAlt = computeAltitudeForIndex(index - 1); +// find distance to previous, and hence climb/descent + SGGeod pos, prevPos; + + if (!computedPositionForIndex(index, pos) || + !computedPositionForIndex(index - 1, prevPos)) + { + SG_LOG(SG_GENERAL, SG_WARN, "unable to compute position for waypoints"); + throw sg_range_exception("unable to compute position for waypoints"); + } + + double d = SGGeodesy::distanceNm(prevPos, pos); + double tMinutes = (d / _pathIAS) * 60.0; // (nm / knots) * 60 = time in minutes + + double deltaFt; // change in altitude in feet + if (w->flag(WPT_ARRIVAL) && !w->flag(WPT_MISS)) { + deltaFt = -_pathDescentFPM * tMinutes; + } else { + deltaFt = _pathClimbFPM * tMinutes; + } + + return prevAlt + deltaFt; +} + +double RoutePath::computeTrackForIndex(int index) const +{ + if ((index < 0) || (index >= (int) _waypts.size())) { + throw sg_range_exception("waypt index out of range", + "RoutePath::computeTrackForIndex"); + } + + WayptRef w = _waypts[index]; + if (w->type() == "radialIntercept") { + RadialIntercept* r = (RadialIntercept*) w.get(); + return r->courseDegMagnetic(); + } else if (w->type() == "dmeIntercept") { + DMEIntercept* d = (DMEIntercept*) w.get(); + return d->courseDegMagnetic(); + } else if (w->type() == "hdgToAlt") { + HeadingToAltitude* h = (HeadingToAltitude*) w.get(); + return h->headingDegMagnetic(); + } else if (w->type() == "hold") { + Hold* h = (Hold*) w.get(); + return h->inboundRadial(); + } else if (w->type() == "vectors") { + SG_LOG(SG_GENERAL, SG_WARN, "asked for track from VECTORS"); + throw sg_range_exception("asked for track from vectors waypt"); + } else if (w->type() == "runway") { + FGRunway* rwy = static_cast(w.get())->runway(); + return rwy->headingDeg(); + } + +// course based upon previous and current pos + SGGeod pos, prevPos; + + if (!computedPositionForIndex(index, pos) || + !computedPositionForIndex(index - 1, prevPos)) + { + SG_LOG(SG_GENERAL, SG_WARN, "unable to compute position for waypoints"); + throw sg_range_exception("unable to compute position for waypoints"); + } + + return SGGeodesy::courseDeg(prevPos, pos); +} + +double RoutePath::distanceForClimb(double climbFt) const +{ + double t = 0.0; // in seconds + if (climbFt > 0.0) { + t = (climbFt / _pathClimbFPM) * 60.0; + } else if (climbFt < 0.0) { + t = (climbFt / _pathDescentFPM) * 60.0; + } + + return _pathIAS * (t / 3600.0); +} + +double RoutePath::magVarFor(const SGGeod& geod) const +{ + double jd = globals->get_time_params()->getJD(); + return sgGetMagVar(geod, jd); +} + diff --git a/src/Navaids/routePath.hxx b/src/Navaids/routePath.hxx new file mode 100644 index 000000000..d04c03d08 --- /dev/null +++ b/src/Navaids/routePath.hxx @@ -0,0 +1,69 @@ +/** + * routePath.hxx - convert a route to straight line segments, for graphical + * output or display. + */ + +// Written by James Turner, started 2010. +// +// Copyright (C) 2010 Curtis L. Olson +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +#ifndef FG_ROUTE_PATH_HXX +#define FG_ROUTE_PATH_HXX + +#include + +namespace flightgear +{ + class Hold; +} + +typedef std::vector SGGeodVec; + +class RoutePath +{ +public: + RoutePath(const flightgear::WayptVec& wpts); + + SGGeodVec pathForIndex(int index) const; + + SGGeod positionForIndex(int index) const; + +private: + class PathCtx; + + SGGeodVec pathForHold(flightgear::Hold* hold) const; + + bool computedPositionForIndex(int index, SGGeod& pos) const; + double computeAltitudeForIndex(int index) const; + double computeTrackForIndex(int index) const; + + /** + * Find the distance (in Nm) to climb/descend a height in feet + */ + double distanceForClimb(double climbFt) const; + + double magVarFor(const SGGeod& gd) const; + + flightgear::WayptVec _waypts; + + int _pathClimbFPM; ///< climb-rate to use for pathing + int _pathDescentFPM; ///< descent rate to use (feet-per-minute) + int _pathIAS; ///< IAS (knots) to use for pathing + double _pathTurnRate; ///< degrees-per-second, defaults to 3, i.e 180 in a minute +}; + +#endif diff --git a/src/Navaids/testnavs.cxx b/src/Navaids/testnavs.cxx index b7215e8c7..d38341aa7 100644 --- a/src/Navaids/testnavs.cxx +++ b/src/Navaids/testnavs.cxx @@ -1,3 +1,21 @@ +// Written by James Turner, started 2009. +// +// Copyright (C) 2009 Curtis L. Olson +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + #include #include "fixlist.hxx" diff --git a/src/Navaids/waypoint.cxx b/src/Navaids/waypoint.cxx new file mode 100644 index 000000000..5992774b1 --- /dev/null +++ b/src/Navaids/waypoint.cxx @@ -0,0 +1,471 @@ +// waypoint.cxx - waypoints that can occur in routes/procedures +// Written by James Turner, started 2009. +// +// Copyright (C) 2009 Curtis L. Olson +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +#ifdef HAVE_CONFIG_H +# include "config.h" +#endif + +#include "waypoint.hxx" + +#include +#include + +#include +#include + +using std::string; + +namespace flightgear +{ + +BasicWaypt::BasicWaypt(const SGGeod& aPos, const string& aIdent, Route* aOwner) : + Waypt(aOwner), + _pos(aPos), + _ident(aIdent) +{ +} + +BasicWaypt::BasicWaypt(const SGWayPoint& aWP, Route* aOwner) : + Waypt(aOwner), + _pos(aWP.get_target()), + _ident(aWP.get_id()) +{ +} + +BasicWaypt::BasicWaypt(Route* aOwner) : + Waypt(aOwner) +{ +} + +void BasicWaypt::initFromProperties(SGPropertyNode_ptr aProp) +{ + if (!aProp->hasChild("lon") || !aProp->hasChild("lat")) { + throw sg_io_exception("missing lon/lat properties", + "BasicWaypt::initFromProperties"); + } + + Waypt::initFromProperties(aProp); + + _pos = SGGeod::fromDeg(aProp->getDoubleValue("lon"), + aProp->getDoubleValue("lat")); + _ident = aProp->getStringValue("ident"); +} + +void BasicWaypt::writeToProperties(SGPropertyNode_ptr aProp) const +{ + Waypt::writeToProperties(aProp); + + aProp->setStringValue("ident", _ident); + aProp->setDoubleValue("lon", _pos.getLongitudeDeg()); + aProp->setDoubleValue("lat", _pos.getLatitudeDeg()); +} + +////////////////////////////////////////////////////////////////////////////// + +NavaidWaypoint::NavaidWaypoint(FGPositioned* aPos, Route* aOwner) : + Waypt(aOwner), + _navaid(aPos) +{ + if (aPos->type() == FGPositioned::RUNWAY) { + SG_LOG(SG_GENERAL, SG_WARN, "sure you don't want to be building a runway waypt here?"); + } +} + +NavaidWaypoint::NavaidWaypoint(Route* aOwner) : + Waypt(aOwner) +{ +} + + +SGGeod NavaidWaypoint::position() const +{ + return SGGeod::fromGeodFt(_navaid->geod(), _altitudeFt); +} + +std::string NavaidWaypoint::ident() const +{ + return _navaid->ident(); +} + +void NavaidWaypoint::initFromProperties(SGPropertyNode_ptr aProp) +{ + if (!aProp->hasChild("ident")) { + throw sg_io_exception("missing navaid value", + "NavaidWaypoint::initFromProperties"); + } + + Waypt::initFromProperties(aProp); + + std::string idn(aProp->getStringValue("ident")); + SGGeod p; + if (aProp->hasChild("lon")) { + p = SGGeod::fromDeg(aProp->getDoubleValue("lon"), aProp->getDoubleValue("lat")); + } + + // FIXME - resolve co-located DME, etc + // is it sufficent just to ignore DMEs, actually? + FGPositionedRef nav = FGPositioned::findClosestWithIdent(idn, p, NULL); + if (!nav) { + throw sg_io_exception("unknown navaid ident:" + idn, + "NavaidWaypoint::initFromProperties"); + } + + _navaid = nav; +} + +void NavaidWaypoint::writeToProperties(SGPropertyNode_ptr aProp) const +{ + Waypt::writeToProperties(aProp); + + aProp->setStringValue("ident", _navaid->ident()); + // write lon/lat to disambiguate + aProp->setDoubleValue("lon", _navaid->geod().getLongitudeDeg()); + aProp->setDoubleValue("lat", _navaid->geod().getLatitudeDeg()); +} + +OffsetNavaidWaypoint::OffsetNavaidWaypoint(FGPositioned* aPos, Route* aOwner, + double aRadial, double aDistNm) : + NavaidWaypoint(aPos, aOwner), + _radial(aRadial), + _distanceNm(aDistNm) +{ + init(); +} + +OffsetNavaidWaypoint::OffsetNavaidWaypoint(Route* aOwner) : + NavaidWaypoint(aOwner) +{ +} + +void OffsetNavaidWaypoint::init() +{ + SGGeod offset; + double az2; + SGGeodesy::direct(_navaid->geod(), _radial, _distanceNm * SG_NM_TO_METER, offset, az2); + _geod = SGGeod::fromGeodFt(offset, _altitudeFt); +} + +void OffsetNavaidWaypoint::initFromProperties(SGPropertyNode_ptr aProp) +{ + if (!aProp->hasChild("radial-deg") || !aProp->hasChild("distance-nm")) { + throw sg_io_exception("missing radial/offset distance", + "OffsetNavaidWaypoint::initFromProperties"); + } + + NavaidWaypoint::initFromProperties(aProp); + _radial = aProp->getDoubleValue("radial-deg"); + _distanceNm = aProp->getDoubleValue("distance-nm"); + init(); +} + +void OffsetNavaidWaypoint::writeToProperties(SGPropertyNode_ptr aProp) const +{ + NavaidWaypoint::writeToProperties(aProp); + aProp->setDoubleValue("radial-deg", _radial); + aProp->setDoubleValue("distance-nm", _distanceNm); +} + +///////////////////////////////////////////////////////////////////////////// + +RunwayWaypt::RunwayWaypt(FGRunway* aPos, Route* aOwner) : + Waypt(aOwner), + _runway(aPos) +{ +} + +RunwayWaypt::RunwayWaypt(Route* aOwner) : + Waypt(aOwner) +{ +} + +SGGeod RunwayWaypt::position() const +{ + return _runway->threshold(); +} + +std::string RunwayWaypt::ident() const +{ + return _runway->airport()->ident() + "-" + _runway->ident(); +} + +FGPositioned* RunwayWaypt::source() const +{ + return _runway; +} + +void RunwayWaypt::initFromProperties(SGPropertyNode_ptr aProp) +{ + if (!aProp->hasChild("icao") || !aProp->hasChild("ident")) { + throw sg_io_exception("missing values: icao or ident", + "RunwayWaypoint::initFromProperties"); + } + + Waypt::initFromProperties(aProp); + std::string idn(aProp->getStringValue("ident")); + const FGAirport* apt = FGAirport::getByIdent(aProp->getStringValue("icao")); + _runway = apt->getRunwayByIdent(aProp->getStringValue("ident")); +} + +void RunwayWaypt::writeToProperties(SGPropertyNode_ptr aProp) const +{ + Waypt::writeToProperties(aProp); + aProp->setStringValue("ident", _runway->ident()); + aProp->setStringValue("icao", _runway->airport()->ident()); +} + +///////////////////////////////////////////////////////////////////////////// + +Hold::Hold(const SGGeod& aPos, const string& aIdent, Route* aOwner) : + BasicWaypt(aPos, aIdent, aOwner), + _righthanded(true), + _isDistance(false) +{ + setFlag(WPT_DYNAMIC); +} + +Hold::Hold(Route* aOwner) : + BasicWaypt(aOwner), + _righthanded(true), + _isDistance(false) +{ +} + +void Hold::setHoldRadial(double aInboundRadial) +{ + _bearing = aInboundRadial; +} + +void Hold::setHoldDistance(double aDistanceNm) +{ + _isDistance = true; + _holdTD = aDistanceNm; +} + +void Hold::setHoldTime(double aTimeSec) +{ + _isDistance = false; + _holdTD = aTimeSec; +} + +void Hold::setRightHanded() +{ + _righthanded = true; +} + +void Hold::setLeftHanded() +{ + _righthanded = false; +} + +void Hold::initFromProperties(SGPropertyNode_ptr aProp) +{ + BasicWaypt::initFromProperties(aProp); + + if (!aProp->hasChild("lon") || !aProp->hasChild("lat")) { + throw sg_io_exception("missing lon/lat properties", + "Hold::initFromProperties"); + } + + _righthanded = aProp->getBoolValue("right-handed"); + _isDistance = aProp->getBoolValue("is-distance"); + _bearing = aProp->getDoubleValue("inbound-radial-deg"); + _holdTD = aProp->getDoubleValue("td"); +} + +void Hold::writeToProperties(SGPropertyNode_ptr aProp) const +{ + BasicWaypt::writeToProperties(aProp); + + aProp->setBoolValue("right-handed", _righthanded); + aProp->setBoolValue("is-distance", _isDistance); + aProp->setDoubleValue("inbound-radial-deg", _bearing); + aProp->setDoubleValue("td", _holdTD); +} + +///////////////////////////////////////////////////////////////////////////// + +HeadingToAltitude::HeadingToAltitude(Route* aOwner, const string& aIdent, + double aMagHdg) : + Waypt(aOwner), + _ident(aIdent), + _magHeading(aMagHdg) +{ + setFlag(WPT_DYNAMIC); +} + +HeadingToAltitude::HeadingToAltitude(Route* aOwner) : + Waypt(aOwner) +{ +} + +void HeadingToAltitude::initFromProperties(SGPropertyNode_ptr aProp) +{ + if (!aProp->hasChild("heading-deg")) { + throw sg_io_exception("missing heading/alt properties", + "HeadingToAltitude::initFromProperties"); + } + + Waypt::initFromProperties(aProp); + _magHeading = aProp->getDoubleValue("heading-deg"); + _ident = aProp->getStringValue("ident"); +} + +void HeadingToAltitude::writeToProperties(SGPropertyNode_ptr aProp) const +{ + Waypt::writeToProperties(aProp); + aProp->setStringValue("ident", _ident); + aProp->setDoubleValue("heading-deg", _magHeading); +} + +///////////////////////////////////////////////////////////////////////////// + +DMEIntercept::DMEIntercept(Route* aOwner, const string& aIdent, const SGGeod& aPos, + double aCourseDeg, double aDistanceNm) : + Waypt(aOwner), + _ident(aIdent), + _pos(aPos), + _magCourse(aCourseDeg), + _dmeDistanceNm(aDistanceNm) +{ + setFlag(WPT_DYNAMIC); +} + +DMEIntercept::DMEIntercept(Route* aOwner) : + Waypt(aOwner) +{ +} + +void DMEIntercept::initFromProperties(SGPropertyNode_ptr aProp) +{ + if (!aProp->hasChild("lon") || !aProp->hasChild("lat")) { + throw sg_io_exception("missing lon/lat properties", + "DMEIntercept::initFromProperties"); + } + + Waypt::initFromProperties(aProp); + _pos = SGGeod::fromDeg(aProp->getDoubleValue("lon"), aProp->getDoubleValue("lat")); + _ident = aProp->getStringValue("ident"); +// check it's a real DME? + _magCourse = aProp->getDoubleValue("course-deg"); + _dmeDistanceNm = aProp->getDoubleValue("dme-distance-nm"); + +} + +void DMEIntercept::writeToProperties(SGPropertyNode_ptr aProp) const +{ + Waypt::writeToProperties(aProp); + + aProp->setStringValue("ident", _ident); + aProp->setDoubleValue("lon", _pos.getLongitudeDeg()); + aProp->setDoubleValue("lat", _pos.getLatitudeDeg()); + aProp->setDoubleValue("course-deg", _magCourse); + aProp->setDoubleValue("dme-distance-nm", _dmeDistanceNm); +} + +///////////////////////////////////////////////////////////////////////////// + +RadialIntercept::RadialIntercept(Route* aOwner, const string& aIdent, const SGGeod& aPos, + double aCourseDeg, double aRadial) : + Waypt(aOwner), + _ident(aIdent), + _pos(aPos), + _magCourse(aCourseDeg), + _radial(aRadial) +{ + setFlag(WPT_DYNAMIC); +} + +RadialIntercept::RadialIntercept(Route* aOwner) : + Waypt(aOwner) +{ +} + +void RadialIntercept::initFromProperties(SGPropertyNode_ptr aProp) +{ + if (!aProp->hasChild("lon") || !aProp->hasChild("lat")) { + throw sg_io_exception("missing lon/lat properties", + "RadialIntercept::initFromProperties"); + } + + Waypt::initFromProperties(aProp); + _pos = SGGeod::fromDeg(aProp->getDoubleValue("lon"), aProp->getDoubleValue("lat")); + _ident = aProp->getStringValue("ident"); +// check it's a real VOR? + _magCourse = aProp->getDoubleValue("course-deg"); + _radial = aProp->getDoubleValue("radial-deg"); + +} + +void RadialIntercept::writeToProperties(SGPropertyNode_ptr aProp) const +{ + Waypt::writeToProperties(aProp); + + aProp->setStringValue("ident", _ident); + aProp->setDoubleValue("lon", _pos.getLongitudeDeg()); + aProp->setDoubleValue("lat", _pos.getLatitudeDeg()); + aProp->setDoubleValue("course-deg", _magCourse); + aProp->setDoubleValue("radial-deg", _radial); +} + +///////////////////////////////////////////////////////////////////////////// + +ATCVectors::ATCVectors(Route* aOwner, FGAirport* aFacility) : + Waypt(aOwner), + _facility(aFacility) +{ + setFlag(WPT_DYNAMIC); +} + +ATCVectors::~ATCVectors() +{ +} + +ATCVectors::ATCVectors(Route* aOwner) : + Waypt(aOwner) +{ +} + +SGGeod ATCVectors::position() const +{ + return _facility->geod(); +} + +string ATCVectors::ident() const +{ + return "VECTORS-" + _facility->ident(); +} + +void ATCVectors::initFromProperties(SGPropertyNode_ptr aProp) +{ + if (!aProp->hasChild("icao")) { + throw sg_io_exception("missing icao propertie", + "ATCVectors::initFromProperties"); + } + + Waypt::initFromProperties(aProp); + _facility = FGAirport::getByIdent(aProp->getStringValue("icao")); +} + +void ATCVectors::writeToProperties(SGPropertyNode_ptr aProp) const +{ + Waypt::writeToProperties(aProp); + aProp->setStringValue("icao", _facility->ident()); +} + +} // of namespace diff --git a/src/Navaids/waypoint.hxx b/src/Navaids/waypoint.hxx new file mode 100644 index 000000000..7b5f30f09 --- /dev/null +++ b/src/Navaids/waypoint.hxx @@ -0,0 +1,314 @@ +// waypoint.hxx - waypoints that can occur in routes/procedures +// Written by James Turner, started 2009. +// +// Copyright (C) 2009 Curtis L. Olson +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +#ifndef FG_WAYPOINT_HXX +#define FG_WAYPOINT_HXX + +#include +#include + +class FGAirport; +typedef SGSharedPtr FGAirportRef; +class SGWayPoint; +class FGRunway; + +namespace flightgear +{ + + +class BasicWaypt : public Waypt +{ +public: + + BasicWaypt(const SGGeod& aPos, const std::string& aIdent, Route* aOwner); + + BasicWaypt(const SGWayPoint& aWP, Route* aOwner); + + BasicWaypt(Route* aOwner); + + virtual SGGeod position() const + { return _pos; } + + virtual std::string ident() const + { return _ident; } + +protected: + virtual void initFromProperties(SGPropertyNode_ptr aProp); + virtual void writeToProperties(SGPropertyNode_ptr aProp) const; + + virtual std::string type() const + { return "basic"; } + + SGGeod _pos; + std::string _ident; + +}; + +/** + * Waypoint based upon a navaid. In practice this means any Positioned + * element, excluding runways (see below) + */ +class NavaidWaypoint : public Waypt +{ +public: + NavaidWaypoint(FGPositioned* aPos, Route* aOwner); + + NavaidWaypoint(Route* aOwner); + + virtual SGGeod position() const; + + virtual FGPositioned* source() const + { return _navaid; } + + virtual std::string ident() const; +protected: + virtual void initFromProperties(SGPropertyNode_ptr aProp); + virtual void writeToProperties(SGPropertyNode_ptr aProp) const; + + virtual std::string type() const + { return "navaid"; } + + FGPositionedRef _navaid; +}; + +class OffsetNavaidWaypoint : public NavaidWaypoint +{ +public: + OffsetNavaidWaypoint(FGPositioned* aPos, Route* aOwner, double aRadial, double aDistNm); + + OffsetNavaidWaypoint(Route* aOwner); + + virtual SGGeod position() const + { return _geod; } + +protected: + virtual void initFromProperties(SGPropertyNode_ptr aProp); + virtual void writeToProperties(SGPropertyNode_ptr aProp) const; + + virtual std::string type() const + { return "offset-navaid"; } + +private: + void init(); + + SGGeod _geod; + double _radial; // true, degrees + double _distanceNm; +}; + +/** + * Waypoint based upon a runway. + * Runways are handled specially in various places, so it's cleaner + * to be able to distuinguish them from other navaid waypoints + */ +class RunwayWaypt : public Waypt +{ +public: + RunwayWaypt(FGRunway* aPos, Route* aOwner); + + RunwayWaypt(Route* aOwner); + + virtual SGGeod position() const; + + virtual FGPositioned* source() const; + + virtual std::string ident() const; + + FGRunway* runway() const + { return _runway; } + +protected: + virtual std::string type() const + { return "runway"; } + + virtual void initFromProperties(SGPropertyNode_ptr aProp); + virtual void writeToProperties(SGPropertyNode_ptr aProp) const; + +private: + FGRunway* _runway; +}; + +class Hold : public BasicWaypt +{ +public: + Hold(const SGGeod& aPos, const std::string& aIdent, Route* aOwner); + + Hold(Route* aOwner); + + void setHoldRadial(double aInboundRadial); + void setHoldDistance(double aDistanceNm); + void setHoldTime(double aTimeSec); + + void setRightHanded(); + void setLeftHanded(); + + double inboundRadial() const + { return _bearing; } + + bool isLeftHanded() const + { return !_righthanded; } + + bool isDistance() const + { return _isDistance; } + + double timeOrDistance() const + { return _holdTD;} + +protected: + virtual void initFromProperties(SGPropertyNode_ptr aProp); + virtual void writeToProperties(SGPropertyNode_ptr aProp) const; + + virtual std::string type() const + { return "hold"; } + +private: + double _bearing; + bool _righthanded; + bool _isDistance; + double _holdTD; +}; + +class HeadingToAltitude : public Waypt +{ +public: + HeadingToAltitude(Route* aOwner, const std::string& aIdent, double aMagHdg); + + HeadingToAltitude(Route* aOwner); + + virtual void initFromProperties(SGPropertyNode_ptr aProp); + virtual void writeToProperties(SGPropertyNode_ptr aProp) const; + + virtual std::string type() const + { return "hdgToAlt"; } + + virtual SGGeod position() const + { return SGGeod(); } + + virtual std::string ident() const + { return _ident; } + + double headingDegMagnetic() const + { return _magHeading; } + +private: + std::string _ident; + double _magHeading; +}; + +class DMEIntercept : public Waypt +{ +public: + DMEIntercept(Route* aOwner, const std::string& aIdent, const SGGeod& aPos, + double aCourseDeg, double aDistanceNm); + + DMEIntercept(Route* aOwner); + + virtual void initFromProperties(SGPropertyNode_ptr aProp); + virtual void writeToProperties(SGPropertyNode_ptr aProp) const; + + virtual std::string type() const + { return "dmeIntercept"; } + + virtual SGGeod position() const + { return _pos; } + + virtual std::string ident() const + { return _ident; } + + double courseDegMagnetic() const + { return _magCourse; } + + double dmeDistanceNm() const + { return _dmeDistanceNm; } + +private: + std::string _ident; + SGGeod _pos; + double _magCourse; + double _dmeDistanceNm; +}; + +class RadialIntercept : public Waypt +{ +public: + RadialIntercept(Route* aOwner, const std::string& aIdent, const SGGeod& aPos, + double aCourseDeg, double aRadialDeg); + + RadialIntercept(Route* aOwner); + + virtual void initFromProperties(SGPropertyNode_ptr aProp); + virtual void writeToProperties(SGPropertyNode_ptr aProp) const; + + virtual std::string type() const + { return "radialIntercept"; } + + virtual SGGeod position() const + { return _pos; } + + virtual std::string ident() const + { return _ident; } + + double courseDegMagnetic() const + { return _magCourse; } + + double radialDegMagnetic() const + { return _radial; } + +private: + std::string _ident; + SGGeod _pos; + double _magCourse; + double _radial; +}; + + +/** + * Represent ATC radar vectored segment. Common at the end of published + * missed approach procedures, and from STAR arrival points to final approach + */ +class ATCVectors : public Waypt +{ +public: + ATCVectors(Route* aOwner, FGAirport* aFacility); + virtual ~ATCVectors(); + + ATCVectors(Route* aOwner); + + virtual void initFromProperties(SGPropertyNode_ptr aProp); + virtual void writeToProperties(SGPropertyNode_ptr aProp) const; + + virtual std::string type() const + { return "vectors"; } + + virtual SGGeod position() const; + + virtual std::string ident() const; + +private: + /** + * ATC facility. Using an airport here is incorrect, since often arrivals + * facilities will be shared between several nearby airports, but it + * suffices until we have a proper facility representation + */ + FGAirportRef _facility; +}; + +} // of namespace flighgear + +#endif // of FG_WAYPOINT_HXX diff --git a/src/Scripting/NasalSys.cxx b/src/Scripting/NasalSys.cxx index ae5d9c0af..17fdb67d5 100644 --- a/src/Scripting/NasalSys.cxx +++ b/src/Scripting/NasalSys.cxx @@ -33,6 +33,7 @@ #include
#include #include +#include #include "NasalSys.hxx" @@ -613,6 +614,28 @@ static naRef f_airportinfo(naContext c, naRef me, int argc, naRef* args) HASHSET("ils_frequency_mhz", 17, naNum(rwy->ILS()->get_freq() / 100.0)); } + std::vector sids(rwy->getSIDs()); + naRef sidVec = naNewVector(c); + + for (unsigned int s=0; s < sids.size(); ++s) { + naRef procId = naStr_fromdata(naNewString(c), + const_cast(sids[s]->ident().c_str()), + sids[s]->ident().length()); + naVec_append(sidVec, procId); + } + HASHSET("sids", 4, sidVec); + + std::vector stars(rwy->getSTARs()); + naRef starVec = naNewVector(c); + + for (unsigned int s=0; s < stars.size(); ++s) { + naRef procId = naStr_fromdata(naNewString(c), + const_cast(stars[s]->ident().c_str()), + stars[s]->ident().length()); + naVec_append(starVec, procId); + } + HASHSET("stars", 5, starVec); + #undef HASHSET naHash_set(rwys, rwyid, rwydata); } From 3fe6209c2017474db28786702215c7404c8a3ebd Mon Sep 17 00:00:00 2001 From: James Turner Date: Wed, 20 Oct 2010 09:09:29 +0100 Subject: [PATCH 3/6] VS2008 project updates. --- projects/VC90/FlightGear/FlightGear.vcproj | 48 ++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/projects/VC90/FlightGear/FlightGear.vcproj b/projects/VC90/FlightGear/FlightGear.vcproj index d8011106c..e97cba54f 100644 --- a/projects/VC90/FlightGear/FlightGear.vcproj +++ b/projects/VC90/FlightGear/FlightGear.vcproj @@ -4203,6 +4203,46 @@ RelativePath="..\..\..\src\Navaids\positioned.hxx" > + + + + + + + + + + + + + + + + + + + + + + + + From 308fd1b11216f5d7e4bba350a7d798a33f06e70a Mon Sep 17 00:00:00 2001 From: James Turner Date: Wed, 20 Oct 2010 11:48:06 +0100 Subject: [PATCH 4/6] Automake build fixes for airways commit. --- src/Instrumentation/Makefile.am | 6 +++--- src/Instrumentation/gps.hxx | 1 + src/Instrumentation/rnav_waypt_controller.cxx | 2 ++ src/Navaids/Makefile.am | 1 + src/Navaids/procedure.cxx | 3 +++ 5 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/Instrumentation/Makefile.am b/src/Instrumentation/Makefile.am index 136929dc7..f519f99a0 100644 --- a/src/Instrumentation/Makefile.am +++ b/src/Instrumentation/Makefile.am @@ -31,8 +31,8 @@ libInstrumentation_a_SOURCES = \ tacan.cxx tacan.hxx mk_viii.cxx mk_viii.hxx \ dclgps.cxx dclgps.hxx \ render_area_2d.cxx render_area_2d.hxx \ - groundradar.cxx groundradar.hxx \ - agradar.cxx agradar.hxx rad_alt.cxx rad_alt.hxx \ - rnav_waypt_controller.cxx rnav_waypt_controller.hxx \ + groundradar.cxx groundradar.hxx \ + agradar.cxx agradar.hxx rad_alt.cxx rad_alt.hxx \ + rnav_waypt_controller.cxx rnav_waypt_controller.hxx INCLUDES = -I$(top_srcdir) -I$(top_srcdir)/src -I$(top_builddir)/src diff --git a/src/Instrumentation/gps.hxx b/src/Instrumentation/gps.hxx index 3005997c0..f69a70a76 100644 --- a/src/Instrumentation/gps.hxx +++ b/src/Instrumentation/gps.hxx @@ -8,6 +8,7 @@ #define __INSTRUMENTS_GPS_HXX 1 #include +#include #include #include diff --git a/src/Instrumentation/rnav_waypt_controller.cxx b/src/Instrumentation/rnav_waypt_controller.cxx index fbdb5eeb1..977452e18 100644 --- a/src/Instrumentation/rnav_waypt_controller.cxx +++ b/src/Instrumentation/rnav_waypt_controller.cxx @@ -19,6 +19,8 @@ #include "rnav_waypt_controller.hxx" +#include + #include #include diff --git a/src/Navaids/Makefile.am b/src/Navaids/Makefile.am index 66c650045..30016afe3 100644 --- a/src/Navaids/Makefile.am +++ b/src/Navaids/Makefile.am @@ -14,6 +14,7 @@ libNavaids_a_SOURCES = \ route.hxx route.cxx \ waypoint.hxx waypoint.cxx \ procedure.hxx procedure.cxx \ + awynet.cxx awynet.hxx # # testnavs_SOURCES = testnavs.cxx diff --git a/src/Navaids/procedure.cxx b/src/Navaids/procedure.cxx index 4080f0a75..5104f4c75 100644 --- a/src/Navaids/procedure.cxx +++ b/src/Navaids/procedure.cxx @@ -19,6 +19,9 @@ #include "procedure.hxx" +#include +#include // for reverse_copy + #include #include From fe167327831067005b275593ba388ea676231025 Mon Sep 17 00:00:00 2001 From: James Turner Date: Wed, 20 Oct 2010 13:13:05 +0100 Subject: [PATCH 5/6] Fix Win32 compilation - avoid SID name clash with windows.h --- src/Airports/simple.cxx | 10 +++++----- src/Autopilot/route_mgr.cxx | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/Airports/simple.cxx b/src/Airports/simple.cxx index f563cf1d0..c7e486f45 100644 --- a/src/Airports/simple.cxx +++ b/src/Airports/simple.cxx @@ -511,13 +511,13 @@ bool FGAirport::buildApproach(Waypt* aEnroute, STAR* aSTAR, FGRunway* aRwy, Wayp return aps.front()->routeFromVectors(aRoute); } -pair +pair FGAirport::selectSID(const SGGeod& aDest, FGRunway* aRwy) { loadProcedures(); WayptRef enroute; - SID* sid = NULL; + flightgear::SID* sid = NULL; double d = 1e9; for (unsigned int i=0; iselectSID(_destination->geod(), runway); @@ -569,7 +569,7 @@ void FGRouteMgr::buildDeparture(WayptRef enroute, WayptVec& wps) FGRunway* r = _departure->getRunwayByIdent(runwayId); string sidId = departure->getStringValue("sid"); - SID* sid = _departure->findSIDWithIdent(sidId); + flightgear::SID* sid = _departure->findSIDWithIdent(sidId); if (!sid) { // valid runway, but no SID selected/found, so just the runway node for now if (!sidId.empty() && (sidId != "(none)")) { From f5c2c630ebe3599f2b72dc4e0bca2ad427767b80 Mon Sep 17 00:00:00 2001 From: James Turner Date: Wed, 20 Oct 2010 17:54:16 +0100 Subject: [PATCH 6/6] Remove references to old (unused) airways code --- src/AIModel/AIAircraft.cxx | 1 + src/AIModel/AIFlightPlan.hxx | 65 ++++++++++------------ src/AIModel/AIFlightPlanCreate.cxx | 1 + src/AIModel/AIFlightPlanCreateCruise.cxx | 5 +- src/AIModel/AIFlightPlanCreatePushBack.cxx | 2 + src/ATC/trafficcontrol.cxx | 2 +- src/Airports/groundnetwork.cxx | 1 + src/Airports/sidstar.hxx | 13 ++--- src/Main/fg_init.cxx | 12 +--- src/Main/globals.cxx | 6 +- src/Main/globals.hxx | 6 -- src/Navaids/Makefile.am | 3 +- 12 files changed, 47 insertions(+), 70 deletions(-) diff --git a/src/AIModel/AIAircraft.cxx b/src/AIModel/AIAircraft.cxx index f117810c1..a525d7ef1 100644 --- a/src/AIModel/AIAircraft.cxx +++ b/src/AIModel/AIAircraft.cxx @@ -29,6 +29,7 @@ #include #include #include +#include #include #include diff --git a/src/AIModel/AIFlightPlan.hxx b/src/AIModel/AIFlightPlan.hxx index 1de5658bd..fd3968305 100644 --- a/src/AIModel/AIFlightPlan.hxx +++ b/src/AIModel/AIFlightPlan.hxx @@ -24,26 +24,18 @@ #include -#include -#include - -#include "AIBase.hxx" - - - -using std::vector; -using std::string; - class FGTaxiRoute; class FGRunway; class FGAIAircraft; +class FGAirport; +class SGGeod; class FGAIFlightPlan { public: typedef struct { - string name; + std::string name; double latitude; double longitude; double altitude; @@ -56,11 +48,11 @@ public: int routeIndex; // For AI/ATC purposes; double time_sec; double trackLength; // distance from previous waypoint (for AI purposes); - string time; + std::string time; } waypoint; FGAIFlightPlan(); - FGAIFlightPlan(const string& filename); + FGAIFlightPlan(const std::string& filename); FGAIFlightPlan(FGAIAircraft *, const std::string& p, double course, @@ -73,9 +65,9 @@ public: double lat, double lon, double speed, - const string& fltType, - const string& acType, - const string& airline); + const std::string& fltType, + const std::string& acType, + const std::string& airline); ~FGAIFlightPlan(); waypoint* const getPreviousWaypoint( void ) const; @@ -91,18 +83,18 @@ public: double getLeadDistance( void ) const {return lead_distance;} double getBearing(waypoint* previous, waypoint* next) const; double getBearing(double lat, double lon, waypoint* next) const; - double checkTrackLength(string wptName); + double checkTrackLength(std::string wptName); time_t getStartTime() const { return start_time; } time_t getArrivalTime() const { return arrivalTime; } void create(FGAIAircraft *, FGAirport *dep, FGAirport *arr, int leg, double alt, double speed, double lat, double lon, - bool firstLeg, double radius, const string& fltType, const string& aircraftType, const string& airline, double distance); + bool firstLeg, double radius, const std::string& fltType, const std::string& aircraftType, const std::string& airline, double distance); void setLeg(int val) { leg = val;} void setTime(time_t st) { start_time = st; } int getGate() const { return gateId; } double getLeadInAngle() const { return leadInAngle; } - const string& getRunway() const; + const std::string& getRunway() const; void setRepeat(bool r) { repeat = r; } bool getRepeat(void) const { return repeat; } @@ -111,16 +103,16 @@ public: int getRouteIndex(int i); // returns the AI related index of this current routes. FGTaxiRoute *getTaxiRoute() { return taxiRoute; } void deleteTaxiRoute(); - string getRunway() { return activeRunway; } + std::string getRunway() { return activeRunway; } bool isActive(time_t time) {return time >= this->getStartTime();} - void setRunway(string rwy) { activeRunway = rwy; }; - string getRunwayClassFromTrafficType(string fltType); + void setRunway(std::string rwy) { activeRunway = rwy; }; + std::string getRunwayClassFromTrafficType(std::string fltType); void addWaypoint(waypoint* wpt) { waypoints.push_back(wpt); }; - void setName(string n) { name = n; }; - string getName() { return name; }; + void setName(std::string n) { name = n; }; + std::string getName() { return name; }; void setSID(FGAIFlightPlan* fp) { sid = fp;}; FGAIFlightPlan* getSID() { return sid; }; @@ -128,7 +120,7 @@ public: private: FGRunway* rwy; FGAIFlightPlan *sid; - typedef vector wpt_vector_type; + typedef std::vector wpt_vector_type; typedef wpt_vector_type::const_iterator wpt_vector_iterator; @@ -143,26 +135,25 @@ private: time_t arrivalTime; // For AI/ATC purposes. int leg; int gateId, lastNodeVisited; - string activeRunway; - FGAirRoute airRoute; + std::string activeRunway; FGTaxiRoute *taxiRoute; - string name; + std::string name; - void createPushBack(FGAIAircraft *, bool, FGAirport*, double, double, double, const string&, const string&, const string&); - void createPushBackFallBack(FGAIAircraft *, bool, FGAirport*, double, double, double, const string&, const string&, const string&); - void createTakeOff(FGAIAircraft *, bool, FGAirport *, double, const string&); - void createClimb(FGAIAircraft *, bool, FGAirport *, double, double, const string&); - void createCruise(FGAIAircraft *, bool, FGAirport*, FGAirport*, double, double, double, double, const string&); - void createDescent(FGAIAircraft *, FGAirport *, double latitude, double longitude, double speed, double alt,const string&, double distance); - void createLanding(FGAIAircraft *, FGAirport *, const string&); + void createPushBack(FGAIAircraft *, bool, FGAirport*, double, double, double, const std::string&, const std::string&, const std::string&); + void createPushBackFallBack(FGAIAircraft *, bool, FGAirport*, double, double, double, const std::string&, const std::string&, const std::string&); + void createTakeOff(FGAIAircraft *, bool, FGAirport *, double, const std::string&); + void createClimb(FGAIAircraft *, bool, FGAirport *, double, double, const std::string&); + void createCruise(FGAIAircraft *, bool, FGAirport*, FGAirport*, double, double, double, double, const std::string&); + void createDescent(FGAIAircraft *, FGAirport *, double latitude, double longitude, double speed, double alt,const std::string&, double distance); + void createLanding(FGAIAircraft *, FGAirport *, const std::string&); void createParking(FGAIAircraft *, FGAirport *, double radius); void deleteWaypoints(); void resetWaypoints(); - void createLandingTaxi(FGAIAircraft *, FGAirport *apt, double radius, const string& fltType, const string& acType, const string& airline); + void createLandingTaxi(FGAIAircraft *, FGAirport *apt, double radius, const std::string& fltType, const std::string& acType, const std::string& airline); void createDefaultLandingTaxi(FGAIAircraft *, FGAirport* aAirport); void createDefaultTakeoffTaxi(FGAIAircraft *, FGAirport* aAirport, FGRunway* aRunway); - void createTakeoffTaxi(FGAIAircraft *, bool firstFlight, FGAirport *apt, double radius, const string& fltType, const string& acType, const string& airline); + void createTakeoffTaxi(FGAIAircraft *, bool firstFlight, FGAirport *apt, double radius, const std::string& fltType, const std::string& acType, const std::string& airline); double getTurnRadius(double, bool); diff --git a/src/AIModel/AIFlightPlanCreate.cxx b/src/AIModel/AIFlightPlanCreate.cxx index 790de6923..a0a396d8f 100644 --- a/src/AIModel/AIFlightPlanCreate.cxx +++ b/src/AIModel/AIFlightPlanCreate.cxx @@ -27,6 +27,7 @@ #include #include +#include #include #include #include "AIAircraft.hxx" diff --git a/src/AIModel/AIFlightPlanCreateCruise.cxx b/src/AIModel/AIFlightPlanCreateCruise.cxx index 269acda94..1932de9ce 100644 --- a/src/AIModel/AIFlightPlanCreateCruise.cxx +++ b/src/AIModel/AIFlightPlanCreateCruise.cxx @@ -27,7 +27,7 @@ #include #include -#include +#include #include #include @@ -40,6 +40,7 @@ using std::iostream; +/* void FGAIFlightPlan::evaluateRoutePart(double deplat, double deplon, double arrlat, @@ -97,7 +98,7 @@ void FGAIFlightPlan::evaluateRoutePart(double deplat, } } - +*/ /* void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep, FGAirport *arr, double latitude, diff --git a/src/AIModel/AIFlightPlanCreatePushBack.cxx b/src/AIModel/AIFlightPlanCreatePushBack.cxx index d97e30da2..34f21a1fc 100644 --- a/src/AIModel/AIFlightPlanCreatePushBack.cxx +++ b/src/AIModel/AIFlightPlanCreatePushBack.cxx @@ -23,6 +23,8 @@ #endif #include + +#include #include #include diff --git a/src/ATC/trafficcontrol.cxx b/src/ATC/trafficcontrol.cxx index 53ddf28b5..8079933b1 100644 --- a/src/ATC/trafficcontrol.cxx +++ b/src/ATC/trafficcontrol.cxx @@ -34,7 +34,7 @@ #include #include #include - +#include using std::sort; diff --git a/src/Airports/groundnetwork.cxx b/src/Airports/groundnetwork.cxx index 3d96ccf7d..2fb8127a5 100644 --- a/src/Airports/groundnetwork.cxx +++ b/src/Airports/groundnetwork.cxx @@ -30,6 +30,7 @@ #include #include +#include #include #include diff --git a/src/Airports/sidstar.hxx b/src/Airports/sidstar.hxx index ef388e49b..ad8d10e94 100644 --- a/src/Airports/sidstar.hxx +++ b/src/Airports/sidstar.hxx @@ -24,6 +24,7 @@ #define _SIDSTAR_HXX_ #include +#include #include @@ -36,12 +37,10 @@ #include "runwayprefs.hxx" -using std::string; - class FGAirport; -typedef vector FlightPlanVec; -typedef vector::iterator FlightPlanVecIterator; +typedef std::vector FlightPlanVec; +typedef std::vector::iterator FlightPlanVecIterator; typedef std::map < std::string, FlightPlanVec > FlightPlanVecMap; @@ -49,7 +48,7 @@ typedef std::map < std::string, FlightPlanVec > FlightPlanVecMap; class FGSidStar { private: - string id; + std::string id; bool initialized; FlightPlanVecMap data; @@ -57,9 +56,9 @@ class FGSidStar FGSidStar(FGAirport *ap); FGSidStar(const FGSidStar &other); - string getId() { return id; }; + std::string getId() { return id; }; void load(SGPath path); - FGAIFlightPlan *getBest(string activeRunway, double heading); + FGAIFlightPlan *getBest(std::string activeRunway, double heading); }; diff --git a/src/Main/fg_init.cxx b/src/Main/fg_init.cxx index 184e5623a..fa2cc45a5 100644 --- a/src/Main/fg_init.cxx +++ b/src/Main/fg_init.cxx @@ -58,6 +58,8 @@ #include #include #include +#include + #include #include #include @@ -1072,16 +1074,6 @@ fgInitNav () fixlist.init( p_fix ); // adds fixes to the DB in positioned.cxx SG_LOG(SG_GENERAL, SG_INFO, " Airways"); - #if 0 - SGPath p_awy( globals->get_fg_root() ); - p_awy.append( "Navaids/awy.dat" ); - FGAirwayNetwork *awyNet = new FGAirwayNetwork; - //cerr << "Loading Airways" << endl; - awyNet->load (p_awy ); - awyNet->init(); - //cerr << "initializing airways" << endl; - globals->set_airwaynet( awyNet ); -#endif flightgear::Airway::load(); return true; diff --git a/src/Main/globals.cxx b/src/Main/globals.cxx index a5eb32563..7fafd0516 100644 --- a/src/Main/globals.cxx +++ b/src/Main/globals.cxx @@ -45,7 +45,6 @@ #include #include #include -#include #include #include #include @@ -149,9 +148,7 @@ FGGlobals::FGGlobals() : dmelist( NULL ), tacanlist( NULL ), carrierlist( NULL ), - channellist( NULL ), - airwaynet( NULL ) - + channellist( NULL ) { simgear::ResourceManager::instance()->addProvider(new AircraftResourceProvider()); } @@ -199,7 +196,6 @@ FGGlobals::~FGGlobals() delete tacanlist; delete carrierlist; delete channellist; - delete airwaynet; soundmgr->unbind(); delete soundmgr; diff --git a/src/Main/globals.hxx b/src/Main/globals.hxx index 50bd24e67..097e3acb5 100644 --- a/src/Main/globals.hxx +++ b/src/Main/globals.hxx @@ -60,7 +60,6 @@ class FGAircraftModel; class FGControls; class FGFlightPlanDispatcher; class FGNavList; -class FGAirwayNetwork; class FGTACANList; class FGModelMgr; class FGRouteMgr; @@ -171,7 +170,6 @@ private: FGNavList *tacanlist; FGNavList *carrierlist; FGTACANList *channellist; - FGAirwayNetwork *airwaynet; /// roots of Aircraft trees string_list fg_aircraft_dirs; @@ -322,10 +320,6 @@ public: inline FGTACANList *get_channellist() const { return channellist; } inline void set_channellist( FGTACANList *c ) { channellist = c; } - inline FGAirwayNetwork *get_airwaynet() const { return airwaynet; } - inline void set_airwaynet( FGAirwayNetwork *a ) { airwaynet = a; } - - /** * Save the current state as the initial state. */ diff --git a/src/Navaids/Makefile.am b/src/Navaids/Makefile.am index 30016afe3..93985cacf 100644 --- a/src/Navaids/Makefile.am +++ b/src/Navaids/Makefile.am @@ -13,8 +13,7 @@ libNavaids_a_SOURCES = \ airways.hxx airways.cxx \ route.hxx route.cxx \ waypoint.hxx waypoint.cxx \ - procedure.hxx procedure.cxx \ - awynet.cxx awynet.hxx + procedure.hxx procedure.cxx # # testnavs_SOURCES = testnavs.cxx