// std
#include <map>
#include <fstream>
+#include <cassert>
// Boost
#include <boost/algorithm/string/case_conv.hpp>
static FPDelegateFactoryVec static_delegateFactories;
FlightPlan::FlightPlan() :
+ _delegateLock(0),
_currentIndex(-1),
_departureRunway(NULL),
_destinationRunway(NULL),
_approach(NULL),
_delegate(NULL)
{
+ _departureChanged = _arrivalChanged = _waypointsChanged = _currentWaypointChanged = false;
+
BOOST_FOREACH(DelegateFactory* factory, static_delegateFactories) {
Delegate* d = factory->createFlightPlanDelegate(this);
if (d) { // factory might not always create a delegate
delete cur;
}
}
+
+// delete legs
+ BOOST_FOREACH(Leg* l, _legs) {
+ delete l;
+ }
}
FlightPlan* FlightPlan::clone(const string& newIdent) const
{
FlightPlan* c = new FlightPlan();
c->_ident = newIdent.empty() ? _ident : newIdent;
+ c->lockDelegate();
// copy destination / departure data.
c->setDeparture(_departure);
c->setSID(_sid);
// copy legs
+ c->_waypointsChanged = true;
for (int l=0; l < numLegs(); ++l) {
c->_legs.push_back(_legs[l]->cloneFor(c));
}
-
+ c->unlockDelegate();
return c;
}
}
insertWayptsAtIndex(wps, index);
- return legAtIndex(aIndex);
+ return legAtIndex(index);
}
void FlightPlan::insertWayptsAtIndex(const WayptVec& wps, int aIndex)
newLegs.push_back(new Leg(this, wp));
}
+ lockDelegate();
+ _waypointsChanged = true;
_legs.insert(it, newLegs.begin(), newLegs.end());
- rebuildLegData();
-
- if (_delegate) {
- _delegate->runWaypointsChanged();
- }
+ unlockDelegate();
}
void FlightPlan::deleteIndex(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;
}
+
+ lockDelegate();
+ _waypointsChanged = true;
+
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;
+ _currentWaypointChanged = true;
} else if (_currentIndex > index) {
--_currentIndex; // shift current index down if necessary
}
-
- rebuildLegData();
- if (_delegate) {
- _delegate->runWaypointsChanged();
- if (curChanged) {
- _delegate->runCurrentWaypointChanged();
- }
- }
+
+ unlockDelegate();
}
void FlightPlan::clear()
{
+ lockDelegate();
+ _waypointsChanged = true;
+ _currentWaypointChanged = true;
+ _arrivalChanged = true;
+ _departureChanged = true;
+
_currentIndex = -1;
BOOST_FOREACH(Leg* l, _legs) {
delete l;
}
_legs.clear();
- rebuildLegData();
+
if (_delegate) {
- _delegate->runDepartureChanged();
- _delegate->runArrivalChanged();
- _delegate->runWaypointsChanged();
- _delegate->runCurrentWaypointChanged();
+ _delegate->runCleared();
}
+ unlockDelegate();
}
+class RemoveWithFlag
+{
+public:
+ RemoveWithFlag(WayptFlag f) : flag(f), delCount(0) { }
+
+ int numDeleted() const { return delCount; }
+
+ bool operator()(FlightPlan::Leg* leg) const
+ {
+ if (leg->waypoint()->flag(flag)) {
+ delete leg;
+ ++delCount;
+ return true;
+ }
+
+ return false;
+ }
+private:
+ WayptFlag flag;
+ mutable int delCount;
+};
+
int FlightPlan::clearWayptsWithFlag(WayptFlag flag)
{
int count = 0;
- for (unsigned int i=0; i<_legs.size(); ++i) {
+// first pass, fix up currentIndex
+ for (int i=0; i<_currentIndex; ++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;
+ if (l->waypoint()->flag(flag)) {
+ ++count;
}
-
- delete l;
- LegVec::iterator it = _legs.begin();
- it += i;
- _legs.erase(it);
}
- if (count == 0) {
+ // 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);
+ LegVec::iterator it = std::remove_if(_legs.begin(), _legs.end(), rf);
+ if (it == _legs.end()) {
return 0; // nothing was cleared, don't fire the delegate
}
- rebuildLegData();
- if (_delegate) {
- _delegate->runWaypointsChanged();
- _delegate->runCurrentWaypointChanged();
+ lockDelegate();
+ _waypointsChanged = true;
+ if ((count > 0) || currentIsBeingCleared) {
+ _currentWaypointChanged = true;
+ }
+
+ _legs.erase(it, _legs.end());
+
+ if (_legs.empty()) { // maybe all legs were deleted
+ if (_delegate) {
+ _delegate->runCleared();
+ }
}
- return count;
+ unlockDelegate();
+ return rf.numDeleted();
}
void FlightPlan::setCurrentIndex(int index)
return;
}
+ lockDelegate();
_currentIndex = index;
- if (_delegate) {
- _delegate->runCurrentWaypointChanged();
- }
+ _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
FlightPlan::Leg* FlightPlan::previousLeg() const
{
- if (_currentIndex == 0) {
+ if (_currentIndex <= 0) {
return NULL;
}
return;
}
+ lockDelegate();
+ _departureChanged = true;
_departure = apt;
_departureRunway = NULL;
setSID((SID*)NULL);
-
- if (_delegate) {
- _delegate->runDepartureChanged();
- }
+ unlockDelegate();
}
void FlightPlan::setDeparture(FGRunway* rwy)
return;
}
+ lockDelegate();
+ _departureChanged = true;
+
_departureRunway = rwy;
if (rwy->airport() != _departure) {
_departure = rwy->airport();
setSID((SID*)NULL);
}
-
- if (_delegate) {
- _delegate->runDepartureChanged();
- }
+ unlockDelegate();
}
void FlightPlan::setSID(SID* sid, const std::string& transition)
return;
}
+ lockDelegate();
+ _departureChanged = true;
_sid = sid;
_sidTransition = transition;
-
- if (_delegate) {
- _delegate->runDepartureChanged();
- }
+ unlockDelegate();
}
void FlightPlan::setSID(Transition* trans)
return;
}
+ lockDelegate();
+ _arrivalChanged = true;
_destination = apt;
_destinationRunway = NULL;
setSTAR((STAR*)NULL);
-
- if (_delegate) {
- _delegate->runArrivalChanged();
- }
+ setApproach(NULL);
+ unlockDelegate();
}
void FlightPlan::setDestination(FGRunway* rwy)
return;
}
+ lockDelegate();
+ _arrivalChanged = true;
_destinationRunway = rwy;
if (_destination != rwy->airport()) {
_destination = rwy->airport();
setSTAR((STAR*)NULL);
}
- if (_delegate) {
- _delegate->runArrivalChanged();
- }
+ unlockDelegate();
}
void FlightPlan::setSTAR(STAR* star, const std::string& transition)
return;
}
+ lockDelegate();
+ _arrivalChanged = true;
_star = star;
_starTransition = transition;
-
- if (_delegate) {
- _delegate->runArrivalChanged();
- }
+ unlockDelegate();
}
void FlightPlan::setSTAR(Transition* trans)
return;
}
+ lockDelegate();
+ _arrivalChanged = true;
_approach = app;
if (app) {
// keep runway + airport in sync
_destination = _destinationRunway->airport();
}
}
-
- if (_delegate) {
- _delegate->runArrivalChanged();
- }
+ unlockDelegate();
}
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;
}
}
{
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 {
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;
}
if (routeData.valid())
}
Status = 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;
}
}
- rebuildLegData();
- if (_delegate) {
- _delegate->runWaypointsChanged();
- }
-
+ unlockDelegate();
return Status;
}
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")) {
Leg* l = new Leg(this, Waypt::createFromProperties(NULL, wpNode));
_legs.push_back(l);
} // of route iteration
+ _waypointsChanged = true;
}
void FlightPlan::loadVersion1XMLRoute(SGPropertyNode_ptr routeData)
Leg* l = new Leg(this, parseVersion1XMLWaypt(wpNode));
_legs.push_back(l);
} // of route iteration
-
+ _waypointsChanged = true;
}
WayptRef FlightPlan::parseVersion1XMLWaypt(SGPropertyNode* aWP)
} 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);
_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;
}
_legs[l]->_distanceAlongPath = _totalDistance;
_totalDistance += crsDist.second * SG_METER_TO_NM;
} // of legs iteration
+
+// set some data on the final leg
+ if (lastLeg > 0) {
+ // keep the same course as the final leg, when passing the final
+ // waypoint
+ _legs[lastLeg]->_courseDeg = _legs[lastLeg - 1]->_courseDeg;
+ _legs[lastLeg]->_pathDistance = 0.0;
+ _legs[lastLeg]->_distanceAlongPath = _totalDistance;
+ }
+}
+
+SGGeod FlightPlan::pointAlongRoute(int aIndex, double aOffsetNm) const
+{
+ if (aIndex >= (int) _legs.size()) {
+ throw sg_range_exception();
+ }
+
+ const int lastLeg = static_cast<int>(_legs.size()) - 1;
+// convert the relative offset and leg index into an absolute, positive
+// distance in nm from the route origin. This means we can simply walk
+// forwards to find the actual leg.
+ Leg* leg = _legs[(aIndex >= 0) ? aIndex : lastLeg];
+ double absolutePathDistance = leg->_distanceAlongPath + aOffsetNm;
+ if (absolutePathDistance < 0.0) {
+ return _legs[0]->waypoint()->position(); // begining of route
+ }
+
+ if (absolutePathDistance > _totalDistance) {
+ return _legs[lastLeg]->waypoint()->position(); // end of route
+ }
+
+// find the leg containing the absolute distance
+ for (int l=0; l<lastLeg; ++l) {
+ leg = _legs[l];
+ if (absolutePathDistance < leg->_pathDistance) {
+ break; // found our matching leg
+ }
+ absolutePathDistance -= leg->_pathDistance;
+ } // of forwards walk along route to find leg
+
+ return SGGeodesy::direct(leg->waypoint()->position(),
+ leg->_courseDeg, absolutePathDistance * SG_NM_TO_METER);
+}
+
+void FlightPlan::lockDelegate()
+{
+ if (_delegateLock == 0) {
+ assert(!_departureChanged && !_arrivalChanged &&
+ !_waypointsChanged && !_currentWaypointChanged);
+ }
+
+ ++_delegateLock;
+}
+
+void FlightPlan::unlockDelegate()
+{
+ assert(_delegateLock > 0);
+ if (_delegateLock > 1) {
+ --_delegateLock;
+ return;
+ }
+
+ if (_departureChanged) {
+ _departureChanged = false;
+ if (_delegate) {
+ _delegate->runDepartureChanged();
+ }
+ }
+
+ if (_arrivalChanged) {
+ _arrivalChanged = false;
+ if (_delegate) {
+ _delegate->runArrivalChanged();
+ }
+ }
+
+ if (_waypointsChanged) {
+ _waypointsChanged = false;
+ rebuildLegData();
+ if (_delegate) {
+ _delegate->runWaypointsChanged();
+ }
+ }
+
+ if (_currentWaypointChanged) {
+ _currentWaypointChanged = false;
+ if (_delegate) {
+ _delegate->runCurrentWaypointChanged();
+ }
+ }
+
+ --_delegateLock;
}
void FlightPlan::registerDelegateFactory(DelegateFactory* df)
_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