#include <simgear/misc/sgstream.hxx>
#include <simgear/misc/strutils.hxx>
#include <simgear/props/props_io.hxx>
+#include <simgear/xml/easyxml.hxx>
// FlightGear
#include <Main/globals.hxx>
#include "Main/fg_props.hxx"
#include <Navaids/procedure.hxx>
#include <Navaids/waypoint.hxx>
+#include <Navaids/routePath.hxx>
using std::string;
using std::vector;
_sid(NULL),
_star(NULL),
_approach(NULL),
+ _totalDistance(0.0),
_delegate(NULL)
{
_departureChanged = _arrivalChanged = _waypointsChanged = _currentWaypointChanged = false;
delete cur;
}
}
+
+// delete legs
+ BOOST_FOREACH(Leg* l, _legs) {
+ delete l;
+ }
}
FlightPlan* FlightPlan::clone(const string& newIdent) const
}
insertWayptsAtIndex(wps, index);
- return legAtIndex(aIndex);
+ return legAtIndex(index);
}
void FlightPlan::insertWayptsAtIndex(const WayptVec& wps, int aIndex)
}
if ((index < 0) || (index >= numLegs())) {
- SG_LOG(SG_AUTOPILOT, SG_WARN, "removeAtIndex with invalid index:" << aIndex);
+ SG_LOG(SG_NAVAID, SG_WARN, "removeAtIndex with invalid index:" << aIndex);
return;
}
}
_legs.clear();
+ if (_delegate) {
+ _delegate->runCleared();
+ }
unlockDelegate();
}
bool operator()(FlightPlan::Leg* leg) const
{
if (leg->waypoint()->flag(flag)) {
- std::cout << "deleting" << leg << std::endl;
delete leg;
++delCount;
return true;
++count;
}
}
+
+ // test if the current leg will be removed
+ bool currentIsBeingCleared = false;
+ Leg* curLeg = currentLeg();
+ if (curLeg) {
+ currentIsBeingCleared = curLeg->waypoint()->flag(flag);
+ }
_currentIndex -= count;
+
+ // if we're clearing the current waypoint, what shall we do with the
+ // index? there's various options, but safest is to select no waypoint
+ // and let the use re-activate.
+ // http://code.google.com/p/flightgear-bugs/issues/detail?id=1134
+ if (currentIsBeingCleared) {
+ SG_LOG(SG_GENERAL, SG_INFO, "currentIsBeingCleared:" << currentIsBeingCleared);
+ _currentIndex = -1;
+ }
// now delete and remove
RemoveWithFlag rf(flag);
lockDelegate();
_waypointsChanged = true;
- if (count > 0) {
+ if ((count > 0) || currentIsBeingCleared) {
_currentWaypointChanged = true;
}
_legs.erase(it, _legs.end());
+
+ if (_legs.empty()) { // maybe all legs were deleted
+ if (_delegate) {
+ _delegate->runCleared();
+ }
+ }
+
unlockDelegate();
return rf.numDeleted();
}
_currentWaypointChanged = true;
unlockDelegate();
}
+
+void FlightPlan::finish()
+{
+ if (_currentIndex == -1) {
+ return;
+ }
+
+ lockDelegate();
+ _currentIndex = -1;
+ _currentWaypointChanged = true;
+
+ if (_delegate) {
+ _delegate->runFinished();
+ }
+
+ unlockDelegate();
+}
int FlightPlan::findWayptIndex(const SGGeod& aPos) const
{
return -1;
}
+
+int FlightPlan::findWayptIndex(const FGPositionedRef aPos) const
+{
+ for (int i=0; i<numLegs(); ++i) {
+ if (_legs[i]->waypoint()->source() == aPos) {
+ return i;
+ }
+ }
+
+ return -1;
+}
FlightPlan::Leg* FlightPlan::currentLeg() const
{
FlightPlan::Leg* FlightPlan::previousLeg() const
{
- if (_currentIndex == 0) {
+ if (_currentIndex <= 0) {
return NULL;
}
bool FlightPlan::save(const SGPath& path)
{
- SG_LOG(SG_IO, SG_INFO, "Saving route to " << path.str());
+ SG_LOG(SG_NAVAID, SG_INFO, "Saving route to " << path.str());
try {
SGPropertyNode_ptr d(new SGPropertyNode);
d->setIntValue("version", 2);
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());
+ SG_LOG(SG_NAVAID, 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()
+ SG_LOG(SG_NAVAID, 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());
+
+ SG_LOG(SG_NAVAID, SG_INFO, "going to read flight-plan from:" << path.str());
bool Status = false;
lockDelegate();
+
+ // try different file formats
+ if (loadGpxFormat(path)) // GPX format
+ Status = true;
+ else
+ if (loadXmlFormat(path)) // XML property data
+ Status = true;
+ else
+ if (loadPlainTextFormat(path)) // simple textual list of waypoints
+ Status = true;
+
+ _waypointsChanged = true;
+ unlockDelegate();
+
+ return Status;
+}
+
+/** XML loader for GPX file format */
+class GpxXmlVisitor : public XMLVisitor
+{
+public:
+ GpxXmlVisitor(FlightPlan* fp) : _fp(fp), _lat(-9999), _lon(-9999) {}
+
+ virtual void startElement (const char * name, const XMLAttributes &atts);
+ virtual void endElement (const char * name);
+ virtual void data (const char * s, int length);
+
+private:
+ FlightPlan* _fp;
+ double _lat, _lon;
+ string _element;
+ string _waypoint;
+};
+
+void GpxXmlVisitor::startElement(const char * name, const XMLAttributes &atts)
+{
+ _element = name;
+ if (strcmp(name, "rtept")==0)
+ {
+ _waypoint = "";
+ _lat = _lon = -9999;
+
+ const char* slat = atts.getValue("lat");
+ const char* slon = atts.getValue("lon");
+ if (slat && slon)
+ {
+ _lat = atof(slat);
+ _lon = atof(slon);
+ }
+ }
+}
+
+void GpxXmlVisitor::data(const char * s, int length)
+{
+ // use "name" when given, otherwise use "cmt" (comment) as ID
+ if ((_element == "name")||
+ ((_waypoint == "")&&(_element == "cmt")))
+ {
+ char* buf = (char*) malloc(length+1);
+ memcpy(buf, s, length);
+ buf[length] = 0;
+ _waypoint = buf;
+ free(buf);
+ }
+}
+
+void GpxXmlVisitor::endElement(const char * name)
+{
+ _element = "";
+ if (strcmp(name, "rtept") == 0)
+ {
+ if (_lon > -9990.0)
+ {
+ _fp->insertWayptAtIndex(new BasicWaypt(SGGeod::fromDeg(_lon, _lat), _waypoint.c_str(), NULL), -1);
+ }
+ }
+}
+
+/** Load a flightplan in GPX format */
+bool FlightPlan::loadGpxFormat(const SGPath& path)
+{
+ if (path.lower_extension() != "gpx")
+ {
+ // not a valid GPX file
+ return false;
+ }
+
+ _legs.clear();
+ GpxXmlVisitor gpxVistor(this);
+ try
+ {
+ readXML(path.str(), gpxVistor);
+ } catch (sg_exception& e)
+ {
+ // XML parsing fails => not a GPX XML file
+ SG_LOG(SG_NAVAID, SG_ALERT, "Failed to load flight-plan in GPX format: '" << e.getOrigin()
+ << "'. " << e.getMessage());
+ return false;
+ }
+
+ if (numLegs() == 0)
+ {
+ SG_LOG(SG_NAVAID, SG_ALERT, "Failed to load flight-plan in GPX format. No route found.");
+ return false;
+ }
+
+ return true;
+}
+
+/** Load a flightplan in FlightGear XML property format */
+bool FlightPlan::loadXmlFormat(const SGPath& path)
+{
+ SGPropertyNode_ptr routeData(new SGPropertyNode);
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;
- _waypointsChanged = true;
+ } catch (sg_exception& e) {
+ SG_LOG(SG_NAVAID, SG_ALERT, "Failed to load flight-plan '" << e.getOrigin()
+ << "'. " << e.getMessage());
+ // XML parsing fails => not a property XML file
+ return false;
}
-
+
if (routeData.valid())
{
try {
} else {
throw sg_io_exception("unsupported XML route version");
}
- Status = true;
+ return true;
} catch (sg_exception& e) {
- SG_LOG(SG_IO, SG_ALERT, "Failed to load flight-plan '" << e.getOrigin()
+ SG_LOG(SG_NAVAID, SG_ALERT, "Failed to load flight-plan '" << e.getOrigin()
<< "'. " << e.getMessage());
- Status = false;
}
}
-
- unlockDelegate();
- return Status;
+
+ return false;
}
void FlightPlan::loadXMLRouteHeader(SGPropertyNode_ptr routeData)
string depIdent = dep->getStringValue("airport");
setDeparture((FGAirport*) fgFindAirportID(depIdent));
if (_departure) {
- if (dep->hasChild("runway")) {
- setDeparture(_departure->getRunwayByIdent(dep->getStringValue("runway")));
+ string rwy(dep->getStringValue("runway"));
+ if (_departure->hasRunwayWithIdent(rwy)) {
+ setDeparture(_departure->getRunwayByIdent(rwy));
}
if (dep->hasChild("sid")) {
if (dst) {
setDestination((FGAirport*) fgFindAirportID(dst->getStringValue("airport")));
if (_destination) {
- if (dst->hasChild("runway")) {
- setDestination(_destination->getRunwayByIdent(dst->getStringValue("runway")));
+ string rwy(dst->getStringValue("runway"));
+ if (_destination->hasRunwayWithIdent(rwy)) {
+ setDestination(_destination->getRunwayByIdent(rwy));
}
if (dst->hasChild("star")) {
// 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
+ SGPropertyNode_ptr routeNode = routeData->getChild("route", 0);
+ if (routeNode.valid()) {
+ 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
+ }
_waypointsChanged = true;
}
} 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;
+
+ if (p) {
+ pos = p->geod();
+ } else {
+ SG_LOG(SG_GENERAL, SG_WARN, "unknown navaid in flightplan:" << nid);
+ pos = SGGeod::fromDeg(aWP->getDoubleValue("longitude-deg"),
+ aWP->getDoubleValue("latitude-deg"));
}
- 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);
+ SGGeodesy::direct(pos, radialDeg, offsetNm * SG_NM_TO_METER, pos, az2);
}
w = new BasicWaypt(pos, ident, NULL);
return w;
}
-bool FlightPlan::loadPlainTextRoute(const SGPath& path)
+/** Load a flightplan in FlightGear plain-text format */
+bool FlightPlan::loadPlainTextFormat(const SGPath& path)
{
try {
sg_gzifstream in(path.str().c_str());
_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());
+ SG_LOG(SG_NAVAID, SG_ALERT, "Failed to load route from: '" << path.str() << "'. " << e.getMessage());
_legs.clear();
return false;
}
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());
+ SG_LOG( SG_NAVAID, SG_INFO, "Unable to find FGPositioned with ident:" << pieces.front());
return NULL;
}
} 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());
+ SG_LOG(SG_NAVAID, 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]);
+ SG_LOG(SG_NAVAID, SG_INFO, "No runway: " << pieces[1] << " at " << pieces[0]);
return NULL;
}
// 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]);
+ SG_LOG( SG_NAVAID, SG_INFO, "Unable to find FGPositioned with ident:" << pieces[2]);
return NULL;
}
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);
+ SG_LOG(SG_NAVAID, SG_INFO, "no valid intersection for:" << target);
return NULL;
}
}
if (!wpt) {
- SG_LOG(SG_AUTOPILOT, SG_INFO, "Unable to parse waypoint:" << target);
+ SG_LOG(SG_NAVAID, SG_INFO, "Unable to parse waypoint:" << target);
return NULL;
}
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];
+ double totalDistanceIncludingMissed = 0.0;
+ RoutePath path(this);
+
+ for (unsigned int l=0; l<_legs.size(); ++l) {
+ _legs[l]->_courseDeg = path.trackForIndex(l);
+ _legs[l]->_pathDistance = path.distanceForIndex(l) * SG_METER_TO_NM;
+
+ totalDistanceIncludingMissed += _legs[l]->_pathDistance;
+ // distance along path includes our own leg distance
+ _legs[l]->_distanceAlongPath = totalDistanceIncludingMissed;
- 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
+ // omit missed-approach waypoints from total distance calculation
+ if (!_legs[l]->waypoint()->flag(WPT_MISS)) {
+ _totalDistance += _legs[l]->_pathDistance;
+ }
+} // of legs iteration
+
}
+SGGeod FlightPlan::pointAlongRoute(int aIndex, double aOffsetNm) const
+{
+ RoutePath rp(this);
+ return rp.positionForDistanceFrom(aIndex, aOffsetNm * SG_NM_TO_METER);
+}
+
void FlightPlan::lockDelegate()
{
if (_delegateLock == 0) {
_deleteWithPlan(false),
_inner(NULL)
{
-
}
FlightPlan::Delegate::~Delegate()
-{
-
+{
}
void FlightPlan::Delegate::removeInner(Delegate* d)
{
if (!_inner) {
- return;
+ throw sg_exception("FlightPlan delegate not found");
}
if (_inner == d) {
if (_inner) _inner->runCurrentWaypointChanged();
currentWaypointChanged();
}
-
+
+void FlightPlan::Delegate::runCleared()
+{
+ if (_inner) _inner->runCleared();
+ cleared();
+}
+
+void FlightPlan::Delegate::runFinished()
+{
+ if (_inner) _inner->runFinished();
+ endOfFlightPlan();
+}
+
} // of namespace flightgear