Fix two bugs in the new autopilot code

[flightgear.git] / src / ATCDCL / AILocalTraffic.cxx

diff --git a/src/ATCDCL/AILocalTraffic.cxx b/src/ATCDCL/AILocalTraffic.cxx
index ec2db175b0124762e92dab2a8dcf506f453eecf5..8a66eda6e0a976cc67e1ac5c96c86599bc9b887c 100644 (file)
--- a/src/ATCDCL/AILocalTraffic.cxx
+++ b/src/ATCDCL/AILocalTraffic.cxx
@@ -164,8 +164,8 @@ void FGAILocalTraffic::GetRwyDetails(const string& id) {
   
        // move to the +l end/center of the runway
                //cout << "Runway center is at " << runway._lon << ", " << runway._lat << '\n';
-       double tshlon, tshlat, tshr;
-               double tolon, tolat, tor;
+       double tshlon = 0.0, tshlat = 0.0, tshr;
+               double tolon = 0.0, tolat = 0.0, tor;
                rwy.length = runway->lengthM();
                rwy.width = runway->widthM();
        geo_direct_wgs_84 ( aptElev, runway->latitude(), runway->longitude(), other_way, 
@@ -1127,43 +1127,24 @@ void FGAILocalTraffic::FlyTrafficPattern(double dt) {
     }
 
        if(inAir) {
-               // FIXME - at the moment this is a bit screwy
-               // The velocity correction is applied based on the relative headings.
-               // Then the heading is changed based on the velocity.
-               // Which comes first, the chicken or the egg?
-               // Does it really matter?
-               
-               // Apply wind to ground-relative velocity if in the air
-               vel = IAS - (cos((_hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
-               //crab = f(track, wind, vel);
-               // The vector we need to fly is our desired vector minus the wind vector
-               // TODO - we probably ought to use plib's built in vector types and operations for this
-               // ie.  There's almost *certainly* a better way to do this!
-               double gxx = vel * sin(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity x component wrt ground
-               double gyy = vel * cos(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity y component wrt ground
-               double wxx = wind_speed * sin((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS);    // Wind velocity x component
-               double wyy = wind_speed * cos((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS);    // Wind velocity y component
-               double axx = gxx - wxx; // Plane in-air velocity x component
-               double ayy = gyy - wyy; // Plane in-air velocity y component
-               // Now we want the angle between gxx and axx (which is the crab)
-               double maga = sqrt(axx*axx + ayy*ayy);
-               double magg = sqrt(gxx*gxx + gyy*gyy);
-               crab = acos((axx*gxx + ayy*gyy) / (maga * magg));
-               // At this point this works except we're getting the modulus of the angle
-               //cout << "crab = " << crab << '\n';
-               
-               // Make sure both headings are in the 0->360 circle in order to get sane differences
-               dclBoundHeading(wind_from);
-               dclBoundHeading(track);
-               if(track > wind_from) {
-                       if((track - wind_from) <= 180) {
-                               crab *= -1.0;
-                       }
+               // calculate ground speed and crab from the wind triangle
+               double wind_angle = GetAngleDiff_deg(wind_from + 180, track);
+               double wind_side = (wind_angle < 0) ? -1.0 : 1.0;
+
+               double sine_of_crab = wind_speed / IAS * sin(fabs(wind_angle) * DCL_DEGREES_TO_RADIANS);
+               if (sine_of_crab >= 1.0) {
+                       // The crosswind component is greater than the IAS,
+                       // we can't keep the aircraft on track.
+                       // Assume increased IAS such that it cancels lateral speed.
+                       // This is unrealistic, but not sure how the rest of the sim
+                       // would react to the aircraft going off course.
+                       // Should be a rare case anyway.
+                       crab = wind_side * 90.0;
                } else {
-                       if((wind_from - track) >= 180) {
-                               crab *= -1.0;
-                       }
+                       crab = asin(sine_of_crab) * DCL_RADIANS_TO_DEGREES * wind_side;
                }
+               vel = cos(wind_angle * DCL_DEGREES_TO_RADIANS) * wind_speed
+                       + cos(crab * DCL_DEGREES_TO_RADIANS) * IAS;
        } else {        // on the ground - crab dosen't apply
                crab = 0.0;
        }
@@ -1171,6 +1152,7 @@ void FGAILocalTraffic::FlyTrafficPattern(double dt) {
        //cout << "X " << orthopos.x() << "  Y " << orthopos.y() << "  SLOPE " << slope << "  elev " << _pos.elev() * SG_METER_TO_FEET << '\n';
        
        _hdg = track + crab;
+       dclBoundHeading(_hdg);
        dist = vel * 0.514444 * dt;
        _pos = dclUpdatePosition(_pos, track, slope, dist);
 }