1 // route_mgr.cxx - manage a route (i.e. a collection of waypoints)
3 // Written by Curtis Olson, started January 2004.
7 // Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
9 // This program is free software; you can redistribute it and/or
10 // modify it under the terms of the GNU General Public License as
11 // published by the Free Software Foundation; either version 2 of the
12 // License, or (at your option) any later version.
14 // This program is distributed in the hope that it will be useful, but
15 // WITHOUT ANY WARRANTY; without even the implied warranty of
16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
17 // General Public License for more details.
19 // You should have received a copy of the GNU General Public License
20 // along with this program; if not, write to the Free Software
21 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
34 #include <simgear/compiler.h>
36 #include "route_mgr.hxx"
38 #include <boost/algorithm/string/case_conv.hpp>
39 #include <boost/tuple/tuple.hpp>
41 #include <simgear/misc/strutils.hxx>
42 #include <simgear/structure/exception.hxx>
43 #include <simgear/structure/commands.hxx>
44 #include <simgear/misc/sgstream.hxx>
46 #include <simgear/props/props_io.hxx>
47 #include <simgear/misc/sg_path.hxx>
48 #include <simgear/route/route.hxx>
49 #include <simgear/sg_inlines.h>
51 #include "Main/fg_props.hxx"
52 #include "Navaids/positioned.hxx"
53 #include <Navaids/waypoint.hxx>
54 #include <Navaids/airways.hxx>
55 #include <Navaids/procedure.hxx>
56 #include "Airports/simple.hxx"
57 #include "Airports/runways.hxx"
59 #define RM "/autopilot/route-manager/"
61 #include <GUI/new_gui.hxx>
62 #include <GUI/dialog.hxx>
64 using namespace flightgear;
66 class PropertyWatcher : public SGPropertyChangeListener
69 void watch(SGPropertyNode* p)
71 p->addChangeListener(this, false);
74 virtual void valueChanged(SGPropertyNode*)
79 virtual void fire() = 0;
83 * Template adapter, created by convenience helper below
86 class MethodPropertyWatcher : public PropertyWatcher
89 typedef void (T::*fire_method)();
91 MethodPropertyWatcher(T* obj, fire_method m) :
98 { // dispatch to the object method we're helping
99 (_object->*_method)();
108 PropertyWatcher* createWatcher(T* obj, void (T::*m)())
110 return new MethodPropertyWatcher<T>(obj, m);
113 static bool commandLoadFlightPlan(const SGPropertyNode* arg)
115 FGRouteMgr* self = (FGRouteMgr*) globals->get_subsystem("route-manager");
116 SGPath path(arg->getStringValue("path"));
117 return self->loadRoute(path);
120 static bool commandSaveFlightPlan(const SGPropertyNode* arg)
122 FGRouteMgr* self = (FGRouteMgr*) globals->get_subsystem("route-manager");
123 SGPath path(arg->getStringValue("path"));
124 return self->saveRoute(path);
127 static bool commandActivateFlightPlan(const SGPropertyNode* arg)
129 FGRouteMgr* self = (FGRouteMgr*) globals->get_subsystem("route-manager");
130 bool activate = arg->getBoolValue("activate", true);
140 static bool commandClearFlightPlan(const SGPropertyNode*)
142 FGRouteMgr* self = (FGRouteMgr*) globals->get_subsystem("route-manager");
147 static bool commandSetActiveWaypt(const SGPropertyNode* arg)
149 FGRouteMgr* self = (FGRouteMgr*) globals->get_subsystem("route-manager");
150 int index = arg->getIntValue("index");
151 if ((index < 0) || (index >= self->numWaypts())) {
155 self->jumpToIndex(index);
159 static bool commandInsertWaypt(const SGPropertyNode* arg)
161 FGRouteMgr* self = (FGRouteMgr*) globals->get_subsystem("route-manager");
162 int index = arg->getIntValue("index");
163 std::string ident(arg->getStringValue("id"));
164 int alt = arg->getIntValue("altitude-ft", -999);
165 int ias = arg->getIntValue("speed-knots", -999);
168 // lat/lon may be supplied to narrow down navaid search, or to specify
171 if (arg->hasChild("longitude-deg")) {
172 pos = SGGeod::fromDeg(arg->getDoubleValue("longitude-deg"),
173 arg->getDoubleValue("latitude-deg"));
176 if (arg->hasChild("navaid")) {
177 FGPositionedRef p = FGPositioned::findClosestWithIdent(arg->getStringValue("navaid"), pos);
179 if (arg->hasChild("navaid", 1)) {
180 // intersection of two radials
181 FGPositionedRef p2 = FGPositioned::findClosestWithIdent(arg->getStringValue("navaid[1]"), pos);
183 SG_LOG( SG_AUTOPILOT, SG_INFO, "Unable to find FGPositioned with ident:" << arg->getStringValue("navaid[1]"));
187 double r1 = arg->getDoubleValue("radial"),
188 r2 = arg->getDoubleValue("radial[1]");
191 bool ok = SGGeodesy::radialIntersection(p->geod(), r1, p2->geod(), r2, intersection);
193 SG_LOG(SG_AUTOPILOT, SG_INFO, "no valid intersection for:" << p->ident()
194 << "," << p2->ident());
198 std::string name = p->ident() + "-" + p2->ident();
199 wp = new BasicWaypt(intersection, name, NULL);
200 } else if (arg->hasChild("offset-nm") && arg->hasChild("radial")) {
201 // offset radial from navaid
202 double radial = arg->getDoubleValue("radial");
203 double distanceNm = arg->getDoubleValue("offset-nm");
204 //radial += magvar->getDoubleValue(); // convert to true bearing
205 wp = new OffsetNavaidWaypoint(p, NULL, radial, distanceNm);
207 wp = new NavaidWaypoint(p, NULL);
209 } else if (arg->hasChild("airport")) {
210 const FGAirport* apt = fgFindAirportID(arg->getStringValue("airport"));
212 SG_LOG(SG_AUTOPILOT, SG_INFO, "no such airport" << arg->getStringValue("airport"));
216 if (arg->hasChild("runway")) {
217 if (!apt->hasRunwayWithIdent(arg->getStringValue("runway"))) {
218 SG_LOG(SG_AUTOPILOT, SG_INFO, "No runway: " << arg->getStringValue("runway") << " at " << apt->ident());
222 FGRunway* runway = apt->getRunwayByIdent(arg->getStringValue("runway"));
223 wp = new RunwayWaypt(runway, NULL);
225 wp = new NavaidWaypoint((FGAirport*) apt, NULL);
227 } else if (arg->hasChild("text")) {
228 wp = self->waypointFromString(arg->getStringValue("text"));
229 } else if (!(pos == SGGeod())) {
230 // just a raw lat/lon
231 wp = new BasicWaypt(pos, ident, NULL);
233 return false; // failed to build waypoint
237 wp->setAltitude(alt, flightgear::RESTRICT_AT);
241 wp->setSpeed(ias, flightgear::RESTRICT_AT);
244 self->insertWayptAtIndex(wp, index);
248 static bool commandDeleteWaypt(const SGPropertyNode* arg)
250 FGRouteMgr* self = (FGRouteMgr*) globals->get_subsystem("route-manager");
251 int index = arg->getIntValue("index");
252 self->removeWayptAtIndex(index);
256 /////////////////////////////////////////////////////////////////////////////
258 FGRouteMgr::FGRouteMgr() :
260 input(fgGetNode( RM "input", true )),
261 mirror(fgGetNode( RM "route", true )),
262 _departureWatcher(NULL),
263 _arrivalWatcher(NULL)
265 listener = new InputListener(this);
266 input->setStringValue("");
267 input->addChangeListener(listener);
269 SGCommandMgr::instance()->addCommand("load-flightplan", commandLoadFlightPlan);
270 SGCommandMgr::instance()->addCommand("save-flightplan", commandSaveFlightPlan);
271 SGCommandMgr::instance()->addCommand("activate-flightplan", commandActivateFlightPlan);
272 SGCommandMgr::instance()->addCommand("clear-flightplan", commandClearFlightPlan);
273 SGCommandMgr::instance()->addCommand("set-active-waypt", commandSetActiveWaypt);
274 SGCommandMgr::instance()->addCommand("insert-waypt", commandInsertWaypt);
275 SGCommandMgr::instance()->addCommand("delete-waypt", commandDeleteWaypt);
279 FGRouteMgr::~FGRouteMgr()
281 input->removeChangeListener(listener);
283 delete _departureWatcher;
284 delete _arrivalWatcher;
288 void FGRouteMgr::init() {
289 SGPropertyNode_ptr rm(fgGetNode(RM));
291 lon = fgGetNode( "/position/longitude-deg", true );
292 lat = fgGetNode( "/position/latitude-deg", true );
293 alt = fgGetNode( "/position/altitude-ft", true );
294 magvar = fgGetNode("/environment/magnetic-variation-deg", true);
296 departure = fgGetNode(RM "departure", true);
297 departure->tie("airport", SGRawValueMethods<FGRouteMgr, const char*>(*this,
298 &FGRouteMgr::getDepartureICAO, &FGRouteMgr::setDepartureICAO));
299 departure->tie("name", SGRawValueMethods<FGRouteMgr, const char*>(*this,
300 &FGRouteMgr::getDepartureName, NULL));
301 departure->setStringValue("runway", "");
303 delete _departureWatcher;
304 _departureWatcher = createWatcher(this, &FGRouteMgr::departureChanged);
305 _departureWatcher->watch(departure->getChild("runway"));
307 departure->getChild("etd", 0, true);
308 _departureWatcher->watch(departure->getChild("sid", 0, true));
309 departure->getChild("takeoff-time", 0, true);
311 destination = fgGetNode(RM "destination", true);
312 destination->getChild("airport", 0, true);
314 destination->tie("airport", SGRawValueMethods<FGRouteMgr, const char*>(*this,
315 &FGRouteMgr::getDestinationICAO, &FGRouteMgr::setDestinationICAO));
316 destination->tie("name", SGRawValueMethods<FGRouteMgr, const char*>(*this,
317 &FGRouteMgr::getDestinationName, NULL));
319 delete _arrivalWatcher;
320 _arrivalWatcher = createWatcher(this, &FGRouteMgr::arrivalChanged);
321 _arrivalWatcher->watch(destination->getChild("runway", 0, true));
323 destination->getChild("eta", 0, true);
324 _arrivalWatcher->watch(destination->getChild("star", 0, true));
325 _arrivalWatcher->watch(destination->getChild("transition", 0, true));
326 destination->getChild("touchdown-time", 0, true);
328 alternate = fgGetNode(RM "alternate", true);
329 alternate->getChild("airport", 0, true);
330 alternate->getChild("runway", 0, true);
332 cruise = fgGetNode(RM "cruise", true);
333 cruise->getChild("altitude-ft", 0, true);
334 cruise->setDoubleValue("altitude-ft", 10000.0);
335 cruise->getChild("flight-level", 0, true);
336 cruise->getChild("speed-kts", 0, true);
337 cruise->setDoubleValue("speed-kts", 160.0);
339 _routingType = cruise->getChild("routing", 0, true);
340 _routingType->setIntValue(ROUTE_HIGH_AIRWAYS);
342 totalDistance = fgGetNode(RM "total-distance", true);
343 totalDistance->setDoubleValue(0.0);
345 ete = fgGetNode(RM "ete", true);
346 ete->setDoubleValue(0.0);
348 elapsedFlightTime = fgGetNode(RM "flight-time", true);
349 elapsedFlightTime->setDoubleValue(0.0);
351 active = fgGetNode(RM "active", true);
352 active->setBoolValue(false);
354 airborne = fgGetNode(RM "airborne", true);
355 airborne->setBoolValue(false);
357 _edited = fgGetNode(RM "signals/edited", true);
358 _finished = fgGetNode(RM "signals/finished", true);
360 _currentWpt = fgGetNode(RM "current-wp", true);
361 _currentWpt->tie(SGRawValueMethods<FGRouteMgr, int>
362 (*this, &FGRouteMgr::currentIndex, &FGRouteMgr::jumpToIndex));
364 // temporary distance / eta calculations, for backward-compatability
365 wp0 = fgGetNode(RM "wp", 0, true);
366 wp0->getChild("id", 0, true);
367 wp0->getChild("dist", 0, true);
368 wp0->getChild("eta", 0, true);
369 wp0->getChild("bearing-deg", 0, true);
371 wp1 = fgGetNode(RM "wp", 1, true);
372 wp1->getChild("id", 0, true);
373 wp1->getChild("dist", 0, true);
374 wp1->getChild("eta", 0, true);
376 wpn = fgGetNode(RM "wp-last", 0, true);
377 wpn->getChild("dist", 0, true);
378 wpn->getChild("eta", 0, true);
381 _pathNode = fgGetNode(RM "file-path", 0, true);
385 void FGRouteMgr::postinit()
387 SGPath path(_pathNode->getStringValue());
388 if (!path.isNull()) {
389 SG_LOG(SG_AUTOPILOT, SG_INFO, "loading flight-plan from: " << path.str());
393 // this code only matters for the --wp option now - perhaps the option
394 // should be deprecated in favour of an explicit flight-plan file?
395 // then the global initial waypoint list could die.
396 string_list *waypoints = globals->get_initial_waypoints();
398 string_list::iterator it;
399 for (it = waypoints->begin(); it != waypoints->end(); ++it) {
400 WayptRef w = waypointFromString(*it);
406 SG_LOG(SG_AUTOPILOT, SG_INFO, "loaded initial waypoints:" << _route.size());
410 weightOnWheels = fgGetNode("/gear/gear[0]/wow", true);
411 // check airbone flag agrees with presets
414 void FGRouteMgr::bind() { }
415 void FGRouteMgr::unbind() { }
417 bool FGRouteMgr::isRouteActive() const
419 return active->getBoolValue();
422 void FGRouteMgr::update( double dt )
425 return; // paused, nothing to do here
428 double groundSpeed = fgGetDouble("/velocities/groundspeed-kt", 0.0);
429 if (airborne->getBoolValue()) {
430 time_t now = time(NULL);
431 elapsedFlightTime->setDoubleValue(difftime(now, _takeoffTime));
432 } else { // not airborne
433 if (weightOnWheels->getBoolValue() || (groundSpeed < 40)) {
437 airborne->setBoolValue(true);
438 _takeoffTime = time(NULL); // start the clock
439 departure->setIntValue("takeoff-time", _takeoffTime);
442 if (!active->getBoolValue()) {
446 // basic course/distance information
447 SGGeod currentPos = SGGeod::fromDegFt(lon->getDoubleValue(),
448 lat->getDoubleValue(),alt->getDoubleValue());
450 Waypt* curWpt = currentWaypt();
457 boost::tie(courseDeg, distanceM) = curWpt->courseAndDistanceFrom(currentPos);
459 // update wp0 / wp1 / wp-last for legacy users
460 wp0->setDoubleValue("dist", distanceM * SG_METER_TO_NM);
461 courseDeg -= magvar->getDoubleValue(); // expose magnetic bearing
462 wp0->setDoubleValue("bearing-deg", courseDeg);
463 setETAPropertyFromDistance(wp0->getChild("eta"), distanceM);
465 double totalDistanceRemaining = distanceM; // distance to current waypoint
467 Waypt* nextWpt = nextWaypt();
469 boost::tie(courseDeg, distanceM) = nextWpt->courseAndDistanceFrom(currentPos);
471 wp1->setDoubleValue("dist", distanceM * SG_METER_TO_NM);
472 courseDeg -= magvar->getDoubleValue(); // expose magnetic bearing
473 wp1->setDoubleValue("bearing-deg", courseDeg);
474 setETAPropertyFromDistance(wp1->getChild("eta"), distanceM);
477 Waypt* prev = curWpt;
478 for (unsigned int i=_currentIndex + 1; i<_route.size(); ++i) {
479 Waypt* w = _route[i];
480 if (w->flag(WPT_DYNAMIC)) continue;
481 totalDistanceRemaining += SGGeodesy::distanceM(prev->position(), w->position());
487 wpn->setDoubleValue("dist", totalDistanceRemaining * SG_METER_TO_NM);
488 ete->setDoubleValue(totalDistanceRemaining * SG_METER_TO_NM / groundSpeed * 3600.0);
489 setETAPropertyFromDistance(wpn->getChild("eta"), totalDistanceRemaining);
492 void FGRouteMgr::setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDistance)
494 double speed = fgGetDouble("/velocities/groundspeed-kt", 0.0);
496 aProp->setStringValue("--:--");
501 double eta = aDistance * SG_METER_TO_NM / speed;
502 if ( eta >= 100.0 ) {
503 eta = 99.999; // clamp
506 if ( eta < (1.0/6.0) ) {
507 eta *= 60.0; // within 10 minutes, bump up to min/secs
510 int major = (int)eta,
511 minor = (int)((eta - (int)eta) * 60.0);
512 snprintf( eta_str, 64, "%d:%02d", major, minor );
513 aProp->setStringValue( eta_str );
516 flightgear::WayptRef FGRouteMgr::removeWayptAtIndex(int aIndex)
519 if (aIndex < 0) { // negative indices count the the end
520 index = _route.size() + index;
523 if ((index < 0) || (index >= numWaypts())) {
524 SG_LOG(SG_AUTOPILOT, SG_WARN, "removeWayptAtIndex with invalid index:" << aIndex);
527 WayptVec::iterator it = _route.begin();
530 WayptRef w = *it; // hold a ref now, in case _route is the only other owner
535 if (_currentIndex == index) {
536 currentWaypointChanged(); // current waypoint was removed
539 if (_currentIndex > index) {
540 --_currentIndex; // shift current index down if necessary
543 _edited->fireValueChanged();
549 struct NotGeneratedWayptPredicate : public std::unary_function<const Waypt*, bool>
551 bool operator() (const Waypt* w) const
553 return (w->flag(WPT_GENERATED) == false);
558 void FGRouteMgr::clearRoute()
560 // erase all non-generated waypoints
561 WayptVec::iterator r =
562 std::remove_if(_route.begin(), _route.end(), NotGeneratedWayptPredicate());
563 _route.erase(r, _route.end());
568 active->setBoolValue(false);
569 _edited->fireValueChanged();
573 * route between index-1 and index, using airways.
575 bool FGRouteMgr::routeToIndex(int index, RouteType aRouteType)
581 index = _route.size(); // can still be zero, of course
586 SG_LOG(SG_AUTOPILOT, SG_WARN, "routeToIndex: no departure set");
590 wp1 = new NavaidWaypoint(_departure.get(), NULL);
592 wp1 = wayptAtIndex(index - 1);
595 if (index >= numWaypts()) {
597 SG_LOG(SG_AUTOPILOT, SG_WARN, "routeToIndex: no destination set");
601 wp2 = new NavaidWaypoint(_destination.get(), NULL);
603 wp2 = wayptAtIndex(index);
606 double distNm = SGGeodesy::distanceNm(wp1->position(), wp2->position());
607 if (distNm < 100.0) {
608 SG_LOG(SG_AUTOPILOT, SG_INFO, "routeToIndex: existing waypoints are nearby, direct route");
613 switch (aRouteType) {
614 case ROUTE_HIGH_AIRWAYS:
615 Airway::highLevel()->route(wp1, wp2, r);
618 case ROUTE_LOW_AIRWAYS:
619 Airway::lowLevel()->route(wp1, wp2, r);
623 throw sg_exception("VOR routing not supported yet");
627 SG_LOG(SG_AUTOPILOT, SG_INFO, "routeToIndex: no route found");
631 WayptVec::iterator it = _route.begin();
633 _route.insert(it, r.begin(), r.end());
636 _edited->fireValueChanged();
640 void FGRouteMgr::autoRoute()
642 if (!_departure || !_destination) {
646 string runwayId(departure->getStringValue("runway"));
647 FGRunway* runway = NULL;
648 if (_departure->hasRunwayWithIdent(runwayId)) {
649 runway = _departure->getRunwayByIdent(runwayId);
652 FGRunway* dstRunway = NULL;
653 runwayId = destination->getStringValue("runway");
654 if (_destination->hasRunwayWithIdent(runwayId)) {
655 dstRunway = _destination->getRunwayByIdent(runwayId);
658 _route.clear(); // clear out the existing, first
660 flightgear::SID* sid;
663 boost::tie(sid, sidTrans) = _departure->selectSID(_destination->geod(), runway);
665 SG_LOG(SG_AUTOPILOT, SG_INFO, "selected SID " << sid->ident());
667 SG_LOG(SG_AUTOPILOT, SG_INFO, "\tvia " << sidTrans->ident() << " transition");
670 sid->route(runway, sidTrans, _route);
671 departure->setStringValue("sid", sid->ident());
673 // use airport location for airway search
674 sidTrans = new NavaidWaypoint(_departure.get(), NULL);
675 departure->setStringValue("sid", "");
679 destination->setStringValue("transition", "");
680 destination->setStringValue("star", "");
684 boost::tie(star, starTrans) = _destination->selectSTAR(_departure->geod(), dstRunway);
686 SG_LOG(SG_AUTOPILOT, SG_INFO, "selected STAR " << star->ident());
688 SG_LOG(SG_AUTOPILOT, SG_INFO, "\tvia " << starTrans->ident() << " transition");
689 destination->setStringValue("transition", starTrans->ident());
691 destination->setStringValue("star", star->ident());
693 // use airport location for search
694 starTrans = new NavaidWaypoint(_destination.get(), NULL);
697 // route between them
698 WayptVec airwayRoute;
699 if (Airway::highLevel()->route(sidTrans, starTrans, airwayRoute)) {
700 _route.insert(_route.end(), airwayRoute.begin(), airwayRoute.end());
703 // add the STAR if we have one
705 _destination->buildApproach(starTrans, star, dstRunway, _route);
709 _edited->fireValueChanged();
712 void FGRouteMgr::departureChanged()
714 // remove existing departure waypoints
715 WayptVec::iterator it = _route.begin();
716 for (; it != _route.end(); ++it) {
717 if (!(*it)->flag(WPT_DEPARTURE)) {
722 // erase() invalidates iterators, so grab now
724 if (it == _route.end()) {
726 enroute = new NavaidWaypoint(_destination.get(), NULL);
732 _route.erase(_route.begin(), it);
739 buildDeparture(enroute, wps);
740 for (it = wps.begin(); it != wps.end(); ++it) {
741 (*it)->setFlag(WPT_DEPARTURE);
742 (*it)->setFlag(WPT_GENERATED);
744 _route.insert(_route.begin(), wps.begin(), wps.end());
750 void FGRouteMgr::buildDeparture(WayptRef enroute, WayptVec& wps)
752 string runwayId(departure->getStringValue("runway"));
753 if (!_departure->hasRunwayWithIdent(runwayId)) {
754 // valid airport, but no runway selected, so just the airport noide itself
755 wps.push_back(new NavaidWaypoint(_departure.get(), NULL));
759 FGRunway* r = _departure->getRunwayByIdent(runwayId);
760 string sidId = departure->getStringValue("sid");
761 flightgear::SID* sid = _departure->findSIDWithIdent(sidId);
763 // valid runway, but no SID selected/found, so just the runway node for now
764 if (!sidId.empty() && (sidId != "(none)")) {
765 SG_LOG(SG_AUTOPILOT, SG_INFO, "SID not found:" << sidId);
768 wps.push_back(new RunwayWaypt(r, NULL));
772 // we have a valid SID, awesome
773 string trans(departure->getStringValue("transition"));
774 WayptRef t = sid->findTransitionByName(trans);
776 t = sid->findBestTransition(enroute->position());
779 sid->route(r, t, wps);
780 if (!wps.empty() && wps.front()->flag(WPT_DYNAMIC)) {
781 // ensure first waypoint is static, to simplify other computations
782 wps.insert(wps.begin(), new RunwayWaypt(r, NULL));
786 void FGRouteMgr::arrivalChanged()
788 // remove existing arrival waypoints
789 WayptVec::reverse_iterator rit = _route.rbegin();
790 for (; rit != _route.rend(); ++rit) {
791 if (!(*rit)->flag(WPT_ARRIVAL)) {
796 // erase() invalidates iterators, so grab now
798 WayptVec::iterator it;
800 if (rit != _route.rend()) {
802 it = rit.base(); // convert to fwd iterator
807 _route.erase(it, _route.end());
810 buildArrival(enroute, wps);
811 for (it = wps.begin(); it != wps.end(); ++it) {
812 (*it)->setFlag(WPT_ARRIVAL);
813 (*it)->setFlag(WPT_GENERATED);
815 _route.insert(_route.end(), wps.begin(), wps.end());
821 void FGRouteMgr::buildArrival(WayptRef enroute, WayptVec& wps)
827 string runwayId(destination->getStringValue("runway"));
828 if (!_destination->hasRunwayWithIdent(runwayId)) {
829 // valid airport, but no runway selected, so just the airport node itself
830 wps.push_back(new NavaidWaypoint(_destination.get(), NULL));
834 FGRunway* r = _destination->getRunwayByIdent(runwayId);
835 string starId = destination->getStringValue("star");
836 STAR* star = _destination->findSTARWithIdent(starId);
838 // valid runway, but no STAR selected/found, so just the runway node for now
839 wps.push_back(new RunwayWaypt(r, NULL));
843 // we have a valid STAR
844 string trans(destination->getStringValue("transition"));
845 WayptRef t = star->findTransitionByName(trans);
847 t = star->findBestTransition(enroute->position());
850 _destination->buildApproach(t, star, r, wps);
853 void FGRouteMgr::waypointsChanged()
858 void FGRouteMgr::insertWayptAtIndex(Waypt* aWpt, int aIndex)
865 if ((aIndex == -1) || (aIndex > (int) _route.size())) {
866 index = _route.size();
869 WayptVec::iterator it = _route.begin();
872 if (_currentIndex >= index) {
876 _route.insert(it, aWpt);
879 _edited->fireValueChanged();
882 WayptRef FGRouteMgr::waypointFromString(const string& tgt )
884 string target(boost::to_upper_copy(tgt));
888 double altFt = cruise->getDoubleValue("altitude-ft");
889 RouteRestriction altSetting = RESTRICT_NONE;
891 size_t pos = target.find( '@' );
892 if ( pos != string::npos ) {
893 altFt = atof( target.c_str() + pos + 1 );
894 target = target.substr( 0, pos );
895 if ( !strcmp(fgGetString("/sim/startup/units"), "meter") )
896 altFt *= SG_METER_TO_FEET;
897 altSetting = RESTRICT_AT;
901 pos = target.find( ',' );
902 if ( pos != string::npos ) {
903 double lon = atof( target.substr(0, pos).c_str());
904 double lat = atof( target.c_str() + pos + 1);
906 char ew = (lon < 0.0) ? 'W' : 'E';
907 char ns = (lat < 0.0) ? 'S' : 'N';
908 snprintf(buf, 32, "%c%03d%c%03d", ew, (int) fabs(lon), ns, (int)fabs(lat));
910 wpt = new BasicWaypt(SGGeod::fromDeg(lon, lat), buf, NULL);
911 if (altSetting != RESTRICT_NONE) {
912 wpt->setAltitude(altFt, altSetting);
918 if (_route.empty()) {
919 // route is empty, use current position
920 basePosition = SGGeod::fromDeg(lon->getDoubleValue(), lat->getDoubleValue());
922 basePosition = _route.back()->position();
925 string_list pieces(simgear::strutils::split(target, "/"));
926 FGPositionedRef p = FGPositioned::findClosestWithIdent(pieces.front(), basePosition);
928 SG_LOG( SG_AUTOPILOT, SG_INFO, "Unable to find FGPositioned with ident:" << pieces.front());
932 if (pieces.size() == 1) {
933 wpt = new NavaidWaypoint(p, NULL);
934 } else if (pieces.size() == 3) {
935 // navaid/radial/distance-nm notation
936 double radial = atof(pieces[1].c_str()),
937 distanceNm = atof(pieces[2].c_str());
938 radial += magvar->getDoubleValue(); // convert to true bearing
939 wpt = new OffsetNavaidWaypoint(p, NULL, radial, distanceNm);
940 } else if (pieces.size() == 2) {
941 FGAirport* apt = dynamic_cast<FGAirport*>(p.ptr());
943 SG_LOG(SG_AUTOPILOT, SG_INFO, "Waypoint is not an airport:" << pieces.front());
947 if (!apt->hasRunwayWithIdent(pieces[1])) {
948 SG_LOG(SG_AUTOPILOT, SG_INFO, "No runway: " << pieces[1] << " at " << pieces[0]);
952 FGRunway* runway = apt->getRunwayByIdent(pieces[1]);
953 wpt = new NavaidWaypoint(runway, NULL);
954 } else if (pieces.size() == 4) {
955 // navid/radial/navid/radial notation
956 FGPositionedRef p2 = FGPositioned::findClosestWithIdent(pieces[2], basePosition);
958 SG_LOG( SG_AUTOPILOT, SG_INFO, "Unable to find FGPositioned with ident:" << pieces[2]);
962 double r1 = atof(pieces[1].c_str()),
963 r2 = atof(pieces[3].c_str());
964 r1 += magvar->getDoubleValue();
965 r2 += magvar->getDoubleValue();
968 bool ok = SGGeodesy::radialIntersection(p->geod(), r1, p2->geod(), r2, intersection);
970 SG_LOG(SG_AUTOPILOT, SG_INFO, "no valid intersection for:" << target);
974 std::string name = p->ident() + "-" + p2->ident();
975 wpt = new BasicWaypt(intersection, name, NULL);
979 SG_LOG(SG_AUTOPILOT, SG_INFO, "Unable to parse waypoint:" << target);
983 if (altSetting != RESTRICT_NONE) {
984 wpt->setAltitude(altFt, altSetting);
989 // mirror internal route to the property system for inspection by other subsystems
990 void FGRouteMgr::update_mirror()
992 mirror->removeChildren("wp");
994 int num = numWaypts();
995 double totalDistanceEnroute = 0.0;
997 for (int i = 0; i < num; i++) {
998 Waypt* wp = _route[i];
999 SGPropertyNode *prop = mirror->getChild("wp", i, 1);
1001 const SGGeod& pos(wp->position());
1002 prop->setStringValue("id", wp->ident().c_str());
1003 prop->setDoubleValue("longitude-deg", pos.getLongitudeDeg());
1004 prop->setDoubleValue("latitude-deg",pos.getLatitudeDeg());
1006 // leg course+distance
1007 if (i < (num - 1)) {
1008 Waypt* next = _route[i+1];
1009 std::pair<double, double> crsDist =
1010 next->courseAndDistanceFrom(pos);
1011 prop->setDoubleValue("leg-bearing-true-deg", crsDist.first);
1012 prop->setDoubleValue("leg-distance-nm", crsDist.second * SG_METER_TO_NM);
1013 prop->setDoubleValue("distance-along-route-nm", totalDistanceEnroute);
1014 totalDistanceEnroute += crsDist.second * SG_METER_TO_NM;
1017 if (wp->altitudeRestriction() != RESTRICT_NONE) {
1018 double ft = wp->altitudeFt();
1019 prop->setDoubleValue("altitude-m", ft * SG_FEET_TO_METER);
1020 prop->setDoubleValue("altitude-ft", ft);
1021 prop->setIntValue("flight-level", static_cast<int>(ft / 1000) * 10);
1023 prop->setDoubleValue("altitude-m", -9999.9);
1024 prop->setDoubleValue("altitude-ft", -9999.9);
1027 if (wp->speedRestriction() == SPEED_RESTRICT_MACH) {
1028 prop->setDoubleValue("speed-mach", wp->speedMach());
1029 } else if (wp->speedRestriction() != RESTRICT_NONE) {
1030 prop->setDoubleValue("speed-kts", wp->speedKts());
1033 if (wp->flag(WPT_ARRIVAL)) {
1034 prop->setBoolValue("arrival", true);
1037 if (wp->flag(WPT_DEPARTURE)) {
1038 prop->setBoolValue("departure", true);
1041 if (wp->flag(WPT_MISS)) {
1042 prop->setBoolValue("missed-approach", true);
1045 prop->setBoolValue("generated", wp->flag(WPT_GENERATED));
1046 } // of waypoint iteration
1048 // set number as listener attachment point
1049 mirror->setIntValue("num", _route.size());
1051 NewGUI * gui = (NewGUI *)globals->get_subsystem("gui");
1052 FGDialog* rmDlg = gui->getDialog("route-manager");
1054 rmDlg->updateValues();
1058 departure->setDoubleValue("field-elevation-ft", _departure->getElevation());
1062 destination->setDoubleValue("field-elevation-ft", _destination->getElevation());
1065 totalDistance->setDoubleValue(totalDistanceEnroute);
1068 // command interface /autopilot/route-manager/input:
1070 // @CLEAR ... clear route
1071 // @POP ... remove first entry
1072 // @DELETE3 ... delete 4th entry
1073 // @INSERT2:KSFO@900 ... insert "KSFO@900" as 3rd entry
1074 // KSFO@900 ... append "KSFO@900"
1076 void FGRouteMgr::InputListener::valueChanged(SGPropertyNode *prop)
1078 const char *s = prop->getStringValue();
1079 if (strlen(s) == 0) {
1083 if (!strcmp(s, "@CLEAR"))
1085 else if (!strcmp(s, "@ACTIVATE"))
1087 else if (!strcmp(s, "@LOAD")) {
1088 SGPath path(mgr->_pathNode->getStringValue());
1089 mgr->loadRoute(path);
1090 } else if (!strcmp(s, "@SAVE")) {
1091 SGPath path(mgr->_pathNode->getStringValue());
1092 mgr->saveRoute(path);
1093 } else if (!strcmp(s, "@NEXT")) {
1094 mgr->jumpToIndex(mgr->_currentIndex + 1);
1095 } else if (!strcmp(s, "@PREVIOUS")) {
1096 mgr->jumpToIndex(mgr->_currentIndex - 1);
1097 } else if (!strncmp(s, "@JUMP", 5)) {
1098 mgr->jumpToIndex(atoi(s + 5));
1099 } else if (!strncmp(s, "@DELETE", 7))
1100 mgr->removeWayptAtIndex(atoi(s + 7));
1101 else if (!strncmp(s, "@INSERT", 7)) {
1103 int pos = strtol(s + 7, &r, 10);
1109 mgr->insertWayptAtIndex(mgr->waypointFromString(r), pos);
1110 } else if (!strncmp(s, "@ROUTE", 6)) {
1112 int endIndex = strtol(s + 6, &r, 10);
1113 RouteType rt = (RouteType) mgr->_routingType->getIntValue();
1114 mgr->routeToIndex(endIndex, rt);
1115 } else if (!strcmp(s, "@AUTOROUTE")) {
1117 } else if (!strcmp(s, "@POSINIT")) {
1118 mgr->initAtPosition();
1120 mgr->insertWayptAtIndex(mgr->waypointFromString(s), -1);
1123 void FGRouteMgr::initAtPosition()
1125 if (isRouteActive()) {
1126 return; // don't mess with the active route
1129 if (haveUserWaypoints()) {
1130 // user has already defined, loaded or entered a route, again
1131 // don't interfere with it
1135 if (airborne->getBoolValue()) {
1136 SG_LOG(SG_AUTOPILOT, SG_INFO, "initAtPosition: airborne, clearing departure info");
1138 departure->setStringValue("runway", "");
1143 SGGeod pos = SGGeod::fromDegFt(lon->getDoubleValue(),
1144 lat->getDoubleValue(), alt->getDoubleValue());
1146 _departure = FGAirport::findClosest(pos, 20.0);
1148 SG_LOG(SG_AUTOPILOT, SG_INFO, "initAtPosition: couldn't find an airport within 20nm");
1149 departure->setStringValue("runway", "");
1154 std::string rwy = departure->getStringValue("runway");
1156 // runway already set, fine
1160 FGRunway* r = _departure->findBestRunwayForPos(pos);
1165 departure->setStringValue("runway", r->ident().c_str());
1166 SG_LOG(SG_AUTOPILOT, SG_INFO, "initAtPosition: starting at "
1167 << _departure->ident() << " on runway " << r->ident());
1170 bool FGRouteMgr::haveUserWaypoints() const
1172 return std::find_if(_route.begin(), _route.end(), NotGeneratedWayptPredicate()) != _route.end();
1175 bool FGRouteMgr::activate()
1177 if (isRouteActive()) {
1178 SG_LOG(SG_AUTOPILOT, SG_WARN, "duplicate route-activation, no-op");
1183 currentWaypointChanged();
1185 /* double routeDistanceNm = _route->total_distance() * SG_METER_TO_NM;
1186 totalDistance->setDoubleValue(routeDistanceNm);
1187 double cruiseSpeedKts = cruise->getDoubleValue("speed", 0.0);
1188 if (cruiseSpeedKts > 1.0) {
1189 // very very crude approximation, doesn't allow for climb / descent
1190 // performance or anything else at all
1191 ete->setDoubleValue(routeDistanceNm / cruiseSpeedKts * (60.0 * 60.0));
1194 active->setBoolValue(true);
1195 SG_LOG(SG_AUTOPILOT, SG_INFO, "route-manager, activate route ok");
1200 void FGRouteMgr::sequence()
1202 if (!active->getBoolValue()) {
1203 SG_LOG(SG_AUTOPILOT, SG_ALERT, "trying to sequence waypoints with no active route");
1207 if (checkFinished()) {
1212 currentWaypointChanged();
1215 bool FGRouteMgr::checkFinished()
1217 if (_currentIndex < (int) _route.size()) {
1221 SG_LOG(SG_AUTOPILOT, SG_INFO, "reached end of active route");
1222 _finished->fireValueChanged();
1223 active->setBoolValue(false);
1227 void FGRouteMgr::jumpToIndex(int index)
1229 if ((index < 0) || (index >= (int) _route.size())) {
1230 SG_LOG(SG_AUTOPILOT, SG_ALERT, "passed invalid index (" <<
1231 index << ") to FGRouteMgr::jumpToIndex");
1235 if (_currentIndex == index) {
1239 // all the checks out the way, go ahead and update state
1240 _currentIndex = index;
1241 currentWaypointChanged();
1242 _currentWpt->fireValueChanged();
1245 void FGRouteMgr::currentWaypointChanged()
1247 Waypt* cur = currentWaypt();
1248 Waypt* next = nextWaypt();
1250 wp0->getChild("id")->setStringValue(cur ? cur->ident() : "");
1251 wp1->getChild("id")->setStringValue(next ? next->ident() : "");
1253 _currentWpt->fireValueChanged();
1254 SG_LOG(SG_AUTOPILOT, SG_INFO, "route manager, current-wp is now " << _currentIndex);
1257 int FGRouteMgr::findWayptIndex(const SGGeod& aPos) const
1259 for (int i=0; i<numWaypts(); ++i) {
1260 if (_route[i]->matches(aPos)) {
1268 Waypt* FGRouteMgr::currentWaypt() const
1270 if ((_currentIndex < 0) || (_currentIndex >= numWaypts()))
1272 return wayptAtIndex(_currentIndex);
1275 Waypt* FGRouteMgr::previousWaypt() const
1277 if (_currentIndex == 0) {
1281 return wayptAtIndex(_currentIndex - 1);
1284 Waypt* FGRouteMgr::nextWaypt() const
1286 if ((_currentIndex < 0) || ((_currentIndex + 1) >= numWaypts())) {
1290 return wayptAtIndex(_currentIndex + 1);
1293 Waypt* FGRouteMgr::wayptAtIndex(int index) const
1295 if ((index < 0) || (index >= numWaypts())) {
1296 throw sg_range_exception("waypt index out of range", "FGRouteMgr::wayptAtIndex");
1299 return _route[index];
1302 SGPropertyNode_ptr FGRouteMgr::wayptNodeAtIndex(int index) const
1304 if ((index < 0) || (index >= numWaypts())) {
1305 throw sg_range_exception("waypt index out of range", "FGRouteMgr::wayptAtIndex");
1308 return mirror->getChild("wp", index);
1311 bool FGRouteMgr::saveRoute(const SGPath& path)
1313 SG_LOG(SG_IO, SG_INFO, "Saving route to " << path.str());
1315 SGPropertyNode_ptr d(new SGPropertyNode);
1316 SGPath path(_pathNode->getStringValue());
1317 d->setIntValue("version", 2);
1320 d->setStringValue("departure/airport", _departure->ident());
1321 d->setStringValue("departure/sid", departure->getStringValue("sid"));
1322 d->setStringValue("departure/runway", departure->getStringValue("runway"));
1326 d->setStringValue("destination/airport", _destination->ident());
1327 d->setStringValue("destination/star", destination->getStringValue("star"));
1328 d->setStringValue("destination/transition", destination->getStringValue("transition"));
1329 d->setStringValue("destination/runway", destination->getStringValue("runway"));
1333 SGPropertyNode* routeNode = d->getChild("route", 0, true);
1334 for (unsigned int i=0; i<_route.size(); ++i) {
1335 Waypt* wpt = _route[i];
1336 wpt->saveAsNode(routeNode->getChild("wp", i, true));
1337 } // of waypoint iteration
1338 writeProperties(path.str(), d, true /* write-all */);
1340 } catch (sg_exception& e) {
1341 SG_LOG(SG_IO, SG_ALERT, "Failed to save flight-plan '" << path.str() << "'. " << e.getMessage());
1346 bool FGRouteMgr::loadRoute(const SGPath& path)
1350 SG_LOG(SG_IO, SG_ALERT, "Failed to load flight-plan '" << path.str()
1351 << "'. The file does not exist.");
1355 // deactivate route first
1356 active->setBoolValue(false);
1358 SGPropertyNode_ptr routeData(new SGPropertyNode);
1360 SG_LOG(SG_IO, SG_INFO, "going to read flight-plan from:" << path.str());
1362 bool Status = false;
1364 readProperties(path.str(), routeData);
1365 } catch (sg_exception& ) {
1366 // if XML parsing fails, the file might be simple textual list of waypoints
1367 Status = loadPlainTextRoute(path);
1371 if (routeData.valid())
1374 int version = routeData->getIntValue("version", 1);
1376 loadVersion1XMLRoute(routeData);
1377 } else if (version == 2) {
1378 loadVersion2XMLRoute(routeData);
1380 throw sg_io_exception("unsupported XML route version");
1383 } catch (sg_exception& e) {
1384 SG_LOG(SG_IO, SG_ALERT, "Failed to load flight-plan '" << e.getOrigin()
1385 << "'. " << e.getMessage());
1395 void FGRouteMgr::loadXMLRouteHeader(SGPropertyNode_ptr routeData)
1398 SGPropertyNode* dep = routeData->getChild("departure");
1400 string depIdent = dep->getStringValue("airport");
1401 _departure = (FGAirport*) fgFindAirportID(depIdent);
1402 departure->setStringValue("runway", dep->getStringValue("runway"));
1403 departure->setStringValue("sid", dep->getStringValue("sid"));
1404 departure->setStringValue("transition", dep->getStringValue("transition"));
1408 SGPropertyNode* dst = routeData->getChild("destination");
1410 _destination = (FGAirport*) fgFindAirportID(dst->getStringValue("airport"));
1411 destination->setStringValue("runway", dst->getStringValue("runway"));
1412 destination->setStringValue("star", dst->getStringValue("star"));
1413 destination->setStringValue("transition", dst->getStringValue("transition"));
1417 SGPropertyNode* alt = routeData->getChild("alternate");
1419 alternate->setStringValue(alt->getStringValue("airport"));
1420 } // of cruise data loading
1423 SGPropertyNode* crs = routeData->getChild("cruise");
1425 cruise->setDoubleValue("speed-kts", crs->getDoubleValue("speed-kts"));
1426 cruise->setDoubleValue("mach", crs->getDoubleValue("mach"));
1427 cruise->setDoubleValue("altitude-ft", crs->getDoubleValue("altitude-ft"));
1428 } // of cruise data loading
1432 void FGRouteMgr::loadVersion2XMLRoute(SGPropertyNode_ptr routeData)
1434 loadXMLRouteHeader(routeData);
1438 SGPropertyNode_ptr routeNode = routeData->getChild("route", 0);
1439 for (int i=0; i<routeNode->nChildren(); ++i) {
1440 SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i);
1441 WayptRef wpt = Waypt::createFromProperties(NULL, wpNode);
1442 wpts.push_back(wpt);
1443 } // of route iteration
1448 void FGRouteMgr::loadVersion1XMLRoute(SGPropertyNode_ptr routeData)
1450 loadXMLRouteHeader(routeData);
1454 SGPropertyNode_ptr routeNode = routeData->getChild("route", 0);
1455 for (int i=0; i<routeNode->nChildren(); ++i) {
1456 SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i);
1457 WayptRef wpt = parseVersion1XMLWaypt(wpNode);
1458 wpts.push_back(wpt);
1459 } // of route iteration
1464 WayptRef FGRouteMgr::parseVersion1XMLWaypt(SGPropertyNode* aWP)
1467 if (!_route.empty()) {
1468 lastPos = _route.back()->position();
1469 } else if (_departure) {
1470 lastPos = _departure->geod();
1474 string ident(aWP->getStringValue("ident"));
1475 if (aWP->hasChild("longitude-deg")) {
1476 // explicit longitude/latitude
1477 w = new BasicWaypt(SGGeod::fromDeg(aWP->getDoubleValue("longitude-deg"),
1478 aWP->getDoubleValue("latitude-deg")), ident, NULL);
1481 string nid = aWP->getStringValue("navid", ident.c_str());
1482 FGPositionedRef p = FGPositioned::findClosestWithIdent(nid, lastPos);
1484 throw sg_io_exception("bad route file, unknown navid:" + nid);
1487 SGGeod pos(p->geod());
1488 if (aWP->hasChild("offset-nm") && aWP->hasChild("offset-radial")) {
1489 double radialDeg = aWP->getDoubleValue("offset-radial");
1490 // convert magnetic radial to a true radial!
1491 radialDeg += magvar->getDoubleValue();
1492 double offsetNm = aWP->getDoubleValue("offset-nm");
1494 SGGeodesy::direct(p->geod(), radialDeg, offsetNm * SG_NM_TO_METER, pos, az2);
1497 w = new BasicWaypt(pos, ident, NULL);
1500 double altFt = aWP->getDoubleValue("altitude-ft", -9999.9);
1501 if (altFt > -9990.0) {
1502 w->setAltitude(altFt, RESTRICT_AT);
1508 bool FGRouteMgr::loadPlainTextRoute(const SGPath& path)
1511 sg_gzifstream in(path.str().c_str());
1512 if (!in.is_open()) {
1513 throw sg_io_exception("Cannot open file for reading.");
1519 getline(in, line, '\n');
1520 // trim CR from end of line, if found
1521 if (line[line.size() - 1] == '\r') {
1522 line.erase(line.size() - 1, 1);
1525 line = simgear::strutils::strip(line);
1526 if (line.empty() || (line[0] == '#')) {
1527 continue; // ignore empty/comment lines
1530 WayptRef w = waypointFromString(line);
1532 throw sg_io_exception("Failed to create waypoint from line '" + line + "'.");
1536 } // of line iteration
1540 } catch (sg_exception& e) {
1541 SG_LOG(SG_IO, SG_ALERT, "Failed to load route from: '" << path.str() << "'. " << e.getMessage());
1546 const char* FGRouteMgr::getDepartureICAO() const
1552 return _departure->ident().c_str();
1555 const char* FGRouteMgr::getDepartureName() const
1561 return _departure->name().c_str();
1564 void FGRouteMgr::setDepartureICAO(const char* aIdent)
1566 if ((aIdent == NULL) || (strlen(aIdent) < 4)) {
1569 _departure = FGAirport::findByIdent(aIdent);
1575 const char* FGRouteMgr::getDestinationICAO() const
1577 if (!_destination) {
1581 return _destination->ident().c_str();
1584 const char* FGRouteMgr::getDestinationName() const
1586 if (!_destination) {
1590 return _destination->name().c_str();
1593 void FGRouteMgr::setDestinationICAO(const char* aIdent)
1595 if ((aIdent == NULL) || (strlen(aIdent) < 4)) {
1596 _destination = NULL;
1598 _destination = FGAirport::findByIdent(aIdent);
1604 FGAirportRef FGRouteMgr::departureAirport() const
1609 FGAirportRef FGRouteMgr::destinationAirport() const
1611 return _destination;
1614 FGRunway* FGRouteMgr::departureRunway() const
1620 string runwayId(departure->getStringValue("runway"));
1621 if (!_departure->hasRunwayWithIdent(runwayId)) {
1625 return _departure->getRunwayByIdent(runwayId);
1628 FGRunway* FGRouteMgr::destinationRunway() const
1630 if (!_destination) {
1634 string runwayId(destination->getStringValue("runway"));
1635 if (!_destination->hasRunwayWithIdent(runwayId)) {
1639 return _destination->getRunwayByIdent(runwayId);