X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAutopilot%2Froute_mgr.cxx;h=0ef72afb53d23bdeebbb332e0e85440bd2f95e13;hb=87141b47a74d84c3d65031076ba1af09d5c291e2;hp=480e6474d08a8438f0b3d4fa41fe20b6409e96ee;hpb=d05121ef4689d2b50b3fe1848cbb0d1f5a1db877;p=flightgear.git diff --git a/src/Autopilot/route_mgr.cxx b/src/Autopilot/route_mgr.cxx index 480e6474d..0ef72afb5 100644 --- a/src/Autopilot/route_mgr.cxx +++ b/src/Autopilot/route_mgr.cxx @@ -1,6 +1,8 @@ // route_mgr.cxx - manage a route (i.e. a collection of waypoints) // // Written by Curtis Olson, started January 2004. +// Norman Vine +// Melchior FRANZ // // Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt // @@ -16,7 +18,7 @@ // // You should have received a copy of the GNU General Public License // along with this program; if not, write to the Free Software -// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. // // $Id$ @@ -25,201 +27,737 @@ # include #endif -#include +#ifdef HAVE_WINDOWS_H +#include +#endif -#include -#include
+#include #include "route_mgr.hxx" +#include + +#include +#include + +#include +#include +#include +#include + +#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 ), - lon( NULL ), - lat( NULL ), - alt( NULL ), - true_hdg_deg( 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 ) + _route( new SGRoute ), + input(fgGetNode( RM "input", true )), + mirror(fgGetNode( RM "route", true )) { + listener = new InputListener(this); + input->setStringValue(""); + input->addChangeListener(listener); } FGRouteMgr::~FGRouteMgr() { - delete route; + input->removeChangeListener(listener); + + delete listener; + delete _route; } void FGRouteMgr::init() { - lon = fgGetNode( "/position/longitude-deg", true ); - lat = fgGetNode( "/position/latitude-deg", true ); - alt = fgGetNode( "/position/altitude-ft", true ); - - true_hdg_deg = fgGetNode( "/autopilot/settings/true-heading-deg", true ); - - wp0_id = fgGetNode( "/autopilot/route-manager/wp[0]/id", true ); - wp0_dist = fgGetNode( "/autopilot/route-manager/wp[0]/dist", true ); - wp0_eta = fgGetNode( "/autopilot/route-manager/wp[0]/eta", true ); + SGPropertyNode_ptr rm(fgGetNode(RM)); + + 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); + + departure = fgGetNode(RM "departure", true); +// init departure information from current location + SGGeod pos = SGGeod::fromDegFt(lon->getDoubleValue(), lat->getDoubleValue(), alt->getDoubleValue()); + FGAirport* apt = FGAirport::findClosest(pos, 20.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); + + destination = fgGetNode(RM "destination", true); + destination->getChild("airport", 0, true); + destination->getChild("runway", 0, true); + destination->getChild("eta", 0, true); + destination->getChild("touchdown-time", 0, true); + + alternate = fgGetNode(RM "alternate", true); + alternate->getChild("airport", 0, true); + alternate->getChild("runway", 0, true); + + cruise = fgGetNode(RM "cruise", true); + cruise->getChild("altitude-ft", 0, true); + cruise->setDoubleValue("altitude-ft", 10000.0); + cruise->getChild("flight-level", 0, true); + cruise->getChild("speed-kts", 0, true); + cruise->setDoubleValue("speed-kts", 160.0); + + 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); + + _edited = fgGetNode(RM "signals/edited", true); + _finished = fgGetNode(RM "signals/finished", true); + + _currentWpt = fgGetNode(RM "current-wp", true); + _currentWpt->tie(SGRawValueMethods + (*this, &FGRouteMgr::currentWaypoint, &FGRouteMgr::jumpToIndex)); + + // 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); +} - wp1_id = fgGetNode( "/autopilot/route-manager/wp[1]/id", true ); - wp1_dist = fgGetNode( "/autopilot/route-manager/wp[1]/dist", true ); - wp1_eta = fgGetNode( "/autopilot/route-manager/wp[1]/eta", true ); - wpn_id = fgGetNode( "/autopilot/route-manager/wp-last/id", true ); - wpn_dist = fgGetNode( "/autopilot/route-manager/wp-last/dist", true ); - wpn_eta = fgGetNode( "/autopilot/route-manager/wp-last/eta", true ); +void FGRouteMgr::postinit() { + string_list *waypoints = globals->get_initial_waypoints(); + if (waypoints) { + vector::iterator it; + for (it = waypoints->begin(); it != waypoints->end(); ++it) + new_waypoint(*it); + } - route->clear(); + weightOnWheels = fgGetNode("/gear/gear[0]/wow", false); + // check airbone flag agrees with presets + } void FGRouteMgr::bind() { } void FGRouteMgr::unbind() { } +bool FGRouteMgr::isRouteActive() const +{ + return active->getBoolValue(); +} -static double get_ground_speed() { - // starts in ft/s so we convert to kts - static const SGPropertyNode * speedup_node = fgGetNode("/sim/speed-up"); +void FGRouteMgr::update( double dt ) { + if (dt <= 0.0) { + // paused, nothing to do here + return; + } + + if (!active->getBoolValue()) { + return; + } + + double groundSpeed = get_ground_speed(); + if (airborne->getBoolValue()) { + time_t now = time(NULL); + elapsedFlightTime->setDoubleValue(difftime(now, _takeoffTime)); + } else { // not airborne + if (weightOnWheels->getBoolValue() || (groundSpeed < 40)) { + return; + } + + airborne->setBoolValue(true); + _takeoffTime = time(NULL); // start the clock + departure->setIntValue("takeoff-time", _takeoffTime); + } + + // basic course/distance information + 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 ); + + // update wp0 / wp1 / wp-last for legacy users + wp0->setDoubleValue("dist", wp_distance * SG_METER_TO_NM); + wp_course -= magvar->getDoubleValue(); // expose magnetic bearing + wp0->setDoubleValue("bearing-deg", wp_course); + setETAPropertyFromDistance(wp0->getChild("eta"), wp_distance); + + if ((_route->current_index() + 1) < _route->size()) { + wp = _route->get_waypoint(_route->current_index() + 1); + double wp1_course, wp1_distance; + wp.CourseAndDistance(lon->getDoubleValue(), lat->getDoubleValue(), + alt->getDoubleValue(), &wp1_course, &wp1_distance); + + wp1->setDoubleValue("dist", wp1_distance * SG_METER_TO_NM); + setETAPropertyFromDistance(wp1->getChild("eta"), wp1_distance); + } + + double totalDistanceRemaining = wp_distance; // distance to current waypoint + for (int i=_route->current_index() + 1; i<_route->size(); ++i) { + totalDistanceRemaining += _route->get_waypoint(i).get_distance(); + } + + 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); +} - 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::setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDistance) { + double speed = get_ground_speed(); + if (speed < 1.0) { + aProp->setStringValue("--:--"); + return; + } + + char eta_str[64]; + double eta = aDistance * SG_METER_TO_NM / get_ground_speed(); + if ( eta >= 100.0 ) { + eta = 99.999; // clamp + } + + if ( eta < (1.0/6.0) ) { + eta *= 60.0; // within 10 minutes, bump up to min/secs + } + + int major = (int)eta, + minor = (int)((eta - (int)eta) * 60.0); + snprintf( eta_str, 64, "%d:%02d", major, minor ); + aProp->setStringValue( eta_str ); } +void FGRouteMgr::add_waypoint( const SGWayPoint& wp, int n ) { + _route->add_waypoint( wp, n ); + + if (_route->current_index() > n) { + _route->set_current(_route->current_index() + 1); + } + + update_mirror(); + _edited->fireValueChanged(); +} -void FGRouteMgr::update( double dt ) { - double accum = 0.0; - double wp_course, wp_distance; - char eta_str[128]; - // first way point - if ( route->size() > 0 ) { - SGWayPoint wp = route->get_waypoint( 0 ); - wp.CourseAndDistance( lon->getDoubleValue(), lat->getDoubleValue(), - alt->getDoubleValue(), &wp_course, &wp_distance ); +SGWayPoint FGRouteMgr::pop_waypoint( int n ) { + if ( _route->size() <= 0 ) { + return SGWayPoint(); + } + + if ( n < 0 ) { + n = _route->size() - 1; + } + + if (_route->current_index() > n) { + _route->set_current(_route->current_index() - 1); + } + + SGWayPoint wp = _route->get_waypoint(n); + _route->delete_waypoint(n); + + update_mirror(); + _edited->fireValueChanged(); + checkFinished(); + + return wp; +} - true_hdg_deg->setDoubleValue( wp_course ); - if ( wp_distance < 200.0 ) { - pop_waypoint(); - } - } +bool FGRouteMgr::build() { + return true; +} - // next way point - if ( route->size() > 0 ) { - SGWayPoint wp = route->get_waypoint( 0 ); - // update the property tree info - wp0_id->setStringValue( wp.get_id().c_str() ); +void FGRouteMgr::new_waypoint( const string& target, int n ) { + SGWayPoint* wp = make_waypoint( target ); + if (!wp) { + return; + } + + add_waypoint( *wp, n ); + delete wp; +} - accum += wp_distance; - wp0_dist->setDoubleValue( accum * SG_METER_TO_NM ); - double eta = accum * SG_METER_TO_NM / get_ground_speed(); - if ( eta >= 100.0 ) { eta = 99.999; } - int major, minor; - if ( eta < (1.0/6.0) ) { - // within 10 minutes, bump up to min/secs - eta *= 60.0; - } - major = (int)eta; - minor = (int)((eta - (int)eta) * 60.0); - snprintf( eta_str, 128, "%d:%02d", major, minor ); - wp0_eta->setStringValue( eta_str ); +SGWayPoint* FGRouteMgr::make_waypoint(const string& tgt ) { + string target(boost::to_upper_copy(tgt)); + + // extract altitude + double alt = cruise->getDoubleValue("altitude-ft") * SG_FEET_TO_METER; + + size_t pos = target.find( '@' ); + if ( pos != string::npos ) { + alt = atof( target.c_str() + pos + 1 ); + target = target.substr( 0, pos ); + if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) + alt *= SG_FEET_TO_METER; } - // next way point - if ( route->size() > 1 ) { - SGWayPoint wp = route->get_waypoint( 1 ); + // check for lon,lat + pos = target.find( ',' ); + if ( pos != string::npos ) { + double lon = atof( target.substr(0, pos).c_str()); + double lat = atof( target.c_str() + pos + 1); + return new SGWayPoint( lon, lat, alt, SGWayPoint::WGS84, target ); + } + + SGGeod basePosition; + if (_route->size() > 0) { + SGWayPoint wp = get_waypoint(_route->size()-1); + 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 pieces(simgear::strutils::split(target, "/")); - // update the property tree info - wp1_id->setStringValue( wp.get_id().c_str() ); + FGPositionedRef p = FGPositioned::findClosestWithIdent(pieces.front(), basePosition); + if (!p) { + SG_LOG( SG_AUTOPILOT, SG_INFO, "Unable to find FGPositioned with ident:" << pieces.front()); + return NULL; + } + + SGGeod geod = SGGeod::fromGeodM(p->geod(), alt); + if (pieces.size() == 1) { + // simple case + return new SGWayPoint(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(geod, radial, distanceNm * SG_NM_TO_METER, offsetPos, az2); + offsetPos.setElevationM(alt); + + SG_LOG(SG_AUTOPILOT, SG_INFO, "final offset radial is " << radial); + return new SGWayPoint(offsetPos, p->ident() + pieces[2], target); + } + + if (pieces.size() == 2) { + FGAirport* apt = dynamic_cast(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; +} - accum += wp.get_distance(); - wp1_dist->setDoubleValue( accum * SG_METER_TO_NM ); - double eta = accum * SG_METER_TO_NM / get_ground_speed(); - if ( eta >= 100.0 ) { eta = 99.999; } - int major, minor; - if ( eta < (1.0/6.0) ) { - // within 10 minutes, bump up to min/secs - eta *= 60.0; - } - major = (int)eta; - minor = (int)((eta - (int)eta) * 60.0); - snprintf( eta_str, 128, "%d:%02d", major, minor ); - wp1_eta->setStringValue( eta_str ); +// 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++) { + 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", pos.getLongitudeDeg()); + prop->setDoubleValue("latitude-deg",pos.getLatitudeDeg()); + prop->setDoubleValue("altitude-m", pos.getElevationM()); + prop->setDoubleValue("altitude-ft", pos.getElevationFt()); } + // set number as listener attachment point + mirror->setIntValue("num", _route->size()); +} - // summarize remaining way points - if ( route->size() > 2 ) { - SGWayPoint wp; - for ( int i = 2; i < route->size(); ++i ) { - wp = route->get_waypoint( i ); - accum += wp.get_distance(); - } +// command interface /autopilot/route-manager/input: +// +// @CLEAR ... clear route +// @POP ... remove first entry +// @DELETE3 ... delete 4th entry +// @INSERT2:KSFO@900 ... insert "KSFO@900" as 3rd entry +// KSFO@900 ... append "KSFO@900" +// +void FGRouteMgr::InputListener::valueChanged(SGPropertyNode *prop) +{ + const char *s = prop->getStringValue(); + if (!strcmp(s, "@CLEAR")) + mgr->init(); + 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)); + else if (!strncmp(s, "@INSERT", 7)) { + char *r; + int pos = strtol(s + 7, &r, 10); + if (*r++ != ':') + return; + while (isspace(*r)) + r++; + if (*r) + mgr->new_waypoint(r, pos); + } else + mgr->new_waypoint(s); +} - // update the property tree info +// 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 = "" ); - wpn_id->setStringValue( wp.get_id().c_str() ); +bool FGRouteMgr::activate() +{ + const FGAirport* depApt = fgFindAirportID(departure->getStringValue("airport")); + if (!depApt) { + SG_LOG(SG_AUTOPILOT, SG_ALERT, + "unable to activate route: departure airport is invalid:" + << departure->getStringValue("airport") ); + return false; + } + + string runwayId(departure->getStringValue("runway")); + FGRunway* runway = NULL; + if (depApt->hasRunwayWithIdent(runwayId)) { + runway = depApt->getRunwayByIdent(runwayId); + } else { + SG_LOG(SG_AUTOPILOT, SG_INFO, + "route-manager, departure runway not found:" << runwayId); + runway = depApt->getActiveRunwayForUsage(); + } + + SGWayPoint swp(runway->threshold(), + depApt->ident() + "-" + runway->ident(), runway->name()); + add_waypoint(swp, 0); + + const FGAirport* destApt = fgFindAirportID(destination->getStringValue("airport")); + if (!destApt) { + SG_LOG(SG_AUTOPILOT, SG_ALERT, + "unable to activate route: destination airport is invalid:" + << destination->getStringValue("airport") ); + return false; + } + + runwayId = (destination->getStringValue("runway")); + if (destApt->hasRunwayWithIdent(runwayId)) { + FGRunway* runway = destApt->getRunwayByIdent(runwayId); + SGWayPoint swp(runway->end(), + destApt->ident() + "-" + runway->ident(), runway->name()); + add_waypoint(swp); + } else { + // quite likely, since destination runway may not be known until enroute + // probably want a listener on the 'destination' node to allow an enroute + // update + add_waypoint(SGWayPoint(destApt->geod(), destApt->ident(), destApt->name())); + } + + _route->set_current(1); + + double routeDistanceNm = _route->total_distance() * SG_METER_TO_NM; + totalDistance->setDoubleValue(routeDistanceNm); + double cruiseSpeedKts = cruise->getDoubleValue("speed", 0.0); + if (cruiseSpeedKts > 1.0) { + // very very crude approximation, doesn't allow for climb / descent + // performance or anything else at all + ete->setDoubleValue(routeDistanceNm / cruiseSpeedKts * (60.0 * 60.0)); + } + + active->setBoolValue(true); + SG_LOG(SG_AUTOPILOT, SG_INFO, "route-manager, activate route ok"); + return true; +} - wpn_dist->setDoubleValue( accum * SG_METER_TO_NM ); - double eta = accum * SG_METER_TO_NM / get_ground_speed(); - if ( eta >= 100.0 ) { eta = 99.999; } - int major, minor; - if ( eta < (1.0/6.0) ) { - // within 10 minutes, bump up to min/secs - eta *= 60.0; - } - major = (int)eta; - minor = (int)((eta - (int)eta) * 60.0); - snprintf( eta_str, 128, "%d:%02d", major, minor ); - wpn_eta->setStringValue( eta_str ); - } +void FGRouteMgr::sequence() +{ + if (!active->getBoolValue()) { + SG_LOG(SG_AUTOPILOT, SG_ALERT, "trying to sequence waypoints with no active route"); + return; + } + + if (checkFinished()) { + return; + } + + _route->increment_current(); + currentWaypointChanged(); + _currentWpt->fireValueChanged(); } +bool FGRouteMgr::checkFinished() +{ + int lastWayptIndex = _route->size() - 1; + if (_route->current_index() < lastWayptIndex) { + return false; + } + + SG_LOG(SG_AUTOPILOT, SG_INFO, "reached end of active route"); + _finished->fireValueChanged(); + active->setBoolValue(false); + return true; +} -SGWayPoint FGRouteMgr::pop_waypoint() { - SGWayPoint wp; +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(); +} - if ( route->size() > 0 ) { - wp = route->get_first(); - route->delete_first(); - } +void FGRouteMgr::currentWaypointChanged() +{ + SGWayPoint previous = _route->get_previous(); + SGWayPoint cur = _route->get_current(); + + wp0->getChild("id")->setStringValue(cur.get_id()); + if ((_route->current_index() + 1) < _route->size()) { + wp1->getChild("id")->setStringValue(_route->get_next().get_id()); + } else { + wp1->getChild("id")->setStringValue(""); + } + + SG_LOG(SG_AUTOPILOT, SG_INFO, "route manager, current-wp is now " << _route->current_index()); +} - if ( route->size() <= 2 ) { - wpn_id->setStringValue( "" ); - wpn_dist->setDoubleValue( 0.0 ); - wpn_eta->setStringValue( "" ); +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; +} - if ( route->size() <= 1 ) { - wp1_id->setStringValue( "" ); - wp1_dist->setDoubleValue( 0.0 ); - wp1_eta->setStringValue( "" ); - } +SGWayPoint FGRouteMgr::get_waypoint( int i ) const +{ + return _route->get_waypoint(i); +} - if ( route->size() <= 0 ) { - wp0_id->setStringValue( "" ); - wp0_dist->setDoubleValue( 0.0 ); - wp0_eta->setStringValue( "" ); - } +int FGRouteMgr::size() const +{ + return _route->size(); +} - return wp; +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); + } +} -bool FGRouteMgr::build() { - return true; +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 = cruise->getDoubleValue("altitude-ft") * SG_FEET_TO_METER; + if (altProp) { + alt = altProp->getDoubleValue(); + } + + string ident(aWP->getStringValue("ident")); + if (aWP->hasChild("longitude-deg")) { + // explicit longitude/latitude + 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); + } + + 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); + } + + SGWayPoint swp(p->longitude(), p->latitude(), alt, + SGWayPoint::WGS84, p->ident(), p->name()); + add_waypoint(swp); + } }