Land the GPS/route-manager re-write. Many things are better, many other things will be better, some things are no doubt broken. Please be patient and report problems on the mailing list.

src/Autopilot/route_mgr.cxx

@@ -29,34 +29,45 @@
 
 #include <simgear/compiler.h>
 
-#include <FDM/flight.hxx>
-#include <Main/fg_props.hxx>
-#include <Navaids/positioned.hxx>
-
 #include "route_mgr.hxx"
 
+#include <simgear/misc/strutils.hxx>
+#include <simgear/structure/exception.hxx>
+
+#include <simgear/props/props_io.hxx>
+#include <simgear/misc/sg_path.hxx>
+#include <simgear/route/route.hxx>
+#include <simgear/sg_inlines.h>
+
+#include "Main/fg_props.hxx"
+#include "Navaids/positioned.hxx"
+#include "Airports/simple.hxx"
+#include "Airports/runways.hxx"
+
+#include "FDM/flight.hxx" // for getting ground speed
+
 #define RM "/autopilot/route-manager/"
 
+static double get_ground_speed() {
+  // starts in ft/s so we convert to kts
+  static const SGPropertyNode * speedup_node = fgGetNode("/sim/speed-up");
+
+  double ft_s = cur_fdm_state->get_V_ground_speed()
+      * speedup_node->getIntValue();
+  double kts = ft_s * SG_FEET_TO_METER * 3600 * SG_METER_TO_NM;
+  return kts;
+}
 
 FGRouteMgr::FGRouteMgr() :
-    route( new SGRoute ),
+    _route( new SGRoute ),
+    _autoSequence(true),
     lon( NULL ),
     lat( NULL ),
     alt( NULL ),
-    true_hdg_deg( NULL ),
     target_altitude_ft( NULL ),
     altitude_lock( NULL ),
-    wp0_id( NULL ),
-    wp0_dist( NULL ),
-    wp0_eta( NULL ),
-    wp1_id( NULL ),
-    wp1_dist( NULL ),
-    wp1_eta( NULL ),
-    wpn_id( NULL ),
-    wpn_dist( NULL ),
-    wpn_eta( NULL ),
     input(fgGetNode( RM "input", true )),
-    listener(new Listener(this)),
+    listener(new InputListener(this)),
     mirror(fgGetNode( RM "route", true )),
     altitude_set( false )
 {
@@ -67,77 +78,128 @@ FGRouteMgr::FGRouteMgr() :
 
 FGRouteMgr::~FGRouteMgr() {
     input->removeChangeListener(listener);
+    
     delete listener;
-    delete route;
+    delete _route;
 }
 
 
 void FGRouteMgr::init() {
-    lon = fgGetNode( "/position/longitude-deg", true );
+  SGPropertyNode_ptr rm(fgGetNode(RM));
-    lat = fgGetNode( "/position/latitude-deg", true );
+  
-    alt = fgGetNode( "/position/altitude-ft", true );
+  lon = fgGetNode( "/position/longitude-deg", true );
+  lat = fgGetNode( "/position/latitude-deg", true );
+  alt = fgGetNode( "/position/altitude-ft", true );
+  magvar = fgGetNode("/environment/magnetic-variation-deg", true);
+  
+  target_altitude_ft = fgGetNode( "/autopilot/settings/target-altitude-ft", true );
+  altitude_lock = fgGetNode( "/autopilot/locks/altitude", true );
 
-    true_hdg_deg = fgGetNode( "/autopilot/settings/true-heading-deg", true );
+  departure = fgGetNode(RM "departure", true);
-    target_altitude_ft = fgGetNode( "/autopilot/settings/target-altitude-ft", true );
+// init departure information from current location
-    altitude_lock = fgGetNode( "/autopilot/locks/altitude", true );
+  SGGeod pos = SGGeod::fromDegFt(lon->getDoubleValue(), lat->getDoubleValue(), alt->getDoubleValue());
+  FGAirport* apt = FGAirport::findClosest(pos, 10.0);
+  if (apt) {
+    departure->setStringValue("airport", apt->ident().c_str());
+    FGRunway* active = apt->getActiveRunwayForUsage();
+    departure->setStringValue("runway", active->ident().c_str());
+  } else {
+    departure->setStringValue("airport", "");
+    departure->setStringValue("runway", "");
+  }
+  
+  departure->getChild("etd", 0, true);
+  departure->getChild("takeoff-time", 0, true);
 
-    wp0_id = fgGetNode( RM "wp[0]/id", true );
+  destination = fgGetNode(RM "destination", true);
-    wp0_dist = fgGetNode( RM "wp[0]/dist", true );
+  destination->getChild("airport", 0, true);
-    wp0_eta = fgGetNode( RM "wp[0]/eta", true );
+  destination->getChild("runway", 0, true);
-
+  destination->getChild("eta", 0, true);
-    wp1_id = fgGetNode( RM "wp[1]/id", true );
+  destination->getChild("touchdown-time", 0, true);
-    wp1_dist = fgGetNode( RM "wp[1]/dist", true );
+  
-    wp1_eta = fgGetNode( RM "wp[1]/eta", true );
+  alternate = fgGetNode(RM "alternate", true);
-
+  alternate->getChild("airport", 0, true);
-    wpn_id = fgGetNode( RM "wp-last/id", true );
+  alternate->getChild("runway", 0, true);
-    wpn_dist = fgGetNode( RM "wp-last/dist", true );
+  
-    wpn_eta = fgGetNode( RM "wp-last/eta", true );
+  cruise = fgGetNode(RM "cruise", true);
-
+  cruise->getChild("altitude", 0, true);
-    route->clear();
+  cruise->getChild("flight-level", 0, true);
-    update_mirror();
+  cruise->getChild("speed", 0, true);
+      
+  totalDistance = fgGetNode(RM "total-distance", true);
+  totalDistance->setDoubleValue(0.0);
+  
+  ete = fgGetNode(RM "ete", true);
+  ete->setDoubleValue(0.0);
+  
+  elapsedFlightTime = fgGetNode(RM "flight-time", true);
+  elapsedFlightTime->setDoubleValue(0.0);
+  
+  active = fgGetNode(RM "active", true);
+  active->setBoolValue(false);
+  
+  airborne = fgGetNode(RM "airborne", true);
+  airborne->setBoolValue(false);
+  
+  rm->tie("auto-sequence", SGRawValuePointer<bool>(&_autoSequence));
+  
+  currentWp = fgGetNode(RM "current-wp", true);
+  currentWp->setIntValue(_route->current_index());
+      
+  // temporary distance / eta calculations, for backward-compatability
+  wp0 = fgGetNode(RM "wp", 0, true);
+  wp0->getChild("id", 0, true);
+  wp0->getChild("dist", 0, true);
+  wp0->getChild("eta", 0, true);
+  wp0->getChild("bearing-deg", 0, true);
+  
+  wp1 = fgGetNode(RM "wp", 1, true);
+  wp1->getChild("id", 0, true);
+  wp1->getChild("dist", 0, true);
+  wp1->getChild("eta", 0, true);
+  
+  wpn = fgGetNode(RM "wp-last", 0, true);
+  wpn->getChild("dist", 0, true);
+  wpn->getChild("eta", 0, true);
+  
+  _route->clear();
+  update_mirror();
+  
+  _pathNode = fgGetNode(RM "file-path", 0, true);
 }
 
 
 void FGRouteMgr::postinit() {
     string_list *waypoints = globals->get_initial_waypoints();
-    if (!waypoints)
+    if (waypoints) {
-        return;
+      vector<string>::iterator it;
-
+      for (it = waypoints->begin(); it != waypoints->end(); ++it)
-    vector<string>::iterator it;
-    for (it = waypoints->begin(); it != waypoints->end(); ++it)
         new_waypoint(*it);
+    }
+
+    weightOnWheels = fgGetNode("/gear/gear[0]/wow", false);
+    // check airbone flag agrees with presets
+    
 }
 
 
 void FGRouteMgr::bind() { }
 void FGRouteMgr::unbind() { }
 
-
-static double get_ground_speed() {
-    // starts in ft/s so we convert to kts
-    static const SGPropertyNode * speedup_node = fgGetNode("/sim/speed-up");
-
-    double ft_s = cur_fdm_state->get_V_ground_speed()
-        * speedup_node->getIntValue();
-    double kts = ft_s * SG_FEET_TO_METER * 3600 * SG_METER_TO_NM;
-
-    return kts;
-}
-
 void FGRouteMgr::updateTargetAltitude() {
-    if (route->size() == 0) {
+    if (_route->current_index() == _route->size()) {
         altitude_set = false;
         return;
     }
     
-    SGWayPoint wp = route->get_waypoint( 0 );
+    SGWayPoint wp = _route->get_current();
-    if (wp.get_target_alt() < -9990.0) {
+    if (wp.get_target().getElevationM() < -9990.0) {
         altitude_set = false;
         return;
     }
     
     altitude_set = true;
-    target_altitude_ft->setDoubleValue( wp.get_target_alt() * SG_METER_TO_FEET );
+    target_altitude_ft->setDoubleValue(wp.get_target().getElevationFt());
             
     if ( !near_ground() ) {
         // James Turner [zakalawe]: there's no explanation for this logic,
@@ -147,60 +209,79 @@ void FGRouteMgr::updateTargetAltitude() {
     }
 }
 
+bool FGRouteMgr::isRouteActive() const
+{
+  return active->getBoolValue();
+}
+
 void FGRouteMgr::update( double dt ) {
-    if (route->size() == 0) {
+    if (!active->getBoolValue()) {
-      return; // no route set, early return
+      return;
     }
     
-    double wp_course, wp_distance;
+    double groundSpeed = get_ground_speed();
-
+    if (!airborne->getBoolValue()) {
-    // first way point
+      if (weightOnWheels->getBoolValue() || (groundSpeed < 40)) {
-    SGWayPoint wp = route->get_waypoint( 0 );
+        return;
+      }
+      
+      airborne->setBoolValue(true);
+      _takeoffTime = time(NULL); // start the clock
+      departure->setIntValue("takeoff-time", _takeoffTime);
+    }
+    
+    time_t now = time(NULL);
+    elapsedFlightTime->setDoubleValue(difftime(now, _takeoffTime));
+    
+    double inboundCourse, dummy, wp_course, wp_distance;
+    SGWayPoint wp = _route->get_current();
+  
+    wp.CourseAndDistance(_route->get_waypoint(_route->current_index() - 1), 
+      &inboundCourse, &dummy);
+    
     wp.CourseAndDistance( lon->getDoubleValue(), lat->getDoubleValue(),
                           alt->getDoubleValue(), &wp_course, &wp_distance );
-    true_hdg_deg->setDoubleValue( wp_course );
 
-    if ( wp_distance < 200.0 ) {
+    if (_autoSequence && (wp_distance < 2000.0)) {
-        pop_waypoint();
+      double courseDeviation = inboundCourse - wp_course;
-        if (route->size() == 0) {
+      SG_NORMALIZE_RANGE(courseDeviation, -180.0, 180.0);
-            return; // end of route, we're done for the time being
+      if (fabs(courseDeviation) < 90.0) {
-        }
+        SG_LOG( SG_AUTOPILOT, SG_INFO, "route manager doing sequencing");
-        
+        sequence();
-        wp = route->get_waypoint( 0 );
+      }
     }
 
-  // update the property tree info for WP0
+    wp0->setDoubleValue("dist", wp_distance * SG_METER_TO_NM);
-    wp0_id->setStringValue( wp.get_id().c_str() );
+    wp_course -= magvar->getDoubleValue(); // expose magnetic bearing
-    double accum = wp_distance;
+    wp0->setDoubleValue("bearing-deg", wp_course);
-    wp0_dist->setDoubleValue( accum * SG_METER_TO_NM );
+    setETAPropertyFromDistance(wp0->getChild("eta"), wp_distance);
-    setETAPropertyFromDistance(wp0_eta, accum);
+    
-
+    if ((_route->current_index() + 1) < _route->size()) {
-    // next way point
+      wp = _route->get_waypoint(_route->current_index() + 1);
-    if ( route->size() > 1 ) {
+      double wp1_course, wp1_distance;
-        SGWayPoint wp = route->get_waypoint( 1 );
+      wp.CourseAndDistance(lon->getDoubleValue(), lat->getDoubleValue(),
-
+                          alt->getDoubleValue(), &wp1_course, &wp1_distance);
-        // update the property tree info
+    
-        wp1_id->setStringValue( wp.get_id().c_str() );
+      wp1->setDoubleValue("dist", wp1_distance * SG_METER_TO_NM);
-        accum += wp.get_distance();
+      setETAPropertyFromDistance(wp1->getChild("eta"), wp1_distance);
-        wp1_dist->setDoubleValue( accum * SG_METER_TO_NM );
-        setETAPropertyFromDistance(wp1_eta, accum);
     }
-
+    
-    // summarize remaining way points
+    double totalDistanceRemaining = wp_distance; // distance to current waypoint
-    if ( route->size() > 2 ) {
+    for (int i=_route->current_index() + 1; i<_route->size(); ++i) {
-        SGWayPoint wp;
+      totalDistanceRemaining += _route->get_waypoint(i).get_distance();
-        for ( int i = 2; i < route->size(); ++i ) {
-            wp = route->get_waypoint( i );
-            accum += wp.get_distance();
-        }
-
-        // update the property tree info
-        wpn_id->setStringValue( wp.get_id().c_str() );
-        wpn_dist->setDoubleValue( accum * SG_METER_TO_NM );
-        setETAPropertyFromDistance(wpn_eta, accum);
     }
+    
+    wpn->setDoubleValue("dist", totalDistanceRemaining * SG_METER_TO_NM);
+    ete->setDoubleValue(totalDistanceRemaining * SG_METER_TO_NM / groundSpeed * 3600.0);
+    setETAPropertyFromDistance(wpn->getChild("eta"), totalDistanceRemaining);
+    
+    // get time now at destination tz as tm struct
+    // add ete seconds
+    // convert to string ... and stash in property
+    //destination->setDoubleValue("eta", eta);
 }
 
+
 void FGRouteMgr::setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDistance) {
     char eta_str[64];
     double eta = aDistance * SG_METER_TO_NM / get_ground_speed();
@@ -209,8 +290,7 @@ void FGRouteMgr::setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDi
     }
     
     if ( eta < (1.0/6.0) ) {
-        // within 10 minutes, bump up to min/secs
+      eta *= 60.0; // within 10 minutes, bump up to min/secs
-        eta *= 60.0;
     }
     
     int major = (int)eta, 
@@ -220,9 +300,9 @@ void FGRouteMgr::setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDi
 }
 
 void FGRouteMgr::add_waypoint( const SGWayPoint& wp, int n ) {
-    route->add_waypoint( wp, n );
+    _route->add_waypoint( wp, n );
     update_mirror();
-    if ((n==0) || (route->size() == 1)) {
+    if ((n==0) || (_route->size() == 1)) {
         updateTargetAltitude();
     }
 }
@@ -231,29 +311,11 @@ void FGRouteMgr::add_waypoint( const SGWayPoint& wp, int n ) {
 SGWayPoint FGRouteMgr::pop_waypoint( int n ) {
     SGWayPoint wp;
 
-    if ( route->size() > 0 ) {
+    if ( _route->size() > 0 ) {
         if ( n < 0 )
-            n = route->size() - 1;
+            n = _route->size() - 1;
-        wp = route->get_waypoint(n);
+        wp = _route->get_waypoint(n);
-        route->delete_waypoint(n);
+        _route->delete_waypoint(n);
-    }
-
-    if ( route->size() <= 2 ) {
-        wpn_id->setStringValue( "" );
-        wpn_dist->setDoubleValue( 0.0 );
-        wpn_eta->setStringValue( "" );
-    }
-
-    if ( route->size() <= 1 ) {
-        wp1_id->setStringValue( "" );
-        wp1_dist->setDoubleValue( 0.0 );
-        wp1_eta->setStringValue( "" );
-    }
-
-    if ( route->size() <= 0 ) {
-        wp0_id->setStringValue( "" );
-        wp0_dist->setDoubleValue( 0.0 );
-        wp0_eta->setStringValue( "" );
     }
 
     updateTargetAltitude();
@@ -286,9 +348,9 @@ SGWayPoint* FGRouteMgr::make_waypoint(const string& tgt ) {
     string target = tgt;
     
     // make upper case
-    for (unsigned int i = 0; i < target.size(); i++)
+    for (unsigned int i = 0; i < target.size(); i++) {
-        if (target[i] >= 'a' && target[i] <= 'z')
+      target[i] = toupper(target[i]);
-            target[i] -= 'a' - 'A';
+    }
 
     // extract altitude
     double alt = -9999.0;
@@ -305,49 +367,90 @@ SGWayPoint* FGRouteMgr::make_waypoint(const string& tgt ) {
     if ( pos != string::npos ) {
         double lon = atof( target.substr(0, pos).c_str());
         double lat = atof( target.c_str() + pos + 1);
-
-        SG_LOG( SG_GENERAL, SG_INFO, "Adding waypoint lon = " << lon << ", lat = " << lat );
         return new SGWayPoint( lon, lat, alt, SGWayPoint::WGS84, target );
     }    
 
     SGGeod basePosition;
-    if (route->size() > 0) {
+    if (_route->size() > 0) {
-        SGWayPoint wp = get_waypoint(route->size()-1);
+        SGWayPoint wp = get_waypoint(_route->size()-1);
-        basePosition = SGGeod::fromDeg(wp.get_target_lon(), wp.get_target_lat());
+        basePosition = wp.get_target();
     } else {
         // route is empty, use current position
         basePosition = SGGeod::fromDeg(
             fgGetNode("/position/longitude-deg")->getDoubleValue(), 
             fgGetNode("/position/latitude-deg")->getDoubleValue());
     }
+    
+    vector<string> pieces(simgear::strutils::split(target, "/"));
 
 
-    FGPositionedRef p = FGPositioned::findClosestWithIdent(target, basePosition);
+    FGPositionedRef p = FGPositioned::findClosestWithIdent(pieces.front(), basePosition);
     if (!p) {
-        SG_LOG( SG_GENERAL, SG_INFO, "Unable to find FGPositioned with ident:" << target);
+      SG_LOG( SG_AUTOPILOT, SG_INFO, "Unable to find FGPositioned with ident:" << pieces.front());
-        return NULL;
+      return NULL;
     }
     
-    return new SGWayPoint(p->longitude(), p->latitude(), alt, SGWayPoint::WGS84, target);
+    if (pieces.size() == 1) {
+      // simple case
+      return new SGWayPoint(p->geod(), target, p->name());
+    }
+        
+    if (pieces.size() == 3) {
+      // navaid/radial/distance-nm notation
+      double radial = atof(pieces[1].c_str()),
+        distanceNm = atof(pieces[2].c_str()),
+        az2;
+      radial += magvar->getDoubleValue(); // convert to true bearing
+      SGGeod offsetPos;
+      SGGeodesy::direct(p->geod(), radial, distanceNm * SG_NM_TO_METER, offsetPos, az2);
+      
+      SG_LOG(SG_AUTOPILOT, SG_INFO, "final offset radial is " << radial);
+      
+      if (alt > -9990) {
+        offsetPos.setElevationM(alt);
+      } // otherwise use the elevation of navid
+      return new SGWayPoint(offsetPos, p->ident() + pieces[2], target);
+    }
+    
+    if (pieces.size() == 2) {
+      FGAirport* apt = dynamic_cast<FGAirport*>(p.ptr());
+      if (!apt) {
+        SG_LOG(SG_AUTOPILOT, SG_INFO, "Waypoint is not an airport:" << pieces.front());
+        return NULL;
+      }
+      
+      if (!apt->hasRunwayWithIdent(pieces[1])) {
+        SG_LOG(SG_AUTOPILOT, SG_INFO, "No runway: " << pieces[1] << " at " << pieces[0]);
+        return NULL;
+      }
+      
+      FGRunway* runway = apt->getRunwayByIdent(pieces[1]);
+      SGGeod t = runway->threshold();
+      return new SGWayPoint(t.getLongitudeDeg(), t.getLatitudeDeg(), alt, SGWayPoint::WGS84, pieces[1]);
+    }
+    
+    SG_LOG(SG_AUTOPILOT, SG_INFO, "Unable to parse waypoint:" << target);
+    return NULL;
 }
 
 
 // mirror internal route to the property system for inspection by other subsystems
 void FGRouteMgr::update_mirror() {
     mirror->removeChildren("wp");
-    for (int i = 0; i < route->size(); i++) {
+    for (int i = 0; i < _route->size(); i++) {
-        SGWayPoint wp = route->get_waypoint(i);
+        SGWayPoint wp = _route->get_waypoint(i);
         SGPropertyNode *prop = mirror->getChild("wp", i, 1);
 
+        const SGGeod& pos(wp.get_target());
         prop->setStringValue("id", wp.get_id().c_str());
         prop->setStringValue("name", wp.get_name().c_str());
-        prop->setDoubleValue("longitude-deg", wp.get_target_lon());
+        prop->setDoubleValue("longitude-deg", pos.getLongitudeDeg());
-        prop->setDoubleValue("latitude-deg", wp.get_target_lat());
+        prop->setDoubleValue("latitude-deg",pos.getLatitudeDeg());
-        prop->setDoubleValue("altitude-m", wp.get_target_alt());
+        prop->setDoubleValue("altitude-m", pos.getElevationM());
-        prop->setDoubleValue("altitude-ft", wp.get_target_alt() * SG_METER_TO_FEET);
+        prop->setDoubleValue("altitude-ft", pos.getElevationFt());
     }
     // set number as listener attachment point
-    mirror->setIntValue("num", route->size());
+    mirror->setIntValue("num", _route->size());
 }
 
 
@@ -372,12 +475,18 @@ bool FGRouteMgr::near_ground() {
 //   @INSERT2:KSFO@900  ... insert "KSFO@900" as 3rd entry
 //   KSFO@900           ... append "KSFO@900"
 //
-void FGRouteMgr::Listener::valueChanged(SGPropertyNode *prop)
+void FGRouteMgr::InputListener::valueChanged(SGPropertyNode *prop)
 {
     const char *s = prop->getStringValue();
     if (!strcmp(s, "@CLEAR"))
         mgr->init();
-    else if (!strcmp(s, "@POP"))
+    else if (!strcmp(s, "@ACTIVATE"))
+        mgr->activate();
+    else if (!strcmp(s, "@LOAD")) {
+      mgr->loadRoute();
+    } else if (!strcmp(s, "@SAVE")) {
+      mgr->saveRoute();
+    } else if (!strcmp(s, "@POP"))
         mgr->pop_waypoint(0);
     else if (!strncmp(s, "@DELETE", 7))
         mgr->pop_waypoint(atoi(s + 7));
@@ -394,4 +503,296 @@ void FGRouteMgr::Listener::valueChanged(SGPropertyNode *prop)
         mgr->new_waypoint(s);
 }
 
+//    SGWayPoint( const double lon = 0.0, const double lat = 0.0,
+//		const double alt = 0.0, const modetype m = WGS84,
+//		const string& s = "", const string& n = "" );
 
+bool FGRouteMgr::activate()
+{
+  const FGAirport* depApt = fgFindAirportID(departure->getStringValue("airport"));
+  if (!depApt) {
+    SG_LOG(SG_AUTOPILOT, SG_ALERT, 
+      "unable to activate route: departure airport is invalid:" 
+        << departure->getStringValue("airport") );
+    return false;
+  }
+  
+  string runwayId(departure->getStringValue("runway"));
+  FGRunway* runway = NULL;
+  if (depApt->hasRunwayWithIdent(runwayId)) {
+    runway = depApt->getRunwayByIdent(runwayId);
+  } else {
+    SG_LOG(SG_AUTOPILOT, SG_INFO, 
+      "route-manager, departure runway not found:" << runwayId);
+    runway = depApt->getActiveRunwayForUsage();
+  }
+  
+  SGWayPoint swp(runway->threshold(), 
+    depApt->ident() + "-" + runway->ident(), runway->name());
+  add_waypoint(swp, 0);
+  
+  const FGAirport* destApt = fgFindAirportID(destination->getStringValue("airport"));
+  if (!destApt) {
+    SG_LOG(SG_AUTOPILOT, SG_ALERT, 
+      "unable to activate route: destination airport is invalid:" 
+        << destination->getStringValue("airport") );
+    return false;
+  }
+
+  runwayId = (destination->getStringValue("runway"));
+  if (destApt->hasRunwayWithIdent(runwayId)) {
+    FGRunway* runway = depApt->getRunwayByIdent(runwayId);
+    SGWayPoint swp(runway->end(), 
+      destApt->ident() + "-" + runway->ident(), runway->name());
+    add_waypoint(swp);
+  } else {
+    // quite likely, since destination runway may not be known until enroute
+    // probably want a listener on the 'destination' node to allow an enroute
+    // update
+    add_waypoint(SGWayPoint(destApt->geod(), destApt->ident(), destApt->name()));
+  }
+  
+  _route->set_current(0);
+  
+  double routeDistanceNm = _route->total_distance() * SG_METER_TO_NM;
+  totalDistance->setDoubleValue(routeDistanceNm);
+  double cruiseSpeedKts = cruise->getDoubleValue("speed", 0.0);
+  if (cruiseSpeedKts > 1.0) {
+    // very very crude approximation, doesn't allow for climb / descent
+    // performance or anything else at all
+    ete->setDoubleValue(routeDistanceNm / cruiseSpeedKts * (60.0 * 60.0));
+  }
+  
+  active->setBoolValue(true);
+  sequence(); // sequence will sync up wp0, wp1 and current-wp
+  SG_LOG(SG_AUTOPILOT, SG_INFO, "route-manager, activate route ok");
+  return true;
+}
+
+
+void FGRouteMgr::sequence()
+{
+  if (!active->getBoolValue()) {
+    SG_LOG(SG_AUTOPILOT, SG_ALERT, "trying to sequence waypoints with no active route");
+    return;
+  }
+  
+  if (_route->current_index() == _route->size()) {
+    SG_LOG(SG_AUTOPILOT, SG_INFO, "reached end of active route");
+    // what now?
+    active->setBoolValue(false);
+    return;
+  }
+  
+  _route->increment_current();
+  currentWaypointChanged();
+}
+
+void FGRouteMgr::jumpToIndex(int index)
+{
+  if (!active->getBoolValue()) {
+    SG_LOG(SG_AUTOPILOT, SG_ALERT, "trying to sequence waypoints with no active route");
+    return;
+  }
+
+  if ((index < 0) || (index >= _route->size())) {
+    SG_LOG(SG_AUTOPILOT, SG_ALERT, "passed invalid index (" << 
+      index << ") to FGRouteMgr::jumpToIndex");
+    return;
+  }
+
+  if (_route->current_index() == index) {
+    return; // no-op
+  }
+  
+  _route->set_current(index);
+  currentWaypointChanged();
+}
+
+void FGRouteMgr::currentWaypointChanged()
+{
+  SGWayPoint previous = _route->get_previous();
+  SGWayPoint cur = _route->get_current();
+  
+  wp0->getChild("id")->setStringValue(cur.get_id());
+  if ((_route->current_index() + 1) < _route->size()) {
+    wp1->getChild("id")->setStringValue(_route->get_next().get_id());
+  } else {
+    wp1->getChild("id")->setStringValue("");
+  }
+  
+  currentWp->setIntValue(_route->current_index());
+  SG_LOG(SG_AUTOPILOT, SG_INFO, "route manager, current-wp is now " << _route->current_index());
+}
+
+int FGRouteMgr::findWaypoint(const SGGeod& aPos) const
+{  
+  for (int i=0; i<_route->size(); ++i) {
+    double d = SGGeodesy::distanceM(aPos, _route->get_waypoint(i).get_target());
+    if (d < 200.0) { // 200 metres seems close enough
+      return i;
+    }
+  }
+  
+  return -1;
+}
+
+SGWayPoint FGRouteMgr::get_waypoint( int i ) const
+{
+  return _route->get_waypoint(i);
+}
+
+int FGRouteMgr::size() const
+{
+  return _route->size();
+}
+
+int FGRouteMgr::currentWaypoint() const
+{
+  return _route->current_index();
+}
+
+void FGRouteMgr::saveRoute()
+{
+  SGPath path(_pathNode->getStringValue());
+  SG_LOG(SG_IO, SG_INFO, "Saving route to " << path.str());
+  try {
+    writeProperties(path.str(), mirror, false, SGPropertyNode::ARCHIVE);
+  } catch (const sg_exception &e) {
+    SG_LOG(SG_IO, SG_WARN, "Error saving route:" << e.getMessage());
+    //guiErrorMessage("Error writing autosave.xml: ", e);
+  }
+}
+
+void FGRouteMgr::loadRoute()
+{
+  try {
+    // deactivate route first
+    active->setBoolValue(false);
+    
+    SGPropertyNode_ptr routeData(new SGPropertyNode);
+    SGPath path(_pathNode->getStringValue());
+    
+    SG_LOG(SG_IO, SG_INFO, "going to read flight-plan from:" << path.str());
+    readProperties(path.str(), routeData);
+    
+  // departure nodes
+    SGPropertyNode* dep = routeData->getChild("departure");
+    if (!dep) {
+      throw sg_io_exception("malformed route file, no departure node");
+    }
+    
+    string depIdent = dep->getStringValue("airport");
+    const FGAirport* depApt = fgFindAirportID(depIdent);
+    if (!depApt) {
+      throw sg_io_exception("bad route file, unknown airport:" + depIdent);
+    }
+    
+    departure->setStringValue("runway", dep->getStringValue("runway"));
+    
+  // destination
+    SGPropertyNode* dst = routeData->getChild("destination");
+    if (!dst) {
+      throw sg_io_exception("malformed route file, no destination node");
+    }
+    
+    destination->setStringValue("airport", dst->getStringValue("airport"));
+    destination->setStringValue("runay", dst->getStringValue("runway"));
+
+  // alternate
+    SGPropertyNode* alt = routeData->getChild("alternate");
+    if (alt) {
+      alternate->setStringValue(alt->getStringValue("airport"));
+    } // of cruise data loading
+    
+  // cruise
+    SGPropertyNode* crs = routeData->getChild("cruise");
+    if (crs) {
+      cruise->setDoubleValue(crs->getDoubleValue("speed"));
+    } // of cruise data loading
+
+  // route nodes
+    _route->clear();
+    SGPropertyNode_ptr _route = routeData->getChild("route", 0);
+    SGGeod lastPos(depApt->geod());
+    
+    for (int i=0; i<_route->nChildren(); ++i) {
+      SGPropertyNode_ptr wp = _route->getChild("wp", i);
+      parseRouteWaypoint(wp);
+    } // of route iteration
+  } catch (sg_exception& e) {
+    SG_LOG(SG_IO, SG_WARN, "failed to load flight-plan (from '" << e.getOrigin()
+      << "'):" << e.getMessage());
+  }
+}
+
+void FGRouteMgr::parseRouteWaypoint(SGPropertyNode* aWP)
+{
+  SGGeod lastPos;
+  if (_route->size() > 0) {
+    lastPos = get_waypoint(_route->size()-1).get_target();
+  } else {
+    // route is empty, use departure airport position
+    const FGAirport* apt = fgFindAirportID(departure->getStringValue("airport"));
+    assert(apt); // shouldn't have got this far with an invalid airport
+    lastPos = apt->geod();
+  }
+
+  SGPropertyNode_ptr altProp = aWP->getChild("altitude-ft");
+  double alt = -9999.0;
+  if (altProp) {
+    alt = altProp->getDoubleValue();
+  }
+      
+  string ident(aWP->getStringValue("ident"));
+  if (aWP->hasChild("longitude-deg")) {
+    // explicit longitude/latitude
+    if (alt < -9990.0) {
+      alt = 0.0; // don't export wyapoints with invalid altitude
+    }
+    
+    SGWayPoint swp(aWP->getDoubleValue("longitude-deg"),
+      aWP->getDoubleValue("latitude-deg"), alt, 
+      SGWayPoint::WGS84, ident, aWP->getStringValue("name"));
+    add_waypoint(swp);
+  } else if (aWP->hasChild("navid")) {
+    // lookup by navid (possibly with offset)
+    string nid(aWP->getStringValue("navid"));
+    FGPositionedRef p = FGPositioned::findClosestWithIdent(nid, lastPos);
+    if (!p) {
+      throw sg_io_exception("bad route file, unknown navid:" + nid);
+    }
+    
+    SGGeod pos(p->geod());
+    if (aWP->hasChild("offset-nm") && aWP->hasChild("offset-radial")) {
+      double radialDeg = aWP->getDoubleValue("offset-radial");
+      // convert magnetic radial to a true radial!
+      radialDeg += magvar->getDoubleValue();
+      double offsetNm = aWP->getDoubleValue("offset-nm");
+      double az2;
+      SGGeodesy::direct(p->geod(), radialDeg, offsetNm * SG_NM_TO_METER, pos, az2);
+    }
+    
+    if (alt < -9990.0) {
+      alt = p->elevation();
+    }
+    
+    SGWayPoint swp(pos.getLongitudeDeg(), pos.getLatitudeDeg(), alt, 
+      SGWayPoint::WGS84, ident, "");
+    add_waypoint(swp);
+  } else {
+    // lookup by ident (symbolic waypoint)
+    FGPositionedRef p = FGPositioned::findClosestWithIdent(ident, lastPos);
+    if (!p) {
+      throw sg_io_exception("bad route file, unknown waypoint:" + ident);
+    }
+    
+    if (alt < -9990.0) {
+      alt = p->elevation();
+    }
+    
+    SGWayPoint swp(p->longitude(), p->latitude(), alt, 
+      SGWayPoint::WGS84, p->ident(), p->name());
+    add_waypoint(swp);
+  }
+}

src/Autopilot/route_mgr.hxx

@@ -25,9 +25,12 @@
 #define _ROUTE_MGR_HXX 1
 
 #include <simgear/props/props.hxx>
-#include <simgear/route/route.hxx>
+#include <simgear/route/waypoint.hxx>
 #include <simgear/structure/subsystem_mgr.hxx>
 
+// forward decls
+class SGRoute;
+class SGPath;
 
 /**
  * Top level route manager class
@@ -38,61 +41,84 @@ class FGRouteMgr : public SGSubsystem
 {
 
 private:
-
+    SGRoute* _route;
-    SGRoute *route;
+    time_t _takeoffTime;
-
+    time_t _touchdownTime;
+    bool _autoSequence; // true if we are doing internal sequencing
+      // false if other code (FMS/GPS) is managing sequencing
+    
     // automatic inputs
     SGPropertyNode_ptr lon;
     SGPropertyNode_ptr lat;
     SGPropertyNode_ptr alt;
-
+    SGPropertyNode_ptr magvar;
+    
     // automatic outputs
-    SGPropertyNode_ptr true_hdg_deg;
     SGPropertyNode_ptr target_altitude_ft;
     SGPropertyNode_ptr altitude_lock;
 
-    SGPropertyNode_ptr wp0_id;
+    SGPropertyNode_ptr departure; ///< departure airport information
-    SGPropertyNode_ptr wp0_dist;
+    SGPropertyNode_ptr destination; ///< destination airport information
-    SGPropertyNode_ptr wp0_eta;
+    SGPropertyNode_ptr alternate; ///< alternate airport information
-
+    SGPropertyNode_ptr cruise; ///< cruise information
-    SGPropertyNode_ptr wp1_id;
+    
-    SGPropertyNode_ptr wp1_dist;
+    SGPropertyNode_ptr totalDistance;
-    SGPropertyNode_ptr wp1_eta;
+    SGPropertyNode_ptr ete;
-
+    SGPropertyNode_ptr elapsedFlightTime;
-    SGPropertyNode_ptr wpn_id;
+    
-    SGPropertyNode_ptr wpn_dist;
+    SGPropertyNode_ptr active;
-    SGPropertyNode_ptr wpn_eta;
+    SGPropertyNode_ptr airborne;
-
+    SGPropertyNode_ptr currentWp;
-
+    
-    class Listener : public SGPropertyChangeListener {
+    SGPropertyNode_ptr wp0;
+    SGPropertyNode_ptr wp1;
+    SGPropertyNode_ptr wpn;
+    
+    
+    SGPropertyNode_ptr _pathNode;
+    
+    void setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDistance);
+    
+    class InputListener : public SGPropertyChangeListener {
     public:
-        Listener(FGRouteMgr *m) : mgr(m) {}
+        InputListener(FGRouteMgr *m) : mgr(m) {}
         virtual void valueChanged (SGPropertyNode * prop);
     private:
         FGRouteMgr *mgr;
     };
 
     SGPropertyNode_ptr input;
-    Listener *listener;
+    SGPropertyNode_ptr weightOnWheels;
+    
+    InputListener *listener;
     SGPropertyNode_ptr mirror;
     bool altitude_set;
 
+    /**
+     * Create a SGWayPoint from a string in the following format:
+     *  - simple identifier
+     *  - decimal-lon,decimal-lat
+     *  - airport-id/runway-id
+     *  - navaid/radial-deg/offset-nm
+     */
     SGWayPoint* make_waypoint(const string& target);
+    
+    
     void update_mirror();
     bool near_ground();
-
+    
-    /**
+    void currentWaypointChanged();
-     * Helper to set a string property to the estimated arrival time (ETA),
-     * formatted as either hours:minutes or minutes:seconds, based on a distance
-     * and the current groundspeed.
-     */
-    void setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDistance);
     
     /**
      * Helper to update the target_altitude_ft and altitude_set flag when wp0
      * changes
      */
     void updateTargetAltitude();
+    
+    /**
+     * Parse a route/wp node (from a saved, property-lsit formatted route)
+     */
+    void parseRouteWaypoint(SGPropertyNode* aWP);
 public:
 
     FGRouteMgr();
@@ -110,14 +136,41 @@ public:
     void add_waypoint( const SGWayPoint& wp, int n = -1 );
     SGWayPoint pop_waypoint( int i = 0 );
 
-    SGWayPoint get_waypoint( int i ) const {
+    SGWayPoint get_waypoint( int i ) const;
-        return route->get_waypoint(i);
+    int size() const;
-    }
+        
-
+    bool isRouteActive() const;
-    int size() const {
+        
-        return route->size();
+    int currentWaypoint() const;
-    }
+       
+    /**
+     * Find a waypoint in the route, by position, and return its index, or
+     * -1 if no matching waypoint was found in the route.
+     */
+    int findWaypoint(const SGGeod& aPos) const;
+          
+    /**
+     * Activate a built route. This checks for various mandatory pieces of
+     * data, such as departure and destination airports, and creates waypoints
+     * for them on the route structure.
+     *
+     * returns true if the route was activated successfully, or false if the
+     * route could not be activated for some reason
+     */
+    bool activate();
 
+    /**
+     * Step to the next waypoint on the active route
+     */
+    void sequence();
+    
+    /**
+     *
+     */
+    void jumpToIndex(int index);
+    
+    void saveRoute();
+    void loadRoute();
 };
 
 

src/Instrumentation/gps.hxx

@@ -11,8 +11,13 @@
 #include <simgear/structure/subsystem_mgr.hxx>
 #include <simgear/math/SGMath.hxx>
 
+#include "Navaids/positioned.hxx"
+
 // forward decls
 class SGRoute;
+class FGRouteMgr;
+class FGAirport;
+class GPSListener;
 
 class SGGeodProperty
 {
@@ -42,15 +47,6 @@ private:
  * /systems/electrical/outputs/gps
  * /instrumentation/gps/serviceable
  * 
- * /instrumentation/gps/wp-longitude-deg
- * /instrumentation/gps/wp-latitude-deg
- * /instrumentation/gps/wp-altitude-ft
- * /instrumentation/gps/wp-ID
- * /instrumentation/gps/wp-name
- * /instrumentation/gps/desired-course-deg
- * /instrumentation/gps/get-nearest-airport
- * /instrumentation/gps/waypoint-type
- * /instrumentation/gps/tracking-bug
  *
  * Output properties:
  *
@@ -73,8 +69,7 @@ private:
  * /instrumentation/gps/trip-odometer
  * /instrumentation/gps/true-bug-error-deg
  * /instrumentation/gps/magnetic-bug-error-deg
- * /instrumentation/gps/true-bearing-error-deg
+
- * /instrumentation/gps/magnetic-bearing-error-deg
  */
 class GPS : public SGSubsystem
 {
@@ -89,117 +84,294 @@ public:
     virtual void update (double delta_time_sec);
 
 private:
-    typedef struct {
+    friend class GPSListener;
-        double dt;
+    friend class SearchFilter;
-        SGGeod pos;
+    
-        SGGeod wp0_pos;
+    /**
-        SGGeod wp1_pos;
+     * Configuration manager, track data relating to aircraft installation
-        bool waypoint_changed;
+     */
-        double speed_kt;
+    class Config
-        double track1_deg;
+    {
-        double track2_deg;
+    public:
-        double magvar_deg;
+      Config();
-        double wp0_distance;
+      
-        double wp0_course_deg;
+      void init(SGPropertyNode*);
-        double wp0_bearing_deg;
+      
-        double wp1_distance;
+      bool turnAnticipationEnabled() const
-        double wp1_course_deg;
+      { return _enableTurnAnticipation; }
-        double wp1_bearing_deg;
+      
-    } UpdateContext;
+      /**
+       * Desired turn rate in degrees/second. From this we derive the turn
+       * radius and hence how early we need to anticipate it.
+       */
+      double turnRateDegSec() const
+      { return _turnRate; }
+      
+      /**
+       * Distance at which we arm overflight sequencing. Once inside this
+       * distance, a change of the wp1 'TO' flag to false will be considered
+       * overlight of the wp.
+       */
+      double overflightArmDistanceNm() const
+      { return _overflightArmDistance; }
+      
+      /**
+       * Time before the next WP to activate an external annunciator
+       */
+      double waypointAlertTime() const
+      { return _waypointAlertTime; }
+            
+      bool tuneNavRadioToRefVor() const
+      { return _tuneRadio1ToRefVor; }
+      
+      bool requireHardSurface() const
+      { return _requireHardSurface; }
+      
+      double minRunwayLengthFt() const
+      { return _minRunwayLengthFt; }
+      
+      double getOBSCourse() const;
+      
+      bool cdiDeflectionIsAngular() const
+      { return (_cdiMaxDeflectionNm <= 0.0); }
+      
+      double cdiDeflectionLinearPeg() const
+      {
+        assert(_cdiMaxDeflectionNm > 0.0);
+        return _cdiMaxDeflectionNm;
+      }
+    private:
+      bool _enableTurnAnticipation;
+      
+      // desired turn rate in degrees per second
+      double _turnRate;
+      
+      // distance from waypoint to arm overflight sequencing (in nm)
+      double _overflightArmDistance;
+      
+      // time before reaching a waypoint to trigger annunicator light/sound 
+      // (in seconds)
+      double _waypointAlertTime;
+      
+      // should GPS automatically tune NAV1 to the reference VOR?
+      bool _tuneRadio1ToRefVor;
+      
+      // minimum runway length to require when filtering
+      double _minRunwayLengthFt;
+      
+      // should we require a hard-surfaced runway when filtering?
+      bool _requireHardSurface;
+      
+      // helpers to tie obs-course-source property
+      const char* getOBSCourseSource() const;
+      void setOBSCourseSource(const char* aPropPath);
+      
+      // property to retrieve the OBS course from
+      SGPropertyNode_ptr _obsCourseSource;
+      
+      double _cdiMaxDeflectionNm;
+    };
+    
+    class SearchFilter : public FGPositioned::Filter
+    {
+    public:      
+      virtual bool pass(FGPositioned* aPos) const;
+          
+      virtual FGPositioned::Type minType() const;
+      virtual FGPositioned::Type maxType() const;
+    };
     
-    void search (double frequency, const SGGeod& pos);
-
     /**
      * reset all output properties to default / non-service values
      */
     void clearOutput();
 
-    void updateWithValid(UpdateContext& ctx);
+    void updateWithValid(double dt);
+    void updateBasicData(double dt);
+    void updateWaypoints();
+
+    void updateTrackingBug();
+    void updateReferenceNavaid(double dt);
+    void referenceNavaidSet(const std::string& aNavaid);
+    void tuneNavRadios();
+    void updateRouteData();
     
-    void updateNearestAirport(UpdateContext& ctx);
+    void routeActivated();
-    void updateWaypoint0(UpdateContext& ctx);
+    void routeManagerSequenced();
-    void updateWaypoint1(UpdateContext& ctx);
+    
-
+    void updateTurn();  
-    void updateLegCourse(UpdateContext& ctx);
+    void updateOverflight();    
-    void updateWaypoint0Course(UpdateContext& ctx);
+    void beginTurn();
-    void updateWaypoint1Course(UpdateContext& ctx);
+    void endTurn();
-
+    
-    void waypointChanged(UpdateContext& ctx);
+    double computeTurnProgress(double aBearing) const;
-    void updateTTWNode(UpdateContext& ctx, double distance_m, SGPropertyNode_ptr node);
+    void computeTurnData();
-    void updateTrackingBug(UpdateContext& ctx);
+    void updateTurnData();
+    double computeTurnRadiusNm(double aGroundSpeedKts) const;
     
-    SGPropertyNode_ptr _magvar_node;
-    SGPropertyNode_ptr _serviceable_node;
-    SGPropertyNode_ptr _electrical_node;
-    SGPropertyNode_ptr _wp0_ID_node;
-    SGPropertyNode_ptr _wp0_name_node;
-    SGPropertyNode_ptr _wp0_course_node;
-    SGPropertyNode_ptr _get_nearest_airport_node;
-    SGPropertyNode_ptr _wp1_ID_node;
-    SGPropertyNode_ptr _wp1_name_node;
-    SGPropertyNode_ptr _wp1_course_node;
-    SGPropertyNode_ptr _tracking_bug_node;
 
-    SGPropertyNode_ptr _raim_node;
+// scratch maintenence utilities
-    SGPropertyNode_ptr _indicated_vertical_speed_node;
+  void setScratchFromPositioned(FGPositioned* aPos, int aIndex);
-    SGPropertyNode_ptr _true_track_node;
+  void setScratchFromCachedSearchResult();
-    SGPropertyNode_ptr _magnetic_track_node;
+  void setScratchFromRouteWaypoint(int aIndex);
-    SGPropertyNode_ptr _speed_node;
+  
-    SGPropertyNode_ptr _wp0_distance_node;
+  /**
-    SGPropertyNode_ptr _wp0_ttw_node;
+   * Add airport-specific information to a scratch result
-    SGPropertyNode_ptr _wp0_bearing_node;
+   */
-    SGPropertyNode_ptr _wp0_mag_bearing_node;
+  void addAirportToScratch(FGAirport* aAirport);
-    SGPropertyNode_ptr _wp0_course_deviation_node;
+  
-    SGPropertyNode_ptr _wp0_course_error_nm_node;
+  void clearScratch();
-    SGPropertyNode_ptr _wp0_to_flag_node;
+  
-    SGPropertyNode_ptr _wp1_distance_node;
+  /**
-    SGPropertyNode_ptr _wp1_ttw_node;
+   * Predicate, determine if the lon/lat position in the scratch is 
-    SGPropertyNode_ptr _wp1_bearing_node;
+   * valid or not.
-    SGPropertyNode_ptr _wp1_mag_bearing_node;
+   */
-    SGPropertyNode_ptr _wp1_course_deviation_node;
+  bool isScratchPositionValid() const;
-    SGPropertyNode_ptr _wp1_course_error_nm_node;
+  
-    SGPropertyNode_ptr _wp1_to_flag_node;
+  FGPositioned::Filter* createFilter(FGPositioned::Type aTy);
-    SGPropertyNode_ptr _odometer_node;
+  
+  /**
+   * Search kernel - called each time we step through a result
+   */
+  void performSearch();
+  
+// command handlers
+  void selectLegMode();
+  void selectOBSMode();
+  void directTo();
+  void loadRouteWaypoint();
+  void loadNearest();
+  void search();
+  void nextResult();
+  void previousResult();
+  void defineWaypoint();
+  
+// tied-property getter/setters
+  void setCommand(const char* aCmd);
+  const char* getCommand() const { return ""; }
+  
+  const char* getMode() const { return _mode.c_str(); }
+  
+  bool getScratchValid() const { return _scratchValid; }
+  double getScratchDistance() const;
+  double getScratchMagBearing() const;
+  double getScratchTrueBearing() const;
+  bool getScratchHasNext() const { return _searchHasNext; }
+  
+  double getSelectedCourse() const { return _selectedCourse; }
+  double getCDIDeflection() const;
+  
+  double getLegDistance() const;
+  double getLegCourse() const;
+  double getLegMagCourse() const;
+  double getAltDistanceRatio() const;
+  
+  double getTrueTrack() const { return _last_true_track; }
+  double getMagTrack() const;
+  double getGroundspeedKts() const { return _last_speed_kts; }
+  double getVerticalSpeed() const { return _last_vertical_speed; }
+  
+  //bool getLegMode() const { return _mode == "leg"; }
+  //bool getObsMode() const { return _mode == "obs"; }
+  
+  const char* getWP0Ident() const;
+  const char* getWP0Name() const;
+  
+  const char* getWP1Ident() const;
+  const char* getWP1Name() const;
+  
+  double getWP1Distance() const;
+  double getWP1TTW() const;
+  const char* getWP1TTWString() const;
+  double getWP1Bearing() const;
+  double getWP1MagBearing() const;
+  double getWP1CourseDeviation() const;
+  double getWP1CourseErrorNm() const;
+  bool getWP1ToFlag() const;
+  // true-bearing-error and mag-bearing-error
+  
+  
+  
+// members
+  SGPropertyNode_ptr _magvar_node;
+  SGPropertyNode_ptr _serviceable_node;
+  SGPropertyNode_ptr _electrical_node;
+  SGPropertyNode_ptr _tracking_bug_node;
+  SGPropertyNode_ptr _raim_node;
+
+      SGPropertyNode_ptr _odometer_node;
     SGPropertyNode_ptr _trip_odometer_node;
     SGPropertyNode_ptr _true_bug_error_node;
     SGPropertyNode_ptr _magnetic_bug_error_node;
-    SGPropertyNode_ptr _true_wp0_bearing_error_node;
+    
-    SGPropertyNode_ptr _magnetic_wp0_bearing_error_node;
+    SGPropertyNode_ptr _ref_navaid_id_node;
-    SGPropertyNode_ptr _true_wp1_bearing_error_node;
+    SGPropertyNode_ptr _ref_navaid_bearing_node;
-    SGPropertyNode_ptr _magnetic_wp1_bearing_error_node;
+    SGPropertyNode_ptr _ref_navaid_distance_node;
-    SGPropertyNode_ptr _leg_distance_node;
+    SGPropertyNode_ptr _ref_navaid_mag_bearing_node;
-    SGPropertyNode_ptr _leg_course_node;
+    SGPropertyNode_ptr _ref_navaid_frequency_node;
-    SGPropertyNode_ptr _leg_magnetic_course_node;
+    SGPropertyNode_ptr _ref_navaid_name_node;
-    SGPropertyNode_ptr _alt_dist_ratio_node;
+    
-    SGPropertyNode_ptr _leg_course_deviation_node;
+    SGPropertyNode_ptr _route_active_node;
-    SGPropertyNode_ptr _leg_course_error_nm_node;
+    SGPropertyNode_ptr _route_current_wp_node;
-    SGPropertyNode_ptr _leg_to_flag_node;
+    SGPropertyNode_ptr _routeDistanceNm;
-    SGPropertyNode_ptr _alt_deviation_node;
+    SGPropertyNode_ptr _routeETE;
-
+    
+    SGPropertyNode_ptr _fromFlagNode;
+    
+    double _selectedCourse;
+    
     bool _last_valid;
     SGGeod _last_pos;
     double _last_speed_kts;
-
+    double _last_true_track;
-    std::string _last_wp0_ID;
+    double _last_vertical_speed;
-    std::string _last_wp1_ID;
+    
-
+    std::string _mode;
-    double _alt_dist_ratio;
+    GPSListener* _listener;
-    double _distance_m;
+    Config _config;
-    double _course_deg;
+    FGRouteMgr* _routeMgr;
-
+    
-    double _bias_length;
+    bool _ref_navaid_set;
-    double _bias_angle;
+    double _ref_navaid_elapsed;
-    double _azimuth_error;
+    FGPositionedRef _ref_navaid;
-    double _range_error;
+    
-    double _elapsed_time;
-
     std::string _name;
     int _num;
-
+  
-    SGGeodProperty _position;
+  SGGeodProperty _position;
-    SGGeodProperty _wp0_position;
+  SGGeod _wp0_position;
-    SGGeodProperty _wp1_position;
+  SGGeod _wp1_position;
-    SGGeodProperty _indicated_pos;
+  SGGeod _indicated_pos;
+  std::string _wp0Ident, _wp0Name, _wp1Ident, _wp1Name;
+  double _wp1DistanceM, _wp1TrueBearing;
+  
+// scratch data
+  SGGeod _scratchPos;
+  SGPropertyNode_ptr _scratchNode;
+  bool _scratchValid;
+  
+// search data
+  int _searchResultIndex;
+  std::string _searchQuery;
+  FGPositioned::Type _searchType;
+  bool _searchExact;
+  bool _searchOrderByRange;
+  bool _searchResultsCached;
+  FGPositioned::List _searchResults;
+  bool _searchIsRoute; ///< set if 'search' is actually the current route
+  bool _searchHasNext; ///< is there a result after this one?
+  bool _searchNames; ///< set if we're searching names instead of idents
+  
+  // turn data
+    bool _computeTurnData; ///< do we need to update the turn data?
+    bool _anticipateTurn; ///< are we anticipating the next turn or not?
+    bool _inTurn; // is a turn in progress?
+    bool _turnSequenced; // have we sequenced the new leg?
+    double _turnAngle; // angle to turn through, in degrees
+    double _turnStartBearing; // bearing of inbound leg
+    double _turnRadius; // radius of turn in nm
+    SGGeod _turnPt;
+    SGGeod _turnCentre;
 };
 
 

src/Instrumentation/instrument_mgr.cxx

@@ -128,7 +128,7 @@ bool FGInstrumentMgr::build ()
             set_subsystem( id, new Altimeter( node ) );
 
         } else if ( name == "gps" ) {
-            set_subsystem( id, new GPS( node ), 0.45 );
+            set_subsystem( id, new GPS( node ) );
 
         } else if ( name == "gsdi" ) {
             set_subsystem( id, new GSDI( node ) );

src/Instrumentation/navradio.cxx

@@ -244,6 +244,7 @@ FGNavRadio::init ()
     gps_to_flag_node = fgGetNode("/instrumentation/gps/to-flag", true);
     gps_from_flag_node = fgGetNode("/instrumentation/gps/from-flag", true);
     gps_has_gs_node = fgGetNode("/instrumentation/gps/has-gs", true);
+    gps_course_node = fgGetNode("/instrumentation/gps/selected-course-deg", true);
     
     std::ostringstream temp;
     temp << _name << "nav-ident" << _num;
@@ -605,6 +606,8 @@ void FGNavRadio::updateGPSSlaved()
   
   _cdiCrossTrackErrorM = 0.0; // FIXME, supply this
   _gsNeedleDeflection = 0.0; // FIXME, supply this
+  
+  //sel_radial_node->setDoubleValue(gps_course_node->getDoubleValue());
 }
 
 void FGNavRadio::updateCDI(double dt)

src/Instrumentation/navradio.hxx

@@ -113,7 +113,8 @@ class FGNavRadio : public SGSubsystem
     SGPropertyNode_ptr gps_to_flag_node;
     SGPropertyNode_ptr gps_from_flag_node;
     SGPropertyNode_ptr gps_has_gs_node;
-
+    SGPropertyNode_ptr gps_course_node;
+    
     // internal (private) values
 
     int play_count;

src/Instrumentation/testgps.cxx

@@ -0,0 +1,212 @@
+
+#include <Main/fg_init.hxx>
+#include <Main/globals.hxx>
+#include <Main/fg_props.hxx>
+
+#include <Instrumentation/gps.hxx>
+#include <Autopilot/route_mgr.hxx>
+#include <Environment/environment_mgr.hxx>
+
+using std::string;
+
+char *homedir = ::getenv( "HOME" );
+char *hostname = ::getenv( "HOSTNAME" );
+bool free_hostname = false;
+
+void testSetPosition(const SGGeod& aPos)
+{
+  fgSetDouble("/position/longitude-deg", aPos.getLongitudeDeg());
+  fgSetDouble("/position/latitude-deg", aPos.getLatitudeDeg());
+  fgSetDouble("/position/altitude-ft", aPos.getElevationFt());
+}
+
+void printScratch(SGPropertyNode* scratch)
+{
+  if (!scratch->getBoolValue("valid", false)) {
+    SG_LOG(SG_GENERAL, SG_ALERT, "Scratch is invalid."); 
+    return;
+  }
+
+  SG_LOG(SG_GENERAL, SG_ALERT, "Scratch:" <<
+    scratch->getStringValue("ident") << "/" << scratch->getStringValue("name"));
+  
+  SG_LOG(SG_GENERAL, SG_ALERT, "\t" << scratch->getDoubleValue("longitude-deg")
+    << " " << scratch->getDoubleValue("latitude-deg") << " @ " << scratch->getDoubleValue("altitude-ft"));
+    
+  SG_LOG(SG_GENERAL, SG_ALERT, "\t" << scratch->getDoubleValue("true-bearing-deg") <<
+    " (" << scratch->getDoubleValue("mag-bearing-deg") << " magnetic) " << scratch->getDoubleValue("distance-nm"));
+  
+  if (scratch->hasChild("result-index")) {
+    SG_LOG(SG_GENERAL, SG_ALERT, "\tresult-index:" << scratch->getIntValue("result-index"));
+  } 
+  
+  if (scratch->hasChild("route-index")) {
+    SG_LOG(SG_GENERAL, SG_ALERT, "\troute-index:" << scratch->getIntValue("route-index"));
+  }
+}
+
+void createDummyRoute(FGRouteMgr* rm)
+{
+  SGPropertyNode* rmInput = fgGetNode("/autopilot/route-manager/input", true);
+  rmInput->setStringValue("UW");
+  rmInput->setStringValue("TLA/347/13");
+  rmInput->setStringValue("TLA");
+  rmInput->setStringValue("HAVEN");
+  rmInput->setStringValue("NEW/305/29");
+  rmInput->setStringValue("NEW");
+  rmInput->setStringValue("OTR");
+}
+
+int main(int argc, char* argv[])
+{
+  globals = new FGGlobals;
+    
+  fgInitFGRoot(argc, argv);
+  if (!fgInitConfig(argc, argv) ) {
+    SG_LOG( SG_GENERAL, SG_ALERT, "Config option parsing failed" );
+    exit(-1);
+  }
+  
+  
+  fgInitNav();
+  
+  SG_LOG(SG_GENERAL, SG_ALERT, "hello world!");
+  
+  FGRouteMgr* rm = new FGRouteMgr;
+  globals->add_subsystem( "route-manager", rm );
+  
+ // FGEnvironmentMgr* envMgr = new FGEnvironmentMgr;
+ // globals->add_subsystem("environment", envMgr);
+ // envMgr->init();
+  
+  
+  SGPropertyNode* nd = fgGetNode("/instrumentation/gps", true);
+  GPS* gps = new GPS(nd);
+  globals->add_subsystem("gps", gps);
+  
+  const FGAirport* egph = fgFindAirportID("EGPH");
+  testSetPosition(egph->geod());
+  
+  // startup the route manager
+  rm->init();
+  
+  nd->setBoolValue("serviceable", true);
+  fgSetBool("/systems/electrical/outputs/gps", true);
+  
+  gps->init();
+  SGPropertyNode* scratch  = nd->getChild("scratch", 0, true);
+  SGPropertyNode* wp = nd->getChild("wp", 0, true);
+  SGPropertyNode* wp1 = wp->getChild("wp", 1, true);
+  
+  // update a few times
+  gps->update(0.05);
+  gps->update(0.05);
+  
+  scratch->setStringValue("ident", "TL");
+  scratch->setStringValue("type", "Vor");
+  scratch->setBoolValue("exact", false);
+  nd->setStringValue("command", "search");
+  printScratch(scratch);
+  
+  nd->setStringValue("command", "next");
+  printScratch(scratch);
+  
+  nd->setStringValue("command", "next");
+  printScratch(scratch);
+  
+// alphanumeric sort, partial matching
+  nd->setDoubleValue("config/min-runway-length-ft", 5000.0);
+  scratch->setBoolValue("exact", false);
+  scratch->setBoolValue("order-by-distance", false);
+  scratch->setStringValue("ident", "KS");
+  scratch->setStringValue("type", "apt");
+  
+  nd->setStringValue("command", "search");
+  printScratch(scratch);
+  
+  nd->setStringValue("command", "next");
+  printScratch(scratch);
+  
+  nd->setStringValue("command", "next");
+  printScratch(scratch);
+  
+// alphanumeric sort, explicit matching
+  scratch->setBoolValue("exact", true);
+  scratch->setBoolValue("order-by-distance", true);
+  scratch->setStringValue("type", "vor");
+  scratch->setStringValue("ident", "DCS");
+  
+  nd->setStringValue("command", "search");
+  printScratch(scratch);
+  
+  nd->setStringValue("command", "next");
+  printScratch(scratch);
+  
+// search on totally missing
+  scratch->setBoolValue("exact", true);
+  scratch->setBoolValue("order-by-distance", true);
+  scratch->setStringValue("ident", "FOFOFOFOF");
+  nd->setStringValue("command", "search");
+  printScratch(scratch);
+  
+// nearest
+  scratch->setStringValue("type", "apt");
+  scratch->setIntValue("max-results", 10);
+  nd->setStringValue("command", "nearest");
+  printScratch(scratch);
+  
+  nd->setStringValue("command", "next");
+  printScratch(scratch);
+  
+  nd->setStringValue("command", "next");
+  printScratch(scratch);
+  
+// direct to
+  nd->setStringValue("command", "direct");
+  SG_LOG(SG_GENERAL, SG_ALERT, "mode:" << nd->getStringValue("mode") << "\n\t"
+    << wp1->getStringValue("ID") << " " << wp1->getDoubleValue("longitude-deg") 
+    << " " << wp1->getDoubleValue("latitude-deg"));
+
+// OBS mode
+  scratch->setStringValue("ident", "UW");
+  scratch->setBoolValue("order-by-distance", true);
+  nd->setStringValue("command", "search");
+  printScratch(scratch);
+  
+  nd->setStringValue("command", "obs");
+  SG_LOG(SG_GENERAL, SG_ALERT, "mode:" << nd->getStringValue("mode") << "\n\t"
+    << wp1->getStringValue("ID") << " " << wp1->getDoubleValue("longitude-deg") 
+    << " " << wp1->getDoubleValue("latitude-deg"));
+  
+// load route waypoints
+  createDummyRoute(rm);
+
+  scratch->setIntValue("route-index", 5);
+  nd->setStringValue("command", "load-route-wpt");
+  printScratch(scratch);
+  
+  nd->setStringValue("command", "next");
+  printScratch(scratch);
+  
+  nd->setStringValue("command", "next");
+  printScratch(scratch);
+  
+  scratch->setIntValue("route-index", 2);
+  nd->setStringValue("command", "load-route-wpt");
+  nd->setStringValue("command", "direct");
+  SG_LOG(SG_GENERAL, SG_ALERT, "mode:" << nd->getStringValue("mode") << "\n\t"
+    << wp1->getStringValue("ID") << " " << wp1->getDoubleValue("longitude-deg") 
+    << " " << wp1->getDoubleValue("latitude-deg"));
+  
+// route editing
+  SGGeod pos = egph->geod();
+  scratch->setStringValue("ident", "FOOBAR");
+  scratch->setDoubleValue("longitude-deg", pos.getLongitudeDeg());
+  scratch->setDoubleValue("latitude-deg", pos.getLatitudeDeg());
+  nd->setStringValue("command", "define-user-wpt");
+  printScratch(scratch);
+  
+  
+  
+  return EXIT_SUCCESS;
+}