+// SGWayPoint( const double lon = 0.0, const double lat = 0.0,
+// const double alt = 0.0, const modetype m = WGS84,
+// const string& s = "", const string& n = "" );
+
+bool FGRouteMgr::activate()
+{
+ const FGAirport* depApt = fgFindAirportID(departure->getStringValue("airport"));
+ if (!depApt) {
+ SG_LOG(SG_AUTOPILOT, SG_ALERT,
+ "unable to activate route: departure airport is invalid:"
+ << departure->getStringValue("airport") );
+ return false;
+ }
+
+ string runwayId(departure->getStringValue("runway"));
+ FGRunway* runway = NULL;
+ if (depApt->hasRunwayWithIdent(runwayId)) {
+ runway = depApt->getRunwayByIdent(runwayId);
+ } else {
+ SG_LOG(SG_AUTOPILOT, SG_INFO,
+ "route-manager, departure runway not found:" << runwayId);
+ runway = depApt->getActiveRunwayForUsage();
+ }
+
+ SGWayPoint swp(runway->threshold(),
+ depApt->ident() + "-" + runway->ident(), runway->name());
+ add_waypoint(swp, 0);
+
+ const FGAirport* destApt = fgFindAirportID(destination->getStringValue("airport"));
+ if (!destApt) {
+ SG_LOG(SG_AUTOPILOT, SG_ALERT,
+ "unable to activate route: destination airport is invalid:"
+ << destination->getStringValue("airport") );
+ return false;
+ }
+
+ runwayId = (destination->getStringValue("runway"));
+ if (destApt->hasRunwayWithIdent(runwayId)) {
+ FGRunway* runway = destApt->getRunwayByIdent(runwayId);
+ SGWayPoint swp(runway->end(),
+ destApt->ident() + "-" + runway->ident(), runway->name());
+ add_waypoint(swp);
+ } else {
+ // quite likely, since destination runway may not be known until enroute
+ // probably want a listener on the 'destination' node to allow an enroute
+ // update
+ add_waypoint(SGWayPoint(destApt->geod(), destApt->ident(), destApt->name()));
+ }
+
+ _route->set_current(1);
+
+ double routeDistanceNm = _route->total_distance() * SG_METER_TO_NM;
+ totalDistance->setDoubleValue(routeDistanceNm);
+ double cruiseSpeedKts = cruise->getDoubleValue("speed", 0.0);
+ if (cruiseSpeedKts > 1.0) {
+ // very very crude approximation, doesn't allow for climb / descent
+ // performance or anything else at all
+ ete->setDoubleValue(routeDistanceNm / cruiseSpeedKts * (60.0 * 60.0));
+ }
+
+ active->setBoolValue(true);
+ SG_LOG(SG_AUTOPILOT, SG_INFO, "route-manager, activate route ok");
+ return true;
+}
+
+
+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);
+ }
+}