#include <simgear/props/props.hxx>
#include <simgear/structure/subsystem_mgr.hxx>
-#include <Navaids/route.hxx>
+#include <Navaids/FlightPlan.hxx>
// forward decls
class SGPath;
class PropertyWatcher;
-class FGAirport;
-class FGRunway;
-
-typedef SGSharedPtr<FGAirport> FGAirportRef;
-
/**
* Top level route manager class
*
routePath.cxx
waypoint.cxx
LevelDXML.cxx
+ FlightPlan.cxx
)
set(HEADERS
routePath.hxx
waypoint.hxx
LevelDXML.hxx
+ FlightPlan.hxx
)
flightgear_component(Navaids "${SOURCES}" "${HEADERS}")
\ No newline at end of file
--- /dev/null
+// FlightPlan.cxx - flight plan object
+
+// Written by James Turner, started 2012.
+//
+// Copyright (C) 2012 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 "FlightPlan.hxx"
+
+// std
+#include <map>
+#include <fstream>
+
+// Boost
+#include <boost/algorithm/string/case_conv.hpp>
+#include <boost/algorithm/string.hpp>
+#include <boost/foreach.hpp>
+
+// SimGear
+#include <simgear/structure/exception.hxx>
+#include <simgear/misc/sg_path.hxx>
+#include <simgear/magvar/magvar.hxx>
+#include <simgear/timing/sg_time.hxx>
+#include <simgear/misc/sgstream.hxx>
+#include <simgear/misc/strutils.hxx>
+#include <simgear/props/props_io.hxx>
+
+// FlightGear
+#include <Main/globals.hxx>
+#include "Main/fg_props.hxx"
+#include <Navaids/procedure.hxx>
+#include <Navaids/waypoint.hxx>
+
+using std::string;
+using std::vector;
+using std::endl;
+using std::fstream;
+
+namespace flightgear {
+
+FlightPlan::FlightPlan() :
+ _currentIndex(-1),
+ _departureRunway(NULL),
+ _destinationRunway(NULL),
+ _sid(NULL),
+ _star(NULL),
+ _approach(NULL),
+ _delegate(NULL)
+{
+
+}
+
+FlightPlan::~FlightPlan()
+{
+
+}
+
+FlightPlan* FlightPlan::clone(const string& newIdent) const
+{
+ FlightPlan* c = new FlightPlan();
+ c->_ident = newIdent.empty() ? _ident : newIdent;
+
+// copy destination / departure data.
+ c->setDeparture(_departure);
+ c->setDeparture(_departureRunway);
+
+ if (_approach) {
+ c->setApproach(_approach);
+ } else if (_destinationRunway) {
+ c->setDestination(_destinationRunway);
+ } else if (_destination) {
+ c->setDestination(_destination);
+ }
+
+ c->setSTAR(_star);
+ c->setSID(_sid);
+
+// copy legs
+ for (int l=0; l < numLegs(); ++l) {
+ c->_legs.push_back(_legs[l]->cloneFor(c));
+ }
+
+ return c;
+}
+
+void FlightPlan::setIdent(const string& s)
+{
+ _ident = s;
+}
+
+string FlightPlan::ident() const
+{
+ return _ident;
+}
+
+FlightPlan::Leg* FlightPlan::insertWayptAtIndex(Waypt* aWpt, int aIndex)
+{
+ if (!aWpt) {
+ return NULL;
+ }
+
+ WayptVec wps;
+ wps.push_back(aWpt);
+
+ int index = aIndex;
+ if ((aIndex == -1) || (aIndex > (int) _legs.size())) {
+ index = _legs.size();
+ }
+
+ insertWayptsAtIndex(wps, index);
+ return legAtIndex(aIndex);
+}
+
+void FlightPlan::insertWayptsAtIndex(const WayptVec& wps, int aIndex)
+{
+ if (wps.empty()) {
+ return;
+ }
+
+ int index = aIndex;
+ if ((aIndex == -1) || (aIndex > (int) _legs.size())) {
+ index = _legs.size();
+ }
+
+ LegVec::iterator it = _legs.begin();
+ it += index;
+
+ int endIndex = index + wps.size() - 1;
+ if (_currentIndex >= endIndex) {
+ _currentIndex += wps.size();
+ }
+
+ LegVec newLegs;
+ BOOST_FOREACH(WayptRef wp, wps) {
+ newLegs.push_back(new Leg(this, wp));
+ }
+
+ _legs.insert(it, newLegs.begin(), newLegs.end());
+ rebuildLegData();
+
+ if (_delegate) {
+ _delegate->runWaypointsChanged();
+ }
+}
+
+void FlightPlan::deleteIndex(int aIndex)
+{
+ int index = aIndex;
+ if (aIndex < 0) { // negative indices count the the end
+ index = _legs.size() + index;
+ }
+
+ if ((index < 0) || (index >= numLegs())) {
+ SG_LOG(SG_AUTOPILOT, SG_WARN, "removeAtIndex with invalid index:" << aIndex);
+ return;
+ }
+ LegVec::iterator it = _legs.begin();
+ it += index;
+ Leg* l = *it;
+ _legs.erase(it);
+ delete l;
+
+ bool curChanged = false;
+ if (_currentIndex == index) {
+ // current waypoint was removed
+ curChanged = true;
+ } else if (_currentIndex > index) {
+ --_currentIndex; // shift current index down if necessary
+ }
+
+ rebuildLegData();
+ if (_delegate) {
+ _delegate->runWaypointsChanged();
+ if (curChanged) {
+ _delegate->runCurrentWaypointChanged();
+ }
+ }
+}
+
+void FlightPlan::clear()
+{
+ _currentIndex = -1;
+ BOOST_FOREACH(Leg* l, _legs) {
+ delete l;
+ }
+ _legs.clear();
+ rebuildLegData();
+ if (_delegate) {
+ _delegate->runDepartureChanged();
+ _delegate->runArrivalChanged();
+ _delegate->runWaypointsChanged();
+ _delegate->runCurrentWaypointChanged();
+ }
+}
+
+int FlightPlan::clearWayptsWithFlag(WayptFlag flag)
+{
+ int count = 0;
+ for (unsigned int i=0; i<_legs.size(); ++i) {
+ Leg* l = _legs[i];
+ if (!l->waypoint()->flag(flag)) {
+ continue;
+ }
+
+ // okay, we're going to clear this leg
+ ++count;
+ if (_currentIndex > (int) i) {
+ --_currentIndex;
+ }
+
+ delete l;
+ LegVec::iterator it = _legs.begin();
+ it += i;
+ _legs.erase(it);
+ }
+
+ if (count == 0) {
+ return 0; // nothing was cleared, don't fire the delegate
+ }
+
+ rebuildLegData();
+ if (_delegate) {
+ _delegate->runWaypointsChanged();
+ _delegate->runCurrentWaypointChanged();
+ }
+
+ return count;
+}
+
+void FlightPlan::setCurrentIndex(int index)
+{
+ if ((index < -1) || (index >= numLegs())) {
+ throw sg_range_exception("invalid leg index", "FlightPlan::setCurrentIndex");
+ }
+
+ if (index == _currentIndex) {
+ return;
+ }
+
+ _currentIndex = index;
+ if (_delegate) {
+ _delegate->runCurrentWaypointChanged();
+ }
+}
+
+int FlightPlan::findWayptIndex(const SGGeod& aPos) const
+{
+ for (int i=0; i<numLegs(); ++i) {
+ if (_legs[i]->waypoint()->matches(aPos)) {
+ return i;
+ }
+ }
+
+ return -1;
+}
+
+FlightPlan::Leg* FlightPlan::currentLeg() const
+{
+ if ((_currentIndex < 0) || (_currentIndex >= numLegs()))
+ return NULL;
+ return legAtIndex(_currentIndex);
+}
+
+FlightPlan::Leg* FlightPlan::previousLeg() const
+{
+ if (_currentIndex == 0) {
+ return NULL;
+ }
+
+ return legAtIndex(_currentIndex - 1);
+}
+
+FlightPlan::Leg* FlightPlan::nextLeg() const
+{
+ if ((_currentIndex < 0) || ((_currentIndex + 1) >= numLegs())) {
+ return NULL;
+ }
+
+ return legAtIndex(_currentIndex + 1);
+}
+
+FlightPlan::Leg* FlightPlan::legAtIndex(int index) const
+{
+ if ((index < 0) || (index >= numLegs())) {
+ throw sg_range_exception("index out of range", "FlightPlan::legAtIndex");
+ }
+
+ return _legs[index];
+}
+
+int FlightPlan::findLegIndex(const Leg *l) const
+{
+ for (unsigned int i=0; i<_legs.size(); ++i) {
+ if (_legs[i] == l) {
+ return i;
+ }
+ }
+
+ return -1;
+}
+
+void FlightPlan::setDeparture(FGAirport* apt)
+{
+ if (apt == _departure) {
+ return;
+ }
+
+ _departure = apt;
+ _departureRunway = NULL;
+ setSID((SID*)NULL);
+
+ if (_delegate) {
+ _delegate->runDepartureChanged();
+ }
+}
+
+void FlightPlan::setDeparture(FGRunway* rwy)
+{
+ if (_departureRunway == rwy) {
+ return;
+ }
+
+ _departureRunway = rwy;
+ if (rwy->airport() != _departure) {
+ _departure = rwy->airport();
+ setSID((SID*)NULL);
+ }
+
+ if (_delegate) {
+ _delegate->runDepartureChanged();
+ }
+}
+
+void FlightPlan::setSID(SID* sid, const std::string& transition)
+{
+ if (sid == _sid) {
+ return;
+ }
+
+ _sid = sid;
+ _sidTransition = transition;
+
+ if (_delegate) {
+ _delegate->runDepartureChanged();
+ }
+}
+
+void FlightPlan::setSID(Transition* trans)
+{
+ if (!trans) {
+ setSID((SID*) NULL);
+ return;
+ }
+
+ if (trans->parent()->type() != PROCEDURE_SID)
+ throw sg_exception("FlightPlan::setSID: transition does not belong to a SID");
+
+ setSID((SID*) trans->parent(), trans->ident());
+}
+
+Transition* FlightPlan::sidTransition() const
+{
+ if (!_sid || _sidTransition.empty()) {
+ return NULL;
+ }
+
+ return _sid->findTransitionByName(_sidTransition);
+}
+
+void FlightPlan::setDestination(FGAirport* apt)
+{
+ if (apt == _destination) {
+ return;
+ }
+
+ _destination = apt;
+ _destinationRunway = NULL;
+ setSTAR((STAR*)NULL);
+
+ if (_delegate) {
+ _delegate->runArrivalChanged();
+ }
+}
+
+void FlightPlan::setDestination(FGRunway* rwy)
+{
+ if (_destinationRunway == rwy) {
+ return;
+ }
+
+ _destinationRunway = rwy;
+ if (_destination != rwy->airport()) {
+ _destination = rwy->airport();
+ setSTAR((STAR*)NULL);
+ }
+
+ if (_delegate) {
+ _delegate->runArrivalChanged();
+ }
+}
+
+void FlightPlan::setSTAR(STAR* star, const std::string& transition)
+{
+ if (_star == star) {
+ return;
+ }
+
+ _star = star;
+ _starTransition = transition;
+
+ if (_delegate) {
+ _delegate->runArrivalChanged();
+ }
+}
+
+void FlightPlan::setSTAR(Transition* trans)
+{
+ if (!trans) {
+ setSTAR((STAR*) NULL);
+ return;
+ }
+
+ if (trans->parent()->type() != PROCEDURE_STAR)
+ throw sg_exception("FlightPlan::setSTAR: transition does not belong to a STAR");
+
+ setSTAR((STAR*) trans->parent(), trans->ident());
+}
+
+Transition* FlightPlan::starTransition() const
+{
+ if (!_star || _starTransition.empty()) {
+ return NULL;
+ }
+
+ return _star->findTransitionByName(_starTransition);
+}
+
+void FlightPlan::setApproach(flightgear::Approach *app)
+{
+ if (_approach == app) {
+ return;
+ }
+
+ _approach = app;
+ if (app) {
+ // keep runway + airport in sync
+ if (_destinationRunway != _approach->runway()) {
+ _destinationRunway = _approach->runway();
+ }
+
+ if (_destination != _destinationRunway->airport()) {
+ _destination = _destinationRunway->airport();
+ }
+ }
+
+ if (_delegate) {
+ _delegate->runArrivalChanged();
+ }
+}
+
+bool FlightPlan::save(const SGPath& path)
+{
+ SG_LOG(SG_IO, SG_INFO, "Saving route to " << path.str());
+ try {
+ SGPropertyNode_ptr d(new SGPropertyNode);
+ d->setIntValue("version", 2);
+
+ if (_departure) {
+ d->setStringValue("departure/airport", _departure->ident());
+ if (_sid) {
+ d->setStringValue("departure/sid", _sid->ident());
+ }
+
+ if (_departureRunway) {
+ d->setStringValue("departure/runway", _departureRunway->ident());
+ }
+ }
+
+ if (_destination) {
+ d->setStringValue("destination/airport", _destination->ident());
+ if (_star) {
+ d->setStringValue("destination/star", _star->ident());
+ }
+
+ if (_approach) {
+ d->setStringValue("destination/approach", _approach->ident());
+ }
+
+ //d->setStringValue("destination/transition", destination->getStringValue("transition"));
+
+ if (_destinationRunway) {
+ d->setStringValue("destination/runway", _destinationRunway->ident());
+ }
+ }
+
+ // route nodes
+ SGPropertyNode* routeNode = d->getChild("route", 0, true);
+ for (unsigned int i=0; i<_legs.size(); ++i) {
+ Waypt* wpt = _legs[i]->waypoint();
+ wpt->saveAsNode(routeNode->getChild("wp", i, true));
+ } // of waypoint iteration
+ writeProperties(path.str(), d, true /* write-all */);
+ return true;
+ } catch (sg_exception& e) {
+ SG_LOG(SG_IO, SG_ALERT, "Failed to save flight-plan '" << path.str() << "'. " << e.getMessage());
+ return false;
+ }
+}
+
+bool FlightPlan::load(const SGPath& path)
+{
+ if (!path.exists())
+ {
+ SG_LOG(SG_IO, SG_ALERT, "Failed to load flight-plan '" << path.str()
+ << "'. The file does not exist.");
+ return false;
+ }
+
+ SGPropertyNode_ptr routeData(new SGPropertyNode);
+ SG_LOG(SG_IO, SG_INFO, "going to read flight-plan from:" << path.str());
+
+ bool Status = false;
+ try {
+ readProperties(path.str(), routeData);
+ } catch (sg_exception& ) {
+ // if XML parsing fails, the file might be simple textual list of waypoints
+ Status = loadPlainTextRoute(path);
+ routeData = 0;
+ }
+
+ if (routeData.valid())
+ {
+ try {
+ 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");
+ }
+ Status = true;
+ } catch (sg_exception& e) {
+ SG_LOG(SG_IO, SG_ALERT, "Failed to load flight-plan '" << e.getOrigin()
+ << "'. " << e.getMessage());
+ Status = false;
+ }
+ }
+
+ rebuildLegData();
+ if (_delegate) {
+ _delegate->runWaypointsChanged();
+ }
+
+ return Status;
+}
+
+void FlightPlan::loadXMLRouteHeader(SGPropertyNode_ptr routeData)
+{
+ // departure nodes
+ SGPropertyNode* dep = routeData->getChild("departure");
+ if (dep) {
+ string depIdent = dep->getStringValue("airport");
+ setDeparture((FGAirport*) fgFindAirportID(depIdent));
+ if (_departure) {
+ if (dep->hasChild("runway")) {
+ setDeparture(_departure->getRunwayByIdent(dep->getStringValue("runway")));
+ }
+
+ if (dep->hasChild("sid")) {
+ setSID(_departure->findSIDWithIdent(dep->getStringValue("sid")));
+ }
+ // departure->setStringValue("transition", dep->getStringValue("transition"));
+ }
+ }
+
+ // destination
+ SGPropertyNode* dst = routeData->getChild("destination");
+ if (dst) {
+ setDestination((FGAirport*) fgFindAirportID(dst->getStringValue("airport")));
+ if (_destination) {
+ if (dst->hasChild("runway")) {
+ setDestination(_destination->getRunwayByIdent(dst->getStringValue("runway")));
+ }
+
+ if (dst->hasChild("star")) {
+ setSTAR(_destination->findSTARWithIdent(dst->getStringValue("star")));
+ }
+
+ if (dst->hasChild("approach")) {
+ setApproach(_destination->findApproachWithIdent(dst->getStringValue("approach")));
+ }
+ }
+
+ // destination->setStringValue("transition", dst->getStringValue("transition"));
+ }
+
+ // alternate
+ SGPropertyNode* alt = routeData->getChild("alternate");
+ if (alt) {
+ //alternate->setStringValue(alt->getStringValue("airport"));
+ }
+
+ // cruise
+ SGPropertyNode* crs = routeData->getChild("cruise");
+ if (crs) {
+ // cruise->setDoubleValue("speed-kts", crs->getDoubleValue("speed-kts"));
+ // cruise->setDoubleValue("mach", crs->getDoubleValue("mach"));
+ // cruise->setDoubleValue("altitude-ft", crs->getDoubleValue("altitude-ft"));
+ } // of cruise data loading
+
+}
+
+void FlightPlan::loadVersion2XMLRoute(SGPropertyNode_ptr routeData)
+{
+ loadXMLRouteHeader(routeData);
+
+ // route nodes
+ _legs.clear();
+ SGPropertyNode_ptr routeNode = routeData->getChild("route", 0);
+ for (int i=0; i<routeNode->nChildren(); ++i) {
+ SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i);
+ Leg* l = new Leg(this, Waypt::createFromProperties(NULL, wpNode));
+ _legs.push_back(l);
+ } // of route iteration
+}
+
+void FlightPlan::loadVersion1XMLRoute(SGPropertyNode_ptr routeData)
+{
+ loadXMLRouteHeader(routeData);
+
+ // _legs nodes
+ _legs.clear();
+ SGPropertyNode_ptr routeNode = routeData->getChild("route", 0);
+ for (int i=0; i<routeNode->nChildren(); ++i) {
+ SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i);
+ Leg* l = new Leg(this, parseVersion1XMLWaypt(wpNode));
+ _legs.push_back(l);
+ } // of route iteration
+
+}
+
+WayptRef FlightPlan::parseVersion1XMLWaypt(SGPropertyNode* aWP)
+{
+ SGGeod lastPos;
+ if (!_legs.empty()) {
+ lastPos = _legs.back()->waypoint()->position();
+ } else if (_departure) {
+ lastPos = _departure->geod();
+ }
+
+ WayptRef w;
+ string ident(aWP->getStringValue("ident"));
+ if (aWP->hasChild("longitude-deg")) {
+ // explicit longitude/latitude
+ 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");
+ // convert magnetic radial to a true radial!
+ radialDeg += magvarDegAt(pos);
+ double offsetNm = aWP->getDoubleValue("offset-nm");
+ double az2;
+ SGGeodesy::direct(p->geod(), radialDeg, offsetNm * SG_NM_TO_METER, pos, az2);
+ }
+
+ 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;
+}
+
+bool FlightPlan::loadPlainTextRoute(const SGPath& path)
+{
+ try {
+ sg_gzifstream in(path.str().c_str());
+ if (!in.is_open()) {
+ throw sg_io_exception("Cannot open file for reading.");
+ }
+
+ _legs.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);
+ }
+
+ 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 + "'.");
+ }
+
+ _legs.push_back(new Leg(this, w));
+ } // of line iteration
+ } catch (sg_exception& e) {
+ SG_LOG(SG_IO, SG_ALERT, "Failed to load route from: '" << path.str() << "'. " << e.getMessage());
+ _legs.clear();
+ return false;
+ }
+
+ return true;
+}
+
+double FlightPlan::magvarDegAt(const SGGeod& pos) const
+{
+ double jd = globals->get_time_params()->getJD();
+ return sgGetMagVar(pos, jd) * SG_RADIANS_TO_DEGREES;
+}
+
+WayptRef FlightPlan::waypointFromString(const string& tgt )
+{
+ string target(boost::to_upper_copy(tgt));
+ WayptRef wpt;
+
+ // extract altitude
+ double altFt = 0.0;
+ 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));
+
+ wpt = new BasicWaypt(SGGeod::fromDeg(lon, lat), buf, NULL);
+ if (altSetting != RESTRICT_NONE) {
+ wpt->setAltitude(altFt, altSetting);
+ }
+ return wpt;
+ }
+
+ SGGeod basePosition;
+ if (_legs.empty()) {
+ // route is empty, use current position
+ basePosition = globals->get_aircraft_position();
+ } else {
+ basePosition = _legs.back()->waypoint()->position();
+ }
+
+ 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;
+ }
+
+ double magvar = magvarDegAt(basePosition);
+
+ 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;
+ 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;
+ }
+
+ 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]);
+ 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;
+ r2 += magvar;
+
+ 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;
+}
+
+FlightPlan::Leg::Leg(FlightPlan* owner, WayptRef wpt) :
+ _parent(owner),
+ _speedRestrict(RESTRICT_NONE),
+ _altRestrict(RESTRICT_NONE),
+ _waypt(wpt)
+{
+ if (!wpt.valid()) {
+ throw sg_exception("can't create FlightPlan::Leg without underlying waypoint");
+ }
+ _speed = _altitudeFt = 0;
+}
+
+FlightPlan::Leg* FlightPlan::Leg::cloneFor(FlightPlan* owner) const
+{
+ Leg* c = new Leg(owner, _waypt);
+// clone local data
+ c->_speed = _speed;
+ c->_speedRestrict = _speedRestrict;
+ c->_altitudeFt = _altitudeFt;
+ c->_altRestrict = _altRestrict;
+
+ return c;
+}
+
+FlightPlan::Leg* FlightPlan::Leg::nextLeg() const
+{
+ return _parent->legAtIndex(index() + 1);
+}
+
+unsigned int FlightPlan::Leg::index() const
+{
+ return _parent->findLegIndex(this);
+}
+
+int FlightPlan::Leg::altitudeFt() const
+{
+ if (_altRestrict != RESTRICT_NONE) {
+ return _altitudeFt;
+ }
+
+ return _waypt->altitudeFt();
+}
+
+int FlightPlan::Leg::speed() const
+{
+ if (_speedRestrict != RESTRICT_NONE) {
+ return _speed;
+ }
+
+ return _waypt->speed();
+}
+
+int FlightPlan::Leg::speedKts() const
+{
+ return speed();
+}
+
+double FlightPlan::Leg::speedMach() const
+{
+ if (!isMachRestrict(_speedRestrict)) {
+ return 0.0;
+ }
+
+ return -(_speed / 100.0);
+}
+
+RouteRestriction FlightPlan::Leg::altitudeRestriction() const
+{
+ if (_altRestrict != RESTRICT_NONE) {
+ return _altRestrict;
+ }
+
+ return _waypt->altitudeRestriction();
+}
+
+RouteRestriction FlightPlan::Leg::speedRestriction() const
+{
+ if (_speedRestrict != RESTRICT_NONE) {
+ return _speedRestrict;
+ }
+
+ return _waypt->speedRestriction();
+}
+
+void FlightPlan::Leg::setSpeed(RouteRestriction ty, double speed)
+{
+ _speedRestrict = ty;
+ if (isMachRestrict(ty)) {
+ _speed = (speed * -100);
+ } else {
+ _speed = speed;
+ }
+}
+
+void FlightPlan::Leg::setAltitude(RouteRestriction ty, int altFt)
+{
+ _altRestrict = ty;
+ _altitudeFt = altFt;
+}
+
+double FlightPlan::Leg::courseDeg() const
+{
+ return _courseDeg;
+}
+
+double FlightPlan::Leg::distanceNm() const
+{
+ return _pathDistance;
+}
+
+double FlightPlan::Leg::distanceAlongRoute() const
+{
+ return _distanceAlongPath;
+}
+
+void FlightPlan::rebuildLegData()
+{
+ _totalDistance = 0.0;
+ int lastLeg = static_cast<int>(_legs.size()) - 1;
+ for (int l=0; l<lastLeg; ++l) {
+ Leg* cur = _legs[l];
+ Leg* next = _legs[l + 1];
+
+ std::pair<double, double> crsDist =
+ next->waypoint()->courseAndDistanceFrom(cur->waypoint()->position());
+ _legs[l]->_courseDeg = crsDist.first;
+ _legs[l]->_pathDistance = crsDist.second * SG_METER_TO_NM;
+ _legs[l]->_distanceAlongPath = _totalDistance;
+ _totalDistance += crsDist.second * SG_METER_TO_NM;
+ } // of legs iteration
+}
+
+void FlightPlan::setDelegate(Delegate* d)
+{
+ // wrap any existing delegate(s) in the new one
+ d->_inner = _delegate;
+ _delegate = d;
+}
+
+void FlightPlan::removeDelegate(Delegate* d)
+{
+ if (d == _delegate) {
+ _delegate = _delegate->_inner;
+ } else if (_delegate) {
+ _delegate->removeInner(d);
+ }
+}
+
+FlightPlan::Delegate::Delegate() :
+ _inner(NULL)
+{
+
+}
+
+FlightPlan::Delegate::~Delegate()
+{
+
+}
+
+void FlightPlan::Delegate::removeInner(Delegate* d)
+{
+ if (!_inner) {
+ return;
+ }
+
+ if (_inner == d) {
+ // replace with grand-child
+ _inner = d->_inner;
+ } else { // recurse downwards
+ _inner->removeInner(d);
+ }
+}
+
+void FlightPlan::Delegate::runDepartureChanged()
+{
+ if (_inner) _inner->runDepartureChanged();
+ departureChanged();
+}
+
+void FlightPlan::Delegate::runArrivalChanged()
+{
+ if (_inner) _inner->runArrivalChanged();
+ arrivalChanged();
+}
+
+void FlightPlan::Delegate::runWaypointsChanged()
+{
+ if (_inner) _inner->runWaypointsChanged();
+ waypointsChanged();
+}
+
+void FlightPlan::Delegate::runCurrentWaypointChanged()
+{
+ if (_inner) _inner->runCurrentWaypointChanged();
+ currentWaypointChanged();
+}
+
+} // of namespace flightgear
--- /dev/null
+/**
+ * FlightPlan.hxx - defines a full flight-plan object, including
+ * departure, cruise, arrival information and waypoints
+ */
+
+// Written by James Turner, started 2012.
+//
+// Copyright (C) 2012 James Turner
+//
+// 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_FLIGHTPLAN_HXX
+#define FG_FLIGHTPLAN_HXX
+
+#include <Navaids/route.hxx>
+#include <Airports/simple.hxx>
+
+typedef SGSharedPtr<FGAirport> FGAirportRef;
+
+namespace flightgear
+{
+
+class Transition;
+
+class FlightPlan : public RouteBase
+{
+public:
+ FlightPlan();
+ virtual ~FlightPlan();
+
+ virtual std::string ident() const;
+ void setIdent(const std::string& s);
+
+ FlightPlan* clone(const std::string& newIdent = std::string()) const;
+
+ /**
+ * flight-plan leg encapsulation
+ */
+ class Leg
+ {
+ public:
+ FlightPlan* owner() const
+ { return _parent; }
+
+ Waypt* waypoint() const
+ { return _waypt; }
+
+ // reutrn the next leg after this one
+ Leg* nextLeg() const;
+
+ unsigned int index() const;
+
+ int altitudeFt() const;
+ int speed() const;
+
+ int speedKts() const;
+ double speedMach() const;
+
+ RouteRestriction altitudeRestriction() const;
+ RouteRestriction speedRestriction() const;
+
+ void setSpeed(RouteRestriction ty, double speed);
+ void setAltitude(RouteRestriction ty, int altFt);
+
+ double courseDeg() const;
+ double distanceNm() const;
+ double distanceAlongRoute() const;
+ private:
+ friend class FlightPlan;
+
+ Leg(FlightPlan* owner, WayptRef wpt);
+
+ Leg* cloneFor(FlightPlan* owner) const;
+
+ FlightPlan* _parent;
+ RouteRestriction _speedRestrict, _altRestrict;
+ int _speed;
+ int _altitudeFt;
+ WayptRef _waypt;
+ /// length of this leg following the flown path
+ mutable double _pathDistance;
+ mutable double _courseDeg;
+ /// total distance of this leg from departure point
+ mutable double _distanceAlongPath;
+ };
+
+ class Delegate
+ {
+ public:
+ virtual ~Delegate();
+
+ virtual void departureChanged() { }
+ virtual void arrivalChanged() { }
+ virtual void waypointsChanged() { }
+
+ virtual void currentWaypointChanged() { }
+
+ protected:
+ Delegate();
+
+ private:
+ void removeInner(Delegate* d);
+
+ void runDepartureChanged();
+ void runArrivalChanged();
+ void runWaypointsChanged();
+ void runCurrentWaypointChanged();
+
+ friend class FlightPlan;
+
+ Delegate* _inner;
+ };
+
+ Leg* insertWayptAtIndex(Waypt* aWpt, int aIndex);
+ void insertWayptsAtIndex(const WayptVec& wps, int aIndex);
+
+ void deleteIndex(int index);
+ void clear();
+ int clearWayptsWithFlag(WayptFlag flag);
+
+ int currentIndex() const
+ { return _currentIndex; }
+
+ void setCurrentIndex(int index);
+
+ Leg* currentLeg() const;
+ Leg* nextLeg() const;
+ Leg* previousLeg() const;
+
+ int numLegs() const
+ { return _legs.size(); }
+
+ Leg* legAtIndex(int index) const;
+ int findLegIndex(const Leg* l) const;
+
+ int findWayptIndex(const SGGeod& aPos) const;
+
+ bool load(const SGPath& p);
+ bool save(const SGPath& p);
+
+ FGAirportRef departureAirport() const
+ { return _departure; }
+
+ FGAirportRef destinationAirport() const
+ { return _destination; }
+
+ FGRunway* departureRunway() const
+ { return _departureRunway; }
+
+ FGRunway* destinationRunway() const
+ { return _destinationRunway; }
+
+ Approach* approach() const
+ { return _approach; }
+
+ void setDeparture(FGAirport* apt);
+ void setDeparture(FGRunway* rwy);
+
+ SID* sid() const
+ { return _sid; }
+
+ Transition* sidTransition() const;
+
+ void setSID(SID* sid, const std::string& transition = std::string());
+
+ void setSID(Transition* sidWithTrans);
+
+ void setDestination(FGAirport* apt);
+ void setDestination(FGRunway* rwy);
+
+ /**
+ * note setting an approach will implicitly update the destination
+ * airport and runway to match
+ */
+ void setApproach(Approach* app);
+
+ STAR* star() const
+ { return _star; }
+
+ Transition* starTransition() const;
+
+ void setSTAR(STAR* star, const std::string& transition = std::string());
+
+ void setSTAR(Transition* starWithTrans);
+
+ double totalDistanceNm() const
+ { return _totalDistance; }
+
+ /**
+ * Create a WayPoint from a string in the following format:
+ * - simple identifier
+ * - decimal-lon,decimal-lat
+ * - airport-id/runway-id
+ * - navaid/radial-deg/offset-nm
+ */
+ WayptRef waypointFromString(const std::string& target);
+
+ void setDelegate(Delegate* d);
+ void removeDelegate(Delegate* d);
+private:
+
+ bool loadPlainTextRoute(const SGPath& path);
+
+ void loadVersion1XMLRoute(SGPropertyNode_ptr routeData);
+ void loadVersion2XMLRoute(SGPropertyNode_ptr routeData);
+ void loadXMLRouteHeader(SGPropertyNode_ptr routeData);
+ WayptRef parseVersion1XMLWaypt(SGPropertyNode* aWP);
+
+ double magvarDegAt(const SGGeod& pos) const;
+
+ std::string _ident;
+ int _currentIndex;
+
+ FGAirportRef _departure, _destination;
+ FGRunway* _departureRunway, *_destinationRunway;
+ SID* _sid;
+ STAR* _star;
+ Approach* _approach;
+ std::string _sidTransition, _starTransition;
+
+ double _totalDistance;
+ void rebuildLegData();
+
+ typedef std::vector<Leg*> LegVec;
+ LegVec _legs;
+
+ Delegate* _delegate;
+};
+
+} // of namespace flightgear
+
+#endif // of FG_FLIGHTPLAN_HXX
"\n\t" << ex.getMessage());
}
}
-
-////////////////////////////////////////////////////////////////////////////
-
-FlightPlan::FlightPlan() :
- _currentIndex(-1),
- _departureRunway(NULL),
- _destinationRunway(NULL),
- _sid(NULL),
- _star(NULL),
- _approach(NULL),
- _delegate(NULL)
-{
-
-}
-
-FlightPlan::~FlightPlan()
-{
-
-}
-
-FlightPlan* FlightPlan::clone(const string& newIdent) const
-{
- FlightPlan* c = new FlightPlan();
- c->_ident = newIdent.empty() ? _ident : newIdent;
-
-// copy destination / departure data.
- c->setDeparture(_departure);
- c->setDeparture(_departureRunway);
-
- if (_approach) {
- c->setApproach(_approach);
- } else if (_destinationRunway) {
- c->setDestination(_destinationRunway);
- } else if (_destination) {
- c->setDestination(_destination);
- }
-
- c->setSTAR(_star);
- c->setSID(_sid);
-
-// copy legs
- for (int l=0; l < numLegs(); ++l) {
- c->_legs.push_back(_legs[l]->cloneFor(c));
- }
-
- return c;
-}
-
-void FlightPlan::setIdent(const string& s)
-{
- _ident = s;
-}
-
-string FlightPlan::ident() const
-{
- return _ident;
-}
-
-FlightPlan::Leg* FlightPlan::insertWayptAtIndex(Waypt* aWpt, int aIndex)
-{
- if (!aWpt) {
- return NULL;
- }
-
- WayptVec wps;
- wps.push_back(aWpt);
-
- int index = aIndex;
- if ((aIndex == -1) || (aIndex > (int) _legs.size())) {
- index = _legs.size();
- }
-
- insertWayptsAtIndex(wps, index);
- return legAtIndex(aIndex);
-}
-
-void FlightPlan::insertWayptsAtIndex(const WayptVec& wps, int aIndex)
-{
- if (wps.empty()) {
- return;
- }
-
- int index = aIndex;
- if ((aIndex == -1) || (aIndex > (int) _legs.size())) {
- index = _legs.size();
- }
-
- LegVec::iterator it = _legs.begin();
- it += index;
-
- int endIndex = index + wps.size() - 1;
- if (_currentIndex >= endIndex) {
- _currentIndex += wps.size();
- }
-
- LegVec newLegs;
- BOOST_FOREACH(WayptRef wp, wps) {
- newLegs.push_back(new Leg(this, wp));
- }
-
- _legs.insert(it, newLegs.begin(), newLegs.end());
- rebuildLegData();
-
- if (_delegate) {
- _delegate->runWaypointsChanged();
- }
-}
-
-void FlightPlan::deleteIndex(int aIndex)
-{
- int index = aIndex;
- if (aIndex < 0) { // negative indices count the the end
- index = _legs.size() + index;
- }
-
- if ((index < 0) || (index >= numLegs())) {
- SG_LOG(SG_AUTOPILOT, SG_WARN, "removeAtIndex with invalid index:" << aIndex);
- return;
- }
- LegVec::iterator it = _legs.begin();
- it += index;
- Leg* l = *it;
- _legs.erase(it);
- delete l;
-
- bool curChanged = false;
- if (_currentIndex == index) {
- // current waypoint was removed
- curChanged = true;
- } else if (_currentIndex > index) {
- --_currentIndex; // shift current index down if necessary
- }
-
- rebuildLegData();
- if (_delegate) {
- _delegate->runWaypointsChanged();
- if (curChanged) {
- _delegate->runCurrentWaypointChanged();
- }
- }
-}
-
-void FlightPlan::clear()
-{
- _currentIndex = -1;
- BOOST_FOREACH(Leg* l, _legs) {
- delete l;
- }
- _legs.clear();
- rebuildLegData();
- if (_delegate) {
- _delegate->runDepartureChanged();
- _delegate->runArrivalChanged();
- _delegate->runWaypointsChanged();
- _delegate->runCurrentWaypointChanged();
- }
-}
-
-int FlightPlan::clearWayptsWithFlag(WayptFlag flag)
-{
- int count = 0;
- for (unsigned int i=0; i<_legs.size(); ++i) {
- Leg* l = _legs[i];
- if (!l->waypoint()->flag(flag)) {
- continue;
- }
-
- // okay, we're going to clear this leg
- ++count;
- if (_currentIndex > (int) i) {
- --_currentIndex;
- }
-
- delete l;
- LegVec::iterator it = _legs.begin();
- it += i;
- _legs.erase(it);
- }
-
- if (count == 0) {
- return 0; // nothing was cleared, don't fire the delegate
- }
-
- rebuildLegData();
- if (_delegate) {
- _delegate->runWaypointsChanged();
- _delegate->runCurrentWaypointChanged();
- }
-
- return count;
-}
-
-void FlightPlan::setCurrentIndex(int index)
-{
- if ((index < -1) || (index >= numLegs())) {
- throw sg_range_exception("invalid leg index", "FlightPlan::setCurrentIndex");
- }
-
- if (index == _currentIndex) {
- return;
- }
-
- _currentIndex = index;
- if (_delegate) {
- _delegate->runCurrentWaypointChanged();
- }
-}
-
-int FlightPlan::findWayptIndex(const SGGeod& aPos) const
-{
- for (int i=0; i<numLegs(); ++i) {
- if (_legs[i]->waypoint()->matches(aPos)) {
- return i;
- }
- }
-
- return -1;
-}
-
-FlightPlan::Leg* FlightPlan::currentLeg() const
-{
- if ((_currentIndex < 0) || (_currentIndex >= numLegs()))
- return NULL;
- return legAtIndex(_currentIndex);
-}
-
-FlightPlan::Leg* FlightPlan::previousLeg() const
-{
- if (_currentIndex == 0) {
- return NULL;
- }
-
- return legAtIndex(_currentIndex - 1);
-}
-
-FlightPlan::Leg* FlightPlan::nextLeg() const
-{
- if ((_currentIndex < 0) || ((_currentIndex + 1) >= numLegs())) {
- return NULL;
- }
-
- return legAtIndex(_currentIndex + 1);
-}
-
-FlightPlan::Leg* FlightPlan::legAtIndex(int index) const
-{
- if ((index < 0) || (index >= numLegs())) {
- throw sg_range_exception("index out of range", "FlightPlan::legAtIndex");
- }
-
- return _legs[index];
-}
-
-int FlightPlan::findLegIndex(const Leg *l) const
-{
- for (unsigned int i=0; i<_legs.size(); ++i) {
- if (_legs[i] == l) {
- return i;
- }
- }
-
- return -1;
-}
-
-void FlightPlan::setDeparture(FGAirport* apt)
-{
- if (apt == _departure) {
- return;
- }
-
- _departure = apt;
- _departureRunway = NULL;
- setSID((SID*)NULL);
-
- if (_delegate) {
- _delegate->runDepartureChanged();
- }
-}
-
-void FlightPlan::setDeparture(FGRunway* rwy)
-{
- if (_departureRunway == rwy) {
- return;
- }
-
- _departureRunway = rwy;
- if (rwy->airport() != _departure) {
- _departure = rwy->airport();
- setSID((SID*)NULL);
- }
-
- if (_delegate) {
- _delegate->runDepartureChanged();
- }
-}
-
-void FlightPlan::setSID(SID* sid, const std::string& transition)
-{
- if (sid == _sid) {
- return;
- }
-
- _sid = sid;
- _sidTransition = transition;
-
- if (_delegate) {
- _delegate->runDepartureChanged();
- }
-}
-
-void FlightPlan::setSID(Transition* trans)
-{
- if (!trans) {
- setSID((SID*) NULL);
- return;
- }
-
- if (trans->parent()->type() != PROCEDURE_SID)
- throw sg_exception("FlightPlan::setSID: transition does not belong to a SID");
-
- setSID((SID*) trans->parent(), trans->ident());
-}
-
-Transition* FlightPlan::sidTransition() const
-{
- if (!_sid || _sidTransition.empty()) {
- return NULL;
- }
-
- return _sid->findTransitionByName(_sidTransition);
-}
-
-void FlightPlan::setDestination(FGAirport* apt)
-{
- if (apt == _destination) {
- return;
- }
-
- _destination = apt;
- _destinationRunway = NULL;
- setSTAR((STAR*)NULL);
-
- if (_delegate) {
- _delegate->runArrivalChanged();
- }
-}
-
-void FlightPlan::setDestination(FGRunway* rwy)
-{
- if (_destinationRunway == rwy) {
- return;
- }
-
- _destinationRunway = rwy;
- if (_destination != rwy->airport()) {
- _destination = rwy->airport();
- setSTAR((STAR*)NULL);
- }
-
- if (_delegate) {
- _delegate->runArrivalChanged();
- }
-}
-
-void FlightPlan::setSTAR(STAR* star, const std::string& transition)
-{
- if (_star == star) {
- return;
- }
-
- _star = star;
- _starTransition = transition;
-
- if (_delegate) {
- _delegate->runArrivalChanged();
- }
-}
-
-void FlightPlan::setSTAR(Transition* trans)
-{
- if (!trans) {
- setSTAR((STAR*) NULL);
- return;
- }
-
- if (trans->parent()->type() != PROCEDURE_STAR)
- throw sg_exception("FlightPlan::setSTAR: transition does not belong to a STAR");
-
- setSTAR((STAR*) trans->parent(), trans->ident());
-}
-
-Transition* FlightPlan::starTransition() const
-{
- if (!_star || _starTransition.empty()) {
- return NULL;
- }
-
- return _star->findTransitionByName(_starTransition);
-}
-
-void FlightPlan::setApproach(flightgear::Approach *app)
-{
- if (_approach == app) {
- return;
- }
-
- _approach = app;
- if (app) {
- // keep runway + airport in sync
- if (_destinationRunway != _approach->runway()) {
- _destinationRunway = _approach->runway();
- }
-
- if (_destination != _destinationRunway->airport()) {
- _destination = _destinationRunway->airport();
- }
- }
-
- if (_delegate) {
- _delegate->runArrivalChanged();
- }
-}
-
-bool FlightPlan::save(const SGPath& path)
-{
- SG_LOG(SG_IO, SG_INFO, "Saving route to " << path.str());
- try {
- SGPropertyNode_ptr d(new SGPropertyNode);
- d->setIntValue("version", 2);
-
- if (_departure) {
- d->setStringValue("departure/airport", _departure->ident());
- if (_sid) {
- d->setStringValue("departure/sid", _sid->ident());
- }
-
- if (_departureRunway) {
- d->setStringValue("departure/runway", _departureRunway->ident());
- }
- }
-
- if (_destination) {
- d->setStringValue("destination/airport", _destination->ident());
- if (_star) {
- d->setStringValue("destination/star", _star->ident());
- }
-
- if (_approach) {
- d->setStringValue("destination/approach", _approach->ident());
- }
-
- //d->setStringValue("destination/transition", destination->getStringValue("transition"));
-
- if (_destinationRunway) {
- d->setStringValue("destination/runway", _destinationRunway->ident());
- }
- }
-
- // route nodes
- SGPropertyNode* routeNode = d->getChild("route", 0, true);
- for (unsigned int i=0; i<_legs.size(); ++i) {
- Waypt* wpt = _legs[i]->waypoint();
- wpt->saveAsNode(routeNode->getChild("wp", i, true));
- } // of waypoint iteration
- writeProperties(path.str(), d, true /* write-all */);
- return true;
- } catch (sg_exception& e) {
- SG_LOG(SG_IO, SG_ALERT, "Failed to save flight-plan '" << path.str() << "'. " << e.getMessage());
- return false;
- }
-}
-
-bool FlightPlan::load(const SGPath& path)
-{
- if (!path.exists())
- {
- SG_LOG(SG_IO, SG_ALERT, "Failed to load flight-plan '" << path.str()
- << "'. The file does not exist.");
- return false;
- }
-
- SGPropertyNode_ptr routeData(new SGPropertyNode);
- SG_LOG(SG_IO, SG_INFO, "going to read flight-plan from:" << path.str());
-
- bool Status = false;
- try {
- readProperties(path.str(), routeData);
- } catch (sg_exception& ) {
- // if XML parsing fails, the file might be simple textual list of waypoints
- Status = loadPlainTextRoute(path);
- routeData = 0;
- }
-
- if (routeData.valid())
- {
- try {
- 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");
- }
- Status = true;
- } catch (sg_exception& e) {
- SG_LOG(SG_IO, SG_ALERT, "Failed to load flight-plan '" << e.getOrigin()
- << "'. " << e.getMessage());
- Status = false;
- }
- }
-
- rebuildLegData();
- if (_delegate) {
- _delegate->runWaypointsChanged();
- }
-
- return Status;
-}
-
-void FlightPlan::loadXMLRouteHeader(SGPropertyNode_ptr routeData)
-{
- // departure nodes
- SGPropertyNode* dep = routeData->getChild("departure");
- if (dep) {
- string depIdent = dep->getStringValue("airport");
- setDeparture((FGAirport*) fgFindAirportID(depIdent));
- if (_departure) {
- if (dep->hasChild("runway")) {
- setDeparture(_departure->getRunwayByIdent(dep->getStringValue("runway")));
- }
-
- if (dep->hasChild("sid")) {
- setSID(_departure->findSIDWithIdent(dep->getStringValue("sid")));
- }
- // departure->setStringValue("transition", dep->getStringValue("transition"));
- }
- }
-
- // destination
- SGPropertyNode* dst = routeData->getChild("destination");
- if (dst) {
- setDestination((FGAirport*) fgFindAirportID(dst->getStringValue("airport")));
- if (_destination) {
- if (dst->hasChild("runway")) {
- setDestination(_destination->getRunwayByIdent(dst->getStringValue("runway")));
- }
-
- if (dst->hasChild("star")) {
- setSTAR(_destination->findSTARWithIdent(dst->getStringValue("star")));
- }
-
- if (dst->hasChild("approach")) {
- setApproach(_destination->findApproachWithIdent(dst->getStringValue("approach")));
- }
- }
-
- // destination->setStringValue("transition", dst->getStringValue("transition"));
- }
-
- // alternate
- SGPropertyNode* alt = routeData->getChild("alternate");
- if (alt) {
- //alternate->setStringValue(alt->getStringValue("airport"));
- }
-
- // cruise
- SGPropertyNode* crs = routeData->getChild("cruise");
- if (crs) {
- // cruise->setDoubleValue("speed-kts", crs->getDoubleValue("speed-kts"));
- // cruise->setDoubleValue("mach", crs->getDoubleValue("mach"));
- // cruise->setDoubleValue("altitude-ft", crs->getDoubleValue("altitude-ft"));
- } // of cruise data loading
-
-}
-
-void FlightPlan::loadVersion2XMLRoute(SGPropertyNode_ptr routeData)
-{
- loadXMLRouteHeader(routeData);
-
- // route nodes
- _legs.clear();
- SGPropertyNode_ptr routeNode = routeData->getChild("route", 0);
- for (int i=0; i<routeNode->nChildren(); ++i) {
- SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i);
- Leg* l = new Leg(this, Waypt::createFromProperties(NULL, wpNode));
- _legs.push_back(l);
- } // of route iteration
-}
-
-void FlightPlan::loadVersion1XMLRoute(SGPropertyNode_ptr routeData)
-{
- loadXMLRouteHeader(routeData);
-
- // _legs nodes
- _legs.clear();
- SGPropertyNode_ptr routeNode = routeData->getChild("route", 0);
- for (int i=0; i<routeNode->nChildren(); ++i) {
- SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i);
- Leg* l = new Leg(this, parseVersion1XMLWaypt(wpNode));
- _legs.push_back(l);
- } // of route iteration
-
-}
-
-WayptRef FlightPlan::parseVersion1XMLWaypt(SGPropertyNode* aWP)
-{
- SGGeod lastPos;
- if (!_legs.empty()) {
- lastPos = _legs.back()->waypoint()->position();
- } else if (_departure) {
- lastPos = _departure->geod();
- }
-
- WayptRef w;
- string ident(aWP->getStringValue("ident"));
- if (aWP->hasChild("longitude-deg")) {
- // explicit longitude/latitude
- 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");
- // convert magnetic radial to a true radial!
- radialDeg += magvarDegAt(pos);
- double offsetNm = aWP->getDoubleValue("offset-nm");
- double az2;
- SGGeodesy::direct(p->geod(), radialDeg, offsetNm * SG_NM_TO_METER, pos, az2);
- }
-
- 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;
-}
-
-bool FlightPlan::loadPlainTextRoute(const SGPath& path)
-{
- try {
- sg_gzifstream in(path.str().c_str());
- if (!in.is_open()) {
- throw sg_io_exception("Cannot open file for reading.");
- }
-
- _legs.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);
- }
-
- 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 + "'.");
- }
-
- _legs.push_back(new Leg(this, w));
- } // of line iteration
- } catch (sg_exception& e) {
- SG_LOG(SG_IO, SG_ALERT, "Failed to load route from: '" << path.str() << "'. " << e.getMessage());
- _legs.clear();
- return false;
- }
-
- return true;
-}
-
-double FlightPlan::magvarDegAt(const SGGeod& pos) const
-{
- double jd = globals->get_time_params()->getJD();
- return sgGetMagVar(pos, jd) * SG_RADIANS_TO_DEGREES;
-}
-
-WayptRef FlightPlan::waypointFromString(const string& tgt )
-{
- string target(boost::to_upper_copy(tgt));
- WayptRef wpt;
-
- // extract altitude
- double altFt = 0.0;
- 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));
-
- wpt = new BasicWaypt(SGGeod::fromDeg(lon, lat), buf, NULL);
- if (altSetting != RESTRICT_NONE) {
- wpt->setAltitude(altFt, altSetting);
- }
- return wpt;
- }
-
- SGGeod basePosition;
- if (_legs.empty()) {
- // route is empty, use current position
- basePosition = globals->get_aircraft_position();
- } else {
- basePosition = _legs.back()->waypoint()->position();
- }
-
- 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;
- }
-
- double magvar = magvarDegAt(basePosition);
-
- 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;
- 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;
- }
-
- 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]);
- 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;
- r2 += magvar;
-
- 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;
-}
-
-FlightPlan::Leg::Leg(FlightPlan* owner, WayptRef wpt) :
- _parent(owner),
- _speedRestrict(RESTRICT_NONE),
- _altRestrict(RESTRICT_NONE),
- _waypt(wpt)
-{
- if (!wpt.valid()) {
- throw sg_exception("can't create FlightPlan::Leg without underlying waypoint");
- }
- _speed = _altitudeFt = 0;
-}
-
-FlightPlan::Leg* FlightPlan::Leg::cloneFor(FlightPlan* owner) const
-{
- Leg* c = new Leg(owner, _waypt);
-// clone local data
- c->_speed = _speed;
- c->_speedRestrict = _speedRestrict;
- c->_altitudeFt = _altitudeFt;
- c->_altRestrict = _altRestrict;
-
- return c;
-}
-
-FlightPlan::Leg* FlightPlan::Leg::nextLeg() const
-{
- return _parent->legAtIndex(index() + 1);
-}
-
-unsigned int FlightPlan::Leg::index() const
-{
- return _parent->findLegIndex(this);
-}
-
-int FlightPlan::Leg::altitudeFt() const
-{
- if (_altRestrict != RESTRICT_NONE) {
- return _altitudeFt;
- }
-
- return _waypt->altitudeFt();
-}
-
-int FlightPlan::Leg::speed() const
-{
- if (_speedRestrict != RESTRICT_NONE) {
- return _speed;
- }
-
- return _waypt->speed();
-}
-
-int FlightPlan::Leg::speedKts() const
-{
- return speed();
-}
-
-double FlightPlan::Leg::speedMach() const
-{
- if (!isMachRestrict(_speedRestrict)) {
- return 0.0;
- }
-
- return -(_speed / 100.0);
-}
-
-RouteRestriction FlightPlan::Leg::altitudeRestriction() const
-{
- if (_altRestrict != RESTRICT_NONE) {
- return _altRestrict;
- }
-
- return _waypt->altitudeRestriction();
-}
-
-RouteRestriction FlightPlan::Leg::speedRestriction() const
-{
- if (_speedRestrict != RESTRICT_NONE) {
- return _speedRestrict;
- }
-
- return _waypt->speedRestriction();
-}
-
-void FlightPlan::Leg::setSpeed(RouteRestriction ty, double speed)
-{
- _speedRestrict = ty;
- if (isMachRestrict(ty)) {
- _speed = (speed * -100);
- } else {
- _speed = speed;
- }
-}
-
-void FlightPlan::Leg::setAltitude(RouteRestriction ty, int altFt)
-{
- _altRestrict = ty;
- _altitudeFt = altFt;
-}
-
-double FlightPlan::Leg::courseDeg() const
-{
- return _courseDeg;
-}
-
-double FlightPlan::Leg::distanceNm() const
-{
- return _pathDistance;
-}
-
-double FlightPlan::Leg::distanceAlongRoute() const
-{
- return _distanceAlongPath;
-}
-
-void FlightPlan::rebuildLegData()
-{
- _totalDistance = 0.0;
- int lastLeg = static_cast<int>(_legs.size()) - 1;
- for (int l=0; l<lastLeg; ++l) {
- Leg* cur = _legs[l];
- Leg* next = _legs[l + 1];
-
- std::pair<double, double> crsDist =
- next->waypoint()->courseAndDistanceFrom(cur->waypoint()->position());
- _legs[l]->_courseDeg = crsDist.first;
- _legs[l]->_pathDistance = crsDist.second * SG_METER_TO_NM;
- _legs[l]->_distanceAlongPath = _totalDistance;
- _totalDistance += crsDist.second * SG_METER_TO_NM;
- } // of legs iteration
-}
-
-void FlightPlan::setDelegate(Delegate* d)
-{
- // wrap any existing delegate(s) in the new one
- d->_inner = _delegate;
- _delegate = d;
-}
-
-void FlightPlan::removeDelegate(Delegate* d)
-{
- if (d == _delegate) {
- _delegate = _delegate->_inner;
- } else if (_delegate) {
- _delegate->removeInner(d);
- }
-}
-
-FlightPlan::Delegate::Delegate() :
- _inner(NULL)
-{
-
-}
-
-FlightPlan::Delegate::~Delegate()
-{
-
-}
-
-void FlightPlan::Delegate::removeInner(Delegate* d)
-{
- if (!_inner) {
- return;
- }
-
- if (_inner == d) {
- // replace with grand-child
- _inner = d->_inner;
- } else { // recurse downwards
- _inner->removeInner(d);
- }
-}
-
-void FlightPlan::Delegate::runDepartureChanged()
-{
- if (_inner) _inner->runDepartureChanged();
- departureChanged();
-}
-
-void FlightPlan::Delegate::runArrivalChanged()
-{
- if (_inner) _inner->runArrivalChanged();
- arrivalChanged();
-}
-
-void FlightPlan::Delegate::runWaypointsChanged()
-{
- if (_inner) _inner->runWaypointsChanged();
- waypointsChanged();
-}
-
-void FlightPlan::Delegate::runCurrentWaypointChanged()
-{
- if (_inner) _inner->runCurrentWaypointChanged();
- currentWaypointChanged();
-}
} // of namespace flightgear
// forward decls
class FGPositioned;
class SGPath;
-class FGRunway;
-
-#include <Airports/simple.hxx>
-typedef SGSharedPtr<FGAirport> FGAirportRef;
+class FGAirport;
namespace flightgear
{
class RouteBase;
class Waypt;
class NavdataVisitor;
-class SID;
-class STAR;
-class Transition;
typedef SGSharedPtr<Waypt> WayptRef;
private:
};
-
-class FlightPlan : public RouteBase
-{
-public:
- FlightPlan();
- virtual ~FlightPlan();
-
- virtual std::string ident() const;
- void setIdent(const std::string& s);
-
- FlightPlan* clone(const std::string& newIdent = std::string()) const;
-
- /**
- * flight-plan leg encapsulation
- */
- class Leg
- {
- public:
- FlightPlan* owner() const
- { return _parent; }
-
- Waypt* waypoint() const
- { return _waypt; }
-
- // reutrn the next leg after this one
- Leg* nextLeg() const;
-
- unsigned int index() const;
-
- int altitudeFt() const;
- int speed() const;
-
- int speedKts() const;
- double speedMach() const;
-
- RouteRestriction altitudeRestriction() const;
- RouteRestriction speedRestriction() const;
-
- void setSpeed(RouteRestriction ty, double speed);
- void setAltitude(RouteRestriction ty, int altFt);
-
- double courseDeg() const;
- double distanceNm() const;
- double distanceAlongRoute() const;
- private:
- friend class FlightPlan;
-
- Leg(FlightPlan* owner, WayptRef wpt);
-
- Leg* cloneFor(FlightPlan* owner) const;
-
- FlightPlan* _parent;
- RouteRestriction _speedRestrict, _altRestrict;
- int _speed;
- int _altitudeFt;
- WayptRef _waypt;
- /// length of this leg following the flown path
- mutable double _pathDistance;
- mutable double _courseDeg;
- /// total distance of this leg from departure point
- mutable double _distanceAlongPath;
- };
-
- class Delegate
- {
- public:
- virtual ~Delegate();
-
- virtual void departureChanged() { }
- virtual void arrivalChanged() { }
- virtual void waypointsChanged() { }
-
- virtual void currentWaypointChanged() { }
-
- protected:
- Delegate();
-
- private:
- void removeInner(Delegate* d);
-
- void runDepartureChanged();
- void runArrivalChanged();
- void runWaypointsChanged();
- void runCurrentWaypointChanged();
-
- friend class FlightPlan;
-
- Delegate* _inner;
- };
-
- Leg* insertWayptAtIndex(Waypt* aWpt, int aIndex);
- void insertWayptsAtIndex(const WayptVec& wps, int aIndex);
-
- void deleteIndex(int index);
- void clear();
- int clearWayptsWithFlag(WayptFlag flag);
-
- int currentIndex() const
- { return _currentIndex; }
-
- void setCurrentIndex(int index);
-
- Leg* currentLeg() const;
- Leg* nextLeg() const;
- Leg* previousLeg() const;
-
- int numLegs() const
- { return _legs.size(); }
-
- Leg* legAtIndex(int index) const;
- int findLegIndex(const Leg* l) const;
-
- int findWayptIndex(const SGGeod& aPos) const;
-
- bool load(const SGPath& p);
- bool save(const SGPath& p);
-
- FGAirportRef departureAirport() const
- { return _departure; }
-
- FGAirportRef destinationAirport() const
- { return _destination; }
-
- FGRunway* departureRunway() const
- { return _departureRunway; }
-
- FGRunway* destinationRunway() const
- { return _destinationRunway; }
-
- Approach* approach() const
- { return _approach; }
-
- void setDeparture(FGAirport* apt);
- void setDeparture(FGRunway* rwy);
-
- SID* sid() const
- { return _sid; }
-
- Transition* sidTransition() const;
-
- void setSID(SID* sid, const std::string& transition = std::string());
-
- void setSID(Transition* sidWithTrans);
-
- void setDestination(FGAirport* apt);
- void setDestination(FGRunway* rwy);
-
- /**
- * note setting an approach will implicitly update the destination
- * airport and runway to match
- */
- void setApproach(Approach* app);
-
- STAR* star() const
- { return _star; }
-
- Transition* starTransition() const;
-
- void setSTAR(STAR* star, const std::string& transition = std::string());
-
- void setSTAR(Transition* starWithTrans);
-
- double totalDistanceNm() const
- { return _totalDistance; }
-
- /**
- * Create a WayPoint from a string in the following format:
- * - simple identifier
- * - decimal-lon,decimal-lat
- * - airport-id/runway-id
- * - navaid/radial-deg/offset-nm
- */
- WayptRef waypointFromString(const std::string& target);
-
- void setDelegate(Delegate* d);
- void removeDelegate(Delegate* d);
-private:
-
- bool loadPlainTextRoute(const SGPath& path);
-
- void loadVersion1XMLRoute(SGPropertyNode_ptr routeData);
- void loadVersion2XMLRoute(SGPropertyNode_ptr routeData);
- void loadXMLRouteHeader(SGPropertyNode_ptr routeData);
- WayptRef parseVersion1XMLWaypt(SGPropertyNode* aWP);
-
- double magvarDegAt(const SGGeod& pos) const;
-
- std::string _ident;
- int _currentIndex;
-
- FGAirportRef _departure, _destination;
- FGRunway* _departureRunway, *_destinationRunway;
- SID* _sid;
- STAR* _star;
- Approach* _approach;
- std::string _sidTransition, _starTransition;
-
- double _totalDistance;
- void rebuildLegData();
-
- typedef std::vector<Leg*> LegVec;
- LegVec _legs;
-
- Delegate* _delegate;
-};
-
+
} // of namespace flightgear
#endif // of FG_ROUTE_HXX
#include <Main/globals.hxx>
#include <Airports/runways.hxx>
#include <Navaids/waypoint.hxx>
+#include <Navaids/FlightPlan.hxx>
#include <Navaids/positioned.hxx>
namespace flightgear {
namespace flightgear
{
class Hold;
+ class FlightPlan;
}
typedef std::vector<SGGeod> SGGeodVec;
#include <Airports/simple.hxx>
#include <Airports/dynamics.hxx>
#include <Airports/parking.hxx>
+#include <Scripting/NasalSys.hxx>
#include <Navaids/navlist.hxx>
#include <Navaids/procedure.hxx>
#include <Main/globals.hxx>
#include <Main/fg_props.hxx>
#include <Scenery/scenery.hxx>
#include <ATC/CommStation.hxx>
-#include <Navaids/route.hxx>
+#include <Navaids/FlightPlan.hxx>
#include <Navaids/waypoint.hxx>
#include <Navaids/fix.hxx>
#include <Autopilot/route_mgr.hxx>
projects / flightgear.git / commitdiff