X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAutopilot%2Froute_mgr.cxx;h=0ef72afb53d23bdeebbb332e0e85440bd2f95e13;hb=87141b47a74d84c3d65031076ba1af09d5c291e2;hp=915f9de2f73e460b8292c0061de1d1f5df692e7a;hpb=cbddd130eced76579a39522d7ca6771199b2114d;p=flightgear.git diff --git a/src/Autopilot/route_mgr.cxx b/src/Autopilot/route_mgr.cxx index 915f9de2f..0ef72afb5 100644 --- a/src/Autopilot/route_mgr.cxx +++ b/src/Autopilot/route_mgr.cxx @@ -27,249 +27,292 @@ # include #endif -#include +#ifdef HAVE_WINDOWS_H +#include +#endif -#include -#include -#include
-#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 ), - 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 ), + _route( new SGRoute ), input(fgGetNode( RM "input", true )), - listener(new Listener(this)), - mirror(fgGetNode( RM "route", true )), - altitude_set( false ) + 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 ); - target_altitude_ft = fgGetNode( "/autopilot/settings/target-altitude-ft", true ); - altitude_lock = fgGetNode( "/autopilot/locks/altitude", true ); - - wp0_id = fgGetNode( RM "wp[0]/id", true ); - wp0_dist = fgGetNode( RM "wp[0]/dist", true ); - wp0_eta = fgGetNode( RM "wp[0]/eta", true ); - - wp1_id = fgGetNode( RM "wp[1]/id", true ); - wp1_dist = fgGetNode( RM "wp[1]/dist", true ); - wp1_eta = fgGetNode( RM "wp[1]/eta", true ); - - wpn_id = fgGetNode( RM "wp-last/id", true ); - wpn_dist = fgGetNode( RM "wp-last/dist", true ); - wpn_eta = fgGetNode( RM "wp-last/eta", true ); - - route->clear(); - update_mirror(); + 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); } void FGRouteMgr::postinit() { string_list *waypoints = globals->get_initial_waypoints(); - if (!waypoints) - return; - - vector::iterator it; - for (it = waypoints->begin(); it != waypoints->end(); ++it) + if (waypoints) { + vector::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; +bool FGRouteMgr::isRouteActive() const +{ + return active->getBoolValue(); } - 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 ); - - true_hdg_deg->setDoubleValue( wp_course ); - double target_alt = wp.get_target_alt(); - - if (!altitude_set && target_alt > -9990) { - target_altitude_ft->setDoubleValue( target_alt * SG_METER_TO_FEET ); - altitude_lock->setStringValue( "altitude-hold" ); - altitude_set = true; - } - - if ( wp_distance < 200.0 ) { - pop_waypoint(); - altitude_set = false; - } + if (dt <= 0.0) { + // paused, nothing to do here + return; } - - // 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() ); - - 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 ); + + if (!active->getBoolValue()) { + return; } - - // next way point - if ( route->size() > 1 ) { - SGWayPoint wp = route->get_waypoint( 1 ); - - // update the property tree info - - wp1_id->setStringValue( wp.get_id().c_str() ); - - 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 ); + + 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); } - - // 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(); - } - - // update the property tree info - - wpn_id->setStringValue( wp.get_id().c_str() ); - - 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 ); + + // 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); } -void FGRouteMgr::add_waypoint( const SGWayPoint& wp, int n ) { - if ( n == 0 || !route->size()) - altitude_set = false; - - route->add_waypoint( wp, n ); - update_mirror(); -} - - -SGWayPoint FGRouteMgr::pop_waypoint( int n ) { - SGWayPoint wp; - - if ( route->size() > 0 ) { - if ( n < 0 ) - n = route->size() - 1; - wp = route->get_waypoint(n); - route->delete_waypoint(n); +void FGRouteMgr::setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDistance) { + double speed = get_ground_speed(); + if (speed < 1.0) { + aProp->setStringValue("--:--"); + return; } - - if ( route->size() <= 2 ) { - wpn_id->setStringValue( "" ); - wpn_dist->setDoubleValue( 0.0 ); - wpn_eta->setStringValue( "" ); + + char eta_str[64]; + double eta = aDistance * SG_METER_TO_NM / get_ground_speed(); + if ( eta >= 100.0 ) { + eta = 99.999; // clamp } - - if ( route->size() <= 1 ) { - wp1_id->setStringValue( "" ); - wp1_dist->setDoubleValue( 0.0 ); - wp1_eta->setStringValue( "" ); + + 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 ); +} - if ( route->size() <= 0 ) { - wp0_id->setStringValue( "" ); - wp0_dist->setDoubleValue( 0.0 ); - wp0_eta->setStringValue( "" ); - } +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(); +} - if ( n == 0 && route->size() ) - altitude_set = false; - update_mirror(); - return wp; +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; } @@ -278,33 +321,24 @@ bool FGRouteMgr::build() { } - -int FGRouteMgr::new_waypoint( const string& Tgt_Alt, int n ) { - string target = Tgt_Alt; - - // make upper case - for (unsigned int i = 0; i < target.size(); i++) - if (target[i] >= 'a' && target[i] <= 'z') - target[i] -= 'a' - 'A'; - - SGWayPoint *wp = 0; - int type = make_waypoint( &wp, target ); - - if (wp) { - add_waypoint( *wp, n ); - fgSetString( "/autopilot/locks/heading", "true-heading-hold" ); - delete wp; +void FGRouteMgr::new_waypoint( const string& target, int n ) { + SGWayPoint* wp = make_waypoint( target ); + if (!wp) { + return; } - return type; + + add_waypoint( *wp, n ); + delete wp; } - -int FGRouteMgr::make_waypoint(SGWayPoint **wp, string& target) { - double alt = -9999.0; - +SGWayPoint* FGRouteMgr::make_waypoint(const string& tgt ) { + string target(boost::to_upper_copy(tgt)); + // extract altitude - unsigned int pos = target.find( '@' ); + 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 ); @@ -317,96 +351,114 @@ int FGRouteMgr::make_waypoint(SGWayPoint **wp, string& target) { 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 ); + } - SG_LOG( SG_GENERAL, SG_INFO, "Adding waypoint lon = " << lon << ", lat = " << lat ); - *wp = new SGWayPoint( lon, lat, alt, SGWayPoint::WGS84, target ); - return 1; + 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, "/")); - // check for airport id - const FGAirport *apt = fgFindAirportID( target ); - if (apt) { - SG_LOG( SG_GENERAL, SG_INFO, "Adding waypoint (airport) = " << target ); - *wp = new SGWayPoint( apt->getLongitude(), apt->getLatitude(), alt, SGWayPoint::WGS84, target ); - return 2; - } - // check for fix id - FGFix f; - if ( globals->get_fixlist()->query( target, &f ) ) { - SG_LOG( SG_GENERAL, SG_INFO, "Adding waypoint (fix) = " << target ); - *wp = new SGWayPoint( f.get_lon(), f.get_lat(), alt, SGWayPoint::WGS84, target ); - return 3; + 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; } - - // Try finding a nav matching the ID - double lat, lon; - // The base lon/lat are determined by the last WP, - // or the current pos if the WP list is empty. - const int wps = this->size(); - - if (wps > 0) { - SGWayPoint wp = get_waypoint(wps-1); - lat = wp.get_target_lat(); - lon = wp.get_target_lon(); - } else { - lat = fgGetNode("/position/latitude-deg")->getDoubleValue(); - lon = fgGetNode("/position/longitude-deg")->getDoubleValue(); + + SGGeod geod = SGGeod::fromGeodM(p->geod(), alt); + if (pieces.size() == 1) { + // simple case + return new SGWayPoint(geod, target, p->name()); } - - lat *= SGD_DEGREES_TO_RADIANS; - lon *= SGD_DEGREES_TO_RADIANS; - - SG_LOG( SG_GENERAL, SG_INFO, "Looking for nav " << target << " at " << lon << " " << lat); - - if (FGNavRecord* nav = globals->get_navlist()->findByIdent(target.c_str(), lon, lat)) { - SG_LOG( SG_GENERAL, SG_INFO, "Adding waypoint (nav) = " << target ); - *wp = new SGWayPoint( nav->get_lon(), nav->get_lat(), alt, SGWayPoint::WGS84, target ); - return 4; + + 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); } - - // unknown target - return 0; + + 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; } // 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); + 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", wp.get_target_lon()); - prop->setDoubleValue("latitude-deg", wp.get_target_lat()); - prop->setDoubleValue("altitude-m", wp.get_target_alt()); - prop->setDoubleValue("altitude-ft", wp.get_target_alt() * SG_METER_TO_FEET); + 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()); + mirror->setIntValue("num", _route->size()); } - // 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" +// @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::Listener::valueChanged(SGPropertyNode *prop) +void FGRouteMgr::InputListener::valueChanged(SGPropertyNode *prop) { const char *s = prop->getStringValue(); - if (!strcmp(s, "@clear")) + 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)) + else if (!strncmp(s, "@DELETE", 7)) mgr->pop_waypoint(atoi(s + 7)); - else if (!strncmp(s, "@insert", 7)) { + else if (!strncmp(s, "@INSERT", 7)) { char *r; int pos = strtol(s + 7, &r, 10); if (*r++ != ':') @@ -419,4 +471,293 @@ 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 = 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; +} + + +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; +} + +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(""); + } + + 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 = 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); + } +}