+
+
+void FGRouteMgr::new_waypoint( const string& target, int n ) {
+ SGWayPoint* wp = make_waypoint( target );
+ if (!wp) {
+ return;
+ }
+
+ add_waypoint( *wp, n );
+ delete wp;
+
+ if ( !near_ground() ) {
+ fgSetString( "/autopilot/locks/heading", "true-heading-hold" );
+ }
+}
+
+
+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
+ 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 );
+ if ( !strcmp(fgGetString("/sim/startup/units"), "feet") )
+ alt *= SG_FEET_TO_METER;
+ }
+
+ // 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);
+
+ 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) {
+ SGWayPoint wp = get_waypoint(route->size()-1);
+ basePosition = SGGeod::fromDeg(wp.get_target_lon(), wp.get_target_lat());
+ } else {
+ // route is empty, use current position
+ basePosition = SGGeod::fromDeg(
+ fgGetNode("/position/longitude-deg")->getDoubleValue(),
+ fgGetNode("/position/latitude-deg")->getDoubleValue());
+ }
+
+
+ FGPositionedRef p = FGPositioned::findClosestWithIdent(target, basePosition);
+ if (!p) {
+ SG_LOG( SG_GENERAL, SG_INFO, "Unable to find FGPositioned with ident:" << target);
+ return NULL;
+ }
+
+ return new SGWayPoint(p->longitude(), p->latitude(), alt, SGWayPoint::WGS84, target);
+}
+
+
+// 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);
+
+ 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);
+ }
+ // set number as listener attachment point
+ mirror->setIntValue("num", route->size());
+}
+
+
+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"
+//
+void FGRouteMgr::Listener::valueChanged(SGPropertyNode *prop)
+{
+ const char *s = prop->getStringValue();
+ if (!strcmp(s, "@CLEAR"))
+ mgr->init();
+ 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);
+}
+
+