}
if (checkFinished()) {
- // maybe we're done
+ endOfRoute();
}
// basic course/distance information
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::waypointsChanged()
{
update_mirror();
- _edited->fireValueChanged();
- checkFinished();
+ _edited->fireValueChanged();
+
+// removing waypoints, deactivate if we hit the end.
+ if (currentIndex() >= numLegs()) {
+ endOfRoute();
+ }
}
// mirror internal route to the property system for inspection by other subsystems
return;
}
- if (checkFinished()) {
+ int nextIndex = _plan->currentIndex() + 1;
+ if (nextIndex >= _plan->numLegs()) {
+ SG_LOG(SG_AUTOPILOT, SG_INFO, "sequenced on final leg, deactivating route");
+ endOfRoute();
return;
}
-
- _plan->setCurrentIndex(_plan->currentIndex() + 1);
+
+ _plan->setCurrentIndex(nextIndex);
+}
+
+void FGRouteMgr::endOfRoute()
+{
+ SG_LOG(SG_AUTOPILOT, SG_INFO, "reached end of active route");
+ _finished->fireValueChanged();
+ active->setBoolValue(false);
}
bool FGRouteMgr::checkFinished()
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);
+ SG_LOG(SG_AUTOPILOT, SG_INFO, "checkFinished: on the ground on destination runway, we're done");
}
return done;
return "";
}
-flightgear::SID* createDefaultSID(FGRunway* aRunway)
+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;
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));
+ 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));
- p.setElevationFt(runwayElevFt + 4000.0);
- wpts.push_back(new BasicWaypt(p, ss.str(), NULL));
-
+ 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));
- p.setElevationFt(runwayElevFt + 6000.0);
- wpts.push_back(new BasicWaypt(p, ss.str(), NULL));
-
+ 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);
}
if (!strcmp(aIdent, "DEFAULT")) {
- _plan->setSID(createDefaultSID(_plan->departureRunway()));
+ double enrouteCourse = -1.0;
+ if (_plan->destinationAirport()) {
+ enrouteCourse = SGGeodesy::courseDeg(apt->geod(), _plan->destinationAirport()->geod());
+ }
+
+ _plan->setSID(createDefaultSID(_plan->departureRunway(), enrouteCourse));
return;
}
return "";
}
-flightgear::Approach* createDefaultApproach(FGRunway* aRunway)
+flightgear::Approach* createDefaultApproach(FGRunway* aRunway, double aEnrouteCourse)
{
if (!aRunway) {
return NULL;
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));
-
+ 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);
- p.setElevationFt(thresholdElevFt + approachHeightFt);
ss.str("");
ss << aRunway->ident() << "-8";
- wpts.push_back(new BasicWaypt(p, ss.str(), NULL));
+ w = new BasicWaypt(p, ss.str(), NULL);
+ w->setAltitude(thresholdElevFt + approachHeightFt, RESTRICT_AT);
+ wpts.push_back(w);
- p = aRunway->pointOnCenterline(-glideslopeDistanceM);
- p.setElevationFt(thresholdElevFt + approachHeightFt);
-
+ p = aRunway->pointOnCenterline(-glideslopeDistanceM);
ss.str("");
ss << aRunway->ident() << "-GS";
- wpts.push_back(new BasicWaypt(p, ss.str(), NULL));
-
- wpts.push_back(new RunwayWaypt(aRunway, NULL));
-
+ 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);
{
FGAirport* apt = _plan->destinationAirport();
if (!strcmp(aIdent, "DEFAULT")) {
- _plan->setApproach(createDefaultApproach(_plan->destinationRunway()));
+ double enrouteCourse = -1.0;
+ if (_plan->departureAirport()) {
+ enrouteCourse = SGGeodesy::courseDeg(_plan->departureAirport()->geod(), apt->geod());
+ }
+
+ _plan->setApproach(createDefaultApproach(_plan->destinationRunway(), enrouteCourse));
return;
}