X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAutopilot%2Froute_mgr.cxx;h=e8c5674c3213605a19f57ed730878c63812b16c4;hb=11d15b451347674fba77648700d23c5aaec3c6c2;hp=78b15f167a1fabf00e4683abbd95effb05063961;hpb=43dce6a2be0e0d0a65d47e1fbf9879dcbd5e481c;p=flightgear.git diff --git a/src/Autopilot/route_mgr.cxx b/src/Autopilot/route_mgr.cxx index 78b15f167..e8c5674c3 100644 --- a/src/Autopilot/route_mgr.cxx +++ b/src/Autopilot/route_mgr.cxx @@ -29,11 +29,9 @@ #include -#include #include #include
-#include -#include +#include #include "route_mgr.hxx" @@ -68,8 +66,9 @@ FGRouteMgr::FGRouteMgr() : FGRouteMgr::~FGRouteMgr() { - delete route; input->removeChangeListener(listener); + delete listener; + delete route; } @@ -125,78 +124,66 @@ static double get_ground_speed() { return kts; } +void FGRouteMgr::updateTargetAltitude() { + if (route->size() == 0) { + altitude_set = false; + return; + } + + SGWayPoint wp = route->get_waypoint( 0 ); + if (wp.get_target_alt() < -9990.0) { + altitude_set = false; + return; + } + + altitude_set = true; + target_altitude_ft->setDoubleValue( wp.get_target_alt() * SG_METER_TO_FEET ); + + if ( !near_ground() ) { + // James Turner [zakalawe]: there's no explanation for this logic, + // it feels like the autopilot should pull the target altitude out of + // wp0 instead of us pushing it through here. Hmmm. + altitude_lock->setStringValue( "altitude-hold" ); + } +} void FGRouteMgr::update( double dt ) { - double accum = 0.0; + if (route->size() == 0) { + return; // no route set, early return + } + 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; + SGWayPoint wp = route->get_waypoint( 0 ); + wp.CourseAndDistance( lon->getDoubleValue(), lat->getDoubleValue(), + alt->getDoubleValue(), &wp_course, &wp_distance ); + true_hdg_deg->setDoubleValue( wp_course ); + + if ( wp_distance < 200.0 ) { + pop_waypoint(); + if (route->size() == 0) { + return; // end of route, we're done for the time being } + + wp = route->get_waypoint( 0 ); } - // 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 ); - } + // update the property tree info for WP0 + wp0_id->setStringValue( wp.get_id().c_str() ); + double accum = wp_distance; + wp0_dist->setDoubleValue( accum * SG_METER_TO_NM ); + setETAPropertyFromDistance(wp0_eta, accum); // 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 ); + setETAPropertyFromDistance(wp1_eta, accum); } // summarize remaining way points @@ -208,29 +195,36 @@ void FGRouteMgr::update( double dt ) { } // 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 ); + setETAPropertyFromDistance(wpn_eta, accum); } } +void FGRouteMgr::setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDistance) { + 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) ) { + // within 10 minutes, bump up to min/secs + eta *= 60.0; + } + + 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 ); update_mirror(); + if ((n==0) || (route->size() == 1)) { + updateTargetAltitude(); + } } @@ -262,6 +256,7 @@ SGWayPoint FGRouteMgr::pop_waypoint( int n ) { wp0_eta->setStringValue( "" ); } + updateTargetAltitude(); update_mirror(); return wp; } @@ -272,33 +267,32 @@ bool FGRouteMgr::build() { } +void FGRouteMgr::new_waypoint( const string& target, int n ) { + SGWayPoint* wp = make_waypoint( target ); + if (!wp) { + return; + } + + add_waypoint( *wp, n ); + delete wp; -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) { + if ( !near_ground() ) { fgSetString( "/autopilot/locks/heading", "true-heading-hold" ); - add_waypoint( *wp, n ); - delete wp; } - return type; } - -int FGRouteMgr::make_waypoint(SGWayPoint **wp, string& target) { - double alt = -9999.0; +SGWayPoint* FGRouteMgr::make_waypoint(const string& tgt ) { + string target = tgt; + + // make upper case + for (unsigned int i = 0; i < target.size(); i++) + if (target[i] >= 'a' && target[i] <= 'z') + target[i] -= 'a' - 'A'; // extract altitude - unsigned int pos = target.find( '@' ); + double alt = -9999.0; + size_t pos = target.find( '@' ); if ( pos != string::npos ) { alt = atof( target.c_str() + pos + 1 ); target = target.substr( 0, pos ); @@ -307,60 +301,34 @@ int FGRouteMgr::make_waypoint(SGWayPoint **wp, string& target) { } // check for lon,lat - pos = target.find(','); + pos = target.find( ',' ); 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 ); - *wp = new SGWayPoint( lon, lat, alt, SGWayPoint::WGS84, target ); - return 1; - } - - // 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; - } + return new SGWayPoint( lon, lat, alt, SGWayPoint::WGS84, target ); + } - // 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(); + SGGeod basePosition; + if (route->size() > 0) { + SGWayPoint wp = get_waypoint(route->size()-1); + basePosition = SGGeod::fromDeg(wp.get_target_lon(), wp.get_target_lat()); } else { - lat = fgGetNode("/position/latitude-deg")->getDoubleValue(); - lon = fgGetNode("/position/longitude-deg")->getDoubleValue(); + // route is empty, use current position + basePosition = SGGeod::fromDeg( + fgGetNode("/position/longitude-deg")->getDoubleValue(), + fgGetNode("/position/latitude-deg")->getDoubleValue()); } - 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; + FGPositionedRef p = FGPositioned::findClosestWithIdent(target, basePosition); + if (!p) { + SG_LOG( SG_GENERAL, SG_INFO, "Unable to find FGPositioned with ident:" << target); + return NULL; } - - // target not identified - return 0; + + return new SGWayPoint(p->longitude(), p->latitude(), alt, SGWayPoint::WGS84, target); } @@ -383,24 +351,37 @@ void FGRouteMgr::update_mirror() { } +bool FGRouteMgr::near_ground() { + SGPropertyNode *gear = fgGetNode( "/gear/gear/wow", false ); + if ( !gear || gear->getType() == simgear::props::NONE ) + return fgGetBool( "/sim/presets/onground", true ); + + if ( fgGetDouble("/position/altitude-agl-ft", 300.0) + < fgGetDouble("/autopilot/route-manager/min-lock-altitude-agl-ft") ) + return true; + + return gear->getBoolValue(); +} + + // 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) { const char *s = prop->getStringValue(); - if (!strcmp(s, "@clear")) + if (!strcmp(s, "@CLEAR")) mgr->init(); - else if (!strcmp(s, "@pop")) + 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++ != ':')