#include <boost/algorithm/string/case_conv.hpp>
#include <boost/tuple/tuple.hpp>
+#include <boost/foreach.hpp>
#include <simgear/misc/strutils.hxx>
#include <simgear/structure/exception.hxx>
#include <simgear/structure/commands.hxx>
-#include <simgear/misc/sgstream.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 <Navaids/waypoint.hxx>
-#include <Navaids/airways.hxx>
#include <Navaids/procedure.hxx>
#include "Airports/simple.hxx"
#include "Airports/runways.hxx"
-
-#define RM "/autopilot/route-manager/"
-
#include <GUI/new_gui.hxx>
#include <GUI/dialog.hxx>
+#define RM "/autopilot/route-manager/"
+
using namespace flightgear;
+using std::string;
static bool commandLoadFlightPlan(const SGPropertyNode* arg)
{
if (activate) {
self->activate();
} else {
-
+ self->deactivate();
}
return true;
cruise->getChild("speed-kts", 0, true);
cruise->setDoubleValue("speed-kts", 160.0);
- _routingType = cruise->getChild("routing", 0, true);
- _routingType->setIntValue(ROUTE_HIGH_AIRWAYS);
-
totalDistance = fgGetNode(RM "total-distance", true);
totalDistance->setDoubleValue(0.0);
distanceToGo = fgGetNode(RM "distance-remaining-nm", true);
_edited = fgGetNode(RM "signals/edited", true);
_finished = fgGetNode(RM "signals/finished", true);
+ _flightplanChanged = fgGetNode(RM "signals/flightplan-changed", true);
_currentWpt = fgGetNode(RM "current-wp", true);
_currentWpt->tie(SGRawValueMethods<FGRouteMgr, int>
wpn->getChild("eta", 0, true);
_pathNode = fgGetNode(RM "file-path", 0, true);
- setFlightPlan(new FlightPlan());
}
void FGRouteMgr::postinit()
{
+ setFlightPlan(new FlightPlan());
+
SGPath path(_pathNode->getStringValue());
if (!path.isNull()) {
SG_LOG(SG_AUTOPILOT, SG_INFO, "loading flight-plan from: " << path.str());
}
weightOnWheels = fgGetNode("/gear/gear[0]/wow", true);
+ groundSpeed = fgGetNode("/velocities/groundspeed-kt", true);
+
// check airbone flag agrees with presets
}
}
if (_plan) {
+ _plan->removeDelegate(this);
delete _plan;
active->setBoolValue(false);
}
_plan = plan;
- _plan->setDelegate(this);
+ _plan->addDelegate(this);
+
+ _flightplanChanged->fireValueChanged();
// fire all the callbacks!
departureChanged();
return; // paused, nothing to do here
}
- double groundSpeed = fgGetDouble("/velocities/groundspeed-kt", 0.0);
+ double gs = groundSpeed->getDoubleValue();
if (airborne->getBoolValue()) {
time_t now = time(NULL);
elapsedFlightTime->setDoubleValue(difftime(now, _takeoffTime));
+
+ if (weightOnWheels->getBoolValue()) {
+ // touch down
+ destination->setIntValue("touchdown-time", time(NULL));
+ airborne->setBoolValue(false);
+ }
} else { // not airborne
- if (weightOnWheels->getBoolValue() || (groundSpeed < 40)) {
- return;
+ if (weightOnWheels->getBoolValue() || (gs < 40)) {
+ // either taking-off or rolling-out after touchdown
+ } else {
+ airborne->setBoolValue(true);
+ _takeoffTime = time(NULL); // start the clock
+ departure->setIntValue("takeoff-time", _takeoffTime);
}
-
- airborne->setBoolValue(true);
- _takeoffTime = time(NULL); // start the clock
- departure->setIntValue("takeoff-time", _takeoffTime);
}
if (!active->getBoolValue()) {
return;
}
+ if (checkFinished()) {
+ // maybe we're done
+ }
+
// basic course/distance information
SGGeod currentPos = globals->get_aircraft_position();
distanceToGo->setDoubleValue(totalDistanceRemaining);
wpn->setDoubleValue("dist", totalDistanceRemaining);
- ete->setDoubleValue(totalDistanceRemaining / groundSpeed * 3600.0);
+ ete->setDoubleValue(totalDistanceRemaining / gs * 3600.0);
setETAPropertyFromDistance(wpn->getChild("eta"), totalDistanceRemaining);
}
void FGRouteMgr::setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDistance)
{
- double speed = fgGetDouble("/velocities/groundspeed-kt", 0.0);
+ double speed = groundSpeed->getDoubleValue();
if (speed < 1.0) {
aProp->setStringValue("--:--");
return;
_plan->deleteIndex(aIndex);
}
-/**
- * route between index-1 and index, using airways.
- */
-bool FGRouteMgr::routeToIndex(int index, RouteType aRouteType)
-{
- WayptRef wp1;
- WayptRef wp2;
-
- if (index == -1) {
- index = numLegs();
- }
-
- if (index == 0) {
- if (!_plan->departureAirport()) {
- SG_LOG(SG_AUTOPILOT, SG_WARN, "routeToIndex: no departure set");
- return false;
- }
-
- wp1 = new NavaidWaypoint(_plan->departureAirport().get(), NULL);
- } else {
- wp1 = wayptAtIndex(index - 1);
- }
-
- if (index >= numLegs()) {
- if (!_plan->destinationAirport()) {
- SG_LOG(SG_AUTOPILOT, SG_WARN, "routeToIndex: no destination set");
- return false;
- }
-
- wp2 = new NavaidWaypoint(_plan->destinationAirport().get(), NULL);
- } else {
- wp2 = wayptAtIndex(index);
- }
-
- double distNm = SGGeodesy::distanceNm(wp1->position(), wp2->position());
- if (distNm < 100.0) {
- SG_LOG(SG_AUTOPILOT, SG_INFO, "routeToIndex: existing waypoints are nearby, direct route");
- return true;
- }
-
- WayptVec r;
- switch (aRouteType) {
- case ROUTE_HIGH_AIRWAYS:
- Airway::highLevel()->route(wp1, wp2, r);
- break;
-
- case ROUTE_LOW_AIRWAYS:
- Airway::lowLevel()->route(wp1, wp2, r);
- break;
-
- case ROUTE_VOR:
- throw sg_exception("VOR routing not supported yet");
- }
-
- if (r.empty()) {
- SG_LOG(SG_AUTOPILOT, SG_INFO, "routeToIndex: no route found");
- return false;
- }
-
- _plan->insertWayptsAtIndex(r, index);
- return true;
-}
-
-void FGRouteMgr::departureChanged()
-{
- _plan->clearWayptsWithFlag(WPT_DEPARTURE);
- WayptRef enroute;
- WayptVec wps;
- buildDeparture(enroute, wps);
- _plan->insertWayptsAtIndex(wps, 0);
-}
-
-void FGRouteMgr::buildDeparture(WayptRef enroute, WayptVec& wps)
-{
- if (!_plan->departureAirport()) {
- return;
- }
-
- if (!_plan->departureRunway()) {
-// valid airport, but no runway selected, so just the airport _plan itself
- WayptRef w = new NavaidWaypoint(_plan->departureAirport(), _plan);
- w->setFlag(WPT_DEPARTURE);
- wps.push_back(w);
- return;
- }
-
- WayptRef rwyWaypt = new RunwayWaypt(_plan->departureRunway(), _plan);
- rwyWaypt->setFlag(WPT_DEPARTURE);
- wps.push_back(rwyWaypt);
-
- if (!_plan->sid()) {
- return;
- }
-
- _plan->sid()->route(_plan->departureRunway(), _plan->sidTransition(), wps);
-}
-
-void FGRouteMgr::arrivalChanged()
-{
- _plan->clearWayptsWithFlag(WPT_ARRIVAL);
- _plan->clearWayptsWithFlag(WPT_APPROACH);
- WayptVec wps;
- WayptRef enroute;
- buildArrival(enroute, wps);
- _plan->insertWayptsAtIndex(wps, -1);
-}
-
-void FGRouteMgr::buildArrival(WayptRef enroute, WayptVec& wps)
-{
- FGAirportRef apt = _plan->departureAirport();
- if (!apt.valid()) {
- return;
- }
-
- if (!_plan->destinationRunway()) {
- WayptRef w = new NavaidWaypoint(apt.ptr(), _plan);
- w->setFlag(WPT_ARRIVAL);
- wps.push_back(w);
- return;
- }
-
- if (_plan->star()) {
- _plan->star()->route(_plan->destinationRunway(), _plan->starTransition(), wps);
- }
-
- if (_plan->approach()) {
- _plan->approach()->route(wps.back(), wps);
- } else {
- WayptRef w = new RunwayWaypt(_plan->destinationRunway(), _plan);
- w->setFlag(WPT_APPROACH);
- wps.push_back(w);
- }
-}
-
void FGRouteMgr::waypointsChanged()
{
update_mirror();
r++;
if (*r)
mgr->flightPlan()->insertWayptAtIndex(mgr->waypointFromString(r), pos);
- } else if (!strncmp(s, "@ROUTE", 6)) {
- char* r;
- int endIndex = strtol(s + 6, &r, 10);
- RouteType rt = (RouteType) mgr->_routingType->getIntValue();
- mgr->routeToIndex(endIndex, rt);
} else if (!strcmp(s, "@POSINIT")) {
mgr->initAtPosition();
} else
return true;
}
+void FGRouteMgr::deactivate()
+{
+ if (!isRouteActive()) {
+ return;
+ }
+
+ SG_LOG(SG_AUTOPILOT, SG_INFO, "deactivating flight plan");
+ active->setBoolValue(false);
+}
void FGRouteMgr::sequence()
{
return true;
}
- if (_plan->currentIndex() < _plan->numLegs()) {
- return false;
+ bool done = false;
+// done if we're stopped on the destination runway
+ if (_plan->currentLeg() &&
+ (_plan->currentLeg()->waypoint()->source() == _plan->destinationRunway()))
+ {
+ double gs = groundSpeed->getDoubleValue();
+ done = weightOnWheels->getBoolValue() && (gs < 25);
}
- SG_LOG(SG_AUTOPILOT, SG_INFO, "reached end of active route");
- _finished->fireValueChanged();
- active->setBoolValue(false);
- return true;
+// also done if we hit the final waypoint
+ if (_plan->currentIndex() >= _plan->numLegs()) {
+ done = true;
+ }
+
+ if (done) {
+ SG_LOG(SG_AUTOPILOT, SG_INFO, "reached end of active route");
+ _finished->fireValueChanged();
+ active->setBoolValue(false);
+ }
+
+ return done;
}
void FGRouteMgr::jumpToIndex(int index)
return "";
}
+flightgear::SID* createDefaultSID(FGRunway* aRunway)
+{
+ if (!aRunway) {
+ return NULL;
+ }
+
+ double runwayElevFt = aRunway->end().getElevationFt();
+ WayptVec wpts;
+ std::ostringstream ss;
+ ss << aRunway->ident() << "-3";
+
+ SGGeod p = aRunway->pointOnCenterline(aRunway->lengthM() + (3.0 * SG_NM_TO_METER));
+ p.setElevationFt(runwayElevFt + 2000.0);
+ wpts.push_back(new BasicWaypt(p, ss.str(), NULL));
+
+ ss.str("");
+ ss << aRunway->ident() << "-6";
+ p = aRunway->pointOnCenterline(aRunway->lengthM() + (6.0 * SG_NM_TO_METER));
+ p.setElevationFt(runwayElevFt + 4000.0);
+ wpts.push_back(new BasicWaypt(p, ss.str(), NULL));
+
+ ss.str("");
+ ss << aRunway->ident() << "-9";
+ p = aRunway->pointOnCenterline(aRunway->lengthM() + (9.0 * SG_NM_TO_METER));
+ p.setElevationFt(runwayElevFt + 6000.0);
+ wpts.push_back(new BasicWaypt(p, ss.str(), NULL));
+
+ BOOST_FOREACH(Waypt* w, wpts) {
+ w->setFlag(WPT_DEPARTURE);
+ w->setFlag(WPT_GENERATED);
+ }
+
+ return flightgear::SID::createTempSID("DEFAULT", aRunway, wpts);
+}
+
void FGRouteMgr::setSID(const char* aIdent)
{
FGAirport* apt = _plan->departureAirport();
return;
}
+ if (!strcmp(aIdent, "DEFAULT")) {
+ _plan->setSID(createDefaultSID(_plan->departureRunway()));
+ return;
+ }
+
string ident(aIdent);
size_t hyphenPos = ident.find('-');
if (hyphenPos != string::npos) {
return "";
}
+flightgear::Approach* createDefaultApproach(FGRunway* aRunway)
+{
+ if (!aRunway) {
+ return NULL;
+ }
+
+ double thresholdElevFt = aRunway->threshold().getElevationFt();
+ const double approachHeightFt = 2000.0;
+ double glideslopeDistanceM = (approachHeightFt * SG_FEET_TO_METER) /
+ tan(3.0 * SG_DEGREES_TO_RADIANS);
+
+ std::ostringstream ss;
+ ss << aRunway->ident() << "-12";
+ WayptVec wpts;
+ SGGeod p = aRunway->pointOnCenterline(-12.0 * SG_NM_TO_METER);
+ p.setElevationFt(thresholdElevFt + 4000);
+ wpts.push_back(new BasicWaypt(p, ss.str(), NULL));
+
+
+ p = aRunway->pointOnCenterline(-8.0 * SG_NM_TO_METER);
+ p.setElevationFt(thresholdElevFt + approachHeightFt);
+ ss.str("");
+ ss << aRunway->ident() << "-8";
+ wpts.push_back(new BasicWaypt(p, ss.str(), NULL));
+
+ p = aRunway->pointOnCenterline(-glideslopeDistanceM);
+ p.setElevationFt(thresholdElevFt + approachHeightFt);
+
+ ss.str("");
+ ss << aRunway->ident() << "-GS";
+ wpts.push_back(new BasicWaypt(p, ss.str(), NULL));
+
+ wpts.push_back(new RunwayWaypt(aRunway, NULL));
+
+ BOOST_FOREACH(Waypt* w, wpts) {
+ w->setFlag(WPT_APPROACH);
+ w->setFlag(WPT_GENERATED);
+ }
+
+ return Approach::createTempApproach("DEFAULT", aRunway, wpts);
+}
+
void FGRouteMgr::setApproach(const char* aIdent)
{
FGAirport* apt = _plan->destinationAirport();
+ if (!strcmp(aIdent, "DEFAULT")) {
+ _plan->setApproach(createDefaultApproach(_plan->destinationRunway()));
+ return;
+ }
+
if (!apt || (aIdent == NULL)) {
_plan->setApproach(NULL);
} else {