Several Bugfixes:

* Landing AIAircraft now land properly!!!
 * Fixed problems with takeoff scheduling.
 * Untangled several vertical speed calculation algorithms.
 * No more bent-over-backwards and jump-in-the-air AI aircraft initializations.
 * Fixed intermixed target speed and target altitude during climb phase.
 * Implemented beginnings of a "line up and wait" procedure.
 * Tried to use actual runway elevation for calculating the decent profile.

src/AIModel/AIAircraft.cxx

@@ -1,4 +1,4 @@
-// // FGAIAircraft - FGAIBase-derived class creates an AI airplane
+// FGAIAircraft - FGAIBase-derived class creates an AI airplane
 //
 // Written by David Culp, started October 2003.
 //
@@ -47,11 +47,10 @@ using std::string;
 #include "AIAircraft.hxx"
 #include "performancedata.hxx"
 #include "performancedb.hxx"
+#include <signal.h>
 
 //#include <Airports/trafficcontroller.hxx>
 
-static string tempReg;
-
 FGAIAircraft::FGAIAircraft(FGAISchedule *ref) :
      /* HOT must be disabled for AI Aircraft,
       * otherwise traffic detection isn't working as expected.*/
@@ -191,6 +190,7 @@ void FGAIAircraft::checkVisibility()
 
 void FGAIAircraft::AccelTo(double speed) {
     tgt_speed = speed;
+    assertSpeed(speed);
     if (!isStationary())
         _needsGroundElevation = true;
 }
@@ -287,6 +287,7 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
     if (! leadPointReached(curr)) {
         controlHeading(curr);
         controlSpeed(curr, next);
+        
             /*
             if (speed < 0) { 
                 cerr << getCallSign() 
@@ -350,13 +351,18 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
             fp->setLeadDistance(tgt_speed, tgt_heading, curr, next);
         }
 
+
         if (!(prev->getOn_ground()))  // only update the tgt altitude from flightplan if not on the ground
         {
             tgt_altitude_ft = prev->getAltitude();
             if (curr->getCrossat() > -1000.0) {
                 use_perf_vs = false;
-                tgt_vs = (curr->getCrossat() - altitude_ft) / (fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr)
+                // Distance to go in meters
-                         / 6076.0 / speed*60.0);
+                double vert_dist_ft = curr->getCrossat() - altitude_ft;
+                double err_dist     = prev->getCrossat() - altitude_ft;
+                double dist_m       = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr);
+                tgt_vs = calcVerticalSpeed(vert_dist_ft, dist_m, speed, err_dist);
+                
                 checkTcas();
                 tgt_altitude_ft = curr->getCrossat();
             } else {
@@ -369,6 +375,49 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
     }
 }
 
+double FGAIAircraft::calcVerticalSpeed(double vert_ft, double dist_m, double speed, double err)
+{
+    // err is negative when we passed too high
+    double vert_m = vert_ft * SG_FEET_TO_METER;
+    double err_m  = err     * SG_FEET_TO_METER;
+    //double angle = atan2(vert_m, dist_m);
+    double speedMs = (speed * SG_NM_TO_METER) / 3600;
+    //double vs = cos(angle) * speedMs; // Now in m/s
+    double vs = 0;
+    //cerr << "Error term = " << err_m << endl;
+    if (dist_m) {
+        vs = ((vert_m) / dist_m) * speedMs;
+    }
+    // Convert to feet per minute
+    vs *= (SG_METER_TO_FEET * 60);
+    //if (getCallSign() == "LUFTHANSA2002")
+    //if (fabs(vs) > 100000) {
+//     if (getCallSign() == "LUFTHANSA2057") {
+//         cerr << getCallSign() << " " << fp->getPreviousWaypoint()->getName() << ". Alt = " << altitude_ft <<  " vertical dist = " << vert_m << " horiz dist = " << dist_m << " << angle  = " << angle * SG_RADIANS_TO_DEGREES << " vs " << vs << " horizontal speed " << speed << "Previous crossAT " << fp->getPreviousWaypoint()->getCrossat() << endl;
+//     //= (curr->getCrossat() - altitude_ft) / (fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr)
+//     //                     / 6076.0 / speed*60.0);
+//         //raise(SIGSEGV);
+//     }
+    return vs;
+}
+
+void FGAIAircraft::assertSpeed(double speed)
+{
+    if ((speed < -50) || (speed > 1000)) {
+        cerr << getCallSign() << " " 
+            << "Previous waypoint " << fp->getPreviousWaypoint()->getName() << " "
+            << "Departure airport " << trafficRef->getDepartureAirport() << " "
+            << "Leg " << fp->getLeg() <<  " "
+            << "target_speed << " << tgt_speed <<  " "
+            << "speedFraction << " << speedFraction << " "
+            << "Currecnt speed << " << speed << " "
+            << endl;
+       raise(SIGSEGV);
+    }
+}
+
+
+
 void FGAIAircraft::checkTcas(void)
 {
     if (props->getIntValue("tcas/threat-level",0)==3)
@@ -497,12 +546,16 @@ void FGAIAircraft::getGroundElev(double dt) {
 
 
 void FGAIAircraft::doGroundAltitude() {
-    if ((fabs(altitude_ft - (tgt_altitude_ft+groundOffset)) > 1000.0)||
+    if ((fp->getLeg() == 7) && ((altitude_ft -  tgt_altitude_ft) > 5)) {
-        (isStationary()))
+        tgt_vs = -500;
-        altitude_ft = (tgt_altitude_ft + groundOffset);
+    } else {
-    else
+        if ((fabs(altitude_ft - (tgt_altitude_ft+groundOffset)) > 1000.0)||
-        altitude_ft += 0.1 * ((tgt_altitude_ft+groundOffset) - altitude_ft);
+            (isStationary()))
-    tgt_vs = 0;
+            altitude_ft = (tgt_altitude_ft + groundOffset);
+        else
+            altitude_ft += 0.1 * ((tgt_altitude_ft+groundOffset) - altitude_ft);
+        tgt_vs = 0;
+    }
 }
 
 
@@ -527,6 +580,7 @@ void FGAIAircraft::announcePositionToController() {
         case 3:              //Take off tower controller
             if (trafficRef->getDepartureAirport()->getDynamics()) {
                 controller = trafficRef->getDepartureAirport()->getDynamics()->getTowerController();
+                towerController = 0;
             } else {
                 cerr << "Error: Could not find Dynamics at airport : " << trafficRef->getDepartureAirport()->getId() << endl;
             }
@@ -643,7 +697,6 @@ void FGAIAircraft::handleFirstWaypoint() {
     FGAIWaypoint* next = 0;// the next plus 1
 
     spinCounter = 0;
-    tempReg = "";
 
     //TODO fp should handle this
     fp->IncrementWaypoint(eraseWaypoints);
@@ -676,9 +729,13 @@ void FGAIAircraft::handleFirstWaypoint() {
     if (curr->getCrossat() > -1000.0) //use a calculated descent/climb rate
     {
         use_perf_vs = false;
-        tgt_vs = (curr->getCrossat() - prev->getAltitude())
+        //tgt_vs = (curr->getCrossat() - prev->getAltitude())
-                 / (fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr)
+        //         / (fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr)
-                    / 6076.0 / prev->getSpeed()*60.0);
+        //            / 6076.0 / prev->getSpeed()*60.0);
+        double vert_dist_ft = curr->getCrossat() - altitude_ft;
+        double err_dist     = prev->getCrossat() - altitude_ft;
+        double dist_m       = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr);
+        tgt_vs = calcVerticalSpeed(vert_dist_ft, dist_m, speed, err_dist);
         checkTcas();
         tgt_altitude_ft = curr->getCrossat();
     } else {
@@ -730,6 +787,7 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr) {
           if (tgt_speed > -0.5) {
                 tgt_speed = -0.5;
           }
+          assertSpeed(tgt_speed);
           if (fp->getPreviousWaypoint()->getSpeed() < tgt_speed) {
               fp->getPreviousWaypoint()->setSpeed(tgt_speed);
           }
@@ -755,7 +813,7 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr) {
                  minBearing = 10;
             }
             if ((minBearing < 360.0) && (minBearing > 10.0)) {
-                speedFraction = cos(minBearing *SG_DEGREES_TO_RADIANS);
+                speedFraction = 0.5 + (cos(minBearing *SG_DEGREES_TO_RADIANS) * 0.5);
             } else {
                 speedFraction = 1.0;
             }
@@ -770,6 +828,7 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr) {
      }
     if ((dist_to_go < lead_dist) || (bearing > (minBearing * 1.1))) {
         minBearing = 360;
+        speedFraction = 1.0;
         return true;
     } else {
         return false;
@@ -816,9 +875,22 @@ bool FGAIAircraft::handleAirportEndPoints(FGAIWaypoint* prev, time_t now) {
     }
     if (prev->contains(string("DepartureHold"))) {
         //cerr << "Passing point DepartureHold" << endl;
-        scheduleForATCTowerDepartureControl(2);
+        scheduleForATCTowerDepartureControl(1);
     }
+    if (prev->contains(string("Accel"))) {
+        takeOffStatus = 3;
+    }
+    /*if (prev->contains(string("final"))) {
         
+         cerr << getCallSign() << " " 
+            << fp->getPreviousWaypoint()->getName() 
+            << ". Alt = " << altitude_ft 
+            << " vs " << vs 
+            << " horizontal speed " << speed 
+            << "Previous crossAT " << fp->getPreviousWaypoint()->getCrossat()
+            << "Airport elevation" << getTrafficRef()->getArrivalAirport()->getElevation() 
+            << "Altitude difference " << (altitude_ft -  fp->getPreviousWaypoint()->getCrossat()) << endl;
+    }*/
     // This is the last taxi waypoint, and marks the the end of the flight plan
     // so, the schedule should update and wait for the next departure time.
     if (prev->contains("END")) {
@@ -877,6 +949,7 @@ void FGAIAircraft::controlSpeed(FGAIWaypoint* curr, FGAIWaypoint* next) {
 
     if (fabs(speed_diff) > 10) {
         prevSpeed = speed;
+        assertSpeed(speed);
         if (next) {
             fp->setLeadDistance(speed, tgt_heading, curr, next);
         }
@@ -1005,13 +1078,14 @@ void FGAIAircraft::updateHeading() {
             //         << hdg << ". Target " << tgt_heading <<  ". Diff " << fabs(sum - tgt_heading) << ". Speed " << speed << endl;
             //if (headingDiff > 60) {
             groundTargetSpeed = tgt_speed; // * cos(headingDiff * SG_DEGREES_TO_RADIANS);
+            assertSpeed(groundTargetSpeed);
                 //groundTargetSpeed = tgt_speed - tgt_speed * (headingDiff/180);
             //} else {
             //    groundTargetSpeed = tgt_speed;
             //}
             if (sign(groundTargetSpeed) != sign(tgt_speed))
                 groundTargetSpeed = 0.21 * sign(tgt_speed); // to prevent speed getting stuck in 'negative' mode
-            
+            assertSpeed(groundTargetSpeed);
             // Only update the target values when we're not moving because otherwise we might introduce an enormous target change rate while waiting a the gate, or holding.
             if (speed != 0) {
                 if (headingDiff > 30.0) {
@@ -1112,7 +1186,12 @@ void FGAIAircraft::updateVerticalSpeedTarget() {
                     tgt_vs = -_performance->descentRate();
             }
         } else {
-            double max_vs = 4*(tgt_altitude_ft - altitude_ft);
+            double vert_dist_ft = fp->getCurrentWaypoint()->getCrossat() - altitude_ft;
+            double err_dist     = 0; //prev->getCrossat() - altitude_ft;
+            double dist_m       = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), fp->getCurrentWaypoint());
+            tgt_vs = calcVerticalSpeed(vert_dist_ft, dist_m, speed, err_dist);
+            //cerr << "Target vs before : " << tgt_vs;
+/*            double max_vs = 10*(tgt_altitude_ft - altitude_ft);
             double min_vs = 100;
             if (tgt_altitude_ft < altitude_ft)
                 min_vs = -100.0;
@@ -1121,7 +1200,8 @@ void FGAIAircraft::updateVerticalSpeedTarget() {
                 tgt_vs = max_vs;
 
             if (fabs(tgt_vs) < fabs(min_vs))
-                tgt_vs = min_vs;
+                tgt_vs = min_vs;*/
+            //cerr << "target vs : after " << tgt_vs << endl;
         }
     } //else 
     //    tgt_vs = 0.0;
@@ -1168,6 +1248,14 @@ void FGAIAircraft::handleATCRequests() {
                                               altitude_ft, dt);
         processATC(controller->getInstruction(getID()));
     }
+    if (towerController) {
+        towerController->updateAircraftInformation(getID(),
+                                              pos.getLatitudeDeg(),
+                                              pos.getLongitudeDeg(),
+                                              hdg,
+                                              speed,
+                                              altitude_ft, dt);
+    }
 }
 
 void FGAIAircraft::updateActualState() {
@@ -1179,7 +1267,7 @@ void FGAIAircraft::updateActualState() {
         speed = _performance->actualSpeed(this, groundTargetSpeed, dt);
     else
         speed = _performance->actualSpeed(this, (tgt_speed *speedFraction), dt);
-
+    assertSpeed(speed);
     updateHeading();
     roll = _performance->actualBankAngle(this, tgt_roll, dt);
 

src/AIModel/AIAircraft.hxx

@@ -103,6 +103,7 @@ public:
     int getTakeOffStatus() { return takeOffStatus; };
 
     void checkTcas();
+    double calcVerticalSpeed(double vert_ft, double dist_m, double speed, double error);
 
     FGATCController * getATCController() { return controller; };
     
@@ -181,6 +182,8 @@ private:
     time_t timeElapsed;
 
     PerformanceData* _performance; // the performance data for this aircraft
+    
+   void assertSpeed(double speed);
 };
 
 

src/AIModel/AIFlightPlan.cxx

@@ -223,12 +223,12 @@ FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac,
       time_t timeDiff = now-start; 
       leg = 1;
       
-      if ((timeDiff > 60) && (timeDiff < 1200))
+      if ((timeDiff > 60) && (timeDiff < 1500))
 	leg = 2;
-      else if ((timeDiff >= 1200) && (timeDiff < 1500)) {
+      //else if ((timeDiff >= 1200) && (timeDiff < 1500)) {
-	leg = 3;
+	//leg = 3;
-        ac->setTakeOffStatus(2);
+        //ac->setTakeOffStatus(2);
-      }
+      //}
       else if ((timeDiff >= 1500) && (timeDiff < 2000))
 	leg = 4;
       else if (timeDiff >= 2000)

src/AIModel/AIFlightPlanCreate.cxx

@@ -316,6 +316,9 @@ bool FGAIFlightPlan::createTakeoffTaxi(FGAIAircraft * ac, bool firstFlight,
             //cerr << "Setting departurehold point: " << endl;
             wpt->setName( wpt->getName() + string("DepartureHold"));
         }
+        if (taxiRoute->nodesLeft() == 0) {
+            wpt->setName(wpt->getName() + string("Accel"));
+        }
         waypoints.push_back(wpt);
     }
     // Acceleration point, 105 meters into the runway,
@@ -530,7 +533,7 @@ bool FGAIFlightPlan::createClimb(FGAIAircraft * ac, bool firstFlight,
         assert( rwy != NULL );
 
         SGGeod climb1 = rwy->pointOnCenterline(10 * SG_NM_TO_METER);
-        wpt = createInAir(ac, "10000ft climb", climb1, vClimb, 10000);
+        wpt = createInAir(ac, "10000ft climb", climb1, 10000, vClimb);
         wpt->setGear_down(true);
         wpt->setFlaps_down(true);
         waypoints.push_back(wpt);
@@ -607,7 +610,17 @@ bool FGAIFlightPlan::createDescent(FGAIAircraft * ac, FGAirport * apt,
     }
 
 
-    double dAlt = alt - (apt->getElevation() + 2000);
+    double dAlt = 0; //  = alt - (apt->getElevation() + 2000);
+    FGTaxiNode * tn = 0;
+    if (apt->getDynamics()->getGroundNetwork()) {
+        int node = apt->getDynamics()->getGroundNetwork()->findNearestNode(refPoint);
+        tn = apt->getDynamics()->getGroundNetwork()->findNode(node);
+    }
+    if (tn) {
+        dAlt = alt - ((tn->getElevationFt(apt->getElevation())) + 2000);
+    } else {
+        dAlt = alt - (apt->getElevation() + 2000);
+    }
 
     double nPoints = 100;
 
@@ -769,10 +782,19 @@ bool FGAIFlightPlan::createDescent(FGAIAircraft * ac, FGAirport * apt,
     }
 
     //cerr << "creating circle between " << startval << " and " << endval << " using " << increment << endl;
+    //FGTaxiNode * tn = apt->getDynamics()->getGroundNetwork()->findNearestNode(initialTarget);
+    double currentAltitude = 0;
+    if (tn) {
+        currentAltitude = (tn->getElevationFt(apt->getElevation())) + 2000;
+    } else {
+        currentAltitude = apt->getElevation() + 2000;
+    }
+        
     double trackLength = (2 * M_PI * initialTurnRadius) / 360.0;
     for (int i = startval; i != endval; i += increment) {
         SGGeod result;
-        double currentAltitude = apt->getElevation() + 2000;
+        //double currentAltitude = apt->getElevation() + 2000;
+        
         SGGeodesy::direct(secondaryTarget, i,
                           initialTurnRadius, result, dummyAz2);
         snprintf(buffer, 16, "turn%03d", i);
@@ -785,18 +807,20 @@ bool FGAIFlightPlan::createDescent(FGAIAircraft * ac, FGAirport * apt,
     }
 
 
-    // The approach leg should bring the aircraft to approximately 4-6 out, after which the landing phase should take over. 
+    // 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;
     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 =
+        //double currentAltitude =
-            apt->getElevation() + 2000 - (i * 2000 / nPoints);
+        //    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);
+        result = rwy->pointOnCenterline((-distanceOut) + currentDist + touchDownPoint);
-        wpt = createInAir(ac, buffer, result, currentAltitude, vApproach);
+        wpt = createInAir(ac, buffer, result, alt, vApproach);
-        wpt->setCrossat(currentAltitude);
+        wpt->setCrossat(alt);
         wpt->setTrackLength((distanceOut / nPoints));
         // account for the extra distance due to an extended downwind leg
         if (i == 1) {
@@ -804,7 +828,9 @@ bool FGAIFlightPlan::createDescent(FGAIAircraft * ac, FGAirport * apt,
         }
         //cerr << "Track Length : " << wpt->trackLength;
         waypoints.push_back(wpt);
-        //cerr << "  Position : " << result.getLatitudeDeg() << " " << result.getLongitudeDeg() << " " << currentAltitude << endl;
+        //if (apt->ident() == fgGetString("/sim/presets/airport-id")) {
+        //    cerr << "  Position : " << result.getLatitudeDeg() << " " << result.getLongitudeDeg() << " " << currentAltitude << " " << apt->getElevation() << " " << distanceOut << endl;
+        //}
     }
 
     //cerr << "Done" << endl;
@@ -837,6 +863,7 @@ 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;
 }
 
@@ -845,6 +872,7 @@ bool FGAIFlightPlan::createDescent(FGAIAircraft * ac, FGAirport * apt,
  * Create a flight path from the "permision to land" point (currently
    hardcoded at 5000 meters from the threshold) to the threshold, at
    a standard glide slope angle of 3 degrees. 
+   Position : 50.0354 8.52592 384 364 11112
  ******************************************************************/
 bool FGAIFlightPlan::createLanding(FGAIAircraft * ac, FGAirport * apt,
                                    const string & fltType)
@@ -860,16 +888,41 @@ bool FGAIFlightPlan::createLanding(FGAIAircraft * ac, FGAirport * apt,
 
     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);
+    }
+    if (tn) {
+        currElev = tn->getElevationFt(apt->getElevation());
+    } else {
+        currElev = apt->getElevation();
+    }
+
 
     SGGeod coord;
-    char buffer[12];
+    
+
+    /*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));
+    }*/
+    
     for (int i = 1; i < 10; i++) {
         snprintf(buffer, 12, "wpt%d", i);
-        FGRunway * rwy = apt->getRunwayByIdent(activeRunway);
-        assert( rwy != NULL );
         
-        coord = rwy->pointOnCenterline(rwy->lengthM() * (i / 10.0));
+        coord = rwy->pointOnCenterline((rwy->lengthM() * 0.9) * (i / 10.0));
-        wpt = createOnGround(ac, buffer, coord, aptElev, (vTouchdown / i));
+        wpt = createOnGround(ac, buffer, coord, currElev, (vTouchdown / i));
         wpt->setCrossat(apt->getElevation());
         waypoints.push_back(wpt);
     }

src/AIModel/performancedata.cxx

@@ -130,7 +130,7 @@ double PerformanceData::actualVerticalSpeed(FGAIAircraft* ac, double tgt_vs, dou
     double vs = ac->getVerticalSpeed();
     double vs_diff = tgt_vs - vs;
 
-    if (fabs(vs_diff) > 10.0) {
+    if (fabs(vs_diff) > .001) {
         if (vs_diff > 0.0) {
             vs += _climbRate * dt / 3.0; //TODO avoid hardcoded 3 secs to attain climb rate from level flight
 

src/ATC/trafficcontrol.cxx

@@ -140,6 +140,27 @@ time_t ActiveRunway::requestTimeSlot(time_t eta)
     return newEta;
 }
 
+void ActiveRunway::printDepartureCue()
+{
+    cout << "Departure cue for " << rwy << ": " << endl;
+    for (AircraftVecIterator atc = departureCue.begin(); atc != departureCue.end(); atc++) {
+        cout << "     " << (*atc)->getCallSign() << " "  << (*atc)->getTakeOffStatus();
+        cout << " " << (*atc)->_getLatitude() << " " << (*atc)->_getLongitude() << (*atc)-> getSpeed() << " " << (*atc)->getAltitude() << endl;
+    }
+    
+}
+
+FGAIAircraft* ActiveRunway::getFirstOfStatus(int stat)
+{
+    for (AircraftVecIterator atc =departureCue.begin(); atc != departureCue.end(); atc++) {
+        if ((*atc)->getTakeOffStatus() == stat)
+            return (*atc);
+    }
+    return 0;
+}
+
+
+
 /***************************************************************************
  * FGTrafficRecord
  **************************************************************************/
@@ -165,7 +186,7 @@ void FGTrafficRecord::setPositionAndIntentions(int pos,
         intVecIterator i = intentions.begin();
         if ((*i) != pos) {
             SG_LOG(SG_GENERAL, SG_ALERT,
-                   "Error in FGTrafficRecord::setPositionAndIntentions");
+                   "Error in FGTrafficRecord::setPositionAndIntentions at " << SG_ORIGIN);
             //cerr << "Pos : " << pos << " Curr " << *(intentions.begin())  << endl;
             for (intVecIterator i = intentions.begin();
                     i != intentions.end(); i++) {
@@ -845,7 +866,7 @@ void FGTowerController::updateAircraftInformation(int id, double lat, double lon
 //    // update position of the current aircraft
     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
         SG_LOG(SG_GENERAL, SG_ALERT,
-               "AI error: updating aircraft without traffic record");
+               "AI error: updating aircraft without traffic record at " << SG_ORIGIN);
     } else {
         i->setPositionAndHeading(lat, lon, heading, speed, alt);
         current = i;
@@ -855,7 +876,15 @@ void FGTowerController::updateAircraftInformation(int id, double lat, double lon
     // see if we already have a clearance record for the currently active runway
     // NOTE: dd. 2011-08-07: Because the active runway has been constructed in the announcePosition function, we may safely assume that is
     // already exists here. So, we can simplify the current code.
+    
     ActiveRunwayVecIterator rwy = activeRunways.begin();
+    if (parent->getId() == fgGetString("/sim/presets/airport-id")) {
+        //for (rwy = activeRunways.begin(); rwy != activeRunways.end(); rwy++) {
+        //    rwy->printDepartureCue();
+        //}
+    }
+    
+    rwy = activeRunways.begin();
     while (rwy != activeRunways.end()) {
         if (rwy->getRunwayName() == current->getRunway()) {
             break;
@@ -878,20 +907,27 @@ void FGTowerController::updateAircraftInformation(int id, double lat, double lon
         }
     } */
     // only bother with aircraft that have a takeoff status of 2, since those are essentially under tower control
+    FGAIAircraft* ac= rwy->getFirstAircraftInDepartureCue();
+    if (ac->getTakeOffStatus() == 1) {
+        ac->setTakeOffStatus(2);
+    }
     if (current->getAircraft()->getTakeOffStatus() == 2) {
+        current -> setHoldPosition(false);
+    } else {
         current->setHoldPosition(true);
-        int clearanceId = rwy->getCleared();
+    }
-        if (clearanceId) {
+    int clearanceId = rwy->getCleared();
-            if (id == clearanceId) {
+    if (clearanceId) {
-                current->setHoldPosition(false);
+        if (id == clearanceId) {
-            }
+            current->setHoldPosition(false);
-        } else {
-            if (current->getAircraft() == rwy->getFirstAircraftInDepartureCue()) {
-                rwy->setCleared(id);
-            }
         }
     } else {
-        
+        if (current->getAircraft() == rwy->getFirstAircraftInDepartureCue()) {
+            rwy->setCleared(id);
+            FGAIAircraft *ac = rwy->getFirstOfStatus(1);
+            if (ac)
+                ac->setTakeOffStatus(2);
+        }
     }
 } 
 
@@ -925,12 +961,12 @@ void FGTowerController::signOff(int id)
             rwy->updateDepartureCue();
         } else {
             SG_LOG(SG_GENERAL, SG_ALERT,
-                   "AI error: Attempting to erase non-existing runway clearance record in FGTowerController::signoff");
+                   "AI error: Attempting to erase non-existing runway clearance record in FGTowerController::signoff at " << SG_ORIGIN);
         }
     }
     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
         SG_LOG(SG_GENERAL, SG_ALERT,
-               "AI error: Aircraft without traffic record is signing off from tower");
+               "AI error: Aircraft without traffic record is signing off from tower at " << SG_ORIGIN);
     } else {
         i->getAircraft()->resetTakeOffStatus();
         i = activeTraffic.erase(i);
@@ -960,7 +996,7 @@ bool FGTowerController::hasInstruction(int id)
     }
     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
         SG_LOG(SG_GENERAL, SG_ALERT,
-               "AI error: checking ATC instruction for aircraft without traffic record");
+               "AI error: checking ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
     } else {
         return i->hasInstruction();
     }
@@ -984,7 +1020,7 @@ FGATCInstruction FGTowerController::getInstruction(int id)
     }
     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
         SG_LOG(SG_GENERAL, SG_ALERT,
-               "AI error: requesting ATC instruction for aircraft without traffic record");
+               "AI error: requesting ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
     } else {
         return i->getInstruction();
     }
@@ -1079,7 +1115,7 @@ bool FGStartupController::hasInstruction(int id)
     }
     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
         SG_LOG(SG_GENERAL, SG_ALERT,
-               "AI error: checking ATC instruction for aircraft without traffic record");
+               "AI error: checking ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
     } else {
         return i->hasInstruction();
     }
@@ -1103,7 +1139,7 @@ FGATCInstruction FGStartupController::getInstruction(int id)
     }
     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
         SG_LOG(SG_GENERAL, SG_ALERT,
-               "AI error: requesting ATC instruction for aircraft without traffic record");
+               "AI error: requesting ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
     } else {
         return i->getInstruction();
     }
@@ -1126,7 +1162,7 @@ void FGStartupController::signOff(int id)
     }
     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
         SG_LOG(SG_GENERAL, SG_ALERT,
-               "AI error: Aircraft without traffic record is signing off from tower");
+               "AI error: Aircraft without traffic record is signing off from tower at " << SG_ORIGIN);
     } else {
         //cerr << i->getAircraft()->getCallSign() << " signing off from startupcontroller" << endl;
         i = activeTraffic.erase(i);
@@ -1187,7 +1223,7 @@ void FGStartupController::updateAircraftInformation(int id, double lat, double l
 
     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
         SG_LOG(SG_GENERAL, SG_ALERT,
-               "AI error: updating aircraft without traffic record");
+               "AI error: updating aircraft without traffic record at " << SG_ORIGIN);
     } else {
         i->setPositionAndHeading(lat, lon, heading, speed, alt);
         current = i;
@@ -1313,7 +1349,7 @@ void FGStartupController::render(bool visible)
                     } else {
                         elevationStart = ((i)->getAircraft()->_getAltitude() * SG_FEET_TO_METER);
                     }
-                    double elevationEnd   = segment->getEnd()->getElevation();
+                    double elevationEnd   = segment->getEnd()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
                     if ((elevationEnd == 0) || (elevationEnd == parent->getElevation())) {
                         SGGeod center2 = end;
                         center2.setElevationM(SG_MAX_ELEVATION_M);
@@ -1375,8 +1411,8 @@ void FGStartupController::render(bool visible)
                         obj_trans->setDataVariance(osg::Object::STATIC);
                         FGTaxiSegment *segment  = parent->getGroundNetwork()->findSegment(k);
 
-                        double elevationStart = segment->getStart()->getElevation();
+                        double elevationStart = segment->getStart()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
-                        double elevationEnd   = segment->getEnd  ()->getElevation();
+                        double elevationEnd   = segment->getEnd  ()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
                         if ((elevationStart == 0) || (elevationStart == parent->getElevation())) {
                             SGGeod center2 = segment->getStart()->getGeod();
                             center2.setElevationM(SG_MAX_ELEVATION_M);
@@ -1529,7 +1565,7 @@ void FGApproachController::updateAircraftInformation(int id, double lat, double
 //    // update position of the current aircraft
     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
         SG_LOG(SG_GENERAL, SG_ALERT,
-               "AI error: updating aircraft without traffic record");
+               "AI error: updating aircraft without traffic record at " << SG_ORIGIN);
     } else {
         i->setPositionAndHeading(lat, lon, heading, speed, alt);
         current = i;
@@ -1577,7 +1613,7 @@ void FGApproachController::signOff(int id)
     }
     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
         SG_LOG(SG_GENERAL, SG_ALERT,
-               "AI error: Aircraft without traffic record is signing off from approach");
+               "AI error: Aircraft without traffic record is signing off from approach at " << SG_ORIGIN);
     } else {
         i = activeTraffic.erase(i);
     }
@@ -1606,7 +1642,7 @@ bool FGApproachController::hasInstruction(int id)
     }
     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
         SG_LOG(SG_GENERAL, SG_ALERT,
-               "AI error: checking ATC instruction for aircraft without traffic record");
+               "AI error: checking ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
     } else {
         return i->hasInstruction();
     }
@@ -1630,7 +1666,7 @@ FGATCInstruction FGApproachController::getInstruction(int id)
     }
     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
         SG_LOG(SG_GENERAL, SG_ALERT,
-               "AI error: requesting ATC instruction for aircraft without traffic record");
+               "AI error: requesting ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
     } else {
         return i->getInstruction();
     }

src/ATC/trafficcontrol.hxx

@@ -372,9 +372,11 @@ public:
     FGAIAircraft* getFirstAircraftInDepartureCue() {
         return departureCue.size() ? *(departureCue.begin()) : NULL;
     };
+    FGAIAircraft* getFirstOfStatus(int stat);
     void updateDepartureCue() {
         departureCue.erase(departureCue.begin());
     }
+    void printDepartureCue();
 };
 
 typedef vector<ActiveRunway> ActiveRunwayVec;

src/Airports/gnnode.cxx

@@ -3,6 +3,11 @@
 
 #include <iostream>
 #include <algorithm>
+
+#include <osg/Geode>
+#include <Main/globals.hxx>
+#include <Scenery/scenery.hxx>
+
 using std::sort;
 
 /*****************************************************************************
@@ -68,12 +73,26 @@ void FGTaxiNode::setLongitude(const string& val)
   geod.setLongitudeDeg(processPosition(val));
 }
   
-//void FGTaxiNode::sortEndSegments(bool byLength)
+double FGTaxiNode::getElevationFt(double refelev)
-//{
+{
-//  if (byLength)
+    double elevF = geod.getElevationFt();
-//    sort(next.begin(), next.end(), sortByLength);
+    double elevationEnd = 0;
-//  else
+    if ((elevF == 0) || (elevF == refelev)) {
-//    sort(next.begin(), next.end(), sortByHeadingDiff);
+        SGGeod center2 = geod;
-//}
+        FGScenery * local_scenery = globals->get_scenery();
-
+        center2.setElevationM(SG_MAX_ELEVATION_M);
+        if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
+            geod.setElevationM(elevationEnd);
+        }
+    }
+    //cerr << "Returning elevation : " << geod.getElevationM() << ". Ref elev (feet) = " << refelev << endl;
+    return geod.getElevationFt();
+}
 
+double FGTaxiNode::getElevationM(double refelev)
+{
+    double refelevFt = refelev * SG_METER_TO_FEET;
+    double retval = getElevationFt(refelevFt);
+    //cerr << "Returning elevation : " << geod.getElevationM() << ". Ref elev (meters) = " << refelev << endl;
+    return geod.getElevationM();
+}

src/Airports/gnnode.hxx

@@ -101,7 +101,8 @@ FGTaxiNode &operator =(const FGTaxiNode &other)
   double getPathScore() { return pathScore; };
   double getLatitude() { return geod.getLatitudeDeg();};
   double getLongitude(){ return geod.getLongitudeDeg();};
-  double getElevation() { return geod.getElevationM();};
+  double getElevationM (double refelev=0);
+  double getElevationFt(double refelev=0);
 
   const SGGeod& getGeod() const { return geod; }
 

src/Airports/groundnetwork.cxx

@@ -252,7 +252,7 @@ FGGroundNetwork::~FGGroundNetwork()
             node != nodes.end(); node++) {
         if (saveData) {
             cachefile << (*node)->getIndex     () << " "
-            << (*node)->getElevation ()   << " "
+            << (*node)->getElevationM (parent->getElevation()*SG_FEET_TO_METER)   << " "
             << endl;
         }
         delete(*node);
@@ -296,7 +296,7 @@ void FGGroundNetwork::saveElevationCache() {
             node != nodes.end(); node++) {
         if (saveData) {
             cachefile << (*node)->getIndex     () << " "
-            << (*node)->getElevation () << " "
+            << (*node)->getElevationM (parent->getElevation()*SG_FEET_TO_METER) << " "
             << endl;
         }
     }
@@ -323,7 +323,7 @@ void FGGroundNetwork::addNodes(FGParkingVec * parkings)
         n.setIndex(i->getIndex());
         n.setLatitude(i->getLatitude());
         n.setLongitude(i->getLongitude());
-        n.setElevation(parent->getElevation());
+        n.setElevation(parent->getElevation()*SG_FEET_TO_METER);
         nodes.push_back(new FGTaxiNode(n));
 
         i++;
@@ -653,7 +653,7 @@ void FGGroundNetwork::signOff(int id)
     }
     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
         SG_LOG(SG_GENERAL, SG_ALERT,
-               "AI error: Aircraft without traffic record is signing off");
+               "AI error: Aircraft without traffic record is signing off at " << SG_ORIGIN);
     } else {
         i = activeTraffic.erase(i);
     }
@@ -733,7 +733,7 @@ void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
     // update position of the current aircraft
     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
         SG_LOG(SG_GENERAL, SG_ALERT,
-               "AI error: updating aircraft without traffic record");
+               "AI error: updating aircraft without traffic record at " << SG_ORIGIN);
     } else {
         i->setPositionAndHeading(lat, lon, heading, speed, alt);
         current = i;
@@ -818,7 +818,7 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
     }
     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
         SG_LOG(SG_GENERAL, SG_ALERT,
-               "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment");
+               "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment at " << SG_ORIGIN);
     }
     current = i;
     //closest = current;
@@ -910,12 +910,13 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
                     current->setSpeedAdjustment(closest->getSpeed() *
                                                 (mindist / 100));
                     needBraking = true;
-                    if (
+                    
-                        closest->getAircraft()->getTakeOffStatus() &&
+//                     if (
-                        (current->getAircraft()->getTrafficRef()->getDepartureAirport() ==  closest->getAircraft()->getTrafficRef()->getDepartureAirport()) &&
+//                         closest->getAircraft()->getTakeOffStatus() &&
-                        (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway())
+//                         (current->getAircraft()->getTrafficRef()->getDepartureAirport() ==  closest->getAircraft()->getTrafficRef()->getDepartureAirport()) &&
-                    )
+//                         (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway())
-                        current->getAircraft()->scheduleForATCTowerDepartureControl(1);
+//                     )
+//                         current->getAircraft()->scheduleForATCTowerDepartureControl(1);
                 } else {
                     current->setSpeedAdjustment(0);     // This can only happen when the user aircraft is the one closest
                 }
@@ -962,9 +963,18 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat,
     }
     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
         SG_LOG(SG_GENERAL, SG_ALERT,
-               "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition");
+               "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition at " << SG_ORIGIN);
     }
     current = i;
+    // 
+    if (current->getAircraft()->getTakeOffStatus() == 1) {
+        current->setHoldPosition(true);
+        return;
+    }
+    if (current->getAircraft()->getTakeOffStatus() == 2) {
+        current->setHoldPosition(false);
+        return;
+    }
     bool origStatus = current->hasHoldPosition();
     current->setHoldPosition(false);
     SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
@@ -1154,7 +1164,7 @@ bool FGGroundNetwork::checkForCircularWaits(int id)
     }
     if (i == activeTraffic.end() || (trafficSize == 0)) {
         SG_LOG(SG_GENERAL, SG_ALERT,
-               "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
+               "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits at " << SG_ORIGIN);
     }
 
     current = i;
@@ -1246,7 +1256,7 @@ bool FGGroundNetwork::hasInstruction(int id)
     }
     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
         SG_LOG(SG_GENERAL, SG_ALERT,
-               "AI error: checking ATC instruction for aircraft without traffic record");
+               "AI error: checking ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
     } else {
         return i->hasInstruction();
     }
@@ -1269,7 +1279,7 @@ FGATCInstruction FGGroundNetwork::getInstruction(int id)
     }
     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
         SG_LOG(SG_GENERAL, SG_ALERT,
-               "AI error: requesting ATC instruction for aircraft without traffic record");
+               "AI error: requesting ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
     } else {
         return i->getInstruction();
     }
@@ -1345,7 +1355,7 @@ void FGGroundNetwork::render(bool visible)
                 } else {
                     elevationStart = ((i)->getAircraft()->_getAltitude());
                 }
-                double elevationEnd   = segments[pos]->getEnd()->getElevation();
+                double elevationEnd   = segments[pos]->getEnd()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
                 //cerr << "Using elevation " << elevationEnd << endl;
 
                 if ((elevationEnd == 0) || (elevationEnd = parent->getElevation())) {
@@ -1406,8 +1416,8 @@ void FGGroundNetwork::render(bool visible)
                     obj_trans->setDataVariance(osg::Object::STATIC);
 
                     // Experimental: Calculate slope here, based on length, and the individual elevations
-                    double elevationStart = segments[k]->getStart()->getElevation();
+                    double elevationStart = segments[k]->getStart()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
-                    double elevationEnd   = segments[k]->getEnd  ()->getElevation();
+                    double elevationEnd   = segments[k]->getEnd  ()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
                     if ((elevationStart == 0)  || (elevationStart == parent->getElevation())) {
                         SGGeod center2 = segments[k]->getStart()->getGeod();
                         center2.setElevationM(SG_MAX_ELEVATION_M);
@@ -1544,7 +1554,7 @@ void FGGroundNetwork::update(double dt)
         int pos = i->getCurrentPosition();
         if (pos > 0) {
             if (segments[pos-1]->hasBlock()) {
-                SG_LOG(SG_GENERAL, SG_ALERT, "Taxiway incursion for AI aircraft" << i->getAircraft()->getCallSign());
+                //SG_LOG(SG_GENERAL, SG_ALERT, "Taxiway incursion for AI aircraft" << i->getAircraft()->getCallSign());
             }
 
         }