+void FGRouteMgr::sequence()
+{
+ if (!active->getBoolValue()) {
+ SG_LOG(SG_AUTOPILOT, SG_ALERT, "trying to sequence waypoints with no active route");
+ return;
+ }
+
+ if (checkFinished()) {
+ return;
+ }
+
+ _route->increment_current();
+ currentWaypointChanged();
+ _currentWpt->fireValueChanged();
+}
+
+bool FGRouteMgr::checkFinished()
+{
+ int lastWayptIndex = _route->size() - 1;
+ if (_route->current_index() < lastWayptIndex) {
+ return false;
+ }
+
+ SG_LOG(SG_AUTOPILOT, SG_INFO, "reached end of active route");
+ _finished->fireValueChanged();
+ active->setBoolValue(false);
+ return true;
+}
+
+void FGRouteMgr::jumpToIndex(int index)
+{
+ if (!active->getBoolValue()) {
+ SG_LOG(SG_AUTOPILOT, SG_ALERT, "trying to sequence waypoints with no active route");
+ return;
+ }
+
+ if ((index < 0) || (index >= _route->size())) {
+ SG_LOG(SG_AUTOPILOT, SG_ALERT, "passed invalid index (" <<
+ index << ") to FGRouteMgr::jumpToIndex");
+ return;
+ }
+
+ if (_route->current_index() == index) {
+ return; // no-op
+ }
+
+ _route->set_current(index);
+ currentWaypointChanged();
+}
+
+void FGRouteMgr::currentWaypointChanged()
+{
+ SGWayPoint previous = _route->get_previous();
+ SGWayPoint cur = _route->get_current();
+
+ wp0->getChild("id")->setStringValue(cur.get_id());
+ if ((_route->current_index() + 1) < _route->size()) {
+ wp1->getChild("id")->setStringValue(_route->get_next().get_id());
+ } else {
+ wp1->getChild("id")->setStringValue("");
+ }
+
+ SG_LOG(SG_AUTOPILOT, SG_INFO, "route manager, current-wp is now " << _route->current_index());
+}
+
+int FGRouteMgr::findWaypoint(const SGGeod& aPos) const
+{
+ for (int i=0; i<_route->size(); ++i) {
+ double d = SGGeodesy::distanceM(aPos, _route->get_waypoint(i).get_target());
+ if (d < 200.0) { // 200 metres seems close enough
+ return i;
+ }
+ }
+
+ return -1;
+}
+
+SGWayPoint FGRouteMgr::get_waypoint( int i ) const
+{
+ return _route->get_waypoint(i);
+}
+
+int FGRouteMgr::size() const
+{
+ return _route->size();
+}
+
+int FGRouteMgr::currentWaypoint() const
+{
+ return _route->current_index();
+}
+
+void FGRouteMgr::saveRoute()
+{
+ SGPath path(_pathNode->getStringValue());
+ SG_LOG(SG_IO, SG_INFO, "Saving route to " << path.str());
+ try {
+ writeProperties(path.str(), mirror, false, SGPropertyNode::ARCHIVE);
+ } catch (const sg_exception &e) {
+ SG_LOG(SG_IO, SG_WARN, "Error saving route:" << e.getMessage());
+ //guiErrorMessage("Error writing autosave.xml: ", e);
+ }
+}
+
+void FGRouteMgr::loadRoute()
+{
+ try {
+ // deactivate route first
+ active->setBoolValue(false);
+
+ SGPropertyNode_ptr routeData(new SGPropertyNode);
+ SGPath path(_pathNode->getStringValue());
+
+ SG_LOG(SG_IO, SG_INFO, "going to read flight-plan from:" << path.str());
+ readProperties(path.str(), routeData);
+
+ // departure nodes
+ SGPropertyNode* dep = routeData->getChild("departure");
+ if (!dep) {
+ throw sg_io_exception("malformed route file, no departure node");
+ }
+
+ string depIdent = dep->getStringValue("airport");
+ const FGAirport* depApt = fgFindAirportID(depIdent);
+ if (!depApt) {
+ throw sg_io_exception("bad route file, unknown airport:" + depIdent);
+ }
+
+ departure->setStringValue("runway", dep->getStringValue("runway"));
+
+ // destination
+ SGPropertyNode* dst = routeData->getChild("destination");
+ if (!dst) {
+ throw sg_io_exception("malformed route file, no destination node");
+ }
+
+ destination->setStringValue("airport", dst->getStringValue("airport"));
+ destination->setStringValue("runay", dst->getStringValue("runway"));
+
+ // alternate
+ SGPropertyNode* alt = routeData->getChild("alternate");
+ if (alt) {
+ alternate->setStringValue(alt->getStringValue("airport"));
+ } // of cruise data loading
+
+ // cruise
+ SGPropertyNode* crs = routeData->getChild("cruise");
+ if (crs) {
+ cruise->setDoubleValue(crs->getDoubleValue("speed"));
+ } // of cruise data loading
+
+ // route nodes
+ _route->clear();
+ SGPropertyNode_ptr _route = routeData->getChild("route", 0);
+ SGGeod lastPos(depApt->geod());
+
+ for (int i=0; i<_route->nChildren(); ++i) {
+ SGPropertyNode_ptr wp = _route->getChild("wp", i);
+ parseRouteWaypoint(wp);
+ } // of route iteration
+ } catch (sg_exception& e) {
+ SG_LOG(SG_IO, SG_WARN, "failed to load flight-plan (from '" << e.getOrigin()
+ << "'):" << e.getMessage());
+ }
+}
+
+void FGRouteMgr::parseRouteWaypoint(SGPropertyNode* aWP)
+{
+ SGGeod lastPos;
+ if (_route->size() > 0) {
+ lastPos = get_waypoint(_route->size()-1).get_target();
+ } else {
+ // route is empty, use departure airport position
+ const FGAirport* apt = fgFindAirportID(departure->getStringValue("airport"));
+ assert(apt); // shouldn't have got this far with an invalid airport
+ lastPos = apt->geod();
+ }
+
+ SGPropertyNode_ptr altProp = aWP->getChild("altitude-ft");
+ double alt = cruise->getDoubleValue("altitude-ft") * SG_FEET_TO_METER;
+ if (altProp) {
+ alt = altProp->getDoubleValue();
+ }
+
+ string ident(aWP->getStringValue("ident"));
+ if (aWP->hasChild("longitude-deg")) {
+ // explicit longitude/latitude
+ SGWayPoint swp(aWP->getDoubleValue("longitude-deg"),
+ aWP->getDoubleValue("latitude-deg"), alt,
+ SGWayPoint::WGS84, ident, aWP->getStringValue("name"));
+ add_waypoint(swp);
+ } else if (aWP->hasChild("navid")) {
+ // lookup by navid (possibly with offset)
+ string nid(aWP->getStringValue("navid"));
+ 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 += magvar->getDoubleValue();
+ double offsetNm = aWP->getDoubleValue("offset-nm");
+ double az2;
+ SGGeodesy::direct(p->geod(), radialDeg, offsetNm * SG_NM_TO_METER, pos, az2);
+ }
+
+ SGWayPoint swp(pos.getLongitudeDeg(), pos.getLatitudeDeg(), alt,
+ SGWayPoint::WGS84, ident, "");
+ add_waypoint(swp);
+ } else {
+ // lookup by ident (symbolic waypoint)
+ FGPositionedRef p = FGPositioned::findClosestWithIdent(ident, lastPos);
+ if (!p) {
+ throw sg_io_exception("bad route file, unknown waypoint:" + ident);
+ }
+
+ SGWayPoint swp(p->longitude(), p->latitude(), alt,
+ SGWayPoint::WGS84, p->ident(), p->name());
+ add_waypoint(swp);
+ }
+}