#include <time.h>
#endif
+#include <cstdio>
+
#include <simgear/compiler.h>
#include "route_mgr.hxx"
#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/procedure.hxx>
-#include "Airports/simple.hxx"
+#include "Airports/airport.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;
input->setStringValue("");
input->addChangeListener(listener);
- SGCommandMgr::instance()->addCommand("load-flightplan", commandLoadFlightPlan);
- SGCommandMgr::instance()->addCommand("save-flightplan", commandSaveFlightPlan);
- SGCommandMgr::instance()->addCommand("activate-flightplan", commandActivateFlightPlan);
- SGCommandMgr::instance()->addCommand("clear-flightplan", commandClearFlightPlan);
- SGCommandMgr::instance()->addCommand("set-active-waypt", commandSetActiveWaypt);
- SGCommandMgr::instance()->addCommand("insert-waypt", commandInsertWaypt);
- SGCommandMgr::instance()->addCommand("delete-waypt", commandDeleteWaypt);
+ SGCommandMgr* cmdMgr = globals->get_commands();
+ cmdMgr->addCommand("define-user-waypoint", this, &FGRouteMgr::commandDefineUserWaypoint);
+ cmdMgr->addCommand("delete-user-waypoint", this, &FGRouteMgr::commandDeleteUserWaypoint);
+
+ cmdMgr->addCommand("load-flightplan", commandLoadFlightPlan);
+ cmdMgr->addCommand("save-flightplan", commandSaveFlightPlan);
+ cmdMgr->addCommand("activate-flightplan", commandActivateFlightPlan);
+ cmdMgr->addCommand("clear-flightplan", commandClearFlightPlan);
+ cmdMgr->addCommand("set-active-waypt", commandSetActiveWaypt);
+ cmdMgr->addCommand("insert-waypt", commandInsertWaypt);
+ cmdMgr->addCommand("delete-waypt", commandDeleteWaypt);
}
{
input->removeChangeListener(listener);
delete listener;
+
+ if (_plan) {
+ _plan->removeDelegate(this);
+ }
+
+ SGCommandMgr* cmdMgr = globals->get_commands();
+ cmdMgr->removeCommand("define-user-waypoint");
+ cmdMgr->removeCommand("delete-user-waypoint");
+ cmdMgr->removeCommand("load-flightplan");
+ cmdMgr->removeCommand("save-flightplan");
+ cmdMgr->removeCommand("activate-flightplan");
+ cmdMgr->removeCommand("clear-flightplan");
+ cmdMgr->removeCommand("set-active-waypt");
+ cmdMgr->removeCommand("insert-waypt");
+ cmdMgr->removeCommand("delete-waypt");
}
departure->tie("name", SGRawValueMethods<FGRouteMgr, const char*>(*this,
&FGRouteMgr::getDepartureName, NULL));
departure->tie("field-elevation-ft", SGRawValueMethods<FGRouteMgr, double>(*this,
- &FGRouteMgr::getDestinationFieldElevation, NULL));
+ &FGRouteMgr::getDepartureFieldElevation, NULL));
departure->getChild("etd", 0, true);
departure->getChild("takeoff-time", 0, true);
&FGRouteMgr::getDestinationFieldElevation, NULL));
destination->getChild("eta", 0, true);
+ destination->getChild("eta-seconds", 0, true);
destination->getChild("touchdown-time", 0, true);
alternate = fgGetNode(RM "alternate", true);
airborne->setBoolValue(false);
_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>
(*this, &FGRouteMgr::currentIndex, &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("eta-seconds", 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);
+ wp1->getChild("eta-seconds", 0, true);
wpn = fgGetNode(RM "wp-last", 0, true);
wpn->getChild("dist", 0, true);
wpn->getChild("eta", 0, true);
+ wpn->getChild("eta-seconds", 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());
return true;
}
-FlightPlan* FGRouteMgr::flightPlan() const
+FlightPlanRef FGRouteMgr::flightPlan() const
{
return _plan;
}
-void FGRouteMgr::setFlightPlan(FlightPlan* plan)
+void FGRouteMgr::setFlightPlan(const FlightPlanRef& plan)
{
if (plan == _plan) {
return;
}
if (_plan) {
- delete _plan;
+ _plan->removeDelegate(this);
active->setBoolValue(false);
}
_plan = plan;
- _plan->setDelegate(this);
+ _plan->addDelegate(this);
_flightplanChanged->fireValueChanged();
return;
}
- if (checkFinished()) {
- // maybe we're done
- }
-
// basic course/distance information
SGGeod currentPos = globals->get_aircraft_position();
wp0->setDoubleValue("true-bearing-deg", courseDeg);
courseDeg -= magvar->getDoubleValue(); // expose magnetic bearing
wp0->setDoubleValue("bearing-deg", courseDeg);
- setETAPropertyFromDistance(wp0->getChild("eta"), distanceM);
+ setETAPropertyFromDistance(wp0, distanceM);
double totalPathDistanceNm = _plan->totalDistanceNm();
double totalDistanceRemaining = distanceM * SG_METER_TO_NM; // distance to current waypoint
wp1->setDoubleValue("true-bearing-deg", courseDeg);
courseDeg -= magvar->getDoubleValue(); // expose magnetic bearing
wp1->setDoubleValue("bearing-deg", courseDeg);
- setETAPropertyFromDistance(wp1->getChild("eta"), distanceM);
+ setETAPropertyFromDistance(wp1, distanceM);
wp1->setDoubleValue("distance-along-route-nm",
nextLeg->distanceAlongRoute());
wp1->setDoubleValue("remaining-distance-nm",
distanceToGo->setDoubleValue(totalDistanceRemaining);
wpn->setDoubleValue("dist", totalDistanceRemaining);
ete->setDoubleValue(totalDistanceRemaining / gs * 3600.0);
- setETAPropertyFromDistance(wpn->getChild("eta"), totalDistanceRemaining);
+ setETAPropertyFromDistance(wpn, totalDistanceRemaining);
}
void FGRouteMgr::clearRoute()
Waypt* FGRouteMgr::wayptAtIndex(int index) const
{
- if (_plan) {
- FlightPlan::Leg* leg = _plan->legAtIndex(index);
- if (leg) {
- return leg->waypoint();
- }
+ if (!_plan) {
+ throw sg_range_exception("wayptAtindex: no flightplan");
}
- return NULL;
+ return _plan->legAtIndex(index)->waypoint();
}
int FGRouteMgr::numLegs() const
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;
char eta_str[64];
double eta = aDistance * SG_METER_TO_NM / speed;
+ aProp->getChild("eta-seconds")->setIntValue( eta * 3600 );
if ( eta >= 100.0 ) {
eta = 99.999; // clamp
}
int major = (int)eta,
minor = (int)((eta - (int)eta) * 60.0);
snprintf( eta_str, 64, "%d:%02d", major, minor );
- aProp->setStringValue( eta_str );
+ aProp->getChild("eta")->setStringValue( eta_str );
}
void FGRouteMgr::removeLegAtIndex(int aIndex)
_plan->deleteIndex(aIndex);
}
-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->destinationAirport();
- 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();
- _edited->fireValueChanged();
- checkFinished();
+ _edited->fireValueChanged();
}
// mirror internal route to the property system for inspection by other subsystems
r++;
if (*r)
mgr->flightPlan()->insertWayptAtIndex(mgr->waypointFromString(r), pos);
- } else if (!strcmp(s, "@POSINIT")) {
- mgr->initAtPosition();
} else
mgr->flightPlan()->insertWayptAtIndex(mgr->waypointFromString(s), -1);
}
-void FGRouteMgr::initAtPosition()
-{
- if (isRouteActive()) {
- return; // don't mess with the active route
- }
-
- if (haveUserWaypoints()) {
- // user has already defined, loaded or entered a route, again
- // don't interfere with it
- return;
- }
-
- if (airborne->getBoolValue()) {
- SG_LOG(SG_AUTOPILOT, SG_INFO, "initAtPosition: airborne, clearing departure info");
- _plan->setDeparture((FGAirport*) NULL);
- return;
- }
-
-// on the ground
- SGGeod pos = globals->get_aircraft_position();
- if (!_plan->departureAirport()) {
- _plan->setDeparture(FGAirport::findClosest(pos, 20.0));
- if (!_plan->departureAirport()) {
- SG_LOG(SG_AUTOPILOT, SG_INFO, "initAtPosition: couldn't find an airport within 20nm");
- return;
- }
- }
-
- std::string rwy = departure->getStringValue("runway");
- FGRunway* r = NULL;
- if (!rwy.empty()) {
- r = _plan->departureAirport()->getRunwayByIdent(rwy);
- } else {
- r = _plan->departureAirport()->findBestRunwayForPos(pos);
- }
-
- if (!r) {
- return;
- }
-
- _plan->setDeparture(r);
- SG_LOG(SG_AUTOPILOT, SG_INFO, "initAtPosition: starting at "
- << _plan->departureAirport()->ident() << " on runway " << r->ident());
-}
-
-bool FGRouteMgr::haveUserWaypoints() const
-{
- // FIXME
- return false;
-}
-
bool FGRouteMgr::activate()
{
if (!_plan) {
return true;
}
-
-void FGRouteMgr::sequence()
+void FGRouteMgr::deactivate()
{
- if (!_plan || !active->getBoolValue()) {
- SG_LOG(SG_AUTOPILOT, SG_ALERT, "trying to sequence waypoints with no active route");
+ if (!isRouteActive()) {
return;
}
- if (checkFinished()) {
- return;
- }
-
- _plan->setCurrentIndex(_plan->currentIndex() + 1);
-}
-
-bool FGRouteMgr::checkFinished()
-{
- if (!_plan) {
- return true;
- }
-
- 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);
- }
-
-// 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;
+ SG_LOG(SG_AUTOPILOT, SG_INFO, "deactivating flight plan");
+ active->setBoolValue(false);
}
void FGRouteMgr::jumpToIndex(int index)
void FGRouteMgr::setDepartureRunway(const char* aIdent)
{
+ if (!_plan) {
+ return;
+ }
+
FGAirport* apt = _plan->departureAirport();
if (!apt || (aIdent == NULL)) {
_plan->setDeparture(apt);
void FGRouteMgr::setDepartureICAO(const char* aIdent)
{
+ if (!_plan) {
+ return;
+ }
+
if ((aIdent == NULL) || (strlen(aIdent) < 4)) {
_plan->setDeparture((FGAirport*) NULL);
} else {
return "";
}
+static double headingDiffDeg(double a, double b)
+{
+ double rawDiff = b - a;
+ SG_NORMALIZE_RANGE(rawDiff, -180.0, 180.0);
+ return rawDiff;
+}
+
+flightgear::SID* createDefaultSID(FGRunway* aRunway, double enrouteCourse)
+{
+ 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));
+ WayptRef w = new BasicWaypt(p, ss.str(), NULL);
+ w->setAltitude(runwayElevFt + 3000.0, RESTRICT_AT);
+ wpts.push_back(w);
+
+ ss.str("");
+ ss << aRunway->ident() << "-6";
+ p = aRunway->pointOnCenterline(aRunway->lengthM() + (6.0 * SG_NM_TO_METER));
+ w = new BasicWaypt(p, ss.str(), NULL);
+ w->setAltitude(runwayElevFt + 6000.0, RESTRICT_AT);
+ wpts.push_back(w);
+
+ if (enrouteCourse >= 0.0) {
+ // valid enroute course
+ int index = 3;
+ double course = aRunway->headingDeg();
+ double diff;
+ while (fabs(diff = headingDiffDeg(course, enrouteCourse)) > 45.0) {
+ // turn in the sign of the heading change 45 degrees
+ course += copysign(45.0, diff);
+ ss.str("");
+ ss << "DEP-" << index++;
+ SGGeod pos = wpts.back()->position();
+ pos = SGGeodesy::direct(pos, course, 3.0 * SG_NM_TO_METER);
+ w = new BasicWaypt(pos, ss.str(), NULL);
+ wpts.push_back(w);
+ }
+ } else {
+ // no enroute course, just keep runway heading
+ ss.str("");
+ ss << aRunway->ident() << "-9";
+ p = aRunway->pointOnCenterline(aRunway->lengthM() + (9.0 * SG_NM_TO_METER));
+ w = new BasicWaypt(p, ss.str(), NULL);
+ w->setAltitude(runwayElevFt + 9000.0, RESTRICT_AT);
+ wpts.push_back(w);
+ }
+
+ 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)
{
+ if (!_plan) {
+ return;
+ }
+
FGAirport* apt = _plan->departureAirport();
if (!apt || (aIdent == NULL)) {
_plan->setSID((flightgear::SID*) NULL);
return;
}
+ if (!strcmp(aIdent, "DEFAULT")) {
+ double enrouteCourse = -1.0;
+ if (_plan->destinationAirport()) {
+ enrouteCourse = SGGeodesy::courseDeg(apt->geod(), _plan->destinationAirport()->geod());
+ }
+
+ _plan->setSID(createDefaultSID(_plan->departureRunway(), enrouteCourse));
+ return;
+ }
+
string ident(aIdent);
size_t hyphenPos = ident.find('-');
if (hyphenPos != string::npos) {
void FGRouteMgr::setDestinationICAO(const char* aIdent)
{
+ if (!_plan) {
+ return;
+ }
+
if ((aIdent == NULL) || (strlen(aIdent) < 4)) {
_plan->setDestination((FGAirport*) NULL);
} else {
void FGRouteMgr::setDestinationRunway(const char* aIdent)
{
+ if (!_plan) {
+ return;
+ }
+
FGAirport* apt = _plan->destinationAirport();
if (!apt || (aIdent == NULL)) {
_plan->setDestination(apt);
return "";
}
+flightgear::Approach* createDefaultApproach(FGRunway* aRunway, double aEnrouteCourse)
+{
+ 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);
+ WayptRef w = new BasicWaypt(p, ss.str(), NULL);
+ w->setAltitude(thresholdElevFt + 4000, RESTRICT_AT);
+ wpts.push_back(w);
+
+// work back form the first point on the centerline
+
+ if (aEnrouteCourse >= 0.0) {
+ // valid enroute course
+ int index = 4;
+ double course = aRunway->headingDeg();
+ double diff;
+ while (fabs(diff = headingDiffDeg(aEnrouteCourse, course)) > 45.0) {
+ // turn in the sign of the heading change 45 degrees
+ course -= copysign(45.0, diff);
+ ss.str("");
+ ss << "APP-" << index++;
+ SGGeod pos = wpts.front()->position();
+ pos = SGGeodesy::direct(pos, course + 180.0, 3.0 * SG_NM_TO_METER);
+ w = new BasicWaypt(pos, ss.str(), NULL);
+ wpts.insert(wpts.begin(), w);
+ }
+ }
+
+ p = aRunway->pointOnCenterline(-8.0 * SG_NM_TO_METER);
+ ss.str("");
+ ss << aRunway->ident() << "-8";
+ w = new BasicWaypt(p, ss.str(), NULL);
+ w->setAltitude(thresholdElevFt + approachHeightFt, RESTRICT_AT);
+ wpts.push_back(w);
+
+ p = aRunway->pointOnCenterline(-glideslopeDistanceM);
+ ss.str("");
+ ss << aRunway->ident() << "-GS";
+ w = new BasicWaypt(p, ss.str(), NULL);
+ w->setAltitude(thresholdElevFt + approachHeightFt, RESTRICT_AT);
+ wpts.push_back(w);
+
+ 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)
{
+ if (!_plan) {
+ return;
+ }
+
FGAirport* apt = _plan->destinationAirport();
+ if (!strcmp(aIdent, "DEFAULT")) {
+ double enrouteCourse = -1.0;
+ if (_plan->departureAirport()) {
+ enrouteCourse = SGGeodesy::courseDeg(_plan->departureAirport()->geod(), apt->geod());
+ }
+
+ _plan->setApproach(createDefaultApproach(_plan->destinationRunway(), enrouteCourse));
+ return;
+ }
+
if (!apt || (aIdent == NULL)) {
_plan->setApproach(NULL);
} else {
void FGRouteMgr::setSTAR(const char* aIdent)
{
+ if (!_plan) {
+ return;
+ }
+
FGAirport* apt = _plan->destinationAirport();
if (!apt || (aIdent == NULL)) {
_plan->setSTAR((STAR*) NULL);
return mirror->getChild("wp", index);
}
+
+bool FGRouteMgr::commandDefineUserWaypoint(const SGPropertyNode* arg)
+{
+ std::string ident = arg->getStringValue("ident");
+ if (ident.empty()) {
+ SG_LOG(SG_AUTOPILOT, SG_WARN, "missing ident defining user waypoint");
+ return false;
+ }
+
+ // check for duplicate idents
+ FGPositioned::TypeFilter f(FGPositioned::WAYPOINT);
+ FGPositionedList dups = FGPositioned::findAllWithIdent(ident, &f);
+ if (!dups.empty()) {
+ SG_LOG(SG_AUTOPILOT, SG_WARN, "defineUserWaypoint: non-unique waypoint identifier:" << ident);
+ return false;
+ }
+
+ SGGeod pos(SGGeod::fromDeg(arg->getDoubleValue("longitude-deg"),
+ arg->getDoubleValue("latitude-deg")));
+ FGPositioned::createUserWaypoint(ident, pos);
+ return true;
+}
+
+bool FGRouteMgr::commandDeleteUserWaypoint(const SGPropertyNode* arg)
+{
+ std::string ident = arg->getStringValue("ident");
+ if (ident.empty()) {
+ SG_LOG(SG_AUTOPILOT, SG_WARN, "missing ident deleting user waypoint");
+ return false;
+ }
+
+ return FGPositioned::deleteUserWaypoint(ident);
+}
+
projects / flightgear.git / blobdiff