diff --git a/src/AIModel/AIAircraft.cxx b/src/AIModel/AIAircraft.cxx
index fe93d938b..2db78de6d 100644
--- a/src/AIModel/AIAircraft.cxx
+++ b/src/AIModel/AIAircraft.cxx
@@ -709,9 +709,9 @@ void FGAIAircraft::handleFirstWaypoint() {
     setAltitude(prev->getAltitude());
 
     if (prev->getSpeed() > 0.0)
-        setHeading(fp->getBearing(prev->getLatitude(), prev->getLongitude(), curr));
+        setHeading(fp->getBearing(prev, curr));
     else
-        setHeading(fp->getBearing(curr->getLatitude(), curr->getLongitude(), prev));
+        setHeading(fp->getBearing(curr, prev));
 
     // If next doesn't exist, as in incrementally created flightplans for
     // AI/Trafficmanager created plans,
@@ -799,7 +799,7 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr) {
     //          << " Ground target speed " << groundTargetSpeed << endl;
     double bearing = 0;
      // don't do bearing calculations for ground traffic
-       bearing = getBearing(fp->getBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr));
+       bearing = getBearing(fp->getBearing(pos, curr));
        if (bearing < minBearing) {
             minBearing = bearing;
             if (minBearing < 10) {
@@ -913,12 +913,11 @@ bool FGAIAircraft::handleAirportEndPoints(FGAIWaypoint* prev, time_t now) {
  * @param curr
  */
 void FGAIAircraft::controlHeading(FGAIWaypoint* curr) {
-    double calc_bearing = fp->getBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr);
+    double calc_bearing = fp->getBearing(pos, curr);
     //cerr << "Bearing = " << calc_bearing << endl;
     if (speed < 0) {
         calc_bearing +=180;
-        if (calc_bearing > 360)
-            calc_bearing -= 360;
+        SG_NORMALIZE_RANGE(calc_bearing, 0.0, 360.0);
     }
 
     if (finite(calc_bearing)) {
@@ -1023,19 +1022,6 @@ void FGAIAircraft::updatePrimaryTargetValues(bool& flightplanActive, bool& aiOut
     }
 }
 
-void FGAIAircraft::updatePosition() {
-    // convert speed to degrees per second
-    double speed_north_deg_sec = cos( hdg * SGD_DEGREES_TO_RADIANS )
-                                 * speed * 1.686 / ft_per_deg_lat;
-    double speed_east_deg_sec  = sin( hdg * SGD_DEGREES_TO_RADIANS )
-                                 * speed * 1.686 / ft_per_deg_lon;
-
-    // set new position
-    pos.setLatitudeDeg( pos.getLatitudeDeg() + speed_north_deg_sec * dt);
-    pos.setLongitudeDeg( pos.getLongitudeDeg() + speed_east_deg_sec * dt);
-}
-
-
 void FGAIAircraft::updateHeading() {
     // adjust heading based on current bank angle
     if (roll == 0.0)
@@ -1181,13 +1167,9 @@ void FGAIAircraft::updateVerticalSpeedTarget() {
         // find target vertical speed
         if (use_perf_vs) {
             if (altitude_ft < tgt_altitude_ft) {
-                tgt_vs = tgt_altitude_ft - altitude_ft;
-                if (tgt_vs > _performance->climbRate())
-                    tgt_vs = _performance->climbRate();
+                tgt_vs = std::min(tgt_altitude_ft - altitude_ft, _performance->climbRate());
             } else {
-                tgt_vs = tgt_altitude_ft - altitude_ft;
-                if (tgt_vs  < (-_performance->descentRate()))
-                    tgt_vs = -_performance->descentRate();
+                tgt_vs = std::max(tgt_altitude_ft - altitude_ft, -_performance->descentRate());
             }
         } else {
             double vert_dist_ft = fp->getCurrentWaypoint()->getCrossat() - altitude_ft;
@@ -1265,7 +1247,9 @@ void FGAIAircraft::handleATCRequests() {
 void FGAIAircraft::updateActualState() {
     //update current state
     //TODO have a single tgt_speed and check speed limit on ground on setting tgt_speed
-    updatePosition();
+    double distance = speed * SG_KT_TO_MPS * dt;
+    pos = SGGeodesy::direct(pos, hdg, distance);
+
 
     if (onGround())
         speed = _performance->actualSpeed(this, groundTargetSpeed, dt, holdPos);
diff --git a/src/AIModel/AIAircraft.hxx b/src/AIModel/AIAircraft.hxx
index 73e98aeb3..dec7a86a4 100644
--- a/src/AIModel/AIAircraft.hxx
+++ b/src/AIModel/AIAircraft.hxx
@@ -148,7 +148,6 @@ private:
     void updatePrimaryTargetValues(bool& flightplanActive, bool& aiOutOfSight);
     
     void updateSecondaryTargetValues();
-    void updatePosition();
     void updateHeading();
     void updateBankAngleTarget();
     void updateVerticalSpeedTarget();
diff --git a/src/AIModel/AIFlightPlan.cxx b/src/AIModel/AIFlightPlan.cxx
index 1ec20e188..14c4de1ac 100644
--- a/src/AIModel/AIFlightPlan.cxx
+++ b/src/AIModel/AIFlightPlan.cxx
@@ -34,6 +34,7 @@
 #include <Main/fg_props.hxx>
 #include <Main/fg_init.hxx>
 #include <Airports/simple.hxx>
+#include <Airports/dynamics.hxx>
 #include <Airports/runways.hxx>
 #include <Airports/groundnetwork.hxx>
 
@@ -124,45 +125,8 @@ FGAIFlightPlan::FGAIFlightPlan(const string& filename)
   lastNodeVisited   = 0;
   taxiRoute         = 0;
 
-  SGPath path( globals->get_fg_root() );
-  path.append( ("/AI/FlightPlans/" + filename).c_str() );
-  SGPropertyNode root;
 
-  try {
-      readProperties(path.str(), &root);
-  } catch (const sg_exception &) {
-      SG_LOG(SG_AI, SG_ALERT,
-       "Error reading AI flight plan: " << path.str());
-       // cout << path.str() << endl;
-     return;
-  }
-
-  SGPropertyNode * node = root.getNode("flightplan");
-  for (int i = 0; i < node->nChildren(); i++) {
-     //cout << "Reading waypoint " << i << endl;        
-     FGAIWaypoint* wpt = new FGAIWaypoint;
-     SGPropertyNode * wpt_node = node->getChild(i);
-     wpt->setName       (wpt_node->getStringValue("name", "END"     ));
-     wpt->setLatitude   (wpt_node->getDoubleValue("lat", 0          ));
-     wpt->setLongitude  (wpt_node->getDoubleValue("lon", 0          ));
-     wpt->setAltitude   (wpt_node->getDoubleValue("alt", 0          ));
-     wpt->setSpeed      (wpt_node->getDoubleValue("ktas", 0         ));
-     wpt->setCrossat    (wpt_node->getDoubleValue("crossat", -10000 ));
-     wpt->setGear_down  (wpt_node->getBoolValue("gear-down", false  ));
-     wpt->setFlaps_down (wpt_node->getBoolValue("flaps-down", false ));
-     wpt->setOn_ground  (wpt_node->getBoolValue("on-ground", false  ));
-     wpt->setTime_sec   (wpt_node->getDoubleValue("time-sec", 0     ));
-     wpt->setTime       (wpt_node->getStringValue("time", ""        ));
-
-     if (wpt->getName() == "END") wpt->setFinished(true);
-     else wpt->setFinished(false);
-
-     pushBackWaypoint( wpt );
-   }
-
-  wpt_iterator = waypoints.begin();
-  isValid = true;
-  //cout << waypoints.size() << " waypoints read." << endl;
+  isValid = parseProperties(filename);
 }
 
 
@@ -186,7 +150,9 @@ FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac,
                                double speed,
                                const string& fltType,
                                const string& acType,
-                               const string& airline)
+                               const string& airline) :
+  departure(dep),
+  arrival(arr)
 {
   sid               = 0;
   repeat            = false;
@@ -199,99 +165,110 @@ FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac,
   lastNodeVisited   = 0;
   taxiRoute         = 0;
 
-  SGPath path( globals->get_fg_root() );
-  path.append( "/AI/FlightPlans" );
-  path.append( p );
-  
-  SGPropertyNode root;
-  isValid = true;
-
-  // This is a bit of a hack:
-  // Normally the value of course will be used to evaluate whether
-  // or not a waypoint will be used for midair initialization of 
-  // an AI aircraft. However, if a course value of 999 will be passed
-  // when an update request is received, which will by definition always be
-  // on the ground and should include all waypoints.
-//  bool useInitialWayPoint = true;
-//  bool useCurrentWayPoint = false;
-//  if (course == 999)
-//    {
-//      useInitialWayPoint = false;
-//      useCurrentWayPoint = true;
-//    }
-
-  if (path.exists()) 
-    {
-      try 
-	{
-	  readProperties(path.str(), &root);
-	  
-	  SGPropertyNode * node = root.getNode("flightplan");
-	  
-	  //pushBackWaypoint( init_waypoint );
-	  for (int i = 0; i < node->nChildren(); i++) { 
-	    //cout << "Reading waypoint " << i << endl;
-	    FGAIWaypoint* wpt = new FGAIWaypoint;
-	    SGPropertyNode * wpt_node = node->getChild(i);
-	    wpt->setName       (wpt_node->getStringValue("name", "END"     ));
-	    wpt->setLatitude   (wpt_node->getDoubleValue("lat", 0          ));
-	    wpt->setLongitude  (wpt_node->getDoubleValue("lon", 0          ));
-	    wpt->setAltitude   (wpt_node->getDoubleValue("alt", 0          ));
-	    wpt->setSpeed      (wpt_node->getDoubleValue("ktas", 0         ));
-	    wpt->setCrossat    (wpt_node->getDoubleValue("crossat", -10000 ));
-	    wpt->setGear_down  (wpt_node->getBoolValue("gear-down", false  ));
-	    wpt->setFlaps_down (wpt_node->getBoolValue("flaps-down", false ));
-	    
-	    if (wpt->getName() == "END") wpt->setFinished(true);
-	    else wpt->setFinished(false);
-	    pushBackWaypoint(wpt);
-	  } // of node loop
-          wpt_iterator = waypoints.begin();
-	} catch (const sg_exception &e) {
-      SG_LOG(SG_AI, SG_WARN, "Error reading AI flight plan: " <<
-        e.getMessage() << " from " << e.getOrigin());
-    }
+  if (parseProperties(p)) {
+    isValid = true;
   } else {
-      // cout << path.str() << endl;
-      // cout << "Trying to create this plan dynamically" << endl;
-      // cout << "Route from " << dep->id << " to " << arr->id << endl;
-      time_t now = time(NULL) + fgGetLong("/sim/time/warp");
-      time_t timeDiff = now-start; 
-      leg = 1;
-      
-      if ((timeDiff > 60) && (timeDiff < 1500))
-	leg = 2;
-      //else if ((timeDiff >= 1200) && (timeDiff < 1500)) {
-	//leg = 3;
-        //ac->setTakeOffStatus(2);
-      //}
-      else if ((timeDiff >= 1500) && (timeDiff < 2000))
-	leg = 4;
-      else if (timeDiff >= 2000)
-	leg = 5;
-      /*
-      if (timeDiff >= 2000)
-          leg = 5;
-      */
-      SG_LOG(SG_AI, SG_INFO, "Route from " << dep->getId() << " to " << arr->getId() << ". Set leg to : " << leg << " " << ac->getTrafficRef()->getCallSign());
-      wpt_iterator = waypoints.begin();
-      bool dist = 0;
-      isValid = create(ac, dep,arr, leg, alt, speed, lat, lon,
-	     firstLeg, radius, fltType, acType, airline, dist);
-      wpt_iterator = waypoints.begin();
-    }
-
+    createWaypoints(ac, course, start, dep, arr, firstLeg, radius,
+                    alt, lat, lon, speed, fltType, acType, airline);
+  }
 }
 
-
-
-
 FGAIFlightPlan::~FGAIFlightPlan()
 {
   deleteWaypoints();
   delete taxiRoute;
+
+// if we're parked at a gate, release it
+  if (gateId >= 0) {
+    FGAirport* apt = (leg >= 7) ? arrival : departure;
+    if (apt) {
+      SG_LOG(SG_AI, SG_INFO, "releasing parking gate " << gateId << " at " << apt->ident());
+      apt->getDynamics()->releaseParking(gateId);
+    }
+  }
 }
 
+void FGAIFlightPlan::createWaypoints(FGAIAircraft *ac,
+                                     double course,
+                                     time_t start,
+                                     FGAirport *dep,
+                                     FGAirport *arr,
+                                     bool firstLeg,
+                                     double radius,
+                                     double alt,
+                                     double lat,
+                                     double lon,
+                                     double speed,
+                                     const string& fltType,
+                                     const string& acType,
+                                     const string& airline)
+{
+  time_t now = time(NULL) + fgGetLong("/sim/time/warp");
+  time_t timeDiff = now-start;
+  leg = 1;
+  
+  if ((timeDiff > 60) && (timeDiff < 1500))
+    leg = 2;
+  //else if ((timeDiff >= 1200) && (timeDiff < 1500)) {
+	//leg = 3;
+  //ac->setTakeOffStatus(2);
+  //}
+  else if ((timeDiff >= 1500) && (timeDiff < 2000))
+    leg = 4;
+  else if (timeDiff >= 2000)
+    leg = 5;
+  /*
+   if (timeDiff >= 2000)
+   leg = 5;
+   */
+  SG_LOG(SG_AI, SG_INFO, "Route from " << dep->getId() << " to " << arr->getId() << ". Set leg to : " << leg << " " << ac->getTrafficRef()->getCallSign());
+  wpt_iterator = waypoints.begin();
+  bool dist = 0;
+  isValid = create(ac, dep, arr, leg, alt, speed, lat, lon,
+                   firstLeg, radius, fltType, acType, airline, dist);
+  wpt_iterator = waypoints.begin();
+ 
+}
+
+bool FGAIFlightPlan::parseProperties(const std::string& filename)
+{
+  SGPath path( globals->get_fg_root() );
+  path.append( "/AI/FlightPlans/" + filename );
+  if (!path.exists()) {
+    return false;
+  }
+  
+  SGPropertyNode root;
+  try {
+    readProperties(path.str(), &root);
+  } catch (const sg_exception &e) {
+    SG_LOG(SG_AI, SG_ALERT, "Error reading AI flight plan: " << path.str()
+           << "message:" << e.getFormattedMessage());
+    return false;
+  }
+  
+  SGPropertyNode * node = root.getNode("flightplan");
+  for (int i = 0; i < node->nChildren(); i++) {
+    FGAIWaypoint* wpt = new FGAIWaypoint;
+    SGPropertyNode * wpt_node = node->getChild(i);
+    wpt->setName       (wpt_node->getStringValue("name", "END"     ));
+    wpt->setLatitude   (wpt_node->getDoubleValue("lat", 0          ));
+    wpt->setLongitude  (wpt_node->getDoubleValue("lon", 0          ));
+    wpt->setAltitude   (wpt_node->getDoubleValue("alt", 0          ));
+    wpt->setSpeed      (wpt_node->getDoubleValue("ktas", 0         ));
+    wpt->setCrossat    (wpt_node->getDoubleValue("crossat", -10000 ));
+    wpt->setGear_down  (wpt_node->getBoolValue("gear-down", false  ));
+    wpt->setFlaps_down (wpt_node->getBoolValue("flaps-down", false ));
+    wpt->setOn_ground  (wpt_node->getBoolValue("on-ground", false  ));
+    wpt->setTime_sec   (wpt_node->getDoubleValue("time-sec", 0     ));
+    wpt->setTime       (wpt_node->getStringValue("time", ""        ));
+    wpt->setFinished   ((wpt->getName() == "END"));
+    pushBackWaypoint( wpt );
+  }
+  
+  wpt_iterator = waypoints.begin();
+  return true;
+}
 
 FGAIWaypoint* const FGAIFlightPlan::getPreviousWaypoint( void ) const
 {
@@ -425,10 +402,9 @@ double FGAIFlightPlan::getBearing(FGAIWaypoint* first, FGAIWaypoint* second) con
   return SGGeodesy::courseDeg(first->getPos(), second->getPos());
 }
 
-
-double FGAIFlightPlan::getBearing(double lat, double lon, FGAIWaypoint* wp) const
+double FGAIFlightPlan::getBearing(const SGGeod& aPos, FGAIWaypoint* wp) const
 {
-  return SGGeodesy::courseDeg(SGGeod::fromDeg(lon, lat), wp->getPos());
+  return SGGeodesy::courseDeg(aPos, wp->getPos());
 }
 
 void FGAIFlightPlan::deleteWaypoints()
diff --git a/src/AIModel/AIFlightPlan.hxx b/src/AIModel/AIFlightPlan.hxx
index c32d88345..5b5586af1 100644
--- a/src/AIModel/AIFlightPlan.hxx
+++ b/src/AIModel/AIFlightPlan.hxx
@@ -19,16 +19,21 @@
 #ifndef _FG_AIFLIGHTPLAN_HXX
 #define _FG_AIFLIGHTPLAN_HXX
 
-#include <simgear/compiler.h>
 #include <vector>
 #include <string>
-#include <simgear/math/SGMath.hxx>
 
+#include <simgear/compiler.h>
+#include <simgear/math/SGMath.hxx>
+#include <simgear/structure/SGSharedPtr.hxx>
+
+// forward decls
 class FGTaxiRoute;
 class FGRunway;
 class FGAIAircraft;
 class FGAirport;
 
+typedef SGSharedPtr<FGAirport> FGAirportRef;
+
 class FGAIWaypoint {
 private:
    std::string name;
@@ -121,7 +126,8 @@ public:
    void setLeadDistance(double distance_ft);
    double getLeadDistance( void ) const {return lead_distance;}
    double getBearing(FGAIWaypoint* previous, FGAIWaypoint* next) const;
-   double getBearing(double lat, double lon, FGAIWaypoint* next) const;
+   double getBearing(const SGGeod& aPos, FGAIWaypoint* next) const;
+  
    double checkTrackLength(std::string wptName);
   time_t getStartTime() const { return start_time; }
    time_t getArrivalTime() const { return arrivalTime; }
@@ -187,9 +193,10 @@ private:
   FGTaxiRoute *taxiRoute;
   std::string name;
   bool isValid;
-
+  FGAirportRef departure, arrival;
+  
   void createPushBackFallBack(FGAIAircraft *, bool, FGAirport*, double radius, const std::string&, const std::string&, const std::string&);
-  bool createClimb(FGAIAircraft *, bool, FGAirport *, double, double, const std::string&);
+  bool createClimb(FGAIAircraft *, bool, FGAirport *, FGAirport* arrival, double, double, const std::string&);
   bool createCruise(FGAIAircraft *, bool, FGAirport*, FGAirport*, double, double, double, double, const std::string&);
   bool createDescent(FGAIAircraft *, FGAirport *,  double latitude, double longitude, double speed, double alt,const std::string&, double distance);
   bool createLanding(FGAIAircraft *, FGAirport *, const std::string&);
@@ -214,6 +221,28 @@ private:
 
   //void createCruiseFallback(bool, FGAirport*, FGAirport*, double, double, double, double);
  void evaluateRoutePart(double deplat, double deplon, double arrlat, double arrlon);
+  
+  /**
+   * look for and parse an PropertyList flight-plan file - essentially
+   * a flat list waypoint objects, encoded to properties
+   */
+  bool parseProperties(const std::string& filename);
+  
+  void createWaypoints(FGAIAircraft *ac,
+                       double course,
+                       time_t start,
+                       FGAirport *dep,
+                       FGAirport *arr,
+                       bool firstLeg,
+                       double radius,
+                       double alt,
+                       double lat,
+                       double lon,
+                       double speed,
+                       const std::string& fltType,
+                       const std::string& acType,
+                       const std::string& airline);
+  
  public:
   wpt_vector_iterator getFirstWayPoint() { return waypoints.begin(); };
   wpt_vector_iterator getLastWayPoint()  { return waypoints.end(); };
diff --git a/src/AIModel/AIFlightPlanCreate.cxx b/src/AIModel/AIFlightPlanCreate.cxx
index 47026b60e..795aceadb 100644
--- a/src/AIModel/AIFlightPlanCreate.cxx
+++ b/src/AIModel/AIFlightPlanCreate.cxx
@@ -38,6 +38,7 @@
 #include <Environment/environment_mgr.hxx>
 #include <Environment/environment.hxx>
 #include <FDM/LaRCsim/basic_aero.h>
+#include <Navaids/navrecord.hxx>
 
 
 /* FGAIFlightPlan::create()
@@ -78,7 +79,7 @@ bool FGAIFlightPlan::create(FGAIAircraft * ac, FGAirport * dep,
         retVal = createTakeOff(ac, firstFlight, dep, speed, fltType);
         break;
     case 4:
-        retVal = createClimb(ac, firstFlight, dep, speed, alt, fltType);
+        retVal = createClimb(ac, firstFlight, dep, arr, speed, alt, fltType);
         break;
     case 5:
         retVal = createCruise(ac, firstFlight, dep, arr, latitude, longitude, speed,
@@ -460,20 +461,18 @@ bool FGAIFlightPlan::createTakeOff(FGAIAircraft * ac, bool firstFlight,
                                    const string & fltType)
 {
     const double ACCEL_POINT = 105.0;
-    const double KNOTS_HOUR_TO_MSEC = SG_NM_TO_METER / 3600.0;
   // climb-out angle in degrees. could move this to the perf-db but this
   // value is pretty sane
-    const double INITIAL_PITCH_ANGLE = 12.5;
+    const double INITIAL_PITCH_ANGLE = 10.0;
   
     double accel = ac->getPerformance()->acceleration();
     double vTaxi = ac->getPerformance()->vTaxi();
     double vRotate = ac->getPerformance()->vRotate();
     double vTakeoff = ac->getPerformance()->vTakeoff();
 
-    double accelMetric = accel * KNOTS_HOUR_TO_MSEC;
-    double vTaxiMetric = vTaxi * KNOTS_HOUR_TO_MSEC;
-    double vRotateMetric = vRotate * KNOTS_HOUR_TO_MSEC;
-    double vTakeoffMetric = vTakeoff * KNOTS_HOUR_TO_MSEC;
+    double accelMetric = accel * SG_KT_TO_MPS;
+    double vTaxiMetric = vTaxi * SG_KT_TO_MPS;
+    double vRotateMetric = vRotate * SG_KT_TO_MPS;
    
     FGAIWaypoint *wpt;
     // Get the current active runway, based on code from David Luff
@@ -498,7 +497,8 @@ bool FGAIFlightPlan::createTakeOff(FGAIAircraft * ac, bool firstFlight,
     wpt = createOnGround(ac, "rotate", accelPoint, airportElev, vTakeoff);
     pushBackWaypoint(wpt);
 
-    double vRef = vTakeoffMetric + 20; // climb-out at v2 + 20kts
+    double vRef = vTakeoff + 20; // climb-out at v2 + 20kts
+  
     double gearUpDist = d + pitchDistance(INITIAL_PITCH_ANGLE, 400 * SG_FEET_TO_METER);
     accelPoint = rwy->pointOnCenterline(gearUpDist);
     
@@ -509,14 +509,22 @@ bool FGAIFlightPlan::createTakeOff(FGAIAircraft * ac, bool firstFlight,
     wpt->setGear_down(false);
     pushBackWaypoint(wpt);
   
+  // limit climbout speed to 240kts below 10000'
+    double vClimbBelow10000 = std::min(240.0, ac->getPerformance()->vClimb());
+  
+  // create two climb-out points. This is important becuase the first climb point will
+  // be a (sometimes large) turn towards the destination, and we don't want to
+  // commence that turn below 2000'
     double climbOut = d + pitchDistance(INITIAL_PITCH_ANGLE, 2000 * SG_FEET_TO_METER);
     accelPoint = rwy->pointOnCenterline(climbOut);
-    wpt = createInAir(ac, "2000'", accelPoint, airportElev + 2000, vRef);
+    wpt = createInAir(ac, "2000'", accelPoint, airportElev + 2000, vClimbBelow10000);
+    pushBackWaypoint(wpt);
+  
+    climbOut = d + pitchDistance(INITIAL_PITCH_ANGLE, 2500 * SG_FEET_TO_METER);
+    accelPoint = rwy->pointOnCenterline(climbOut);
+    wpt = createInAir(ac, "2500'", accelPoint, airportElev + 2500, vClimbBelow10000);
     pushBackWaypoint(wpt);
 
-  // as soon as we pass 2000', hand off to departure so the next acft can line up
-  // ideally the next aircraft would be able to line-up + hold but that's tricky
-  // with the current design.
     return true;
 }
 
@@ -525,14 +533,14 @@ bool FGAIFlightPlan::createTakeOff(FGAIAircraft * ac, bool firstFlight,
  * initialize the Aircraft at the parking location
  ******************************************************************/
 bool FGAIFlightPlan::createClimb(FGAIAircraft * ac, bool firstFlight,
-                                 FGAirport * apt, double speed, double alt,
+                                 FGAirport * apt, FGAirport* arrival,
+                                 double speed, double alt,
                                  const string & fltType)
 {
     FGAIWaypoint *wpt;
-//  bool planLoaded = false;
-    string fPLName;
+  //  string fPLName;
     double vClimb = ac->getPerformance()->vClimb();
-
+  
     if (firstFlight) {
         string rwyClass = getRunwayClassFromTrafficType(fltType);
         double heading = ac->getTrafficRef()->getCourse();
@@ -546,18 +554,23 @@ bool FGAIFlightPlan::createClimb(FGAIAircraft * ac, bool firstFlight,
             //cerr << " Cloning waypoint " << endl;
         }
     } else {
-        FGRunway * rwy = apt->getRunwayByIdent(activeRunway);
-        assert( rwy != NULL );
-
-        SGGeod climb1 = rwy->pointOnCenterline(10 * SG_NM_TO_METER);
+        FGRunway* runway = apt->getRunwayByIdent(activeRunway);
+        SGGeod cur = runway->end();
+        if (!waypoints.empty()) {
+          cur = waypoints.back()->getPos();
+        }
+      
+      // compute course towards destination
+        double course = SGGeodesy::courseDeg(cur, arrival->geod());
+      
+        SGGeod climb1 = SGGeodesy::direct(cur, course, 10 * SG_NM_TO_METER);
         wpt = createInAir(ac, "10000ft climb", climb1, 10000, vClimb);
         wpt->setGear_down(true);
         wpt->setFlaps_down(true);
         pushBackWaypoint(wpt);
 
-        SGGeod climb2 = rwy->pointOnCenterline(20 * SG_NM_TO_METER);
-        wpt = cloneWithPos(ac, wpt, "18000ft climb", climb2);
-        wpt->setAltitude(18000);
+        SGGeod climb2 = SGGeodesy::direct(cur, course, 20 * SG_NM_TO_METER);
+        wpt = createInAir(ac, "18000ft climb", climb2, 18000, vClimb);
         pushBackWaypoint(wpt);
     }
     return true;
@@ -580,8 +593,6 @@ bool FGAIFlightPlan::createDescent(FGAIAircraft * ac, FGAirport * apt,
     FGAIWaypoint *wpt;
     double vDescent = ac->getPerformance()->vDescent();
     double vApproach = ac->getPerformance()->vApproach();
-    double vTouchdown = ac->getPerformance()->vTouchdown();
-
 
     //Beginning of Descent 
     string rwyClass = getRunwayClassFromTrafficType(fltType);
@@ -827,34 +838,7 @@ bool FGAIFlightPlan::createDescent(FGAIAircraft * ac, FGAirport * apt,
 
     // The approach leg should bring the aircraft to approximately 4-6 nm out, after which the landing phase should take over. 
     //cerr << "Phase 3: Approach" << endl;
-    double tgt_speed = vApproach;
-    distanceOut -= distanceCovered;
-    double touchDownPoint = 0; //(rwy->lengthM() * 0.1);
-    for (int i = 1; i < nPoints; i++) {
-        SGGeod result;
-        double currentDist = i * (distanceOut / nPoints);
-        //double currentAltitude =
-        //    apt->getElevation() + 2000 - (i * 2000 / (nPoints-1));
-        double alt = currentAltitude -  (i * 2000 / (nPoints - 1));
-        snprintf(buffer, 16, "final%03d", i);
-        result = rwy->pointOnCenterline((-distanceOut) + currentDist + touchDownPoint);
-        if (i == nPoints - 30) {
-            tgt_speed = vTouchdown;
-        }
-        wpt = createInAir(ac, buffer, result, alt, tgt_speed);
-        wpt->setCrossat(alt);
-        wpt->setTrackLength((distanceOut / nPoints));
-        // account for the extra distance due to an extended downwind leg
-        if (i == 1) {
-            wpt->setTrackLength(wpt->getTrackLength() + distanceCovered);
-        }
-        //cerr << "Track Length : " << wpt->trackLength;
-        pushBackWaypoint(wpt);
-        //if (apt->ident() == fgGetString("/sim/presets/airport-id")) {
-        //    cerr << "  Position : " << result.getLatitudeDeg() << " " << result.getLongitudeDeg() << " " << currentAltitude << " " << apt->getElevation() << " " << distanceOut << endl;
-        //}
-    }
-
+    
     //cerr << "Done" << endl;
 
     // Erase the two bogus BOD points: Note check for conflicts with scripted AI flightPlans
@@ -885,10 +869,31 @@ bool FGAIFlightPlan::createDescent(FGAIAircraft * ac, FGAirport * apt,
         ac->resetPositionFromFlightPlan();
     }
     waypoints[1]->setName( (waypoints[1]->getName() + string("legend"))); 
-    waypoints.back()->setName(waypoints.back()->getName() + "LandingThreshold");
     return true;
 }
 
+/**
+ * compute the distance along the centerline, to the ILS glideslope
+ * transmitter. Return -1 if there's no GS for the runway
+ */
+static double runwayGlideslopeTouchdownDistance(FGRunway* rwy)
+{
+  FGNavRecord* gs = rwy->glideslope();
+  if (!gs) {
+    return -1;
+  }
+  
+  SGVec3d runwayPosCart = SGVec3d::fromGeod(rwy->pointOnCenterline(0.0));
+  // compute a unit vector in ECF cartesian space, from the runway beginning to the end
+  SGVec3d runwayDirectionVec = normalize(SGVec3d::fromGeod(rwy->end()) - runwayPosCart);
+  SGVec3d gsTransmitterVec = gs->cart() - runwayPosCart;
+  
+// project the gsTransmitterVec along the runwayDirctionVec to get out
+// final value (in metres)
+  double dist = dot(runwayDirectionVec, gsTransmitterVec);
+  return dist;
+}
+
 /*******************************************************************
  * CreateLanding
  * Create a flight path from the "permision to land" point (currently
@@ -902,108 +907,81 @@ bool FGAIFlightPlan::createLanding(FGAIAircraft * ac, FGAirport * apt,
     double vTouchdown = ac->getPerformance()->vTouchdown();
     double vTaxi      = ac->getPerformance()->vTaxi();
     double decel     = ac->getPerformance()->deceleration() * 1.4;
-    
+    double vApproach = ac->getPerformance()->vApproach();
+  
     double vTouchdownMetric = (vTouchdown  * SG_NM_TO_METER) / 3600;
     double vTaxiMetric      = (vTaxi       * SG_NM_TO_METER) / 3600;
     double decelMetric      = (decel       * SG_NM_TO_METER) / 3600;
 
-    //string rwyClass = getRunwayClassFromTrafficType(fltType);
-    //double heading = ac->getTrafficRef()->getCourse();
-    //apt->getDynamics()->getActiveRunway(rwyClass, 2, activeRunway, heading);
-    //rwy = apt->getRunwayByIdent(activeRunway);
-
-
-    FGAIWaypoint *wpt;
-    //double aptElev = apt->getElevation();
-    double currElev = 0;
     char buffer[12];
     FGRunway * rwy = apt->getRunwayByIdent(activeRunway);
     assert( rwy != NULL );
-    SGGeod refPoint = rwy->pointOnCenterline(0);
-    FGTaxiNode *tn = 0;
-    if (apt->getDynamics()->getGroundNetwork()) {
-        int node = apt->getDynamics()->getGroundNetwork()->findNearestNode(refPoint);
-        tn = apt->getDynamics()->getGroundNetwork()->findNode(node);
+    SGGeod threshold = rwy->threshold();
+    double currElev = threshold.getElevationFt();
+  
+    double touchdownDistance = runwayGlideslopeTouchdownDistance(rwy);
+    if (touchdownDistance < 0.0) {
+      double landingLength = rwy->lengthM() - (rwy->displacedThresholdM());
+      // touchdown 25% of the way along the landing area
+      touchdownDistance = rwy->displacedThresholdM() + (landingLength * 0.25);
     }
-    if (tn) {
-        currElev = tn->getElevationFt(apt->getElevation());
-    } else {
-        currElev = apt->getElevation();
-    }
-
-
+  
     SGGeod coord;
-    
-
-    /*double distanceOut = rwy->lengthM() * .1;
-    double nPoints = 20;
-    for (int i = 1; i < nPoints; i++) {
-        snprintf(buffer, 12, "flare%d", i);
-        double currentDist = i * (distanceOut / nPoints);
-        double currentAltitude = apt->getElevation() + 20 - (i * 20 / nPoints);
-        coord = rwy->pointOnCenterline((currentDist * (i / nPoints)));
-        wpt = createInAir(ac, buffer, coord, currentAltitude, (vTouchdown));
-    }*/
-    double rolloutDistance =
-        (vTouchdownMetric * vTouchdownMetric - vTaxiMetric * vTaxiMetric) / (2 * decelMetric);
-    //cerr << " touchdown speed = " << vTouchdown << ". Rollout distance " << rolloutDistance << endl;
+  // find glideslope entry point, 2000' above touchdown elevation
+    double glideslopeEntry = -((2000 * SG_FEET_TO_METER) / tan(3.0)) + touchdownDistance;
+    FGAIWaypoint *wpt = createInAir(ac, "Glideslope begin", rwy->pointOnCenterline(glideslopeEntry),
+                                  currElev + 2000, vApproach);
+    pushBackWaypoint(wpt);
+  
+  // deceleration point, 500' above touchdown elevation - slow from approach speed
+  // to touchdown speed
+    double decelPoint = -((500 * SG_FEET_TO_METER) / tan(3.0)) + touchdownDistance;
+    wpt = createInAir(ac, "500' decel", rwy->pointOnCenterline(decelPoint),
+                                  currElev + 2000, vTouchdown);
+    pushBackWaypoint(wpt);
+  
+  // compute elevation above the runway start, based on a 3-degree glideslope
+    double heightAboveRunwayStart = touchdownDistance *
+      tan(3.0 * SG_DEGREES_TO_RADIANS) * SG_METER_TO_FEET;
+    wpt = createInAir(ac, "CrossThreshold", rwy->begin(),
+                      heightAboveRunwayStart + currElev, vTouchdown);
+    pushBackWaypoint(wpt);
+  
+    double rolloutDistance = accelDistance(vTouchdownMetric, vTaxiMetric, decelMetric);
+  
     int nPoints = 50;
     for (int i = 1; i < nPoints; i++) {
         snprintf(buffer, 12, "landing03%d", i);
-        
-        coord = rwy->pointOnCenterline((rolloutDistance * ((double) i / (double) nPoints)));
-        wpt = createOnGround(ac, buffer, coord, currElev, 2*vTaxi);
+        double t = ((double) i) / nPoints;
+        coord = rwy->pointOnCenterline(touchdownDistance + (rolloutDistance * t));
+        double vel = (vTouchdownMetric * (1.0 - t)) + (vTaxiMetric * t);
+        wpt = createOnGround(ac, buffer, coord, currElev, vel);
         wpt->setCrossat(currElev);
         pushBackWaypoint(wpt);
     }
+  
     wpt->setSpeed(vTaxi);
-    double mindist = 1.1 * rolloutDistance;
-    double maxdist = rwy->lengthM();
-    //cerr << "Finding nearest exit" << endl;
+    double mindist = (1.1 * rolloutDistance) + touchdownDistance;
+
     FGGroundNetwork *gn = apt->getDynamics()->getGroundNetwork();
-    if (gn) {
-        double min = 0;
-        for (int i = ceil(mindist); i < floor(maxdist); i++) {
-            coord = rwy->pointOnCenterline(mindist);
-            int nodeId = 0;
-            if (gn->getVersion() > 0) {
-                nodeId = gn->findNearestNodeOnRunway(coord);
-            } else {
-                nodeId = gn->findNearestNode(coord);
-            }
-            if (tn)
-                tn = gn->findNode(nodeId);
-            if (!tn)
-                break;
-            
-            double dist = SGGeodesy::distanceM(coord, tn->geod());
-            if (dist < (min + 0.75)) {
-                break;
-            }
-            min = dist;
-        }
-        if (tn) {
-            wpt = createOnGround(ac, buffer, tn->geod(), currElev, vTaxi);
-            pushBackWaypoint(wpt);
-        }
+    if (!gn) {
+      return true;
+    }
+  
+    coord = rwy->pointOnCenterline(mindist);
+    int nodeId = 0;
+    if (gn->getVersion() > 0) {
+        nodeId = gn->findNearestNodeOnRunway(coord, rwy);
+    } else {
+        nodeId = gn->findNearestNode(coord);
+    }
+      
+    FGTaxiNode* tn = gn->findNode(nodeId);
+    if (tn) {
+        wpt = createOnGround(ac, buffer, tn->geod(), currElev, vTaxi);
+        pushBackWaypoint(wpt);
     }
-    //cerr << "Done. " << endl;
 
-    /*
-       //Runway Threshold
-       wpt = createOnGround(ac, "Threshold", rwy->threshold(), aptElev, vTouchdown);
-       wpt->crossat = apt->getElevation();
-       pushBackWaypoint(wpt); 
-
-       // Roll-out
-       wpt = createOnGround(ac, "Center", rwy->geod(), aptElev, vTaxi*2);
-       pushBackWaypoint(wpt);
-
-       SGGeod rollOut = rwy->pointOnCenterline(rwy->lengthM() * 0.9);
-       wpt = createOnGround(ac, "Roll Out", rollOut, aptElev, vTaxi);
-       wpt->crossat   = apt->getElevation();
-       pushBackWaypoint(wpt); 
-     */
     return true;
 }
 
diff --git a/src/Airports/groundnetwork.cxx b/src/Airports/groundnetwork.cxx
index c992f1e7b..3c65c8e48 100644
--- a/src/Airports/groundnetwork.cxx
+++ b/src/Airports/groundnetwork.cxx
@@ -43,6 +43,7 @@
 
 #include <Airports/simple.hxx>
 #include <Airports/dynamics.hxx>
+#include <Airports/runways.hxx>
 
 #include <AIModel/AIAircraft.hxx>
 #include <AIModel/performancedata.hxx>
@@ -455,7 +456,7 @@ int FGGroundNetwork::findNearestNode(const SGGeod & aGeod)
     return index;
 }
 
-int FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod)
+int FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod, FGRunway* aRunway)
 {
     double minDist = HUGE_VAL;
     int index = -1;
@@ -465,6 +466,16 @@ int FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod)
         if (!i->second->getIsOnRunway()) {
             continue;
         }
+      // check point lies on the runway - i.e that course from aGeod to the
+      // runway end, matches the runway heading
+        if (aRunway) {
+          double course = SGGeodesy::courseDeg(i->second->geod(), aRunway->end());
+          double headingDiff = course - aRunway->headingDeg();
+          SG_NORMALIZE_RANGE(headingDiff, -180.0, 180.0);
+          if (fabs(headingDiff) > 3.0) { // 3 degrees tolerance
+            continue;
+          }
+        }
       
         double d = SGGeodesy::distanceM(aGeod, i->second->geod());
         if (d < minDist) {
diff --git a/src/Airports/groundnetwork.hxx b/src/Airports/groundnetwork.hxx
index 35fd03c0d..9b73e2e81 100644
--- a/src/Airports/groundnetwork.hxx
+++ b/src/Airports/groundnetwork.hxx
@@ -31,13 +31,13 @@
 #include <list>
 #include <map>
 
-class Block;
-
 #include "gnnode.hxx"
 #include "parking.hxx"
 #include <ATC/trafficcontrol.hxx>
 
 
+class Block;
+class FGRunway;
 class FGTaxiSegment; // forward reference
 class FGAIFlightPlan; // forward reference
 class FGAirport;      // forward reference
@@ -275,7 +275,7 @@ public:
     };
 
     int findNearestNode(const SGGeod& aGeod);
-    int findNearestNodeOnRunway(const SGGeod& aGeod);
+    int findNearestNodeOnRunway(const SGGeod& aGeod, FGRunway* aRunway = NULL);
 
     FGTaxiNode *findNode(unsigned idx);
     FGTaxiSegment *findSegment(unsigned idx);
diff --git a/src/Airports/runways.cxx b/src/Airports/runways.cxx
index d4b319e75..70a2ee1b8 100644
--- a/src/Airports/runways.cxx
+++ b/src/Airports/runways.cxx
@@ -149,6 +149,17 @@ FGNavRecord* FGRunway::ILS() const
   return (FGNavRecord*) flightgear::NavDataCache::instance()->loadById(_ils);
 }
 
+FGNavRecord* FGRunway::glideslope() const
+{
+  flightgear::NavDataCache* cache = flightgear::NavDataCache::instance();
+  PositionedID gsId = cache->findNavaidForRunway(guid(), FGPositioned::GS);
+  if (gsId == 0) {
+    return NULL;
+  }
+  
+  return (FGNavRecord*) cache->loadById(gsId);
+}
+
 std::vector<flightgear::SID*> FGRunway::getSIDs() const
 {
   FGAirport* apt = airport();
diff --git a/src/Airports/runways.hxx b/src/Airports/runways.hxx
index d824aa3e3..36f342c00 100644
--- a/src/Airports/runways.hxx
+++ b/src/Airports/runways.hxx
@@ -108,6 +108,11 @@ public:
   
   FGNavRecord* ILS() const;
   
+  /**
+   * retrieve the associated glideslope transmitter, if one is defined.
+   */
+  FGNavRecord* glideslope() const;
+  
   void setILS(PositionedID nav) { _ils = nav; }
   
   FGRunway* reciprocalRunway() const;
diff --git a/src/Navaids/NavDataCache.cxx b/src/Navaids/NavDataCache.cxx
index d39f438a5..5d228c4bc 100644
--- a/src/Navaids/NavDataCache.cxx
+++ b/src/Navaids/NavDataCache.cxx
@@ -528,6 +528,9 @@ public:
     findNavsByFreqNoPos = prepare("SELECT positioned.rowid FROM positioned, navaid WHERE "
                                   "positioned.rowid=navaid.rowid AND freq=?1 " AND_TYPED);
     
+    findNavaidForRunway = prepare("SELECT positioned.rowid FROM positioned, navaid WHERE "
+                                  "positioned.rowid=navaid.rowid AND runway=?1 AND type=?2");
+    
   // for an octree branch, return the child octree nodes which exist,
   // described as a bit-mask
     getOctreeChildren = prepare("SELECT children FROM octree WHERE rowid=?1");
@@ -808,7 +811,7 @@ public:
 
   sqlite3_stmt_ptr searchAirports;
   sqlite3_stmt_ptr findCommByFreq, findNavsByFreq,
-  findNavsByFreqNoPos;
+  findNavsByFreqNoPos, findNavaidForRunway;
   sqlite3_stmt_ptr getAirportItems, getAirportItemByIdent;
   sqlite3_stmt_ptr findAirportRunway,
     findILS;
@@ -1707,6 +1710,18 @@ AirwayEdgeVec NavDataCache::airwayEdgesFrom(int network, PositionedID pos)
   }
   return result;
 }
+
+PositionedID NavDataCache::findNavaidForRunway(PositionedID runway, FGPositioned::Type ty)
+{
+  d->reset(d->findNavaidForRunway);
+  sqlite3_bind_int64(d->findNavaidForRunway, 1, runway);
+  sqlite3_bind_int(d->findNavaidForRunway, 2, ty);
+  if (!d->execSelect(d->findNavaidForRunway)) {
+    return 0;
+  }
+  
+  return sqlite3_column_int64(d->findNavaidForRunway, 0);
+}
   
 } // of namespace flightgear
 
diff --git a/src/Navaids/NavDataCache.hxx b/src/Navaids/NavDataCache.hxx
index e7909818c..4080a608e 100644
--- a/src/Navaids/NavDataCache.hxx
+++ b/src/Navaids/NavDataCache.hxx
@@ -140,6 +140,11 @@ public:
   /// returning results. Only used by TACAN carrier search
   PositionedIDVec findNavaidsByFreq(int freqKhz, FGPositioned::Filter* filt);
   
+  /**
+   * Given a runway and type, find the corresponding navaid (ILS / GS / OM)
+   */
+  PositionedID findNavaidForRunway(PositionedID runway, FGPositioned::Type ty);
+  
   /**
    * given a navaid name (or similar) from apt.dat / nav.dat, find the
    * corresponding airport and runway IDs.
diff --git a/src/Traffic/TrafficMgr.cxx b/src/Traffic/TrafficMgr.cxx
index 464dca5e0..acc9fa90c 100644
--- a/src/Traffic/TrafficMgr.cxx
+++ b/src/Traffic/TrafficMgr.cxx
@@ -200,15 +200,15 @@ void FGTrafficManager::shutdown()
             cachefile << "[TrafficManagerCachedata:ref:2011:09:04]" << endl;
         }
     }
-    for (ScheduleVectorIterator sched = scheduledAircraft.begin();
-         sched != scheduledAircraft.end(); sched++) {
+  
+    BOOST_FOREACH(FGAISchedule* acft, scheduledAircraft) {
         if (saveData) {
-            cachefile << (*sched)->getRegistration() << " "
-                << (*sched)->getRunCount() << " "
-                << (*sched)->getHits() << " "
-                << (*sched)->getLastUsed() << endl;
+            cachefile << acft->getRegistration() << " "
+                << acft->getRunCount() << " "
+                << acft->getHits() << " "
+                << acft->getLastUsed() << endl;
         }
-        delete(*sched);
+        delete acft;
     }
     if (saveData) {
         cachefile.close();
@@ -271,10 +271,9 @@ void FGTrafficManager::finishInit()
     SG_LOG(SG_AI, SG_INFO, "finishing AI-Traffic init");
     loadHeuristics();
     
-    // Do sorting and scoring separately, to take advantage of the "homeport| variable
-    for (currAircraft = scheduledAircraft.begin();
-         currAircraft != scheduledAircraft.end(); currAircraft++) {
-        (*currAircraft)->setScore();
+    // Do sorting and scoring separately, to take advantage of the "homeport" variable
+    BOOST_FOREACH(FGAISchedule* schedule, scheduledAircraft) {
+        schedule->setScore();
     }
     
     sort(scheduledAircraft.begin(), scheduledAircraft.end(),
@@ -372,7 +371,7 @@ void FGTrafficManager::update(double /*dt */ )
     }
         
     time_t now = time(NULL) + fgGetLong("/sim/time/warp");
-    if (scheduledAircraft.size() == 0) {
+    if (scheduledAircraft.empty()) {
         return;
     }