#include "runways.hxx"
+#include <Airports/simple.hxx>
+#include <Navaids/procedure.hxx>
+
using std::string;
static std::string cleanRunwayNo(const std::string& aRwyNo)
_reciprocal = other;
}
+std::vector<flightgear::SID*> FGRunway::getSIDs()
+{
+ std::vector<flightgear::SID*> 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<flightgear::STAR*> FGRunway::getSTARs()
+{
+ std::vector<flightgear::STAR*> 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;
+}
+
+
class FGNavRecord;
class SGPropertyNode;
+namespace flightgear {
+ class SID;
+ class STAR;
+}
+
class FGRunway : public FGRunwayBase
{
FGAirport* _airport;
* 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<flightgear::SID*> getSIDs();
+
+ /**
+ * Get STARs associared with this runway
+ */
+ std::vector<flightgear::STAR*> getSTARs();
};
#endif // _FG_RUNWAYS_HXX
#include <Airports/pavement.hxx>
#include <Airports/dynamics.hxx>
#include <Airports/xmlloader.hxx>
+#include <Navaids/procedure.hxx>
+#include <Navaids/waypoint.hxx>
+
+using std::vector;
+using namespace flightgear;
// magic import of a helper which uses FGPositioned internals
extern char** searchAirportNamesAndIdents(const std::string& aFilter);
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();
}
}
+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<FGAirport*>(this));
+}
+
void FGAirport::loadSceneryDefintions() const
{
// allow users to disable the scenery data in the short-term
_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<Approach*> aps;
+ for (unsigned int j=0; j<mApproaches.size();++j) {
+ if (mApproaches[j]->runway() == 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; k<aps.size(); ++k) {
+ if (aps[k]->route(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<SID*, WayptRef>
+FGAirport::selectSID(const SGGeod& aDest, FGRunway* aRwy)
+{
+ loadProcedures();
+
+ WayptRef enroute;
+ SID* sid = NULL;
+ double d = 1e9;
+
+ for (unsigned int i=0; i<mSIDs.size(); ++i) {
+ if (aRwy && !mSIDs[i]->isForRunway(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<STAR*, WayptRef>
+FGAirport::selectSTAR(const SGGeod& aOrigin, FGRunway* aRwy)
+{
+ loadProcedures();
+
+ WayptRef enroute;
+ STAR* star = NULL;
+ double d = 1e9;
+
+ for (unsigned int i=0; i<mSTARs.size(); ++i) {
+ if (!mSTARs[i]->isForRunway(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; i<mSIDs.size(); ++i) {
+ if (mSIDs[i]->ident() == 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; i<mSTARs.size(); ++i) {
+ if (mSTARs[i]->ident() == 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 )
{
typedef SGSharedPtr<FGTaxiway> FGTaxiwayPtr;
typedef SGSharedPtr<FGPavement> FGPavementPtr;
+namespace flightgear {
+ class SID;
+ class STAR;
+ class Approach;
+ class Waypt;
+
+
+ typedef SGSharedPtr<Waypt> WayptRef;
+ typedef std::vector<WayptRef> WayptVec;
+}
+
+
+
/***************************************************************************************
*
**************************************************************************************/
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.
double mMinLengthFt;
};
+
+ void setProcedures(const std::vector<flightgear::SID*>& aSids,
+ const std::vector<flightgear::STAR*>& aStars,
+ const std::vector<flightgear::Approach*>& 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
* 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<flightgear::SID*, flightgear::WayptRef> 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<flightgear::STAR*, flightgear::WayptRef> selectSTAR(const SGGeod& aOrigin, FGRunway* aRwy);
+
private:
typedef std::vector<FGRunwayPtr>::const_iterator Runway_iterator;
/**
void loadRunways() const;
void loadTaxiways() const;
+ void loadProcedures() const;
mutable bool mRunwaysLoaded;
mutable bool mTaxiwaysLoaded;
+ mutable bool mProceduresLoaded;
std::vector<FGRunwayPtr> mRunways;
std::vector<FGTaxiwayPtr> mTaxiways;
std::vector<FGPavementPtr> mPavements;
+
+ std::vector<flightgear::SID*> mSIDs;
+ std::vector<flightgear::STAR*> mSTARs;
+ std::vector<flightgear::Approach*> mApproaches;
};
// find basic airport location info from airport database
#include "route_mgr.hxx"
#include <boost/algorithm/string/case_conv.hpp>
+#include <boost/tuple/tuple.hpp>
#include <simgear/misc/strutils.hxx>
#include <simgear/structure/exception.hxx>
#include "Main/fg_props.hxx"
#include "Navaids/positioned.hxx"
+#include <Navaids/waypoint.hxx>
+#include <Navaids/airways.hxx>
+#include <Navaids/procedure.hxx>
#include "Airports/simple.hxx"
#include "Airports/runways.hxx"
#define RM "/autopilot/route-manager/"
+#include <GUI/new_gui.hxx>
+#include <GUI/dialog.hxx>
+
+using namespace flightgear;
+
+class PropertyWatcher : public SGPropertyChangeListener
+{
+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 T>
+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 <class T>
+PropertyWatcher* createWatcher(T* obj, void (T::*m)())
+{
+ return new MethodPropertyWatcher<T>(obj, m);
+}
+
FGRouteMgr::FGRouteMgr() :
- _route( new SGRoute ),
- input(fgGetNode( RM "input", true )),
- mirror(fgGetNode( RM "route", true ))
+ _currentIndex(0),
+ input(fgGetNode( RM "input", true )),
+ mirror(fgGetNode( RM "route", true ))
{
- listener = new InputListener(this);
- input->setStringValue("");
- input->addChangeListener(listener);
+ 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;
}
&FGRouteMgr::getDepartureICAO, &FGRouteMgr::setDepartureICAO));
departure->tie("name", SGRawValueMethods<FGRouteMgr, const char*>(*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);
&FGRouteMgr::getDestinationICAO, &FGRouteMgr::setDestinationICAO));
destination->tie("name", SGRawValueMethods<FGRouteMgr, const char*>(*this,
&FGRouteMgr::getDestinationName, NULL));
-
- destination->getChild("runway", 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);
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);
_currentWpt = fgGetNode(RM "current-wp", true);
_currentWpt->tie(SGRawValueMethods<FGRouteMgr, int>
- (*this, &FGRouteMgr::currentWaypoint, &FGRouteMgr::jumpToIndex));
+ (*this, &FGRouteMgr::currentIndex, &FGRouteMgr::jumpToIndex));
// temporary distance / eta calculations, for backward-compatability
wp0 = fgGetNode(RM "wp", 0, true);
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<string>::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() { }
return active->getBoolValue();
}
-void FGRouteMgr::update( double dt ) {
- if (dt <= 0.0) {
- // paused, nothing to do here
- return;
- }
+void FGRouteMgr::update( double dt )
+{
+ if (dt <= 0.0) {
+ return; // paused, nothing to do here
+ }
- if (!active->getBoolValue()) {
+ 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;
}
- 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);
+ 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::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 );
+}
+
+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;
}
- // 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);
+ 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;
}
- 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();
- }
+ 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;
- wpn->setDoubleValue("dist", totalDistanceRemaining * SG_METER_TO_NM);
- ete->setDoubleValue(totalDistanceRemaining * SG_METER_TO_NM / groundSpeed * 3600.0);
- setETAPropertyFromDistance(wpn->getChild("eta"), totalDistanceRemaining);
+ case ROUTE_LOW_AIRWAYS:
+ Airway::lowLevel()->route(wp1, wp2, r);
+ break;
- // get time now at destination tz as tm struct
- // add ete seconds
- // convert to string ... and stash in property
- //destination->setDoubleValue("eta", eta);
-}
+ 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());
-void FGRouteMgr::setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDistance) {
- double speed =fgGetDouble("/velocities/groundspeed-kt", 0.0);
- if (speed < 1.0) {
- aProp->setStringValue("--:--");
- return;
- }
+ update_mirror();
+ _edited->fireValueChanged();
+ return true;
+}
+
+void FGRouteMgr::autoRoute()
+{
+ if (!_departure || !_destination) {
+ return;
+ }
- char eta_str[64];
- double eta = aDistance * SG_METER_TO_NM / speed;
- if ( eta >= 100.0 ) {
- eta = 99.999; // clamp
- }
+ 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);
+ }
- if ( eta < (1.0/6.0) ) {
- eta *= 60.0; // within 10 minutes, bump up to min/secs
+ _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");
}
- int major = (int)eta,
- minor = (int)((eta - (int)eta) * 60.0);
- snprintf( eta_str, 64, "%d:%02d", major, minor );
- aProp->setStringValue( eta_str );
+ 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();
}
-void FGRouteMgr::add_waypoint( const SGWayPoint& wp, int n )
+void FGRouteMgr::departureChanged()
{
- _route->add_waypoint( wp, n );
-
- if ((n >= 0) && (_route->current_index() > n)) {
- _route->set_current(_route->current_index() + 1);
+// remove existing departure waypoints
+ WayptVec::iterator it = _route.begin();
+ for (; it != _route.end(); ++it) {
+ if (!(*it)->flag(WPT_DEPARTURE)) {
+ break;
+ }
}
+ // erase() invalidates iterators, so grab now
+ WayptRef enroute;
+ if (it == _route.end()) {
+ if (_destination) {
+ enroute = new NavaidWaypoint(_destination.get(), NULL);
+ }
+ } else {
+ enroute = *it;
+ }
+
+ _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();
}
-void FGRouteMgr::waypointsChanged()
+void FGRouteMgr::buildDeparture(WayptRef enroute, WayptVec& wps)
{
- 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));
+ 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());
}
- update_mirror();
- _edited->fireValueChanged();
- checkFinished();
+ 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));
+ }
}
-SGWayPoint FGRouteMgr::pop_waypoint( int n ) {
- if ( _route->size() <= 0 ) {
- return SGWayPoint();
+void FGRouteMgr::arrivalChanged()
+{
+ // remove existing arrival waypoints
+ WayptVec::reverse_iterator rit = _route.rbegin();
+ for (; rit != _route.rend(); ++rit) {
+ if (!(*rit)->flag(WPT_ARRIVAL)) {
+ break;
+ }
}
- if ( n < 0 ) {
- n = _route->size() - 1;
- }
+ // erase() invalidates iterators, so grab now
+ WayptRef enroute;
+ WayptVec::iterator it;
- if (_route->current_index() > n) {
- _route->set_current(_route->current_index() - 1);
+ if (rit != _route.rend()) {
+ enroute = *rit;
+ it = rit.base(); // convert to fwd iterator
+ } else {
+ it = _route.begin();
}
- SGWayPoint wp = _route->get_waypoint(n);
- _route->delete_waypoint(n);
-
+ _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();
- return wp;
}
-
-bool FGRouteMgr::build() {
- return true;
+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);
}
+void FGRouteMgr::waypointsChanged()
+{
-void FGRouteMgr::new_waypoint( const string& target, int n ) {
- SGWayPoint* wp = make_waypoint( target );
- if (!wp) {
- return;
- }
-
- add_waypoint( *wp, n );
- delete wp;
}
+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();
+}
-SGWayPoint* FGRouteMgr::make_waypoint(const string& tgt ) {
- string target(boost::to_upper_copy(tgt));
+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<string> 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<FGAirport*>(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);
+ 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();
- if (pieces.size() == 2) {
- FGAirport* apt = dynamic_cast<FGAirport*>(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]);
+ 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);
-
- 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());
+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->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:
}
if (!strcmp(s, "@CLEAR"))
- mgr->init();
+ mgr->clearRoute();
else if (!strcmp(s, "@ACTIVATE"))
mgr->activate();
else if (!strcmp(s, "@LOAD")) {
} 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);
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()
{
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);
- }
-
- 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()));
- }
+
+ _currentIndex = 0;
+ currentWaypointChanged();
+
+ /* 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;
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;
}
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();
-
- 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("");
- }
+ Waypt* cur = currentWaypt();
+ Waypt* next = nextWaypt();
+
+ wp0->getChild("id")->setStringValue(cur ? cur->ident() : "");
+ wp1->getChild("id")->setStringValue(next ? next->ident() : "");
- 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; i<numWaypts(); ++i) {
+ if (_route[i]->matches(aPos)) {
return i;
}
}
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()
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());
d->setStringValue("destination/transition", destination->getStringValue("transition"));
d->setStringValue("destination/runway", destination->getStringValue("runway"));
}
-
- // route nodes
+
+ // 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());
- }
+ 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());
}
}
}
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");
}
-
+ } catch (sg_exception& e) {
+ SG_LOG(SG_IO, SG_WARN, "failed to load flight-plan (from '" << e.getOrigin()
+ << "'):" << e.getMessage());
+ }
+}
+
+void FGRouteMgr::loadXMLRouteHeader(SGPropertyNode_ptr routeData)
+{
+ // departure nodes
+ SGPropertyNode* dep = routeData->getChild("departure");
+ if (dep) {
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");
- }
-
+ 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"));
+ }
- // 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
+// 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::loadVersion2XMLRoute(SGPropertyNode_ptr routeData)
+{
+ loadXMLRouteHeader(routeData);
+
+// route nodes
+ WayptVec wpts;
+ SGPropertyNode_ptr routeNode = routeData->getChild("route", 0);
+ for (int i=0; i<routeNode->nChildren(); ++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; i<routeNode->nChildren(); ++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->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();
+ if (!_route.empty()) {
+ lastPos = _route.back()->position();
+ } else if (_departure) {
+ lastPos = _departure->geod();
}
- SGPropertyNode_ptr altProp = aWP->getChild("altitude-ft");
- double altM = -9999.0;
- if (altProp) {
- altM = altProp->getDoubleValue() * SG_FEET_TO_METER;
- }
-
+ 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");
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)
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
} else {
_departure = FGAirport::findByIdent(aIdent);
}
+
+ departureChanged();
}
const char* FGRouteMgr::getDestinationICAO() const
} else {
_destination = FGAirport::findByIdent(aIdent);
}
+
+ arrivalChanged();
}
#include <simgear/route/waypoint.hxx>
#include <simgear/structure/subsystem_mgr.hxx>
+#include <Navaids/route.hxx>
+
// forward decls
-class SGRoute;
class SGPath;
+class PropertyWatcher;
class FGAirport;
typedef SGSharedPtr<FGAirport> 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;
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
* - 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.
*/
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;
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;
};
#include <Airports/simple.hxx>
#include <Airports/runways.hxx>
#include <Main/fg_os.hxx> // fgGetKeyModifiers()
+#include <Navaids/routePath.hxx>
const char* RULER_LEGEND_KEY = "ruler-legend";
void MapWidget::paintRoute()
{
- if (_route->size() < 2) {
+ if (_route->numWaypts() < 2) {
return;
}
-// first pass, draw the actual line
- glLineWidth(2.0);
- glBegin(GL_LINE_STRIP);
+ RoutePath path(_route->waypts());
- SGVec2d prev = project(_route->get_waypoint(0).get_target());
- glVertex2d(prev.x(), prev.y());
+// first pass, draw the actual lines
+ glLineWidth(2.0);
- for (int w=1; w < _route->size(); ++w) {
-
- SGVec2d p = project(_route->get_waypoint(w).get_target());
+ for (int w=0; w<_route->numWaypts(); ++w) {
+ SGGeodVec gv(path.pathForIndex(w));
+ if (gv.empty()) {
+ continue;
+ }
- 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; i<gv.size(); ++i) {
+ SGVec2d p = project(gv[i]);
+ glVertex2d(p.x(), p.y());
+ }
+ glEnd();
+ glDisable(GL_LINE_STIPPLE);
}
- glEnd();
glLineWidth(1.0);
// second pass, draw waypoint symbols and data
- for (int w=0; w < _route->size(); ++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<void*>(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);
-
- 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";
+ d->setPriority(w < _route->currentIndex() ? 9000 : 12000);
- MapData* ld = getOrCreateDataForKey(reinterpret_cast<void*>(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
}
#include "WaypointList.hxx"
#include <algorithm>
+#include <boost/tuple/tuple.hpp>
+
#include <plib/puAux.h>
#include <simgear/route/waypoint.hxx>
#include <Main/globals.hxx>
#include <Main/fg_props.hxx>
+#include <Navaids/positioned.hxx>
#include <Autopilot/route_mgr.hxx>
+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 :
// 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)
_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()
}
_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;
}
_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)
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];
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
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));
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
+ if (isCurrent) {
+ f->drawString(">", xx, yy);
+ }
+
+ int x = xx;
+ x += _arrowWidth + PUSTR_LGAP;
+
// 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())) {
+ 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)", wp.get_name().c_str());
+ ::snprintf(buffer + count, 128 - count, " (%s)", src->name().c_str());
}
-
- glColor4fv ( colour [ PUCOL_LEGEND ] ) ;
- drawClippedString(legendFont, buffer, xx, yy, 300);
+
+ drawClippedString(legendFont, buffer, x, yy, 300);
+ x += 300 + PUSTR_LGAP;
if (_showLatLon) {
- char ns = (wp.get_target_lat() > 0.0) ? 'N' : 'S';
- char ew = (wp.get_target_lon() > 0.0) ? 'E' : 'W';
+ 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(wp.get_target_lon()), ew, fabs(wp.get_target_lat()), ns);
- } else {
+ 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",
- wp.get_track(), wp.get_distance() * SG_METER_TO_NM);
+ courseDeg, distanceM * SG_METER_TO_NM);
}
- legendFont.drawString(buffer, xx + 300 + PUSTR_LGAP, yy);
+ f->drawString(buffer, x, yy);
+ x += 100 + 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 (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);
}
}
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<int>(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<int>(wp->altitudeFt()) + 50) / 100;
+ wp->setAltitude((curAlt + 10) * 100, wp->altitudeRestriction());
}
}
break;
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);
}
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
// forward decls
class puaScrollBar;
-class SGWayPoint;
class SGCallback;
+namespace flightgear {
+ class Waypt;
+}
+
class WaypointList : public puFrame, public GUI_ID
{
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;
};
int numFullyVisibleRows() const;
int firstFullyVisibleRow() const;
int lastFullyVisibleRow() const;
-
- int wayptAltFtHundreds(int index) const;
-
+
void modelUpdateCallback();
int _scrollPx; // scroll ammount (in pixels)
Model* _model;
SGCallback* _updateCallback;
SGCallback* _scrollCallback;
+
+ SGTimeStamp _blinkTimer;
+ bool _blink;
+ int _arrowWidth;
};
class ScrolledWaypointList : public puGroup
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
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 {
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
#include "gps.hxx"
+#include <boost/tuple/tuple.hpp>
+
#include <memory>
#include <set>
#include <cstring>
#include "Main/globals.hxx" // for get_subsystem
#include "Main/util.hxx" // for fgLowPass
#include "Navaids/positioned.hxx"
+#include <Navaids/waypoint.hxx>
#include "Navaids/navrecord.hxx"
#include "Airports/simple.hxx"
#include "Airports/runways.hxx"
using std::auto_ptr;
using std::string;
+using namespace flightgear;
///////////////////////////////////////////////////////////////////
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 ()
_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);
// 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);
tie(_scratchNode, "mag-bearing-deg", SGRawValueMethods<GPS, double>(*this, &GPS::getScratchMagBearing, NULL));
tie(_scratchNode, "has-next", SGRawValueMethods<GPS, bool>(*this, &GPS::getScratchHasNext, NULL));
_scratchValid = false;
+
-// waypoint data (including various historical things)
SGPropertyNode *wp_node = _gpsNode->getChild("wp", 0, true);
- SGPropertyNode *wp0_node = wp_node->getChild("wp", 0, true);
- SGPropertyNode *wp1_node = wp_node->getChild("wp", 1, true);
-
+ SGPropertyNode* wp0_node = wp_node->getChild("wp", 0, true);
+
tieSGGeodReadOnly(wp0_node, _wp0_position, "longitude-deg", "latitude-deg", "altitude-ft");
- tie(wp0_node, "ID", SGRawValueMethods<GPS, const char*>
- (*this, &GPS::getWP0Ident, NULL));
- tie(wp0_node, "name", SGRawValueMethods<GPS, const char*>
- (*this, &GPS::getWP0Name, NULL));
-
- tieSGGeodReadOnly(wp1_node, _wp1_position, "longitude-deg", "latitude-deg", "altitude-ft");
- tie(wp1_node, "ID", SGRawValueMethods<GPS, const char*>
+ tie(_currentWayptNode, "ID", SGRawValueMethods<GPS, const char*>
(*this, &GPS::getWP1Ident, NULL));
- tie(wp1_node, "name", SGRawValueMethods<GPS, const char*>
- (*this, &GPS::getWP1Name, NULL));
- tie(wp1_node, "distance-nm", SGRawValueMethods<GPS, double>
+ tie(_currentWayptNode, "distance-nm", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1Distance, NULL));
- tie(wp1_node, "bearing-true-deg", SGRawValueMethods<GPS, double>
+ tie(_currentWayptNode, "bearing-true-deg", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1Bearing, NULL));
- tie(wp1_node, "bearing-mag-deg", SGRawValueMethods<GPS, double>
+ tie(_currentWayptNode, "bearing-mag-deg", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1MagBearing, NULL));
- tie(wp1_node, "TTW-sec", SGRawValueMethods<GPS, double>
+ tie(_currentWayptNode, "TTW-sec", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1TTW, NULL));
- tie(wp1_node, "TTW", SGRawValueMethods<GPS, const char*>
+ tie(_currentWayptNode, "TTW", SGRawValueMethods<GPS, const char*>
(*this, &GPS::getWP1TTWString, NULL));
- tie(wp1_node, "course-deviation-deg", SGRawValueMethods<GPS, double>
+ tie(_currentWayptNode, "course-deviation-deg", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1CourseDeviation, NULL));
- tie(wp1_node, "course-error-nm", SGRawValueMethods<GPS, double>
+ tie(_currentWayptNode, "course-error-nm", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1CourseErrorNm, NULL));
- tie(wp1_node, "to-flag", SGRawValueMethods<GPS, bool>
+ tie(_currentWayptNode, "to-flag", SGRawValueMethods<GPS, bool>
(*this, &GPS::getWP1ToFlag, NULL));
- tie(wp1_node, "from-flag", SGRawValueMethods<GPS, bool>
+ tie(_currentWayptNode, "from-flag", SGRawValueMethods<GPS, bool>
(*this, &GPS::getWP1FromFlag, NULL));
// leg properties (only valid in DTO/LEG modes, not OBS)
tie(wp_node, "leg-distance-nm", SGRawValueMethods<GPS, double>(*this, &GPS::getLegDistance, NULL));
tie(wp_node, "leg-true-course-deg", SGRawValueMethods<GPS, double>(*this, &GPS::getLegCourse, NULL));
tie(wp_node, "leg-mag-course-deg", SGRawValueMethods<GPS, double>(*this, &GPS::getLegMagCourse, NULL));
- tie(wp_node, "alt-dist-ratio", SGRawValueMethods<GPS, double>(*this, &GPS::getAltDistanceRatio, NULL));
// navradio slaving properties
tie(_gpsNode, "cdi-deflection", SGRawValueMethods<GPS,double>
_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);
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") {
+ if (_wayptController.get()) {
+ _wayptController->update();
+ SGGeod p(_wayptController->position());
+ _currentWayptNode->setDoubleValue("longitude-deg", p.getLongitudeDeg());
+ _currentWayptNode->setDoubleValue("latitude-deg", p.getLatitudeDeg());
+ _currentWayptNode->setDoubleValue("altitude-ft", p.getElevationFt());
+
+ _desiredCourse = getLegMagCourse();
+
updateTurn();
+ updateRouteData();
}
-
- updateWaypoints();
+
+
updateTrackingBug();
updateReferenceNavaid(delta_time_sec);
- updateRouteData();
driveAutopilot();
}
_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)
{
_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) {
}
} 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();
}
}
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;
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();
}
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();
}
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);
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();
return;
}
- int curIndex = _routeMgr->currentWaypoint();
- if ((curIndex + 1) >= _routeMgr->size()) {
+ WayptRef next = _routeMgr->nextWaypt();
+ if (!next || next->flag(WPT_DYNAMIC)) {
_anticipateTurn = false;
return;
}
_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;
", 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;
}
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);
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);
}
}
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
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) {
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
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) {
- return -1.0;
- }
-
- if (_last_speed_kts < 1.0) {
+ if (!_dataValid || !_wayptController.get()) {
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
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;
}
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;
}
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 {
} 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;
}
}
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();
}
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;
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);
}
auto_ptr<FGPositioned::Filter> 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);
{
int lastResult;
if (_searchIsRoute) {
- lastResult = _routeMgr->size() - 1;
+ lastResult = _routeMgr->numWaypts() - 1;
} else {
lastResult = (int) _searchResults.size() - 1;
}
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();
}
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");
}
}
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,
#include <simgear/structure/subsystem_mgr.hxx>
#include <simgear/math/SGMath.hxx>
-#include "Navaids/positioned.hxx"
+#include <Navaids/positioned.hxx>
+#include <Instrumentation/rnav_waypt_controller.hxx>
// forward decls
class SGRoute;
* /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;
void clearOutput();
void updateBasicData(double dt);
- void updateWaypoints();
void updateTrackingBug();
void updateReferenceNavaid(double dt);
double getLegDistance() const;
double getLegCourse() const;
double getLegMagCourse() const;
- double getAltDistanceRatio() const;
double getTrueTrack() const { return _last_true_track; }
double getMagTrack() const;
// members
SGPropertyNode_ptr _gpsNode;
+ SGPropertyNode_ptr _currentWayptNode;
SGPropertyNode_ptr _magvar_node;
SGPropertyNode_ptr _serviceable_node;
SGPropertyNode_ptr _electrical_node;
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;
SGGeod _turnPt;
SGGeod _turnCentre;
+ std::auto_ptr<flightgear::WayptController> _wayptController;
+
SGPropertyNode_ptr _realismSimpleGps; ///< should the GPS be simple or realistic?
+ flightgear::WayptRef _prevWaypt;
+ flightgear::WayptRef _currentWaypt;
// autopilot drive properties
SGPropertyNode_ptr _apDrivingFlag;
--- /dev/null
+// 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 <simgear/sg_inlines.h>
+#include <simgear/structure/exception.hxx>
+
+#include <Airports/runways.hxx>
+
+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<RunwayWaypt*>(_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
+
--- /dev/null
+// 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 <Navaids/waypoint.hxx>
+
+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
+#include <fstream>
+
+#include <simgear/misc/sg_path.hxx>
+#include <simgear/structure/exception.hxx>
+
#include <Main/fg_init.hxx>
#include <Main/globals.hxx>
#include <Main/fg_props.hxx>
#include <Instrumentation/gps.hxx>
#include <Autopilot/route_mgr.hxx>
#include <Environment/environment_mgr.hxx>
+#include <Navaids/airways.hxx>
+#include <Navaids/waypoint.hxx>
+#include <Navaids/procedure.hxx>
using std::string;
+using namespace flightgear;
char *homedir = ::getenv( "HOME" );
char *hostname = ::getenv( "HOSTNAME" );
}
}
+void printRoute(const WayptVec& aRoute)
+{
+ SG_LOG(SG_GENERAL, SG_INFO, "route size=" << aRoute.size());
+ for (unsigned int r=0; r<aRoute.size();++r) {
+ Waypt* w = aRoute[r];
+ SG_LOG(SG_GENERAL, SG_ALERT, "\t" << r << ": " << w->ident() << " "
+ << w->owner()->ident());
+ }
+}
+
void createDummyRoute(FGRouteMgr* rm)
{
SGPropertyNode* rmInput = fgGetNode("/autopilot/route-manager/input", true);
int main(int argc, char* argv[])
{
+
+try{
globals = new FGGlobals;
fgInitFGRoot(argc, argv);
fgInitNav();
+ fgSetDouble("/environment/magnetic-variation-deg", 0.0);
+
+ Airway::load();
SG_LOG(SG_GENERAL, SG_ALERT, "hello world!");
// 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
// update a few times
gps->update(0.05);
gps->update(0.05);
+ gps->update(0.05);
scratch->setStringValue("query", "TL");
scratch->setStringValue("type", "Vor");
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 << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
+ "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n"
+ "<Document>\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 << "</Document>\n"
+ "</kml>" << 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 << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
+ "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n"
+ "<Document>\n";
+
+ Route::dumpRouteToLineString("REDFA 18R", approach, f);
+ Route::dumpRouteToLineString("EGLL 27R", approach2, f);
+
+ // post-amble
+ f << "</Document>\n"
+ "</kml>" << endl;
+ f.close();
+
+
return EXIT_SUCCESS;
+
+
+
+} catch (sg_exception& ex) {
+ SG_LOG(SG_GENERAL, SG_ALERT, "exception:" << ex.getFormattedMessage());
+}
+ return EXIT_FAILURE;
}
#include <Navaids/navlist.hxx>
#include <Navaids/fix.hxx>
#include <Navaids/fixlist.hxx>
+#include <Navaids/airways.hxx>
#include <Scenery/scenery.hxx>
#include <Scenery/tilemgr.hxx>
#include <Scripting/NasalSys.hxx>
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;
awyNet->init();
//cerr << "initializing airways" << endl;
globals->set_airwaynet( awyNet );
-
+#endif
+ flightgear::Airway::load();
+
return true;
}
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
--- /dev/null
+// 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 <algorithm>
+#include <set>
+
+#include <simgear/sg_inlines.h>
+#include <simgear/structure/exception.hxx>
+#include <simgear/misc/sgstream.hxx>
+#include <simgear/misc/sg_path.hxx>
+
+#include <boost/tuple/tuple.hpp>
+
+#include <Main/globals.hxx>
+#include <Navaids/positioned.hxx>
+#include <Navaids/waypoint.hxx>
+
+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<AStarOpenNode> previous;
+ double distanceFromStart; // aka 'g(x)'
+ double directDistanceToDestination; // aka 'h(x)'
+
+ /**
+ * aka 'f(x)'
+ */
+ double totalCost() const {
+ return distanceFromStart + directDistanceToDestination;
+ }
+};
+
+typedef SGSharedPtr<AStarOpenNode> 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<FGPositioned*>(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<FGPositionedRef, bool>
+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<FGPositionedRef, bool>
+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<AStarOpenNodeRef> 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; i<aHeap.size(); ++i) {
+ if (aHeap[i]->node == 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<FGPositioned*> ClosedNodeSet;
+ typedef std::pair<AdjacencyMap::iterator,AdjacencyMap::iterator> 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
--- /dev/null
+// 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 <map>
+#include <vector>
+
+#include <Navaids/route.hxx>
+
+class FGPositioned;
+typedef SGSharedPtr<FGPositioned> 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 <em>not</em> 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<FGPositioned*, AdjacentWaypoint*> AdjacencyMap;
+ AdjacencyMap _graph;
+
+ typedef std::vector<AdjacentWaypoint*> AdjacentWaypointVec;
+
+ typedef std::map<std::string, Airway*> 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<FGPositionedRef, bool> findClosestNode(WayptRef aRef);
+
+ /**
+ * Overloaded version working with a raw SGGeod
+ */
+ std::pair<FGPositionedRef, bool> 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
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()) {
}
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) {
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();
}
}
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
TAXIWAY,
PAVEMENT,
PARK_STAND,
+ WAYPOINT,
FIX,
VOR,
NDB,
DME,
TACAN,
OBSTACLE,
- WAYPOINT, // user-defined waypoint
FREQ_GND,
FREQ_TWR,
FREQ_ATIS,
* 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
--- /dev/null
+// 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 <simgear/structure/exception.hxx>
+
+#include <Navaids/waypoint.hxx>
+
+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<WayptVec> 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
--- /dev/null
+/// 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 <set>
+
+#include <Navaids/route.hxx>
+#include <Airports/runways.hxx>
+
+typedef SGSharedPtr<FGRunway> 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<WayptRef, Transition*> 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<FGRunwayRef, Transition*> 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<WayptRef, Transition*> 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
--- /dev/null
+// 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 <map>
+#include <fstream>
+
+// Boost
+#include <boost/algorithm/string/case_conv.hpp>
+#include <boost/algorithm/string.hpp>
+
+// SimGear
+#include <simgear/structure/exception.hxx>
+#include <simgear/xml/easyxml.hxx>
+#include <simgear/misc/sg_path.hxx>
+
+// FlightGear
+#include <Navaids/procedure.hxx>
+#include <Navaids/waypoint.hxx>
+#include <Airports/simple.hxx>
+
+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<double, double>
+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 << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
+ "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n"
+ "<Document>\n";
+
+ dumpRouteToLineString(aName, aRoute, f);
+
+// post-amble
+ f << "</Document>\n"
+ "</kml>" << endl;
+ f.close();
+}
+
+void Route::dumpRouteToLineString(const std::string& aIdent,
+ const WayptVec& aRoute, std::ostream& aStream)
+{
+ // preamble
+ aStream << "<Placemark>\n";
+ aStream << "<name>" << aIdent << "</name>\n";
+ aStream << "<LineString>\n";
+ aStream << "<tessellate>1</tessellate>\n";
+ aStream << "<coordinates>\n";
+
+ // waypoints
+ for (unsigned int i=0; i<aRoute.size(); ++i) {
+ SGGeod pos = aRoute[i]->position();
+ aStream << pos.getLongitudeDeg() << "," << pos.getLatitudeDeg() << " " << endl;
+ }
+
+ // postable
+ aStream << "</coordinates>\n"
+ "</LineString>\n"
+ "</Placemark>\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<string> rwys;
+ boost::split(rwys, v, boost::is_any_of(" ,"));
+ for (unsigned int r=0; r<rwys.size(); ++r) {
+ FGRunway* rwy = _airport->getRunwayByIdent(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
--- /dev/null
+/**
+ * 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 <vector>
+#include <map>
+#include <iosfwd>
+
+// Simgear
+#include <simgear/math/SGMath.hxx>
+#include <simgear/structure/SGReferenced.hxx>
+#include <simgear/structure/SGSharedPtr.hxx>
+#include <simgear/props/props.hxx>
+
+// forward decls
+class FGPositioned;
+class SGPath;
+class FGAirport;
+
+namespace flightgear
+{
+
+// forward decls
+class Route;
+class Waypt;
+class NavdataVisitor;
+
+typedef SGSharedPtr<Waypt> 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<double, double> 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<WayptRef> 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
--- /dev/null
+#include <Navaids/routePath.hxx>
+
+#include <simgear/structure/exception.hxx>
+#include <simgear/magvar/magvar.hxx>
+#include <simgear/timing/sg_time.hxx>
+
+#include <Main/globals.hxx>
+#include <Airports/runways.hxx>
+#include <Navaids/waypoint.hxx>
+#include <Navaids/positioned.hxx>
+
+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<RunwayWaypt*>(_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<turnSteps; ++i) {
+ hdg += turnDelta;
+ SGGeodesy::direct(pos, hdg, stepDist, pos, az2);
+ r.push_back(pos);
+ }
+
+ // leg
+ SGGeodesy::direct(pos, hdg, legDist, pos, az2);
+ r.push_back(pos);
+ } // of leg+turn duplication
+
+ return r;
+}
+
+/**
+ * the path context holds the state of of an imaginary aircraft traversing
+ * the route, and limits the rate at which heading / altitude / position can
+ * change
+ */
+class RoutePath::PathCtx
+{
+public:
+ SGGeod pos;
+ double heading;
+};
+
+bool RoutePath::computedPositionForIndex(int index, SGGeod& r) const
+{
+ if ((index < 0) || (index >= (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<RunwayWaypt*>(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<RunwayWaypt*>(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);
+}
+
--- /dev/null
+/**
+ * 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 <Navaids/route.hxx>
+
+namespace flightgear
+{
+ class Hold;
+}
+
+typedef std::vector<SGGeod> 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
+// 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 <simgear/misc/sg_path.hxx>
#include "fixlist.hxx"
--- /dev/null
+// 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 <simgear/structure/exception.hxx>
+#include <simgear/route/waypoint.hxx>
+
+#include <Airports/simple.hxx>
+#include <Airports/runways.hxx>
+
+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
--- /dev/null
+// 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 <Navaids/route.hxx>
+#include <Navaids/positioned.hxx>
+
+class FGAirport;
+typedef SGSharedPtr<FGAirport> 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
#include <Main/util.hxx>
#include <Scenery/scenery.hxx>
#include <Navaids/navrecord.hxx>
+#include <Navaids/procedure.hxx>
#include "NasalSys.hxx"
HASHSET("ils_frequency_mhz", 17, naNum(rwy->ILS()->get_freq() / 100.0));
}
+ std::vector<flightgear::SID*> sids(rwy->getSIDs());
+ naRef sidVec = naNewVector(c);
+
+ for (unsigned int s=0; s < sids.size(); ++s) {
+ naRef procId = naStr_fromdata(naNewString(c),
+ const_cast<char *>(sids[s]->ident().c_str()),
+ sids[s]->ident().length());
+ naVec_append(sidVec, procId);
+ }
+ HASHSET("sids", 4, sidVec);
+
+ std::vector<flightgear::STAR*> stars(rwy->getSTARs());
+ naRef starVec = naNewVector(c);
+
+ for (unsigned int s=0; s < stars.size(); ++s) {
+ naRef procId = naStr_fromdata(naNewString(c),
+ const_cast<char *>(stars[s]->ident().c_str()),
+ stars[s]->ident().length());
+ naVec_append(starVec, procId);
+ }
+ HASHSET("stars", 5, starVec);
+
#undef HASHSET
naHash_set(rwys, rwyid, rwydata);
}
projects / flightgear.git / commitdiff