Remove un-needed header.

[flightgear.git] / src / Autopilot / route_mgr.cxx

diff --git a/src/Autopilot/route_mgr.cxx b/src/Autopilot/route_mgr.cxx
index 524bffa902da09ca5328c7aa782aaf0c7d6f67d9..337f2e9f2007751691e1fec38f7219d15cf98d66 100644 (file)
--- a/src/Autopilot/route_mgr.cxx
+++ b/src/Autopilot/route_mgr.cxx
@@ -453,7 +453,7 @@ void FGRouteMgr::update( double dt )
   }
 
   if (checkFinished()) {
-    // maybe we're done
+    endOfRoute();
   }
   
 // basic course/distance information
@@ -535,14 +535,11 @@ int FGRouteMgr::currentIndex() const
 
 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
@@ -590,8 +587,12 @@ void FGRouteMgr::removeLegAtIndex(int aIndex)
 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
@@ -802,11 +803,21 @@ void FGRouteMgr::sequence()
     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()
@@ -824,15 +835,8 @@ 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;
@@ -914,7 +918,14 @@ const char* FGRouteMgr::getSID() const
   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;
@@ -926,21 +937,42 @@ flightgear::SID* createDefaultSID(FGRunway* aRunway)
   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);
@@ -958,7 +990,12 @@ void FGRouteMgr::setSID(const char* aIdent)
   } 
   
   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;
   }
   
@@ -1031,7 +1068,7 @@ const char* FGRouteMgr::getApproach() const
   return "";
 }
 
-flightgear::Approach* createDefaultApproach(FGRunway* aRunway)
+flightgear::Approach* createDefaultApproach(FGRunway* aRunway, double aEnrouteCourse)
 {
   if (!aRunway) {
     return NULL;
@@ -1046,22 +1083,42 @@ flightgear::Approach* createDefaultApproach(FGRunway* aRunway)
   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));
+  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);
@@ -1075,7 +1132,12 @@ void FGRouteMgr::setApproach(const char* aIdent)
 {
   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;
   }