Some tweaks and finetuning made possible by the multihreaded modelloader

and the pushback code:
 - Traffic manager starts modelload requests immediately upon program
   loading
 - Only create legs 1 (push back) or five (cruise) of AI traffic.
 - AIAircraft's rather obsessive behavior to circle around a waypoint is
   largely resolved
 - More realistic ground steering for AI aircraft.

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.
 //
@@ -290,7 +290,7 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
         }
 
         if (next) {
-            fp->setLeadDistance(speed, tgt_heading, curr, next);
+            fp->setLeadDistance(tgt_speed, tgt_heading, curr, next);
         }
 
         if (!(prev->on_ground))  // only update the tgt altitude from flightplan if not on the ground
@@ -522,6 +522,7 @@ void FGAIAircraft::processATC(FGATCInstruction instruction) {
 
 void FGAIAircraft::handleFirstWaypoint() {
     bool eraseWaypoints;         //TODO YAGNI
+    headingError = 0;
     if (trafficRef) {
         eraseWaypoints = true;
     } else {
@@ -609,7 +610,6 @@ bool FGAIAircraft::leadPointReached(FGAIFlightPlan::waypoint* curr) {
 
     //cerr << "2" << endl;
     double lead_dist = fp->getLeadDistance();
-    //cerr << " Distance : " << dist_to_go << ": Lead distance " << lead_dist << endl;
     // experimental: Use fabs, because speed can be negative (I hope) during push_back.
 
     if (lead_dist < fabs(2*speed)) {
@@ -619,6 +619,8 @@ bool FGAIAircraft::leadPointReached(FGAIFlightPlan::waypoint* curr) {
     }
 
     //prev_dist_to_go = dist_to_go;
+    //if (dist_to_go < lead_dist)
+    //    cerr << trafficRef->getCallSign() << " Distance : " << dist_to_go << ": Lead distance " << lead_dist << " " << curr->name << endl;
     return dist_to_go < lead_dist;
 }
 
@@ -819,23 +821,54 @@ void FGAIAircraft::updateHeading() {
         // If on ground, calculate heading change directly
         if (onGround()) {
             double headingDiff = fabs(hdg-tgt_heading);
+            double bank_sense = 0.0;
+        /*
+        double diff = fabs(hdg - tgt_heading);
+        if (diff > 180)
+            diff = fabs(diff - 360);
 
+        double sum = hdg + diff;
+        if (sum > 360.0)
+            sum -= 360.0;
+        if (fabs(sum - tgt_heading) < 1.0) {
+            bank_sense = 1.0;    // right turn
+        } else {
+            bank_sense = -1.0;   // left turn
+        }*/
             if (headingDiff > 180)
                 headingDiff = fabs(headingDiff - 360);
-
+            double sum = hdg + headingDiff;
-            groundTargetSpeed = tgt_speed - (tgt_speed * (headingDiff/45));
+            if (sum > 360.0) 
+                sum -= 360.0;
+            if (fabs(sum - tgt_heading) > 0.0001) {
+                bank_sense = -1.0;
+            } else {
+                bank_sense = 1.0;
+            }
+            if (trafficRef)
+            	//cerr << trafficRef->getCallSign() << " Heading " 
+                //     << hdg << ". Target " << tgt_heading <<  ". Diff " << fabs(sum - tgt_heading) << ". Speed " << speed << endl;
+            //if (headingDiff > 60) {
+            groundTargetSpeed = tgt_speed; // * cos(headingDiff * SG_DEGREES_TO_RADIANS);
+                //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
 
             if (headingDiff > 30.0) {
                 // invert if pushed backward
-                headingChangeRate += dt * sign(roll);
+                headingChangeRate += 10.0 * dt * sign(roll);
 
+                // Clamp the maximum steering rate to 30 degrees per second,
+                // But only do this when the heading error is decreasing.
+                if ((headingDiff < headingError)) {
                     if (headingChangeRate > 30)
                         headingChangeRate = 30;
                     else if (headingChangeRate < -30)
                         headingChangeRate = -30;
-
+                }
             } else {
                    if (fabs(headingChangeRate) > headingDiff)
                        headingChangeRate = headingDiff*sign(roll);
@@ -843,6 +876,7 @@ void FGAIAircraft::updateHeading() {
                        headingChangeRate += dt * sign(roll);
             }
 	    hdg += headingChangeRate * dt;
+            headingError = headingDiff;
         } else {
             if (fabs(speed) > 1.0) {
                 turn_radius_ft = 0.088362 * speed * speed

src/AIModel/AIAircraft.hxx

@@ -95,6 +95,7 @@ private:
     double dt_count;
     double dt_elev_count;
     double headingChangeRate;
+    double headingError;
     double groundTargetSpeed;
     double groundOffset;
     double dt;

src/AIModel/AIFlightPlan.cxx

@@ -172,7 +172,7 @@ FGAIFlightPlan::FGAIFlightPlan(const std::string& p,
 	cerr << "Errno = " << errno << endl;
 	if (errno == ENOENT)
 	  {
-	    cerr << "Reason: No such file or directory" << endl;
+	    SG_LOG(SG_GENERAL, SG_WARN, "Reason: No such file or directory");
 	  }
       }
     }
@@ -184,6 +184,7 @@ FGAIFlightPlan::FGAIFlightPlan(const std::string& p,
       time_t now = time(NULL) + fgGetLong("/sim/time/warp");
       time_t timeDiff = now-start; 
       leg = 1;
+      /*
       if ((timeDiff > 300) && (timeDiff < 1200))
 	leg = 2;
       else if ((timeDiff >= 1200) && (timeDiff < 1500))
@@ -192,6 +193,9 @@ FGAIFlightPlan::FGAIFlightPlan(const std::string& p,
 	leg = 4;
       else if (timeDiff >= 2000)
 	leg = 5;
+      */
+      if (timeDiff >= 2000)
+          leg = 5;
 
       SG_LOG(SG_GENERAL, SG_INFO, "Route from " << dep->getId() << " to " << arr->getId() << ". Set leg to : " << leg);
       wpt_iterator = waypoints.begin();
@@ -353,39 +357,44 @@ void FGAIFlightPlan::IncrementWaypoint(bool eraseWaypoints )
 
 // gives distance in feet from a position to a waypoint
 double FGAIFlightPlan::getDistanceToGo(double lat, double lon, waypoint* wp) const{
+   double course, distance;
    // get size of a degree2 at the present latitude
    // this won't work over large distances
-   double ft_per_deg_lat = 366468.96 - 3717.12 * cos(lat / SG_RADIANS_TO_DEGREES);
+   //double ft_per_deg_lat = 366468.96 - 3717.12 * cos(lat / SG_RADIANS_TO_DEGREES);
-   double ft_per_deg_lon = 365228.16 * cos(lat / SG_RADIANS_TO_DEGREES);
+   //double ft_per_deg_lon = 365228.16 * cos(lat / SG_RADIANS_TO_DEGREES);
-   double lat_diff_ft = fabs(wp->latitude - lat) * ft_per_deg_lat;
+   //double lat_diff_ft = fabs(wp->latitude - lat) * ft_per_deg_lat;
-   double lon_diff_ft = fabs(wp->longitude - lon) * ft_per_deg_lon;
+   //double lon_diff_ft = fabs(wp->longitude - lon) * ft_per_deg_lon;
-   return sqrt((lat_diff_ft * lat_diff_ft) + (lon_diff_ft * lon_diff_ft));
+   //return sqrt((lat_diff_ft * lat_diff_ft) + (lon_diff_ft * lon_diff_ft));
+   SGWayPoint sgWp(wp->longitude,wp->latitude, wp->altitude, SGWayPoint::WGS84, string("temp"));
+   sgWp.CourseAndDistance(lon, lat, wp->altitude, &course, &distance);
+   return distance;
 }
 
 // sets distance in feet from a lead point to the current waypoint
 void FGAIFlightPlan::setLeadDistance(double speed, double bearing, 
                                      waypoint* current, waypoint* next){
   double turn_radius;
-    if (fabs(speed) > 1) 
+  // Handle Ground steering
+  // At a turn rate of 30 degrees per second, it takes 12 seconds to do a full 360 degree turn
+  // So, to get an estimate of the turn radius, calculate the cicumference of the circle
+  // we travel on. Get the turn radius by dividing by PI (*2).
+  if (speed < 25) {
+       turn_radius = ((360/30)*15) / (2*M_PI);
+  } else 
       turn_radius = 0.1911 * speed * speed; // an estimate for 25 degrees bank
-    else
-      turn_radius = 1.0;
 
   double inbound = bearing;
   double outbound = getBearing(current, next);
   leadInAngle = fabs(inbound - outbound);
   if (leadInAngle > 180.0) 
     leadInAngle = 360.0 - leadInAngle;
-  if (leadInAngle < 1.0) // To prevent lead_dist from getting so small it is skipped 
+  //if (leadInAngle < 30.0) // To prevent lead_dist from getting so small it is skipped 
-    leadInAngle = 1.0;
+  //  leadInAngle = 30.0;
   
-  lead_distance = turn_radius * sin(leadInAngle * SG_DEGREES_TO_RADIANS); 
+  //lead_distance = turn_radius * sin(leadInAngle * SG_DEGREES_TO_RADIANS); 
+  lead_distance = turn_radius * tan((leadInAngle * SG_DEGREES_TO_RADIANS)/2);
   //  if ((errno == EDOM) || (errno == ERANGE) || lead_distance < 1.0)
   //  {
-  //    cerr << "Lead Distance = " << lead_distance
-  //	   << "Diff          = " << diff
-  //	   << "Turn Radius   = " << turn_radius 
-  //	   << "Speed         = " << speed << endl;
   //  }
 }
 

src/Traffic/TrafficMgr.cxx

@@ -167,16 +167,7 @@ void FGTrafficManager::init()
 
 void FGTrafficManager::update(double /*dt*/)
 {
-  //SG_LOG( SG_GENERAL, SG_INFO, "Running TrafficManager::Update() ");
+
-  // Hack alert: Skip running for the first frames 1000 after 
-  // initialization to allow proper initialization of wheather stuff 
-  // and runway assignments
-  if (runCount < 1000)
-    {
-      runCount++;
-      return;
-    }
-  //runCount = 0;
   time_t now = time(NULL) + fgGetLong("/sim/time/warp");
   if (scheduledAircraft.size() == 0) {
     //SG_LOG( SG_GENERAL, SG_INFO, "Returned Running TrafficManager::Update() ");
@@ -184,7 +175,6 @@ void FGTrafficManager::update(double /*dt*/)
   }
   if(currAircraft == scheduledAircraft.end())
     {
-      //cerr << "resetting schedule " << endl;
       currAircraft = scheduledAircraft.begin();
     }
   if (!((*currAircraft)->update(now)))