X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FNavaids%2FFlightPlan.cxx;h=83edf5c78aded2b7b566f55fbaf73da7d0692ff0;hb=5c659b3970e9a4542519e0d528016c08f7ecb6c1;hp=5f5ba5e713847f5afce27c34be410514df96ae39;hpb=9fa790bcac7e535bb06cd228ae15178f1b10b26f;p=flightgear.git

diff --git a/src/Navaids/FlightPlan.cxx b/src/Navaids/FlightPlan.cxx
index 5f5ba5e71..83edf5c78 100644
--- a/src/Navaids/FlightPlan.cxx
+++ b/src/Navaids/FlightPlan.cxx
@@ -42,12 +42,14 @@
 #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;
@@ -67,6 +69,7 @@ FlightPlan::FlightPlan() :
   _sid(NULL),
   _star(NULL),
   _approach(NULL),
+  _totalDistance(0.0),
   _delegate(NULL)
 {
   _departureChanged = _arrivalChanged = _waypointsChanged = _currentWaypointChanged = false;
@@ -91,6 +94,11 @@ FlightPlan::~FlightPlan()
       delete cur;
     }
   }
+    
+// delete legs
+    BOOST_FOREACH(Leg* l, _legs) {
+        delete l;
+    }
 }
   
 FlightPlan* FlightPlan::clone(const string& newIdent) const
@@ -148,7 +156,7 @@ FlightPlan::Leg* FlightPlan::insertWayptAtIndex(Waypt* aWpt, int aIndex)
   }
   
   insertWayptsAtIndex(wps, index);
-  return legAtIndex(aIndex);
+  return legAtIndex(index);
 }
   
 void FlightPlan::insertWayptsAtIndex(const WayptVec& wps, int aIndex)
@@ -189,7 +197,7 @@ 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;
   }
   
@@ -226,6 +234,9 @@ void FlightPlan::clear()
   }
   _legs.clear();  
   
+  if (_delegate) {
+    _delegate->runCleared();
+  }
   unlockDelegate();
 }
   
@@ -239,7 +250,6 @@ public:
   bool operator()(FlightPlan::Leg* leg) const
   {
     if (leg->waypoint()->flag(flag)) {
-      std::cout << "deleting" << leg << std::endl;
       delete leg;
       ++delCount;
       return true;
@@ -262,8 +272,24 @@ int FlightPlan::clearWayptsWithFlag(WayptFlag flag)
       ++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);
@@ -274,11 +300,18 @@ int FlightPlan::clearWayptsWithFlag(WayptFlag 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();
 }
@@ -298,6 +331,23 @@ void FlightPlan::setCurrentIndex(int index)
   _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
 {  
@@ -309,6 +359,17 @@ 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
 {
@@ -319,7 +380,7 @@ FlightPlan::Leg* FlightPlan::currentLeg() const
 
 FlightPlan::Leg* FlightPlan::previousLeg() const
 {
-  if (_currentIndex == 0) {
+  if (_currentIndex <= 0) {
     return NULL;
   }
   
@@ -512,7 +573,7 @@ void FlightPlan::setApproach(flightgear::Approach *app)
   
 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);
@@ -554,34 +615,146 @@ bool FlightPlan::save(const SGPath& path)
     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 {
@@ -593,16 +766,14 @@ bool FlightPlan::load(const SGPath& path)
       } 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)
@@ -613,8 +784,9 @@ 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")) {
@@ -629,8 +801,9 @@ void FlightPlan::loadXMLRouteHeader(SGPropertyNode_ptr routeData)
   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")) {
@@ -667,12 +840,14 @@ void FlightPlan::loadVersion2XMLRoute(SGPropertyNode_ptr 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
+  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;
 }
 
@@ -710,18 +885,23 @@ 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);
@@ -735,7 +915,8 @@ WayptRef FlightPlan::parseVersion1XMLWaypt(SGPropertyNode* aWP)
   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());
@@ -765,7 +946,7 @@ bool FlightPlan::loadPlainTextRoute(const SGPath& path)
       _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;
   }
@@ -825,7 +1006,7 @@ WayptRef FlightPlan::waypointFromString(const string& tgt )
   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;
   }
   
@@ -842,12 +1023,12 @@ WayptRef FlightPlan::waypointFromString(const string& tgt )
   } 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;
     }
     
@@ -857,7 +1038,7 @@ WayptRef FlightPlan::waypointFromString(const string& tgt )
     // 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;
     }
     
@@ -869,7 +1050,7 @@ WayptRef FlightPlan::waypointFromString(const string& tgt )
     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;
     }
     
@@ -878,7 +1059,7 @@ WayptRef FlightPlan::waypointFromString(const string& tgt )
   }
   
   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;
   }
   
@@ -1006,20 +1187,31 @@ double FlightPlan::Leg::distanceAlongRoute() const
 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) {
@@ -1112,18 +1304,16 @@ FlightPlan::Delegate::Delegate() :
   _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) {
@@ -1157,5 +1347,17 @@ void FlightPlan::Delegate::runCurrentWaypointChanged()
   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