]> git.mxchange.org Git - flightgear.git/blob - src/Autopilot/route_mgr.cxx
Make route editing sane.
[flightgear.git] / src / Autopilot / route_mgr.cxx
1 // route_mgr.cxx - manage a route (i.e. a collection of waypoints)
2 //
3 // Written by Curtis Olson, started January 2004.
4 //            Norman Vine
5 //            Melchior FRANZ
6 //
7 // Copyright (C) 2004  Curtis L. Olson  - http://www.flightgear.org/~curt
8 //
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.
13 //
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.
18 //
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.
22 //
23 // $Id$
24
25
26 #ifdef HAVE_CONFIG_H
27 #  include <config.h>
28 #endif
29
30 #ifdef HAVE_WINDOWS_H
31 #include <time.h>
32 #endif
33
34 #include <simgear/compiler.h>
35
36 #include "route_mgr.hxx"
37
38 #include <boost/algorithm/string/case_conv.hpp>
39 #include <boost/tuple/tuple.hpp>
40
41 #include <simgear/misc/strutils.hxx>
42 #include <simgear/structure/exception.hxx>
43 #include <simgear/misc/sgstream.hxx>
44
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>
49
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"
57
58 #define RM "/autopilot/route-manager/"
59
60 #include <GUI/new_gui.hxx>
61 #include <GUI/dialog.hxx>
62
63 using namespace flightgear;
64
65 class PropertyWatcher : public SGPropertyChangeListener
66 {
67 public:
68   void watch(SGPropertyNode* p)
69   {
70     p->addChangeListener(this, false);
71   }
72
73   virtual void valueChanged(SGPropertyNode*)
74   {
75     fire();
76   }
77 protected:
78   virtual void fire() = 0;
79 };
80
81 /**
82  * Template adapter, created by convenience helper below
83  */
84 template <class T>
85 class MethodPropertyWatcher : public PropertyWatcher
86 {
87 public:
88   typedef void (T::*fire_method)();
89
90   MethodPropertyWatcher(T* obj, fire_method m) :
91     _object(obj),
92     _method(m)
93   { ; }
94   
95 protected:
96   virtual void fire()
97   { // dispatch to the object method we're helping
98     (_object->*_method)();
99   }
100   
101 private:
102   T* _object;
103   fire_method _method;
104 };
105
106 template <class T>
107 PropertyWatcher* createWatcher(T* obj, void (T::*m)())
108 {
109   return new MethodPropertyWatcher<T>(obj, m);
110 }
111
112 FGRouteMgr::FGRouteMgr() :
113   _currentIndex(0),
114   input(fgGetNode( RM "input", true )),
115   mirror(fgGetNode( RM "route", true ))
116 {
117   listener = new InputListener(this);
118   input->setStringValue("");
119   input->addChangeListener(listener);
120 }
121
122
123 FGRouteMgr::~FGRouteMgr()
124 {
125   input->removeChangeListener(listener);
126   delete listener;
127 }
128
129
130 void FGRouteMgr::init() {
131   SGPropertyNode_ptr rm(fgGetNode(RM));
132   
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);
137      
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", "");
144   
145   _departureWatcher = createWatcher(this, &FGRouteMgr::departureChanged);
146   _departureWatcher->watch(departure->getChild("runway"));
147   
148   departure->getChild("etd", 0, true);
149   _departureWatcher->watch(departure->getChild("sid", 0, true));
150   departure->getChild("takeoff-time", 0, true);
151
152   destination = fgGetNode(RM "destination", true);
153   destination->getChild("airport", 0, true);
154   
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));
159   
160   _arrivalWatcher = createWatcher(this, &FGRouteMgr::arrivalChanged);
161   _arrivalWatcher->watch(destination->getChild("runway", 0, true));
162   
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);
167
168   alternate = fgGetNode(RM "alternate", true);
169   alternate->getChild("airport", 0, true);
170   alternate->getChild("runway", 0, true);
171   
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);
178   
179   _routingType = cruise->getChild("routing", 0, true);
180   _routingType->setIntValue(ROUTE_HIGH_AIRWAYS);
181   
182   totalDistance = fgGetNode(RM "total-distance", true);
183   totalDistance->setDoubleValue(0.0);
184   
185   ete = fgGetNode(RM "ete", true);
186   ete->setDoubleValue(0.0);
187   
188   elapsedFlightTime = fgGetNode(RM "flight-time", true);
189   elapsedFlightTime->setDoubleValue(0.0);
190   
191   active = fgGetNode(RM "active", true);
192   active->setBoolValue(false);
193   
194   airborne = fgGetNode(RM "airborne", true);
195   airborne->setBoolValue(false);
196     
197   _edited = fgGetNode(RM "signals/edited", true);
198   _finished = fgGetNode(RM "signals/finished", true);
199   
200   _currentWpt = fgGetNode(RM "current-wp", true);
201   _currentWpt->tie(SGRawValueMethods<FGRouteMgr, int>
202     (*this, &FGRouteMgr::currentIndex, &FGRouteMgr::jumpToIndex));
203       
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);
210   
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);
215   
216   wpn = fgGetNode(RM "wp-last", 0, true);
217   wpn->getChild("dist", 0, true);
218   wpn->getChild("eta", 0, true);
219   
220   update_mirror();
221   _pathNode = fgGetNode(RM "file-path", 0, true);
222 }
223
224
225 void FGRouteMgr::postinit()
226 {
227   string_list *waypoints = globals->get_initial_waypoints();
228   if (waypoints) {
229     string_list::iterator it;
230     for (it = waypoints->begin(); it != waypoints->end(); ++it) {
231       WayptRef w = waypointFromString(*it);
232       if (w) {
233         _route.push_back(w);
234       }
235     }
236     
237     SG_LOG(SG_AUTOPILOT, SG_INFO, "loaded initial waypoints:" << _route.size());
238   }
239
240   weightOnWheels = fgGetNode("/gear/gear[0]/wow", true);
241   // check airbone flag agrees with presets
242 }
243
244 void FGRouteMgr::bind() { }
245 void FGRouteMgr::unbind() { }
246
247 bool FGRouteMgr::isRouteActive() const
248 {
249   return active->getBoolValue();
250 }
251
252 void FGRouteMgr::update( double dt )
253 {
254   if (dt <= 0.0) {
255     return; // paused, nothing to do here
256   }
257   
258   double groundSpeed = fgGetDouble("/velocities/groundspeed-kt", 0.0);
259   if (airborne->getBoolValue()) {
260     time_t now = time(NULL);
261     elapsedFlightTime->setDoubleValue(difftime(now, _takeoffTime));
262   } else { // not airborne
263     if (weightOnWheels->getBoolValue() || (groundSpeed < 40)) {
264       return;
265     }
266     
267     airborne->setBoolValue(true);
268     _takeoffTime = time(NULL); // start the clock
269     departure->setIntValue("takeoff-time", _takeoffTime);
270   }
271   
272   if (!active->getBoolValue()) {
273     return;
274   }
275
276 // basic course/distance information
277   SGGeod currentPos = SGGeod::fromDegFt(lon->getDoubleValue(), 
278     lat->getDoubleValue(),alt->getDoubleValue());
279
280   Waypt* curWpt = currentWaypt();
281   if (!curWpt) {
282     return;
283   }
284   
285   double courseDeg;
286   double distanceM;
287   boost::tie(courseDeg, distanceM) = curWpt->courseAndDistanceFrom(currentPos);
288   
289 // update wp0 / wp1 / wp-last for legacy users
290   wp0->setDoubleValue("dist", distanceM * SG_METER_TO_NM);
291   courseDeg -= magvar->getDoubleValue(); // expose magnetic bearing
292   wp0->setDoubleValue("bearing-deg", courseDeg);
293   setETAPropertyFromDistance(wp0->getChild("eta"), distanceM);
294   
295   double totalDistanceRemaining = distanceM; // distance to current waypoint
296   
297   Waypt* nextWpt = nextWaypt();
298   if (nextWpt) {
299     boost::tie(courseDeg, distanceM) = nextWpt->courseAndDistanceFrom(currentPos);
300      
301     wp1->setDoubleValue("dist", distanceM * SG_METER_TO_NM);
302     courseDeg -= magvar->getDoubleValue(); // expose magnetic bearing
303     wp1->setDoubleValue("bearing-deg", courseDeg);
304     setETAPropertyFromDistance(wp1->getChild("eta"), distanceM);
305   }
306   
307   Waypt* prev = curWpt;
308   for (unsigned int i=_currentIndex + 1; i<_route.size(); ++i) {
309     Waypt* w = _route[i];
310     if (w->flag(WPT_DYNAMIC)) continue;
311     totalDistanceRemaining += SGGeodesy::distanceM(prev->position(), w->position());
312     prev = w;
313   }
314   
315   wpn->setDoubleValue("dist", totalDistanceRemaining * SG_METER_TO_NM);
316   ete->setDoubleValue(totalDistanceRemaining * SG_METER_TO_NM / groundSpeed * 3600.0);
317   setETAPropertyFromDistance(wpn->getChild("eta"), totalDistanceRemaining);
318 }
319
320 void FGRouteMgr::setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDistance)
321 {
322   double speed = fgGetDouble("/velocities/groundspeed-kt", 0.0);
323   if (speed < 1.0) {
324     aProp->setStringValue("--:--");
325     return;
326   }
327
328   char eta_str[64];
329   double eta = aDistance * SG_METER_TO_NM / speed;
330   if ( eta >= 100.0 ) { 
331       eta = 99.999; // clamp
332   }
333   
334   if ( eta < (1.0/6.0) ) {
335     eta *= 60.0; // within 10 minutes, bump up to min/secs
336   }
337   
338   int major = (int)eta, 
339       minor = (int)((eta - (int)eta) * 60.0);
340   snprintf( eta_str, 64, "%d:%02d", major, minor );
341   aProp->setStringValue( eta_str );
342 }
343
344 flightgear::WayptRef FGRouteMgr::removeWayptAtIndex(int aIndex)
345 {
346   int index = aIndex;
347   if (aIndex < 0) { // negative indices count the the end
348     index = _route.size() + index;
349   }
350   
351   if ((index < 0) || (index >= numWaypts())) {
352     SG_LOG(SG_AUTOPILOT, SG_WARN, "removeWayptAtIndex with invalid index:" << aIndex);
353     return NULL;
354   }
355   WayptVec::iterator it = _route.begin();
356   it += index;
357   
358   WayptRef w = *it; // hold a ref now, in case _route is the only other owner
359   _route.erase(it);
360   
361   update_mirror();
362   
363   if (_currentIndex == index) {
364     currentWaypointChanged(); // current waypoint was removed
365   }
366   else
367   if (_currentIndex > index) {
368     --_currentIndex; // shift current index down if necessary
369   }
370
371   _edited->fireValueChanged();
372   checkFinished();
373   
374   return w;
375 }
376   
377 void FGRouteMgr::clearRoute()
378 {
379   _route.clear();
380   _currentIndex = -1;
381   
382   update_mirror();
383   active->setBoolValue(false);
384   _edited->fireValueChanged();
385 }
386
387 /**
388  * route between index-1 and index, using airways.
389  */
390 bool FGRouteMgr::routeToIndex(int index, RouteType aRouteType)
391 {
392   WayptRef wp1;
393   WayptRef wp2;
394   
395   if (index == -1) {
396     index = _route.size(); // can still be zero, of course
397   }
398   
399   if (index == 0) {
400     if (!_departure) {
401       SG_LOG(SG_AUTOPILOT, SG_WARN, "routeToIndex: no departure set");
402       return false;
403     }
404     
405     wp1 = new NavaidWaypoint(_departure.get(), NULL);
406   } else {
407     wp1 = wayptAtIndex(index - 1);
408   }
409   
410   if (index >= numWaypts()) {
411     if (!_destination) {
412       SG_LOG(SG_AUTOPILOT, SG_WARN, "routeToIndex: no destination set");
413       return false;
414     }
415     
416     wp2 = new NavaidWaypoint(_destination.get(), NULL);
417   } else {
418     wp2 = wayptAtIndex(index);
419   }
420   
421   double distNm = SGGeodesy::distanceNm(wp1->position(), wp2->position());
422   if (distNm < 100.0) {
423     SG_LOG(SG_AUTOPILOT, SG_INFO, "routeToIndex: existing waypoints are nearby, direct route");
424     return true;
425   }
426   
427   WayptVec r;
428   switch (aRouteType) {
429   case ROUTE_HIGH_AIRWAYS:
430     Airway::highLevel()->route(wp1, wp2, r);
431     break;
432     
433   case ROUTE_LOW_AIRWAYS:
434     Airway::lowLevel()->route(wp1, wp2, r);
435     break;
436     
437   case ROUTE_VOR:
438     throw sg_exception("VOR routing not supported yet");
439   }
440   
441   if (r.empty()) {
442     SG_LOG(SG_AUTOPILOT, SG_INFO, "routeToIndex: no route found");
443     return false;
444   }
445
446   WayptVec::iterator it = _route.begin();
447   it += index;
448   _route.insert(it, r.begin(), r.end());
449
450   update_mirror();
451   _edited->fireValueChanged();
452   return true;
453 }
454
455 void FGRouteMgr::autoRoute()
456 {
457   if (!_departure || !_destination) {
458     return;
459   }
460   
461   string runwayId(departure->getStringValue("runway"));
462   FGRunway* runway = NULL;
463   if (_departure->hasRunwayWithIdent(runwayId)) {
464     runway = _departure->getRunwayByIdent(runwayId);
465   }
466   
467   FGRunway* dstRunway = NULL;
468   runwayId = destination->getStringValue("runway");
469   if (_destination->hasRunwayWithIdent(runwayId)) {
470     dstRunway = _destination->getRunwayByIdent(runwayId);
471   }
472     
473   _route.clear(); // clear out the existing, first
474 // SID
475   flightgear::SID* sid;
476   WayptRef sidTrans;
477   
478   boost::tie(sid, sidTrans) = _departure->selectSID(_destination->geod(), runway);
479   if (sid) { 
480     SG_LOG(SG_AUTOPILOT, SG_INFO, "selected SID " << sid->ident());
481     if (sidTrans) {
482       SG_LOG(SG_AUTOPILOT, SG_INFO, "\tvia " << sidTrans->ident() << " transition");
483     }
484     
485     sid->route(runway, sidTrans, _route);
486     departure->setStringValue("sid", sid->ident());
487   } else {
488     // use airport location for airway search
489     sidTrans = new NavaidWaypoint(_departure.get(), NULL);
490     departure->setStringValue("sid", "");
491   }
492   
493 // STAR
494   destination->setStringValue("transition", "");
495   destination->setStringValue("star", "");
496   
497   STAR* star;
498   WayptRef starTrans;
499   boost::tie(star, starTrans) = _destination->selectSTAR(_departure->geod(), dstRunway);
500   if (star) {
501     SG_LOG(SG_AUTOPILOT, SG_INFO, "selected STAR " << star->ident());
502     if (starTrans) {
503       SG_LOG(SG_AUTOPILOT, SG_INFO, "\tvia " << starTrans->ident() << " transition");
504       destination->setStringValue("transition", starTrans->ident());
505     }    
506     destination->setStringValue("star", star->ident());
507   } else {
508     // use airport location for search
509     starTrans = new NavaidWaypoint(_destination.get(), NULL);
510   }
511   
512 // route between them
513   WayptVec airwayRoute;
514   if (Airway::highLevel()->route(sidTrans, starTrans, airwayRoute)) {
515     _route.insert(_route.end(), airwayRoute.begin(), airwayRoute.end());
516   }
517   
518 // add the STAR if we have one
519   if (star) {
520     _destination->buildApproach(starTrans, star, dstRunway, _route);
521   }
522
523   update_mirror();
524   _edited->fireValueChanged();
525 }
526
527 void FGRouteMgr::departureChanged()
528 {
529 // remove existing departure waypoints
530   WayptVec::iterator it = _route.begin();
531   for (; it != _route.end(); ++it) {
532     if (!(*it)->flag(WPT_DEPARTURE)) {
533       break;
534     }
535   }
536   
537   // erase() invalidates iterators, so grab now
538   WayptRef enroute;
539   if (it == _route.end()) {
540     if (_destination) {
541       enroute = new NavaidWaypoint(_destination.get(), NULL);
542     }
543   } else {
544     enroute = *it;
545   }
546
547   _route.erase(_route.begin(), it);
548   if (!_departure) {
549     waypointsChanged();
550     return;
551   }
552   
553   WayptVec wps;
554   buildDeparture(enroute, wps);
555   for (it = wps.begin(); it != wps.end(); ++it) {
556     (*it)->setFlag(WPT_DEPARTURE);
557     (*it)->setFlag(WPT_GENERATED);
558   }
559   _route.insert(_route.begin(), wps.begin(), wps.end());
560   
561   update_mirror();
562   waypointsChanged();
563 }
564
565 void FGRouteMgr::buildDeparture(WayptRef enroute, WayptVec& wps)
566 {
567   string runwayId(departure->getStringValue("runway"));
568   if (!_departure->hasRunwayWithIdent(runwayId)) {
569 // valid airport, but no runway selected, so just the airport noide itself
570     wps.push_back(new NavaidWaypoint(_departure.get(), NULL));
571     return;
572   }
573   
574   FGRunway* r = _departure->getRunwayByIdent(runwayId);
575   string sidId = departure->getStringValue("sid");
576   flightgear::SID* sid = _departure->findSIDWithIdent(sidId);
577   if (!sid) {
578 // valid runway, but no SID selected/found, so just the runway node for now
579     if (!sidId.empty() && (sidId != "(none)")) {
580       SG_LOG(SG_AUTOPILOT, SG_INFO, "SID not found:" << sidId);
581     }
582     
583     wps.push_back(new RunwayWaypt(r, NULL));
584     return;
585   }
586   
587 // we have a valid SID, awesome
588   string trans(departure->getStringValue("transition"));
589   WayptRef t = sid->findTransitionByName(trans);
590   if (!t && enroute) {
591     t = sid->findBestTransition(enroute->position());
592   }
593
594   sid->route(r, t, wps);
595   if (!wps.empty() && wps.front()->flag(WPT_DYNAMIC)) {
596     // ensure first waypoint is static, to simplify other computations
597     wps.insert(wps.begin(), new RunwayWaypt(r, NULL));
598   }
599 }
600
601 void FGRouteMgr::arrivalChanged()
602 {  
603   // remove existing arrival waypoints
604   WayptVec::reverse_iterator rit = _route.rbegin();
605   for (; rit != _route.rend(); ++rit) {
606     if (!(*rit)->flag(WPT_ARRIVAL)) {
607       break;
608     }
609   }
610   
611   // erase() invalidates iterators, so grab now
612   WayptRef enroute;
613   WayptVec::iterator it;
614   
615   if (rit != _route.rend()) {
616     enroute = *rit;
617     it = rit.base(); // convert to fwd iterator
618   } else {
619     it = _route.begin();
620   }
621
622   _route.erase(it, _route.end());
623   
624   WayptVec wps;
625   buildArrival(enroute, wps);
626   for (it = wps.begin(); it != wps.end(); ++it) {
627     (*it)->setFlag(WPT_ARRIVAL);
628     (*it)->setFlag(WPT_GENERATED);
629   }
630   _route.insert(_route.end(), wps.begin(), wps.end());
631   
632   update_mirror();
633   waypointsChanged();
634 }
635
636 void FGRouteMgr::buildArrival(WayptRef enroute, WayptVec& wps)
637 {
638   if (!_destination) {
639     return;
640   }
641   
642   string runwayId(destination->getStringValue("runway"));
643   if (!_destination->hasRunwayWithIdent(runwayId)) {
644 // valid airport, but no runway selected, so just the airport node itself
645     wps.push_back(new NavaidWaypoint(_destination.get(), NULL));
646     return;
647   }
648   
649   FGRunway* r = _destination->getRunwayByIdent(runwayId);
650   string starId = destination->getStringValue("star");
651   STAR* star = _destination->findSTARWithIdent(starId);
652   if (!star) {
653 // valid runway, but no STAR selected/found, so just the runway node for now
654     wps.push_back(new RunwayWaypt(r, NULL));
655     return;
656   }
657   
658 // we have a valid STAR
659   string trans(destination->getStringValue("transition"));
660   WayptRef t = star->findTransitionByName(trans);
661   if (!t && enroute) {
662     t = star->findBestTransition(enroute->position());
663   }
664   
665   _destination->buildApproach(t, star, r, wps);
666 }
667
668 void FGRouteMgr::waypointsChanged()
669 {
670
671 }
672
673 void FGRouteMgr::insertWayptAtIndex(Waypt* aWpt, int aIndex)
674 {
675   if (!aWpt) {
676     return;
677   }
678   
679   int index = aIndex;
680   if ((aIndex == -1) || (aIndex > (int) _route.size())) {
681     index = _route.size();
682   }
683   
684   WayptVec::iterator it = _route.begin();
685   it += index;
686       
687   if (_currentIndex >= index) {
688     ++_currentIndex;
689   }
690   
691   _route.insert(it, aWpt);
692   
693   update_mirror();
694   _edited->fireValueChanged();
695 }
696
697 WayptRef FGRouteMgr::waypointFromString(const string& tgt )
698 {
699   string target(boost::to_upper_copy(tgt));
700   WayptRef wpt;
701   
702 // extract altitude
703   double altFt = cruise->getDoubleValue("altitude-ft");
704   RouteRestriction altSetting = RESTRICT_NONE;
705     
706   size_t pos = target.find( '@' );
707   if ( pos != string::npos ) {
708     altFt = atof( target.c_str() + pos + 1 );
709     target = target.substr( 0, pos );
710     if ( !strcmp(fgGetString("/sim/startup/units"), "meter") )
711       altFt *= SG_METER_TO_FEET;
712     altSetting = RESTRICT_AT;
713   }
714
715 // check for lon,lat
716   pos = target.find( ',' );
717   if ( pos != string::npos ) {
718     double lon = atof( target.substr(0, pos).c_str());
719     double lat = atof( target.c_str() + pos + 1);
720     char buf[32];
721     char ew = (lon < 0.0) ? 'W' : 'E';
722     char ns = (lat < 0.0) ? 'S' : 'N';
723     snprintf(buf, 32, "%c%03d%c%03d", ew, (int) fabs(lon), ns, (int)fabs(lat));
724     
725     wpt = new BasicWaypt(SGGeod::fromDeg(lon, lat), buf, NULL);
726     if (altSetting != RESTRICT_NONE) {
727       wpt->setAltitude(altFt, altSetting);
728     }
729     return wpt;
730   }
731
732   SGGeod basePosition;
733   if (_route.empty()) {
734     // route is empty, use current position
735     basePosition = SGGeod::fromDeg(lon->getDoubleValue(), lat->getDoubleValue());
736   } else {
737     basePosition = _route.back()->position();
738   }
739     
740   string_list pieces(simgear::strutils::split(target, "/"));
741   FGPositionedRef p = FGPositioned::findClosestWithIdent(pieces.front(), basePosition);
742   if (!p) {
743     SG_LOG( SG_AUTOPILOT, SG_INFO, "Unable to find FGPositioned with ident:" << pieces.front());
744     return NULL;
745   }
746
747   if (pieces.size() == 1) {
748     wpt = new NavaidWaypoint(p, NULL);
749   } else if (pieces.size() == 3) {
750     // navaid/radial/distance-nm notation
751     double radial = atof(pieces[1].c_str()),
752       distanceNm = atof(pieces[2].c_str());
753     radial += magvar->getDoubleValue(); // convert to true bearing
754     wpt = new OffsetNavaidWaypoint(p, NULL, radial, distanceNm);
755   } else if (pieces.size() == 2) {
756     FGAirport* apt = dynamic_cast<FGAirport*>(p.ptr());
757     if (!apt) {
758       SG_LOG(SG_AUTOPILOT, SG_INFO, "Waypoint is not an airport:" << pieces.front());
759       return NULL;
760     }
761     
762     if (!apt->hasRunwayWithIdent(pieces[1])) {
763       SG_LOG(SG_AUTOPILOT, SG_INFO, "No runway: " << pieces[1] << " at " << pieces[0]);
764       return NULL;
765     }
766       
767     FGRunway* runway = apt->getRunwayByIdent(pieces[1]);
768     wpt = new NavaidWaypoint(runway, NULL);
769   } else if (pieces.size() == 4) {
770     // navid/radial/navid/radial notation     
771     FGPositionedRef p2 = FGPositioned::findClosestWithIdent(pieces[2], basePosition);
772     if (!p2) {
773       SG_LOG( SG_AUTOPILOT, SG_INFO, "Unable to find FGPositioned with ident:" << pieces[2]);
774       return NULL;
775     }
776
777     double r1 = atof(pieces[1].c_str()),
778       r2 = atof(pieces[3].c_str());
779     r1 += magvar->getDoubleValue();
780     r2 += magvar->getDoubleValue();
781     
782     SGGeod intersection;
783     bool ok = SGGeodesy::radialIntersection(p->geod(), r1, p2->geod(), r2, intersection);
784     if (!ok) {
785       SG_LOG(SG_AUTOPILOT, SG_INFO, "no valid intersection for:" << target);
786       return NULL;
787     }
788     
789     std::string name = p->ident() + "-" + p2->ident();
790     wpt = new BasicWaypt(intersection, name, NULL);
791   }
792   
793   if (!wpt) {
794     SG_LOG(SG_AUTOPILOT, SG_INFO, "Unable to parse waypoint:" << target);
795     return NULL;
796   }
797   
798   if (altSetting != RESTRICT_NONE) {
799     wpt->setAltitude(altFt, altSetting);
800   }
801   return wpt;
802 }
803
804 // mirror internal route to the property system for inspection by other subsystems
805 void FGRouteMgr::update_mirror()
806 {
807   mirror->removeChildren("wp");
808   for (int i = 0; i < numWaypts(); i++) {
809     Waypt* wp = _route[i];
810     SGPropertyNode *prop = mirror->getChild("wp", i, 1);
811
812     const SGGeod& pos(wp->position());
813     prop->setStringValue("id", wp->ident().c_str());
814     //prop->setStringValue("name", wp.get_name().c_str());
815     prop->setDoubleValue("longitude-deg", pos.getLongitudeDeg());
816     prop->setDoubleValue("latitude-deg",pos.getLatitudeDeg());
817    
818     if (wp->altitudeRestriction() != RESTRICT_NONE) {
819       double ft = wp->altitudeFt();
820       prop->setDoubleValue("altitude-m", ft * SG_FEET_TO_METER);
821       prop->setDoubleValue("altitude-ft", ft);
822     } else {
823       prop->setDoubleValue("altitude-m", -9999.9);
824       prop->setDoubleValue("altitude-ft", -9999.9);
825     }
826     
827     if (wp->speedRestriction() != RESTRICT_NONE) {
828       prop->setDoubleValue("speed-kts", wp->speedKts());
829     }
830     
831     if (wp->flag(WPT_ARRIVAL)) {
832       prop->setBoolValue("arrival", true);
833     }
834     
835     if (wp->flag(WPT_DEPARTURE)) {
836       prop->setBoolValue("departure", true);
837     }
838     
839     if (wp->flag(WPT_MISS)) {
840       prop->setBoolValue("missed-approach", true);
841     }
842     
843     prop->setBoolValue("generated", wp->flag(WPT_GENERATED));
844   } // of waypoint iteration
845   
846   // set number as listener attachment point
847   mirror->setIntValue("num", _route.size());
848     
849   NewGUI * gui = (NewGUI *)globals->get_subsystem("gui");
850   FGDialog* rmDlg = gui->getDialog("route-manager");
851   if (rmDlg) {
852     rmDlg->updateValues();
853   }
854 }
855
856 // command interface /autopilot/route-manager/input:
857 //
858 //   @CLEAR             ... clear route
859 //   @POP               ... remove first entry
860 //   @DELETE3           ... delete 4th entry
861 //   @INSERT2:KSFO@900  ... insert "KSFO@900" as 3rd entry
862 //   KSFO@900           ... append "KSFO@900"
863 //
864 void FGRouteMgr::InputListener::valueChanged(SGPropertyNode *prop)
865 {
866     const char *s = prop->getStringValue();
867     if (strlen(s) == 0) {
868       return;
869     }
870     
871     if (!strcmp(s, "@CLEAR"))
872         mgr->clearRoute();
873     else if (!strcmp(s, "@ACTIVATE"))
874         mgr->activate();
875     else if (!strcmp(s, "@LOAD")) {
876       mgr->loadRoute();
877     } else if (!strcmp(s, "@SAVE")) {
878       mgr->saveRoute();
879     } else if (!strcmp(s, "@POP")) {
880       SG_LOG(SG_AUTOPILOT, SG_WARN, "route-manager @POP command is deprecated");
881     } else if (!strcmp(s, "@NEXT")) {
882       mgr->jumpToIndex(mgr->_currentIndex + 1);
883     } else if (!strcmp(s, "@PREVIOUS")) {
884       mgr->jumpToIndex(mgr->_currentIndex - 1);
885     } else if (!strncmp(s, "@JUMP", 5)) {
886       mgr->jumpToIndex(atoi(s + 5));
887     } else if (!strncmp(s, "@DELETE", 7))
888         mgr->removeWayptAtIndex(atoi(s + 7));
889     else if (!strncmp(s, "@INSERT", 7)) {
890         char *r;
891         int pos = strtol(s + 7, &r, 10);
892         if (*r++ != ':')
893             return;
894         while (isspace(*r))
895             r++;
896         if (*r)
897             mgr->insertWayptAtIndex(mgr->waypointFromString(r), pos);
898     } else if (!strncmp(s, "@ROUTE", 6)) {
899       char* r;
900       int endIndex = strtol(s + 6, &r, 10);
901       RouteType rt = (RouteType) mgr->_routingType->getIntValue();
902       mgr->routeToIndex(endIndex, rt);
903     } else if (!strcmp(s, "@AUTOROUTE")) {
904       mgr->autoRoute();
905     } else if (!strcmp(s, "@POSINIT")) {
906       mgr->initAtPosition();
907     } else
908       mgr->insertWayptAtIndex(mgr->waypointFromString(s), -1);
909 }
910
911 void FGRouteMgr::initAtPosition()
912 {
913   if (isRouteActive()) {
914     return; // don't mess with the active route
915   }
916   
917   if (haveUserWaypoints()) {
918     // user has already defined, loaded or entered a route, again
919     // don't interfere with it
920     return; 
921   }
922   
923   if (airborne->getBoolValue()) {
924     SG_LOG(SG_AUTOPILOT, SG_INFO, "initAtPosition: airborne, clearing departure info");
925     _departure = NULL;
926     departure->setStringValue("runway", "");
927     return;
928   }
929   
930 // on the ground
931   SGGeod pos = SGGeod::fromDegFt(lon->getDoubleValue(), lat->getDoubleValue(), alt->getDoubleValue());
932   _departure = FGAirport::findClosest(pos, 20.0);
933   if (!_departure) {
934     SG_LOG(SG_AUTOPILOT, SG_INFO, "initAtPosition: couldn't find an airport within 20nm");
935     departure->setStringValue("runway", "");
936     return;
937   }
938   
939   FGRunway* r = _departure->findBestRunwayForPos(pos);
940   if (!r) {
941     return;
942   }
943   
944   departure->setStringValue("runway", r->ident().c_str());
945   SG_LOG(SG_AUTOPILOT, SG_INFO, "initAtPosition: starting at " 
946     << _departure->ident() << " on runway " << r->ident());
947 }
948
949 bool FGRouteMgr::haveUserWaypoints() const
950 {
951   for (int i = 0; i < numWaypts(); i++) {
952     if (!_route[i]->flag(WPT_GENERATED)) {
953       // have a non-generated waypoint, we're done
954       return true;
955     }
956   }
957   
958   // all waypoints are generated
959   return false;
960 }
961
962 bool FGRouteMgr::activate()
963 {
964   if (isRouteActive()) {
965     SG_LOG(SG_AUTOPILOT, SG_WARN, "duplicate route-activation, no-op");
966     return false;
967   }
968  
969   _currentIndex = 0;
970   currentWaypointChanged();
971   
972  /* double routeDistanceNm = _route->total_distance() * SG_METER_TO_NM;
973   totalDistance->setDoubleValue(routeDistanceNm);
974   double cruiseSpeedKts = cruise->getDoubleValue("speed", 0.0);
975   if (cruiseSpeedKts > 1.0) {
976     // very very crude approximation, doesn't allow for climb / descent
977     // performance or anything else at all
978     ete->setDoubleValue(routeDistanceNm / cruiseSpeedKts * (60.0 * 60.0));
979   }
980   */
981   active->setBoolValue(true);
982   SG_LOG(SG_AUTOPILOT, SG_INFO, "route-manager, activate route ok");
983   return true;
984 }
985
986
987 void FGRouteMgr::sequence()
988 {
989   if (!active->getBoolValue()) {
990     SG_LOG(SG_AUTOPILOT, SG_ALERT, "trying to sequence waypoints with no active route");
991     return;
992   }
993   
994   if (checkFinished()) {
995     return;
996   }
997   
998   _currentIndex++;
999   currentWaypointChanged();
1000 }
1001
1002 bool FGRouteMgr::checkFinished()
1003 {
1004   if (_currentIndex < (int) _route.size()) {
1005     return false;
1006   }
1007   
1008   SG_LOG(SG_AUTOPILOT, SG_INFO, "reached end of active route");
1009   _finished->fireValueChanged();
1010   active->setBoolValue(false);
1011   return true;
1012 }
1013
1014 void FGRouteMgr::jumpToIndex(int index)
1015 {
1016   if ((index < 0) || (index >= (int) _route.size())) {
1017     SG_LOG(SG_AUTOPILOT, SG_ALERT, "passed invalid index (" << 
1018       index << ") to FGRouteMgr::jumpToIndex");
1019     return;
1020   }
1021
1022   if (_currentIndex == index) {
1023     return; // no-op
1024   }
1025   
1026 // all the checks out the way, go ahead and update state
1027   _currentIndex = index;
1028   currentWaypointChanged();
1029   _currentWpt->fireValueChanged();
1030 }
1031
1032 void FGRouteMgr::currentWaypointChanged()
1033 {
1034   Waypt* cur = currentWaypt();
1035   Waypt* next = nextWaypt();
1036
1037   wp0->getChild("id")->setStringValue(cur ? cur->ident() : "");
1038   wp1->getChild("id")->setStringValue(next ? next->ident() : "");
1039   
1040   _currentWpt->fireValueChanged();
1041   SG_LOG(SG_AUTOPILOT, SG_INFO, "route manager, current-wp is now " << _currentIndex);
1042 }
1043
1044 int FGRouteMgr::findWayptIndex(const SGGeod& aPos) const
1045 {  
1046   for (int i=0; i<numWaypts(); ++i) {
1047     if (_route[i]->matches(aPos)) {
1048       return i;
1049     }
1050   }
1051   
1052   return -1;
1053 }
1054
1055 Waypt* FGRouteMgr::currentWaypt() const
1056 {
1057   if ((_currentIndex < 0) || (_currentIndex >= numWaypts()))
1058       return NULL;
1059   return wayptAtIndex(_currentIndex);
1060 }
1061
1062 Waypt* FGRouteMgr::previousWaypt() const
1063 {
1064   if (_currentIndex == 0) {
1065     return NULL;
1066   }
1067   
1068   return wayptAtIndex(_currentIndex - 1);
1069 }
1070
1071 Waypt* FGRouteMgr::nextWaypt() const
1072 {
1073   if ((_currentIndex < 0) || ((_currentIndex + 1) >= numWaypts())) {
1074     return NULL;
1075   }
1076   
1077   return wayptAtIndex(_currentIndex + 1);
1078 }
1079
1080 Waypt* FGRouteMgr::wayptAtIndex(int index) const
1081 {
1082   if ((index < 0) || (index >= numWaypts())) {
1083     throw sg_range_exception("waypt index out of range", "FGRouteMgr::wayptAtIndex");
1084   }
1085   
1086   return _route[index];
1087 }
1088
1089 void FGRouteMgr::saveRoute()
1090 {
1091   SGPath path(_pathNode->getStringValue());
1092   SG_LOG(SG_IO, SG_INFO, "Saving route to " << path.str());
1093   try {
1094     SGPropertyNode_ptr d(new SGPropertyNode);
1095     SGPath path(_pathNode->getStringValue());
1096     d->setIntValue("version", 2);
1097     
1098     if (_departure) {
1099       d->setStringValue("departure/airport", _departure->ident());
1100       d->setStringValue("departure/sid", departure->getStringValue("sid"));
1101       d->setStringValue("departure/runway", departure->getStringValue("runway"));
1102     }
1103     
1104     if (_destination) {
1105       d->setStringValue("destination/airport", _destination->ident());
1106       d->setStringValue("destination/star", destination->getStringValue("star"));
1107       d->setStringValue("destination/transition", destination->getStringValue("transition"));
1108       d->setStringValue("destination/runway", destination->getStringValue("runway"));
1109     }
1110     
1111   // route nodes
1112     SGPropertyNode* routeNode = d->getChild("route", 0, true);
1113     for (unsigned int i=0; i<_route.size(); ++i) {
1114       Waypt* wpt = _route[i];
1115       wpt->saveAsNode(routeNode->getChild("wp", i, true));
1116     } // of waypoint iteration
1117     writeProperties(path.str(), d, true /* write-all */);
1118   } catch (sg_exception& e) {
1119     SG_LOG(SG_IO, SG_WARN, "failed to save flight-plan:" << e.getMessage());
1120   }
1121 }
1122
1123 void FGRouteMgr::loadRoute()
1124 {
1125   // deactivate route first
1126   active->setBoolValue(false);
1127   
1128   SGPropertyNode_ptr routeData(new SGPropertyNode);
1129   SGPath path(_pathNode->getStringValue());
1130   
1131   SG_LOG(SG_IO, SG_INFO, "going to read flight-plan from:" << path.str());
1132     
1133   try {
1134     readProperties(path.str(), routeData);
1135   } catch (sg_exception& ) {
1136     // if XML parsing fails, the file might be simple textual list of waypoints
1137     loadPlainTextRoute(path);
1138     return;
1139   }
1140   
1141   try {
1142     int version = routeData->getIntValue("version", 1);
1143     if (version == 1) {
1144       loadVersion1XMLRoute(routeData);
1145     } else if (version == 2) {
1146       loadVersion2XMLRoute(routeData);
1147     } else {
1148       throw sg_io_exception("unsupported XML route version");
1149     }
1150   } catch (sg_exception& e) {
1151     SG_LOG(SG_IO, SG_WARN, "failed to load flight-plan (from '" << e.getOrigin()
1152       << "'):" << e.getMessage());
1153   }
1154 }
1155
1156 void FGRouteMgr::loadXMLRouteHeader(SGPropertyNode_ptr routeData)
1157 {
1158   // departure nodes
1159   SGPropertyNode* dep = routeData->getChild("departure");
1160   if (dep) {
1161     string depIdent = dep->getStringValue("airport");
1162     _departure = (FGAirport*) fgFindAirportID(depIdent);
1163     departure->setStringValue("runway", dep->getStringValue("runway"));
1164     departure->setStringValue("sid", dep->getStringValue("sid"));
1165     departure->setStringValue("transition", dep->getStringValue("transition"));
1166   }
1167   
1168 // destination
1169   SGPropertyNode* dst = routeData->getChild("destination");
1170   if (dst) {
1171     _destination = (FGAirport*) fgFindAirportID(dst->getStringValue("airport"));
1172     destination->setStringValue("runway", dst->getStringValue("runway"));
1173     destination->setStringValue("star", dst->getStringValue("star"));
1174     destination->setStringValue("transition", dst->getStringValue("transition"));
1175   }
1176
1177 // alternate
1178   SGPropertyNode* alt = routeData->getChild("alternate");
1179   if (alt) {
1180     alternate->setStringValue(alt->getStringValue("airport"));
1181   } // of cruise data loading
1182   
1183 // cruise
1184   SGPropertyNode* crs = routeData->getChild("cruise");
1185   if (crs) {
1186     cruise->setDoubleValue("speed-kts", crs->getDoubleValue("speed-kts"));
1187     cruise->setDoubleValue("mach", crs->getDoubleValue("mach"));
1188     cruise->setDoubleValue("altitude-ft", crs->getDoubleValue("altitude-ft"));
1189   } // of cruise data loading
1190
1191 }
1192
1193 void FGRouteMgr::loadVersion2XMLRoute(SGPropertyNode_ptr routeData)
1194 {
1195   loadXMLRouteHeader(routeData);
1196   
1197 // route nodes
1198   WayptVec wpts;
1199   SGPropertyNode_ptr routeNode = routeData->getChild("route", 0);    
1200   for (int i=0; i<routeNode->nChildren(); ++i) {
1201     SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i);
1202     WayptRef wpt = Waypt::createFromProperties(NULL, wpNode);
1203     wpts.push_back(wpt);
1204   } // of route iteration
1205   
1206   _route = wpts;
1207 }
1208
1209 void FGRouteMgr::loadVersion1XMLRoute(SGPropertyNode_ptr routeData)
1210 {
1211   loadXMLRouteHeader(routeData);
1212
1213 // route nodes
1214   WayptVec wpts;
1215   SGPropertyNode_ptr routeNode = routeData->getChild("route", 0);    
1216   for (int i=0; i<routeNode->nChildren(); ++i) {
1217     SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i);
1218     WayptRef wpt = parseVersion1XMLWaypt(wpNode);
1219     wpts.push_back(wpt);
1220   } // of route iteration
1221   
1222   _route = wpts;
1223 }
1224
1225 WayptRef FGRouteMgr::parseVersion1XMLWaypt(SGPropertyNode* aWP)
1226 {
1227   SGGeod lastPos;
1228   if (!_route.empty()) {
1229     lastPos = _route.back()->position();
1230   } else if (_departure) {
1231     lastPos = _departure->geod();
1232   }
1233
1234   WayptRef w;
1235   string ident(aWP->getStringValue("ident"));
1236   if (aWP->hasChild("longitude-deg")) {
1237     // explicit longitude/latitude
1238     w = new BasicWaypt(SGGeod::fromDeg(aWP->getDoubleValue("longitude-deg"), 
1239       aWP->getDoubleValue("latitude-deg")), ident, NULL);
1240     
1241   } else {
1242     string nid = aWP->getStringValue("navid", ident.c_str());
1243     FGPositionedRef p = FGPositioned::findClosestWithIdent(nid, lastPos);
1244     if (!p) {
1245       throw sg_io_exception("bad route file, unknown navid:" + nid);
1246     }
1247       
1248     SGGeod pos(p->geod());
1249     if (aWP->hasChild("offset-nm") && aWP->hasChild("offset-radial")) {
1250       double radialDeg = aWP->getDoubleValue("offset-radial");
1251       // convert magnetic radial to a true radial!
1252       radialDeg += magvar->getDoubleValue();
1253       double offsetNm = aWP->getDoubleValue("offset-nm");
1254       double az2;
1255       SGGeodesy::direct(p->geod(), radialDeg, offsetNm * SG_NM_TO_METER, pos, az2);
1256     }
1257
1258     w = new BasicWaypt(pos, ident, NULL);
1259   }
1260   
1261   double altFt = aWP->getDoubleValue("altitude-ft", -9999.9);
1262   if (altFt > -9990.0) {
1263     w->setAltitude(altFt, RESTRICT_AT);
1264   }
1265
1266   return w;
1267 }
1268
1269 void FGRouteMgr::loadPlainTextRoute(const SGPath& path)
1270 {
1271   sg_gzifstream in(path.str().c_str());
1272   if (!in.is_open()) {
1273     return;
1274   }
1275   
1276   try {
1277     WayptVec wpts;
1278     while (!in.eof()) {
1279       string line;
1280       getline(in, line, '\n');
1281     // trim CR from end of line, if found
1282       if (line[line.size() - 1] == '\r') {
1283         line.erase(line.size() - 1, 1);
1284       }
1285       
1286       line = simgear::strutils::strip(line);
1287       if (line.empty() || (line[0] == '#')) {
1288         continue; // ignore empty/comment lines
1289       }
1290       
1291       WayptRef w = waypointFromString(line);
1292       if (!w) {
1293         throw sg_io_exception("failed to create waypoint from line:" + line);
1294       }
1295       
1296       wpts.push_back(w);
1297     } // of line iteration
1298   
1299     _route = wpts;
1300   } catch (sg_exception& e) {
1301     SG_LOG(SG_IO, SG_WARN, "failed to load route from:" << path.str() << ":" << e.getMessage());
1302   }
1303 }
1304
1305 const char* FGRouteMgr::getDepartureICAO() const
1306 {
1307   if (!_departure) {
1308     return "";
1309   }
1310   
1311   return _departure->ident().c_str();
1312 }
1313
1314 const char* FGRouteMgr::getDepartureName() const
1315 {
1316   if (!_departure) {
1317     return "";
1318   }
1319   
1320   return _departure->name().c_str();
1321 }
1322
1323 void FGRouteMgr::setDepartureICAO(const char* aIdent)
1324 {
1325   if ((aIdent == NULL) || (strlen(aIdent) < 4)) {
1326     _departure = NULL;
1327   } else {
1328     _departure = FGAirport::findByIdent(aIdent);
1329   }
1330   
1331   departureChanged();
1332 }
1333
1334 const char* FGRouteMgr::getDestinationICAO() const
1335 {
1336   if (!_destination) {
1337     return "";
1338   }
1339   
1340   return _destination->ident().c_str();
1341 }
1342
1343 const char* FGRouteMgr::getDestinationName() const
1344 {
1345   if (!_destination) {
1346     return "";
1347   }
1348   
1349   return _destination->name().c_str();
1350 }
1351
1352 void FGRouteMgr::setDestinationICAO(const char* aIdent)
1353 {
1354   if ((aIdent == NULL) || (strlen(aIdent) < 4)) {
1355     _destination = NULL;
1356   } else {
1357     _destination = FGAirport::findByIdent(aIdent);
1358   }
1359   
1360   arrivalChanged();
1361 }