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/misc/sgstream.hxx>
45 #include <simgear/props/props_io.hxx>
46 #include <simgear/misc/sg_path.hxx>
47 #include <simgear/route/route.hxx>
48 #include <simgear/sg_inlines.h>
50 #include "Main/fg_props.hxx"
51 #include "Navaids/positioned.hxx"
52 #include <Navaids/waypoint.hxx>
53 #include <Navaids/airways.hxx>
54 #include <Navaids/procedure.hxx>
55 #include "Airports/simple.hxx"
56 #include "Airports/runways.hxx"
58 #define RM "/autopilot/route-manager/"
60 #include <GUI/new_gui.hxx>
61 #include <GUI/dialog.hxx>
63 using namespace flightgear;
65 class PropertyWatcher : public SGPropertyChangeListener
68 void watch(SGPropertyNode* p)
70 p->addChangeListener(this, false);
73 virtual void valueChanged(SGPropertyNode*)
78 virtual void fire() = 0;
82 * Template adapter, created by convenience helper below
85 class MethodPropertyWatcher : public PropertyWatcher
88 typedef void (T::*fire_method)();
90 MethodPropertyWatcher(T* obj, fire_method m) :
97 { // dispatch to the object method we're helping
98 (_object->*_method)();
107 PropertyWatcher* createWatcher(T* obj, void (T::*m)())
109 return new MethodPropertyWatcher<T>(obj, m);
112 FGRouteMgr::FGRouteMgr() :
114 input(fgGetNode( RM "input", true )),
115 mirror(fgGetNode( RM "route", true ))
117 listener = new InputListener(this);
118 input->setStringValue("");
119 input->addChangeListener(listener);
123 FGRouteMgr::~FGRouteMgr()
125 input->removeChangeListener(listener);
130 void FGRouteMgr::init() {
131 SGPropertyNode_ptr rm(fgGetNode(RM));
133 lon = fgGetNode( "/position/longitude-deg", true );
134 lat = fgGetNode( "/position/latitude-deg", true );
135 alt = fgGetNode( "/position/altitude-ft", true );
136 magvar = fgGetNode("/environment/magnetic-variation-deg", true);
138 departure = fgGetNode(RM "departure", true);
139 departure->tie("airport", SGRawValueMethods<FGRouteMgr, const char*>(*this,
140 &FGRouteMgr::getDepartureICAO, &FGRouteMgr::setDepartureICAO));
141 departure->tie("name", SGRawValueMethods<FGRouteMgr, const char*>(*this,
142 &FGRouteMgr::getDepartureName, NULL));
143 departure->setStringValue("runway", "");
145 _departureWatcher = createWatcher(this, &FGRouteMgr::departureChanged);
146 _departureWatcher->watch(departure->getChild("runway"));
148 departure->getChild("etd", 0, true);
149 _departureWatcher->watch(departure->getChild("sid", 0, true));
150 departure->getChild("takeoff-time", 0, true);
152 destination = fgGetNode(RM "destination", true);
153 destination->getChild("airport", 0, true);
155 destination->tie("airport", SGRawValueMethods<FGRouteMgr, const char*>(*this,
156 &FGRouteMgr::getDestinationICAO, &FGRouteMgr::setDestinationICAO));
157 destination->tie("name", SGRawValueMethods<FGRouteMgr, const char*>(*this,
158 &FGRouteMgr::getDestinationName, NULL));
160 _arrivalWatcher = createWatcher(this, &FGRouteMgr::arrivalChanged);
161 _arrivalWatcher->watch(destination->getChild("runway", 0, true));
163 destination->getChild("eta", 0, true);
164 _arrivalWatcher->watch(destination->getChild("star", 0, true));
165 _arrivalWatcher->watch(destination->getChild("transition", 0, true));
166 destination->getChild("touchdown-time", 0, true);
168 alternate = fgGetNode(RM "alternate", true);
169 alternate->getChild("airport", 0, true);
170 alternate->getChild("runway", 0, true);
172 cruise = fgGetNode(RM "cruise", true);
173 cruise->getChild("altitude-ft", 0, true);
174 cruise->setDoubleValue("altitude-ft", 10000.0);
175 cruise->getChild("flight-level", 0, true);
176 cruise->getChild("speed-kts", 0, true);
177 cruise->setDoubleValue("speed-kts", 160.0);
179 _routingType = cruise->getChild("routing", 0, true);
180 _routingType->setIntValue(ROUTE_HIGH_AIRWAYS);
182 totalDistance = fgGetNode(RM "total-distance", true);
183 totalDistance->setDoubleValue(0.0);
185 ete = fgGetNode(RM "ete", true);
186 ete->setDoubleValue(0.0);
188 elapsedFlightTime = fgGetNode(RM "flight-time", true);
189 elapsedFlightTime->setDoubleValue(0.0);
191 active = fgGetNode(RM "active", true);
192 active->setBoolValue(false);
194 airborne = fgGetNode(RM "airborne", true);
195 airborne->setBoolValue(false);
197 _edited = fgGetNode(RM "signals/edited", true);
198 _finished = fgGetNode(RM "signals/finished", true);
200 _currentWpt = fgGetNode(RM "current-wp", true);
201 _currentWpt->tie(SGRawValueMethods<FGRouteMgr, int>
202 (*this, &FGRouteMgr::currentIndex, &FGRouteMgr::jumpToIndex));
204 // temporary distance / eta calculations, for backward-compatability
205 wp0 = fgGetNode(RM "wp", 0, true);
206 wp0->getChild("id", 0, true);
207 wp0->getChild("dist", 0, true);
208 wp0->getChild("eta", 0, true);
209 wp0->getChild("bearing-deg", 0, true);
211 wp1 = fgGetNode(RM "wp", 1, true);
212 wp1->getChild("id", 0, true);
213 wp1->getChild("dist", 0, true);
214 wp1->getChild("eta", 0, true);
216 wpn = fgGetNode(RM "wp-last", 0, true);
217 wpn->getChild("dist", 0, true);
218 wpn->getChild("eta", 0, true);
221 _pathNode = fgGetNode(RM "file-path", 0, true);
225 void FGRouteMgr::postinit()
227 SGPath path(_pathNode->getStringValue());
229 SG_LOG(SG_AUTOPILOT, SG_INFO, "loading flight-plan from:" << path.str());
233 // this code only matters for the --wp option now - perhaps the option
234 // should be deprecated in favour of an explicit flight-plan file?
235 // then the global initial waypoint list could die.
236 string_list *waypoints = globals->get_initial_waypoints();
238 string_list::iterator it;
239 for (it = waypoints->begin(); it != waypoints->end(); ++it) {
240 WayptRef w = waypointFromString(*it);
246 SG_LOG(SG_AUTOPILOT, SG_INFO, "loaded initial waypoints:" << _route.size());
249 weightOnWheels = fgGetNode("/gear/gear[0]/wow", true);
250 // check airbone flag agrees with presets
253 void FGRouteMgr::bind() { }
254 void FGRouteMgr::unbind() { }
256 bool FGRouteMgr::isRouteActive() const
258 return active->getBoolValue();
261 void FGRouteMgr::update( double dt )
264 return; // paused, nothing to do here
267 double groundSpeed = fgGetDouble("/velocities/groundspeed-kt", 0.0);
268 if (airborne->getBoolValue()) {
269 time_t now = time(NULL);
270 elapsedFlightTime->setDoubleValue(difftime(now, _takeoffTime));
271 } else { // not airborne
272 if (weightOnWheels->getBoolValue() || (groundSpeed < 40)) {
276 airborne->setBoolValue(true);
277 _takeoffTime = time(NULL); // start the clock
278 departure->setIntValue("takeoff-time", _takeoffTime);
281 if (!active->getBoolValue()) {
285 // basic course/distance information
286 SGGeod currentPos = SGGeod::fromDegFt(lon->getDoubleValue(),
287 lat->getDoubleValue(),alt->getDoubleValue());
289 Waypt* curWpt = currentWaypt();
296 boost::tie(courseDeg, distanceM) = curWpt->courseAndDistanceFrom(currentPos);
298 // update wp0 / wp1 / wp-last for legacy users
299 wp0->setDoubleValue("dist", distanceM * SG_METER_TO_NM);
300 courseDeg -= magvar->getDoubleValue(); // expose magnetic bearing
301 wp0->setDoubleValue("bearing-deg", courseDeg);
302 setETAPropertyFromDistance(wp0->getChild("eta"), distanceM);
304 double totalDistanceRemaining = distanceM; // distance to current waypoint
306 Waypt* nextWpt = nextWaypt();
308 boost::tie(courseDeg, distanceM) = nextWpt->courseAndDistanceFrom(currentPos);
310 wp1->setDoubleValue("dist", distanceM * SG_METER_TO_NM);
311 courseDeg -= magvar->getDoubleValue(); // expose magnetic bearing
312 wp1->setDoubleValue("bearing-deg", courseDeg);
313 setETAPropertyFromDistance(wp1->getChild("eta"), distanceM);
316 Waypt* prev = curWpt;
317 for (unsigned int i=_currentIndex + 1; i<_route.size(); ++i) {
318 Waypt* w = _route[i];
319 if (w->flag(WPT_DYNAMIC)) continue;
320 totalDistanceRemaining += SGGeodesy::distanceM(prev->position(), w->position());
326 wpn->setDoubleValue("dist", totalDistanceRemaining * SG_METER_TO_NM);
327 ete->setDoubleValue(totalDistanceRemaining * SG_METER_TO_NM / groundSpeed * 3600.0);
328 setETAPropertyFromDistance(wpn->getChild("eta"), totalDistanceRemaining);
331 void FGRouteMgr::setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDistance)
333 double speed = fgGetDouble("/velocities/groundspeed-kt", 0.0);
335 aProp->setStringValue("--:--");
340 double eta = aDistance * SG_METER_TO_NM / speed;
341 if ( eta >= 100.0 ) {
342 eta = 99.999; // clamp
345 if ( eta < (1.0/6.0) ) {
346 eta *= 60.0; // within 10 minutes, bump up to min/secs
349 int major = (int)eta,
350 minor = (int)((eta - (int)eta) * 60.0);
351 snprintf( eta_str, 64, "%d:%02d", major, minor );
352 aProp->setStringValue( eta_str );
355 flightgear::WayptRef FGRouteMgr::removeWayptAtIndex(int aIndex)
358 if (aIndex < 0) { // negative indices count the the end
359 index = _route.size() + index;
362 if ((index < 0) || (index >= numWaypts())) {
363 SG_LOG(SG_AUTOPILOT, SG_WARN, "removeWayptAtIndex with invalid index:" << aIndex);
366 WayptVec::iterator it = _route.begin();
369 WayptRef w = *it; // hold a ref now, in case _route is the only other owner
374 if (_currentIndex == index) {
375 currentWaypointChanged(); // current waypoint was removed
378 if (_currentIndex > index) {
379 --_currentIndex; // shift current index down if necessary
382 _edited->fireValueChanged();
388 void FGRouteMgr::clearRoute()
394 active->setBoolValue(false);
395 _edited->fireValueChanged();
399 * route between index-1 and index, using airways.
401 bool FGRouteMgr::routeToIndex(int index, RouteType aRouteType)
407 index = _route.size(); // can still be zero, of course
412 SG_LOG(SG_AUTOPILOT, SG_WARN, "routeToIndex: no departure set");
416 wp1 = new NavaidWaypoint(_departure.get(), NULL);
418 wp1 = wayptAtIndex(index - 1);
421 if (index >= numWaypts()) {
423 SG_LOG(SG_AUTOPILOT, SG_WARN, "routeToIndex: no destination set");
427 wp2 = new NavaidWaypoint(_destination.get(), NULL);
429 wp2 = wayptAtIndex(index);
432 double distNm = SGGeodesy::distanceNm(wp1->position(), wp2->position());
433 if (distNm < 100.0) {
434 SG_LOG(SG_AUTOPILOT, SG_INFO, "routeToIndex: existing waypoints are nearby, direct route");
439 switch (aRouteType) {
440 case ROUTE_HIGH_AIRWAYS:
441 Airway::highLevel()->route(wp1, wp2, r);
444 case ROUTE_LOW_AIRWAYS:
445 Airway::lowLevel()->route(wp1, wp2, r);
449 throw sg_exception("VOR routing not supported yet");
453 SG_LOG(SG_AUTOPILOT, SG_INFO, "routeToIndex: no route found");
457 WayptVec::iterator it = _route.begin();
459 _route.insert(it, r.begin(), r.end());
462 _edited->fireValueChanged();
466 void FGRouteMgr::autoRoute()
468 if (!_departure || !_destination) {
472 string runwayId(departure->getStringValue("runway"));
473 FGRunway* runway = NULL;
474 if (_departure->hasRunwayWithIdent(runwayId)) {
475 runway = _departure->getRunwayByIdent(runwayId);
478 FGRunway* dstRunway = NULL;
479 runwayId = destination->getStringValue("runway");
480 if (_destination->hasRunwayWithIdent(runwayId)) {
481 dstRunway = _destination->getRunwayByIdent(runwayId);
484 _route.clear(); // clear out the existing, first
486 flightgear::SID* sid;
489 boost::tie(sid, sidTrans) = _departure->selectSID(_destination->geod(), runway);
491 SG_LOG(SG_AUTOPILOT, SG_INFO, "selected SID " << sid->ident());
493 SG_LOG(SG_AUTOPILOT, SG_INFO, "\tvia " << sidTrans->ident() << " transition");
496 sid->route(runway, sidTrans, _route);
497 departure->setStringValue("sid", sid->ident());
499 // use airport location for airway search
500 sidTrans = new NavaidWaypoint(_departure.get(), NULL);
501 departure->setStringValue("sid", "");
505 destination->setStringValue("transition", "");
506 destination->setStringValue("star", "");
510 boost::tie(star, starTrans) = _destination->selectSTAR(_departure->geod(), dstRunway);
512 SG_LOG(SG_AUTOPILOT, SG_INFO, "selected STAR " << star->ident());
514 SG_LOG(SG_AUTOPILOT, SG_INFO, "\tvia " << starTrans->ident() << " transition");
515 destination->setStringValue("transition", starTrans->ident());
517 destination->setStringValue("star", star->ident());
519 // use airport location for search
520 starTrans = new NavaidWaypoint(_destination.get(), NULL);
523 // route between them
524 WayptVec airwayRoute;
525 if (Airway::highLevel()->route(sidTrans, starTrans, airwayRoute)) {
526 _route.insert(_route.end(), airwayRoute.begin(), airwayRoute.end());
529 // add the STAR if we have one
531 _destination->buildApproach(starTrans, star, dstRunway, _route);
535 _edited->fireValueChanged();
538 void FGRouteMgr::departureChanged()
540 // remove existing departure waypoints
541 WayptVec::iterator it = _route.begin();
542 for (; it != _route.end(); ++it) {
543 if (!(*it)->flag(WPT_DEPARTURE)) {
548 // erase() invalidates iterators, so grab now
550 if (it == _route.end()) {
552 enroute = new NavaidWaypoint(_destination.get(), NULL);
558 _route.erase(_route.begin(), it);
565 buildDeparture(enroute, wps);
566 for (it = wps.begin(); it != wps.end(); ++it) {
567 (*it)->setFlag(WPT_DEPARTURE);
568 (*it)->setFlag(WPT_GENERATED);
570 _route.insert(_route.begin(), wps.begin(), wps.end());
576 void FGRouteMgr::buildDeparture(WayptRef enroute, WayptVec& wps)
578 string runwayId(departure->getStringValue("runway"));
579 if (!_departure->hasRunwayWithIdent(runwayId)) {
580 // valid airport, but no runway selected, so just the airport noide itself
581 wps.push_back(new NavaidWaypoint(_departure.get(), NULL));
585 FGRunway* r = _departure->getRunwayByIdent(runwayId);
586 string sidId = departure->getStringValue("sid");
587 flightgear::SID* sid = _departure->findSIDWithIdent(sidId);
589 // valid runway, but no SID selected/found, so just the runway node for now
590 if (!sidId.empty() && (sidId != "(none)")) {
591 SG_LOG(SG_AUTOPILOT, SG_INFO, "SID not found:" << sidId);
594 wps.push_back(new RunwayWaypt(r, NULL));
598 // we have a valid SID, awesome
599 string trans(departure->getStringValue("transition"));
600 WayptRef t = sid->findTransitionByName(trans);
602 t = sid->findBestTransition(enroute->position());
605 sid->route(r, t, wps);
606 if (!wps.empty() && wps.front()->flag(WPT_DYNAMIC)) {
607 // ensure first waypoint is static, to simplify other computations
608 wps.insert(wps.begin(), new RunwayWaypt(r, NULL));
612 void FGRouteMgr::arrivalChanged()
614 // remove existing arrival waypoints
615 WayptVec::reverse_iterator rit = _route.rbegin();
616 for (; rit != _route.rend(); ++rit) {
617 if (!(*rit)->flag(WPT_ARRIVAL)) {
622 // erase() invalidates iterators, so grab now
624 WayptVec::iterator it;
626 if (rit != _route.rend()) {
628 it = rit.base(); // convert to fwd iterator
633 _route.erase(it, _route.end());
636 buildArrival(enroute, wps);
637 for (it = wps.begin(); it != wps.end(); ++it) {
638 (*it)->setFlag(WPT_ARRIVAL);
639 (*it)->setFlag(WPT_GENERATED);
641 _route.insert(_route.end(), wps.begin(), wps.end());
647 void FGRouteMgr::buildArrival(WayptRef enroute, WayptVec& wps)
653 string runwayId(destination->getStringValue("runway"));
654 if (!_destination->hasRunwayWithIdent(runwayId)) {
655 // valid airport, but no runway selected, so just the airport node itself
656 wps.push_back(new NavaidWaypoint(_destination.get(), NULL));
660 FGRunway* r = _destination->getRunwayByIdent(runwayId);
661 string starId = destination->getStringValue("star");
662 STAR* star = _destination->findSTARWithIdent(starId);
664 // valid runway, but no STAR selected/found, so just the runway node for now
665 wps.push_back(new RunwayWaypt(r, NULL));
669 // we have a valid STAR
670 string trans(destination->getStringValue("transition"));
671 WayptRef t = star->findTransitionByName(trans);
673 t = star->findBestTransition(enroute->position());
676 _destination->buildApproach(t, star, r, wps);
679 void FGRouteMgr::waypointsChanged()
684 void FGRouteMgr::insertWayptAtIndex(Waypt* aWpt, int aIndex)
691 if ((aIndex == -1) || (aIndex > (int) _route.size())) {
692 index = _route.size();
695 WayptVec::iterator it = _route.begin();
698 if (_currentIndex >= index) {
702 _route.insert(it, aWpt);
705 _edited->fireValueChanged();
708 WayptRef FGRouteMgr::waypointFromString(const string& tgt )
710 string target(boost::to_upper_copy(tgt));
714 double altFt = cruise->getDoubleValue("altitude-ft");
715 RouteRestriction altSetting = RESTRICT_NONE;
717 size_t pos = target.find( '@' );
718 if ( pos != string::npos ) {
719 altFt = atof( target.c_str() + pos + 1 );
720 target = target.substr( 0, pos );
721 if ( !strcmp(fgGetString("/sim/startup/units"), "meter") )
722 altFt *= SG_METER_TO_FEET;
723 altSetting = RESTRICT_AT;
727 pos = target.find( ',' );
728 if ( pos != string::npos ) {
729 double lon = atof( target.substr(0, pos).c_str());
730 double lat = atof( target.c_str() + pos + 1);
732 char ew = (lon < 0.0) ? 'W' : 'E';
733 char ns = (lat < 0.0) ? 'S' : 'N';
734 snprintf(buf, 32, "%c%03d%c%03d", ew, (int) fabs(lon), ns, (int)fabs(lat));
736 wpt = new BasicWaypt(SGGeod::fromDeg(lon, lat), buf, NULL);
737 if (altSetting != RESTRICT_NONE) {
738 wpt->setAltitude(altFt, altSetting);
744 if (_route.empty()) {
745 // route is empty, use current position
746 basePosition = SGGeod::fromDeg(lon->getDoubleValue(), lat->getDoubleValue());
748 basePosition = _route.back()->position();
751 string_list pieces(simgear::strutils::split(target, "/"));
752 FGPositionedRef p = FGPositioned::findClosestWithIdent(pieces.front(), basePosition);
754 SG_LOG( SG_AUTOPILOT, SG_INFO, "Unable to find FGPositioned with ident:" << pieces.front());
758 if (pieces.size() == 1) {
759 wpt = new NavaidWaypoint(p, NULL);
760 } else if (pieces.size() == 3) {
761 // navaid/radial/distance-nm notation
762 double radial = atof(pieces[1].c_str()),
763 distanceNm = atof(pieces[2].c_str());
764 radial += magvar->getDoubleValue(); // convert to true bearing
765 wpt = new OffsetNavaidWaypoint(p, NULL, radial, distanceNm);
766 } else if (pieces.size() == 2) {
767 FGAirport* apt = dynamic_cast<FGAirport*>(p.ptr());
769 SG_LOG(SG_AUTOPILOT, SG_INFO, "Waypoint is not an airport:" << pieces.front());
773 if (!apt->hasRunwayWithIdent(pieces[1])) {
774 SG_LOG(SG_AUTOPILOT, SG_INFO, "No runway: " << pieces[1] << " at " << pieces[0]);
778 FGRunway* runway = apt->getRunwayByIdent(pieces[1]);
779 wpt = new NavaidWaypoint(runway, NULL);
780 } else if (pieces.size() == 4) {
781 // navid/radial/navid/radial notation
782 FGPositionedRef p2 = FGPositioned::findClosestWithIdent(pieces[2], basePosition);
784 SG_LOG( SG_AUTOPILOT, SG_INFO, "Unable to find FGPositioned with ident:" << pieces[2]);
788 double r1 = atof(pieces[1].c_str()),
789 r2 = atof(pieces[3].c_str());
790 r1 += magvar->getDoubleValue();
791 r2 += magvar->getDoubleValue();
794 bool ok = SGGeodesy::radialIntersection(p->geod(), r1, p2->geod(), r2, intersection);
796 SG_LOG(SG_AUTOPILOT, SG_INFO, "no valid intersection for:" << target);
800 std::string name = p->ident() + "-" + p2->ident();
801 wpt = new BasicWaypt(intersection, name, NULL);
805 SG_LOG(SG_AUTOPILOT, SG_INFO, "Unable to parse waypoint:" << target);
809 if (altSetting != RESTRICT_NONE) {
810 wpt->setAltitude(altFt, altSetting);
815 // mirror internal route to the property system for inspection by other subsystems
816 void FGRouteMgr::update_mirror()
818 mirror->removeChildren("wp");
820 int num = numWaypts();
821 for (int i = 0; i < num; i++) {
822 Waypt* wp = _route[i];
823 SGPropertyNode *prop = mirror->getChild("wp", i, 1);
825 const SGGeod& pos(wp->position());
826 prop->setStringValue("id", wp->ident().c_str());
827 //prop->setStringValue("name", wp.get_name().c_str());
828 prop->setDoubleValue("longitude-deg", pos.getLongitudeDeg());
829 prop->setDoubleValue("latitude-deg",pos.getLatitudeDeg());
831 // leg course+distance
833 Waypt* next = _route[i+1];
834 std::pair<double, double> crsDist =
835 next->courseAndDistanceFrom(pos);
836 prop->setDoubleValue("leg-bearing-true-deg", crsDist.first);
837 prop->setDoubleValue("leg-distance-nm", crsDist.second * SG_METER_TO_NM);
840 if (wp->altitudeRestriction() != RESTRICT_NONE) {
841 double ft = wp->altitudeFt();
842 prop->setDoubleValue("altitude-m", ft * SG_FEET_TO_METER);
843 prop->setDoubleValue("altitude-ft", ft);
845 prop->setDoubleValue("altitude-m", -9999.9);
846 prop->setDoubleValue("altitude-ft", -9999.9);
849 if (wp->speedRestriction() == SPEED_RESTRICT_MACH) {
850 prop->setDoubleValue("speed-mach", wp->speedMach());
851 } else if (wp->speedRestriction() != RESTRICT_NONE) {
852 prop->setDoubleValue("speed-kts", wp->speedKts());
855 if (wp->flag(WPT_ARRIVAL)) {
856 prop->setBoolValue("arrival", true);
859 if (wp->flag(WPT_DEPARTURE)) {
860 prop->setBoolValue("departure", true);
863 if (wp->flag(WPT_MISS)) {
864 prop->setBoolValue("missed-approach", true);
867 prop->setBoolValue("generated", wp->flag(WPT_GENERATED));
868 } // of waypoint iteration
870 // set number as listener attachment point
871 mirror->setIntValue("num", _route.size());
873 NewGUI * gui = (NewGUI *)globals->get_subsystem("gui");
874 FGDialog* rmDlg = gui->getDialog("route-manager");
876 rmDlg->updateValues();
880 // command interface /autopilot/route-manager/input:
882 // @CLEAR ... clear route
883 // @POP ... remove first entry
884 // @DELETE3 ... delete 4th entry
885 // @INSERT2:KSFO@900 ... insert "KSFO@900" as 3rd entry
886 // KSFO@900 ... append "KSFO@900"
888 void FGRouteMgr::InputListener::valueChanged(SGPropertyNode *prop)
890 const char *s = prop->getStringValue();
891 if (strlen(s) == 0) {
895 if (!strcmp(s, "@CLEAR"))
897 else if (!strcmp(s, "@ACTIVATE"))
899 else if (!strcmp(s, "@LOAD")) {
901 } else if (!strcmp(s, "@SAVE")) {
903 } else if (!strcmp(s, "@POP")) {
904 SG_LOG(SG_AUTOPILOT, SG_WARN, "route-manager @POP command is deprecated");
905 } else if (!strcmp(s, "@NEXT")) {
906 mgr->jumpToIndex(mgr->_currentIndex + 1);
907 } else if (!strcmp(s, "@PREVIOUS")) {
908 mgr->jumpToIndex(mgr->_currentIndex - 1);
909 } else if (!strncmp(s, "@JUMP", 5)) {
910 mgr->jumpToIndex(atoi(s + 5));
911 } else if (!strncmp(s, "@DELETE", 7))
912 mgr->removeWayptAtIndex(atoi(s + 7));
913 else if (!strncmp(s, "@INSERT", 7)) {
915 int pos = strtol(s + 7, &r, 10);
921 mgr->insertWayptAtIndex(mgr->waypointFromString(r), pos);
922 } else if (!strncmp(s, "@ROUTE", 6)) {
924 int endIndex = strtol(s + 6, &r, 10);
925 RouteType rt = (RouteType) mgr->_routingType->getIntValue();
926 mgr->routeToIndex(endIndex, rt);
927 } else if (!strcmp(s, "@AUTOROUTE")) {
929 } else if (!strcmp(s, "@POSINIT")) {
930 mgr->initAtPosition();
932 mgr->insertWayptAtIndex(mgr->waypointFromString(s), -1);
935 void FGRouteMgr::initAtPosition()
937 if (isRouteActive()) {
938 return; // don't mess with the active route
941 if (haveUserWaypoints()) {
942 // user has already defined, loaded or entered a route, again
943 // don't interfere with it
947 if (airborne->getBoolValue()) {
948 SG_LOG(SG_AUTOPILOT, SG_INFO, "initAtPosition: airborne, clearing departure info");
950 departure->setStringValue("runway", "");
955 SGGeod pos = SGGeod::fromDegFt(lon->getDoubleValue(),
956 lat->getDoubleValue(), alt->getDoubleValue());
958 _departure = FGAirport::findClosest(pos, 20.0);
960 SG_LOG(SG_AUTOPILOT, SG_INFO, "initAtPosition: couldn't find an airport within 20nm");
961 departure->setStringValue("runway", "");
966 std::string rwy = departure->getStringValue("runway");
968 // runway already set, fine
972 FGRunway* r = _departure->findBestRunwayForPos(pos);
977 departure->setStringValue("runway", r->ident().c_str());
978 SG_LOG(SG_AUTOPILOT, SG_INFO, "initAtPosition: starting at "
979 << _departure->ident() << " on runway " << r->ident());
982 bool FGRouteMgr::haveUserWaypoints() const
984 for (int i = 0; i < numWaypts(); i++) {
985 if (!_route[i]->flag(WPT_GENERATED)) {
986 // have a non-generated waypoint, we're done
991 // all waypoints are generated
995 bool FGRouteMgr::activate()
997 if (isRouteActive()) {
998 SG_LOG(SG_AUTOPILOT, SG_WARN, "duplicate route-activation, no-op");
1003 currentWaypointChanged();
1005 /* double routeDistanceNm = _route->total_distance() * SG_METER_TO_NM;
1006 totalDistance->setDoubleValue(routeDistanceNm);
1007 double cruiseSpeedKts = cruise->getDoubleValue("speed", 0.0);
1008 if (cruiseSpeedKts > 1.0) {
1009 // very very crude approximation, doesn't allow for climb / descent
1010 // performance or anything else at all
1011 ete->setDoubleValue(routeDistanceNm / cruiseSpeedKts * (60.0 * 60.0));
1014 active->setBoolValue(true);
1015 SG_LOG(SG_AUTOPILOT, SG_INFO, "route-manager, activate route ok");
1020 void FGRouteMgr::sequence()
1022 if (!active->getBoolValue()) {
1023 SG_LOG(SG_AUTOPILOT, SG_ALERT, "trying to sequence waypoints with no active route");
1027 if (checkFinished()) {
1032 currentWaypointChanged();
1035 bool FGRouteMgr::checkFinished()
1037 if (_currentIndex < (int) _route.size()) {
1041 SG_LOG(SG_AUTOPILOT, SG_INFO, "reached end of active route");
1042 _finished->fireValueChanged();
1043 active->setBoolValue(false);
1047 void FGRouteMgr::jumpToIndex(int index)
1049 if ((index < 0) || (index >= (int) _route.size())) {
1050 SG_LOG(SG_AUTOPILOT, SG_ALERT, "passed invalid index (" <<
1051 index << ") to FGRouteMgr::jumpToIndex");
1055 if (_currentIndex == index) {
1059 // all the checks out the way, go ahead and update state
1060 _currentIndex = index;
1061 currentWaypointChanged();
1062 _currentWpt->fireValueChanged();
1065 void FGRouteMgr::currentWaypointChanged()
1067 Waypt* cur = currentWaypt();
1068 Waypt* next = nextWaypt();
1070 wp0->getChild("id")->setStringValue(cur ? cur->ident() : "");
1071 wp1->getChild("id")->setStringValue(next ? next->ident() : "");
1073 _currentWpt->fireValueChanged();
1074 SG_LOG(SG_AUTOPILOT, SG_INFO, "route manager, current-wp is now " << _currentIndex);
1077 int FGRouteMgr::findWayptIndex(const SGGeod& aPos) const
1079 for (int i=0; i<numWaypts(); ++i) {
1080 if (_route[i]->matches(aPos)) {
1088 Waypt* FGRouteMgr::currentWaypt() const
1090 if ((_currentIndex < 0) || (_currentIndex >= numWaypts()))
1092 return wayptAtIndex(_currentIndex);
1095 Waypt* FGRouteMgr::previousWaypt() const
1097 if (_currentIndex == 0) {
1101 return wayptAtIndex(_currentIndex - 1);
1104 Waypt* FGRouteMgr::nextWaypt() const
1106 if ((_currentIndex < 0) || ((_currentIndex + 1) >= numWaypts())) {
1110 return wayptAtIndex(_currentIndex + 1);
1113 Waypt* FGRouteMgr::wayptAtIndex(int index) const
1115 if ((index < 0) || (index >= numWaypts())) {
1116 throw sg_range_exception("waypt index out of range", "FGRouteMgr::wayptAtIndex");
1119 return _route[index];
1122 void FGRouteMgr::saveRoute()
1124 SGPath path(_pathNode->getStringValue());
1125 SG_LOG(SG_IO, SG_INFO, "Saving route to " << path.str());
1127 SGPropertyNode_ptr d(new SGPropertyNode);
1128 SGPath path(_pathNode->getStringValue());
1129 d->setIntValue("version", 2);
1132 d->setStringValue("departure/airport", _departure->ident());
1133 d->setStringValue("departure/sid", departure->getStringValue("sid"));
1134 d->setStringValue("departure/runway", departure->getStringValue("runway"));
1138 d->setStringValue("destination/airport", _destination->ident());
1139 d->setStringValue("destination/star", destination->getStringValue("star"));
1140 d->setStringValue("destination/transition", destination->getStringValue("transition"));
1141 d->setStringValue("destination/runway", destination->getStringValue("runway"));
1145 SGPropertyNode* routeNode = d->getChild("route", 0, true);
1146 for (unsigned int i=0; i<_route.size(); ++i) {
1147 Waypt* wpt = _route[i];
1148 wpt->saveAsNode(routeNode->getChild("wp", i, true));
1149 } // of waypoint iteration
1150 writeProperties(path.str(), d, true /* write-all */);
1151 } catch (sg_exception& e) {
1152 SG_LOG(SG_IO, SG_WARN, "failed to save flight-plan:" << e.getMessage());
1156 void FGRouteMgr::loadRoute()
1158 // deactivate route first
1159 active->setBoolValue(false);
1161 SGPropertyNode_ptr routeData(new SGPropertyNode);
1162 SGPath path(_pathNode->getStringValue());
1164 SG_LOG(SG_IO, SG_INFO, "going to read flight-plan from:" << path.str());
1167 readProperties(path.str(), routeData);
1168 } catch (sg_exception& ) {
1169 // if XML parsing fails, the file might be simple textual list of waypoints
1170 loadPlainTextRoute(path);
1175 int version = routeData->getIntValue("version", 1);
1177 loadVersion1XMLRoute(routeData);
1178 } else if (version == 2) {
1179 loadVersion2XMLRoute(routeData);
1181 throw sg_io_exception("unsupported XML route version");
1183 } catch (sg_exception& e) {
1184 SG_LOG(SG_IO, SG_WARN, "failed to load flight-plan (from '" << e.getOrigin()
1185 << "'):" << e.getMessage());
1189 void FGRouteMgr::loadXMLRouteHeader(SGPropertyNode_ptr routeData)
1192 SGPropertyNode* dep = routeData->getChild("departure");
1194 string depIdent = dep->getStringValue("airport");
1195 _departure = (FGAirport*) fgFindAirportID(depIdent);
1196 departure->setStringValue("runway", dep->getStringValue("runway"));
1197 departure->setStringValue("sid", dep->getStringValue("sid"));
1198 departure->setStringValue("transition", dep->getStringValue("transition"));
1202 SGPropertyNode* dst = routeData->getChild("destination");
1204 _destination = (FGAirport*) fgFindAirportID(dst->getStringValue("airport"));
1205 destination->setStringValue("runway", dst->getStringValue("runway"));
1206 destination->setStringValue("star", dst->getStringValue("star"));
1207 destination->setStringValue("transition", dst->getStringValue("transition"));
1211 SGPropertyNode* alt = routeData->getChild("alternate");
1213 alternate->setStringValue(alt->getStringValue("airport"));
1214 } // of cruise data loading
1217 SGPropertyNode* crs = routeData->getChild("cruise");
1219 cruise->setDoubleValue("speed-kts", crs->getDoubleValue("speed-kts"));
1220 cruise->setDoubleValue("mach", crs->getDoubleValue("mach"));
1221 cruise->setDoubleValue("altitude-ft", crs->getDoubleValue("altitude-ft"));
1222 } // of cruise data loading
1226 void FGRouteMgr::loadVersion2XMLRoute(SGPropertyNode_ptr routeData)
1228 loadXMLRouteHeader(routeData);
1232 SGPropertyNode_ptr routeNode = routeData->getChild("route", 0);
1233 for (int i=0; i<routeNode->nChildren(); ++i) {
1234 SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i);
1235 WayptRef wpt = Waypt::createFromProperties(NULL, wpNode);
1236 wpts.push_back(wpt);
1237 } // of route iteration
1242 void FGRouteMgr::loadVersion1XMLRoute(SGPropertyNode_ptr routeData)
1244 loadXMLRouteHeader(routeData);
1248 SGPropertyNode_ptr routeNode = routeData->getChild("route", 0);
1249 for (int i=0; i<routeNode->nChildren(); ++i) {
1250 SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i);
1251 WayptRef wpt = parseVersion1XMLWaypt(wpNode);
1252 wpts.push_back(wpt);
1253 } // of route iteration
1258 WayptRef FGRouteMgr::parseVersion1XMLWaypt(SGPropertyNode* aWP)
1261 if (!_route.empty()) {
1262 lastPos = _route.back()->position();
1263 } else if (_departure) {
1264 lastPos = _departure->geod();
1268 string ident(aWP->getStringValue("ident"));
1269 if (aWP->hasChild("longitude-deg")) {
1270 // explicit longitude/latitude
1271 w = new BasicWaypt(SGGeod::fromDeg(aWP->getDoubleValue("longitude-deg"),
1272 aWP->getDoubleValue("latitude-deg")), ident, NULL);
1275 string nid = aWP->getStringValue("navid", ident.c_str());
1276 FGPositionedRef p = FGPositioned::findClosestWithIdent(nid, lastPos);
1278 throw sg_io_exception("bad route file, unknown navid:" + nid);
1281 SGGeod pos(p->geod());
1282 if (aWP->hasChild("offset-nm") && aWP->hasChild("offset-radial")) {
1283 double radialDeg = aWP->getDoubleValue("offset-radial");
1284 // convert magnetic radial to a true radial!
1285 radialDeg += magvar->getDoubleValue();
1286 double offsetNm = aWP->getDoubleValue("offset-nm");
1288 SGGeodesy::direct(p->geod(), radialDeg, offsetNm * SG_NM_TO_METER, pos, az2);
1291 w = new BasicWaypt(pos, ident, NULL);
1294 double altFt = aWP->getDoubleValue("altitude-ft", -9999.9);
1295 if (altFt > -9990.0) {
1296 w->setAltitude(altFt, RESTRICT_AT);
1302 void FGRouteMgr::loadPlainTextRoute(const SGPath& path)
1304 sg_gzifstream in(path.str().c_str());
1305 if (!in.is_open()) {
1313 getline(in, line, '\n');
1314 // trim CR from end of line, if found
1315 if (line[line.size() - 1] == '\r') {
1316 line.erase(line.size() - 1, 1);
1319 line = simgear::strutils::strip(line);
1320 if (line.empty() || (line[0] == '#')) {
1321 continue; // ignore empty/comment lines
1324 WayptRef w = waypointFromString(line);
1326 throw sg_io_exception("failed to create waypoint from line:" + line);
1330 } // of line iteration
1333 } catch (sg_exception& e) {
1334 SG_LOG(SG_IO, SG_WARN, "failed to load route from:" << path.str() << ":" << e.getMessage());
1338 const char* FGRouteMgr::getDepartureICAO() const
1344 return _departure->ident().c_str();
1347 const char* FGRouteMgr::getDepartureName() const
1353 return _departure->name().c_str();
1356 void FGRouteMgr::setDepartureICAO(const char* aIdent)
1358 if ((aIdent == NULL) || (strlen(aIdent) < 4)) {
1361 _departure = FGAirport::findByIdent(aIdent);
1367 const char* FGRouteMgr::getDestinationICAO() const
1369 if (!_destination) {
1373 return _destination->ident().c_str();
1376 const char* FGRouteMgr::getDestinationName() const
1378 if (!_destination) {
1382 return _destination->name().c_str();
1385 void FGRouteMgr::setDestinationICAO(const char* aIdent)
1387 if ((aIdent == NULL) || (strlen(aIdent) < 4)) {
1388 _destination = NULL;
1390 _destination = FGAirport::findByIdent(aIdent);