src/ATCDCL/AILocalTraffic.cxx

   1 // FGAILocalTraffic - AIEntity derived class with enough logic to
   2 // fly and interact with the traffic pattern.
   3 //
   4 // Written by David Luff, started March 2002.
   5 //
   6 // Copyright (C) 2002  David C. Luff - david.luff@nottingham.ac.uk
   7 //
   8 // This program is free software; you can redistribute it and/or
   9 // modify it under the terms of the GNU General Public License as
  10 // published by the Free Software Foundation; either version 2 of the
  11 // License, or (at your option) any later version.
  12 //
  13 // This program is distributed in the hope that it will be useful, but
  14 // WITHOUT ANY WARRANTY; without even the implied warranty of
  15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16 // General Public License for more details.
  17 //
  18 // You should have received a copy of the GNU General Public License
  19 // along with this program; if not, write to the Free Software
  20 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  21
  22 /*==========================================================
  23
  24 TODO list.
  25
  26 Should get pattern direction from tower.
  27
  28 Need to continually monitor and adjust deviation from glideslope
  29 during descent to avoid occasionally landing short or long.
  30
  31 ============================================================*/
  32
  33 #ifdef HAVE_CONFIG_H
  34 #  include <config.h>
  35 #endif
  36
  37 #include <Airports/runways.hxx>
  38 #include <Main/globals.hxx>
  39 #include <Main/viewer.hxx>
  40 #include <Scenery/scenery.hxx>
  41 #include <Scenery/tilemgr.hxx>
  42 #include <simgear/math/SGMath.hxx>
  43 #include <simgear/misc/sg_path.hxx>
  44 #include <string>
  45 #include <math.h>
  46
  47 using std::string;
  48
  49 #include "ATCmgr.hxx"
  50 #include "AILocalTraffic.hxx"
  51 #include "ATCutils.hxx"
  52 #include "AIMgr.hxx"
  53
  54 FGAILocalTraffic::FGAILocalTraffic() {
  55         ATC = globals->get_ATC_mgr();
  56
  57         // TODO - unhardwire this
  58         plane.type = GA_SINGLE;
  59
  60         _roll = 0.0;
  61         _pitch = 0.0;
  62         _hdg = 270.0;
  63
  64         //Hardwire initialisation for now - a lot of this should be read in from config eventually
  65         Vr = 70.0;
  66         best_rate_of_climb_speed = 70.0;
  67         //best_rate_of_climb;
  68         //nominal_climb_speed;
  69         //nominal_climb_rate;
  70         //nominal_circuit_speed;
  71         //min_circuit_speed;
  72         //max_circuit_speed;
  73         nominal_descent_rate = 500.0;
  74         nominal_final_speed = 65.0;
  75         //nominal_approach_speed;
  76         //stall_speed_landing_config;
  77         nominalTaxiSpeed = 7.5;
  78         taxiTurnRadius = 8.0;
  79         wheelOffset = 1.45;     // Warning - hardwired to the C172 - we need to read this in from file.
  80         elevInitGood = false;
  81         // Init the property nodes
  82         wind_from_hdg = fgGetNode("/environment/wind-from-heading-deg", true);
  83         wind_speed_knots = fgGetNode("/environment/wind-speed-kt", true);
  84         circuitsToFly = 0;
  85         liningUp = false;
  86         taxiRequestPending = false;
  87         taxiRequestCleared = false;
  88         holdingShort = false;
  89         clearedToLineUp = false;
  90         clearedToTakeOff = false;
  91         _clearedToLand = false;
  92         reportReadyForDeparture = false;
  93         contactTower = false;
  94         contactGround = false;
  95         _taxiToGA = false;
  96         _removeSelf = false;
  97
  98         descending = false;
  99         targetDescentRate = 0.0;
 100         goAround = false;
 101         goAroundCalled = false;
 102
 103         transmitted = false;
 104
 105         freeTaxi = false;
 106         _savedSlope = 0.0;
 107
 108         _controlled = false;
 109
 110         _invisible = false;
 111 }
 112
 113 FGAILocalTraffic::~FGAILocalTraffic() {
 114 }
 115
 116 void FGAILocalTraffic::GetAirportDetails(const string& id) {
 117         AirportATC a;
 118         if(ATC->GetAirportATCDetails(airportID, &a)) {
 119                 if(a.tower_freq) {      // Has a tower - TODO - check the opening hours!!!
 120                         tower = (FGTower*)ATC->GetATCPointer(airportID, TOWER);
 121                         if(tower == NULL) {
 122                                 // Something has gone wrong - abort or carry on with un-towered operation?
 123                                 SG_LOG(SG_ATC, SG_ALERT, "ERROR - can't get a tower pointer from tower control for " << airportID << " in FGAILocalTraffic::GetAirportDetails() :-(");
 124                                 _controlled = false;
 125                         } else {
 126                                 _controlled = true;
 127                         }
 128                         if(tower) {
 129                                 ground = tower->GetGroundPtr();
 130                                 if(ground == NULL) {
 131                                         // Something has gone wrong :-(
 132                                         SG_LOG(SG_ATC, SG_ALERT, "ERROR - can't get a ground pointer from tower control in FGAILocalTraffic::GetAirportDetails() :-(");
 133                                 }
 134                         }
 135                 } else {
 136                         _controlled = false;
 137                         // TODO - Check CTAF, unicom etc
 138                 }
 139         } else {
 140                 SG_LOG(SG_ATC, SG_ALERT, "Unable to find airport details in for " << airportID << " in FGAILocalTraffic::GetAirportDetails() :-(");
 141                 _controlled = false;
 142         }
 143         // Get the airport elevation
 144         aptElev = fgGetAirportElev(airportID.c_str());
 145         //cout << "Airport elev in AILocalTraffic = " << aptElev << '\n';
 146         // WARNING - we use this elev for the whole airport - some assumptions in the code
 147         // might fall down with very slopey airports.
 148 }
 149
 150 // Get details of the active runway
 151 // It is assumed that by the time this is called the tower control and airport code will have been set up.
 152 void FGAILocalTraffic::GetRwyDetails(const string& id) {
 153         //cout << "GetRwyDetails called" << endl;
 154
 155   const FGAirport* apt = fgFindAirportID(id);
 156   assert(apt);
 157   FGRunway* runway(apt->getActiveRunwayForUsage());
 158
 159   double hdg = runway->headingDeg();
 160   double other_way = hdg - 180.0;
 161   while(other_way <= 0.0) {
 162     other_way += 360.0;
 163   }
 164
 165         // move to the +l end/center of the runway
 166                 //cout << "Runway center is at " << runway._lon << ", " << runway._lat << '\n';
 167         double tshlon, tshlat, tshr;
 168                 double tolon, tolat, tor;
 169                 rwy.length = runway->lengthM();
 170                 rwy.width = runway->widthM();
 171         geo_direct_wgs_84 ( aptElev, runway->latitude(), runway->longitude(), other_way,
 172                                 rwy.length / 2.0 - 25.0, &tshlat, &tshlon, &tshr );
 173         geo_direct_wgs_84 ( aptElev, runway->latitude(), runway->longitude(), hdg,
 174                                 rwy.length / 2.0 - 25.0, &tolat, &tolon, &tor );
 175                 // Note - 25 meters in from the runway end is a bit of a hack to put the plane ahead of the user.
 176                 // now copy what we need out of runway into rwy
 177         rwy.threshold_pos = SGGeod::fromDegM(tshlon, tshlat, aptElev);
 178                 SGGeod takeoff_end = SGGeod::fromDegM(tolon, tolat, aptElev);
 179                 //cout << "Threshold position = " << tshlon << ", " << tshlat << ", " << aptElev << '\n';
 180                 //cout << "Takeoff position = " << tolon << ", " << tolat << ", " << aptElev << '\n';
 181                 rwy.hdg = hdg;
 182                 // Set the projection for the local area
 183                 //cout << "Initing ortho for airport " << id << '\n';
 184                 ortho.Init(rwy.threshold_pos, rwy.hdg);
 185                 rwy.end1ortho = ortho.ConvertToLocal(rwy.threshold_pos);        // should come out as zero
 186                 rwy.end2ortho = ortho.ConvertToLocal(takeoff_end);
 187 }
 188
 189
 190 /*
 191 There are two possible scenarios during initialisation:
 192 The first is that the user is flying towards the airport, and hence the traffic
 193 could be initialised anywhere, as long as the AI planes are consistent with
 194 each other.
 195 The second is that the user has started the sim at or close to the airport, and
 196 hence the traffic must be initialised with respect to the user as well as each other.
 197 To a certain extent it's FGAIMgr that has to worry about this, but we need to provide
 198 sufficient initialisation functionality within the plane classes to allow the manager
 199 to initially position them where and how required.
 200 */
 201 bool FGAILocalTraffic::Init(const string& callsign, const string& ICAO, OperatingState initialState, PatternLeg initialLeg) {
 202         //cout << "FGAILocalTraffic.Init(...) called" << endl;
 203         airportID = ICAO;
 204
 205         plane.callsign = callsign;
 206
 207         if(initialState == EN_ROUTE) return(true);
 208
 209         // Get the ATC pointers and airport elev
 210         GetAirportDetails(airportID);
 211
 212         // Get the active runway details (and copy them into rwy)
 213         GetRwyDetails(airportID);
 214         //cout << "Runway is " << rwy.rwyID << '\n';
 215
 216         // FIXME TODO - pattern direction is still hardwired
 217         patternDirection = -1;          // Left
 218         // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
 219         if(rwy.rwyID.size() == 3) {
 220                 patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
 221         }
 222
 223         if(_controlled) {
 224                 if((initialState == PARKED) || (initialState == TAXIING)) {
 225                         freq = (double)ground->get_freq() / 100.0;
 226                 } else {
 227                         freq = (double)tower->get_freq() / 100.0;
 228                 }
 229         } else {
 230                 freq = 122.8;
 231                 // TODO - find the proper freq if CTAF or unicom or after-hours.
 232         }
 233
 234         //cout << "In Init(), initialState = " << initialState << endl;
 235         operatingState = initialState;
 236         SGVec3d orthopos;
 237         switch(operatingState) {
 238         case PARKED:
 239                 tuned_station = ground;
 240                 ourGate = ground->GetGateNode();
 241                 if(ourGate == NULL) {
 242                         // Implies no available gates - what shall we do?
 243                         // For now just vanish the plane - possibly we can make this more elegant in the future
 244                         SG_LOG(SG_ATC, SG_ALERT, "No gate found by FGAILocalTraffic whilst attempting Init at " << airportID << '\n');
 245                         return(false);
 246                 }
 247                 _pitch = 0.0;
 248                 _roll = 0.0;
 249                 vel = 0.0;
 250                 slope = 0.0;
 251                 _pos = ourGate->pos;
 252                 _pos.setElevationM(aptElev);
 253                 _hdg = ourGate->heading;
 254                 Transform();
 255
 256                 // Now we've set the position we can do the ground elev
 257                 elevInitGood = false;
 258                 inAir = false;
 259                 DoGroundElev();
 260
 261                 break;
 262         case TAXIING:
 263                 //tuned_station = ground;
 264                 // FIXME - implement this case properly
 265                 // For now we'll assume that the plane should start at the hold short in this case
 266                 // and that we're working without ground network elements.  Ie. an airport with no facility file.
 267                 if(_controlled) {
 268                         tuned_station = tower;
 269                 } else {
 270                         tuned_station = NULL;
 271                 }
 272                 freeTaxi = true;
 273                 // Set a position and orientation in an approximate place for hold short.
 274                 //cout << "rwy.width = " << rwy.width << '\n';
 275                 orthopos = SGVec3d((rwy.width / 2.0 + 10.0) * -1.0, 0.0, 0.0);
 276                 // TODO - set the x pos to be +ve if a RH parallel rwy.
 277                 _pos = ortho.ConvertFromLocal(orthopos);
 278                 _pos.setElevationM(aptElev);
 279                 _hdg = rwy.hdg + 90.0;
 280                 // TODO - reset the heading if RH rwy.
 281                 _pitch = 0.0;
 282                 _roll = 0.0;
 283                 vel = 0.0;
 284                 slope = 0.0;
 285                 elevInitGood = false;
 286                 inAir = false;
 287                 Transform();
 288                 DoGroundElev();
 289                 //Transform();
 290
 291                 responseCounter = 0.0;
 292                 contactTower = false;
 293                 changeFreq = true;
 294                 holdingShort = true;
 295                 clearedToLineUp = false;
 296                 changeFreqType = TOWER;
 297
 298                 break;
 299         case IN_PATTERN:
 300                 // For now we'll always start the in_pattern case on the threshold ready to take-off
 301                 // since we've got the implementation for this case already.
 302                 // TODO - implement proper generic in_pattern startup.
 303
 304                 // 18/10/03 - adding the ability to start on downwind (mainly to speed testing of the go-around code!!)
 305
 306                 //cout << "Starting in pattern...\n";
 307
 308                 if(_controlled) {
 309                         tuned_station = tower;
 310                 } else {
 311                         tuned_station = NULL;
 312                 }
 313
 314                 circuitsToFly = 0;              // ie just fly this circuit and then stop
 315                 touchAndGo = false;
 316
 317                 if(initialLeg == DOWNWIND) {
 318                         _pos = ortho.ConvertFromLocal(SGVec3d(1000*patternDirection, 800, 0.0));
 319                         _pos.setElevationM(rwy.threshold_pos.getElevationM() + 1000 * SG_FEET_TO_METER);
 320                         _hdg = rwy.hdg + 180.0;
 321                         leg = DOWNWIND;
 322                         elevInitGood = false;
 323                         inAir = true;
 324                         SetTrack(rwy.hdg - (180 * patternDirection));
 325                         slope = 0.0;
 326                         _pitch = 0.0;
 327                         _roll = 0.0;
 328                         IAS = 90.0;
 329                         descending = false;
 330                         _aip.setVisible(true);
 331                         if(_controlled) {
 332                                 tower->RegisterAIPlane(plane, this, CIRCUIT, DOWNWIND);
 333                         }
 334                         Transform();
 335                 } else {
 336                         // Default to initial position on threshold for now
 337                         _pos = rwy.threshold_pos;
 338                         _hdg = rwy.hdg;
 339
 340                         // Now we've set the position we can do the ground elev
 341                         // This might not always be necessary if we implement in-air start
 342                         elevInitGood = false;
 343                         inAir = false;
 344
 345                         _pitch = 0.0;
 346                         _roll = 0.0;
 347                         leg = TAKEOFF_ROLL;
 348                         vel = 0.0;
 349                         slope = 0.0;
 350
 351                         Transform();
 352                         DoGroundElev();
 353                 }
 354
 355                 operatingState = IN_PATTERN;
 356
 357                 break;
 358         case EN_ROUTE:
 359                 // This implies we're being init'd by AIGAVFRTraffic - simple return now
 360                 return(true);
 361         default:
 362                 SG_LOG(SG_ATC, SG_ALERT, "Attempt to set unknown operating state in FGAILocalTraffic.Init(...)\n");
 363                 return(false);
 364         }
 365
 366
 367         return(true);
 368 }
 369
 370
 371 // Set up downwind state - this is designed to be called from derived classes who are already tuned to tower
 372 void FGAILocalTraffic::DownwindEntry() {
 373         circuitsToFly = 0;              // ie just fly this circuit and then stop
 374         touchAndGo = false;
 375         operatingState = IN_PATTERN;
 376         leg = DOWNWIND;
 377         elevInitGood = false;
 378         inAir = true;
 379         SetTrack(rwy.hdg - (180 * patternDirection));
 380         slope = 0.0;
 381         _pitch = 0.0;
 382         _roll = 0.0;
 383         IAS = 90.0;
 384         descending = false;
 385 }
 386
 387 void FGAILocalTraffic::StraightInEntry(bool des) {
 388         //cout << "************ STRAIGHT-IN ********************\n";
 389         circuitsToFly = 0;              // ie just fly this circuit and then stop
 390         touchAndGo = false;
 391         operatingState = IN_PATTERN;
 392         leg = FINAL;
 393         elevInitGood = false;
 394         inAir = true;
 395         SetTrack(rwy.hdg);
 396         transmitted = true;     // TODO - fix this hack.
 397         // TODO - set up the next 5 properly for a descent!
 398         slope = -5.5;
 399         _pitch = 0.0;
 400         _roll = 0.0;
 401         IAS = 90.0;
 402         descending = des;
 403 }
 404
 405
 406 // Return what type of landing we're doing on this circuit
 407 LandingType FGAILocalTraffic::GetLandingOption() {
 408         //cout << "circuitsToFly = " << circuitsToFly << '\n';
 409         if(circuitsToFly) {
 410                 return(touchAndGo ? TOUCH_AND_GO : STOP_AND_GO);
 411         } else {
 412                 return(FULL_STOP);
 413         }
 414 }
 415
 416
 417 // Commands to do something from higher level logic
 418 void FGAILocalTraffic::FlyCircuits(int numCircuits, bool tag) {
 419         //cout << "FlyCircuits called" << endl;
 420
 421         switch(operatingState) {
 422         case IN_PATTERN:
 423                 circuitsToFly += numCircuits;
 424                 return;
 425                 break;
 426         case TAXIING:
 427                 // HACK - assume that we're taxiing out for now
 428                 circuitsToFly += numCircuits;
 429                 touchAndGo = tag;
 430                 break;
 431         case PARKED:
 432                 circuitsToFly = numCircuits;    // Note that one too many circuits gets flown because we only test and decrement circuitsToFly after landing
 433                                                                                 // thus flying one too many circuits.  TODO - Need to sort this out better!
 434                 touchAndGo = tag;
 435                 break;
 436         case EN_ROUTE:
 437                 break;
 438         }
 439 }
 440
 441 // Run the internal calculations
 442 void FGAILocalTraffic::Update(double dt) {
 443         //cout << "U" << flush;
 444
 445         // we shouldn't really need this since there's a LOD of 10K on the whole plane anyway I think.
 446         // At the moment though I need to to avoid DList overflows - the whole plane LOD obviously isn't getting picked up.
 447         if(!_invisible) {
 448                 if(dclGetHorizontalSeparation(_pos, SGGeod::fromDegM(fgGetDouble("/position/longitude-deg"), fgGetDouble("/position/latitude-deg"), 0.0)) > 8000) _aip.setVisible(false);
 449                 else _aip.setVisible(true);
 450         } else {
 451                 _aip.setVisible(false);
 452         }
 453
 454         //double responseTime = 10.0;           // seconds - this should get more sophisticated at some point
 455         responseCounter += dt;
 456         if((contactTower) && (responseCounter >= 8.0)) {
 457                 // Acknowledge request before changing frequency so it gets rendered if the user is on the same freq
 458                 string trns = "Tower ";
 459                 double f = globals->get_ATC_mgr()->GetFrequency(airportID, TOWER) / 100.0;
 460                 char buf[10];
 461                 sprintf(buf, "%.2f", f);
 462                 trns += buf;
 463                 trns += " ";
 464                 trns += plane.callsign;
 465                 pending_transmission = trns;
 466                 ConditionalTransmit(30.0);
 467                 responseCounter = 0.0;
 468                 contactTower = false;
 469                 changeFreq = true;
 470                 changeFreqType = TOWER;
 471         }
 472
 473         if((contactGround) && (responseCounter >= 8.0)) {
 474                 // Acknowledge request before changing frequency so it gets rendered if the user is on the same freq
 475                 string trns = "Ground ";
 476                 double f = globals->get_ATC_mgr()->GetFrequency(airportID, GROUND) / 100.0;
 477                 char buf[10];
 478                 sprintf(buf, "%.2f", f);
 479                 trns += buf;
 480                 trns += " ";
 481                 trns += "Good Day";
 482                 pending_transmission = trns;
 483                 ConditionalTransmit(5.0);
 484                 responseCounter = 0.0;
 485                 contactGround = false;
 486                 changeFreq = true;
 487                 changeFreqType = GROUND;
 488         }
 489
 490         if((_taxiToGA) && (responseCounter >= 8.0)) {
 491                 // Acknowledge request before changing frequency so it gets rendered if the user is on the same freq
 492                 string trns = "GA Parking, Thank you and Good Day";
 493                 //double f = globals->get_ATC_mgr()->GetFrequency(airportID, GROUND) / 100.0;
 494                 pending_transmission = trns;
 495                 ConditionalTransmit(5.0, 99);
 496                 _taxiToGA = false;
 497                 if(_controlled) {
 498                         tower->DeregisterAIPlane(plane.callsign);
 499                 }
 500                 // NOTE - we can't delete this instance yet since then the frequency won't get release when the message display finishes.
 501         }
 502
 503         if((_removeSelf) && (responseCounter >= 8.0)) {
 504                 _removeSelf = false;
 505                 // MEGA HACK - check if we are at a simple airport or not first instead of simply hardwiring KEMT as the only non-simple airport.
 506                 // TODO FIXME TODO FIXME !!!!!!!
 507                 if(airportID != "KEMT") globals->get_AI_mgr()->ScheduleRemoval(plane.callsign);
 508         }
 509
 510         if((changeFreq) && (responseCounter > 8.0)) {
 511                 switch(changeFreqType) {
 512                 case TOWER:
 513                         if(!tower) {
 514                                 SG_LOG(SG_ATC, SG_ALERT, "ERROR: Trying to change frequency to tower in FGAILocalTraffic, but tower is NULL!!!");
 515                                 break;
 516                         }
 517                         tuned_station = tower;
 518                         freq = (double)tower->get_freq() / 100.0;
 519                         //Transmit("DING!");
 520                         // Contact the tower, even if only virtually
 521                         pending_transmission = plane.callsign;
 522                         pending_transmission += " at hold short for runway ";
 523                         pending_transmission += ConvertRwyNumToSpokenString(rwy.rwyID);
 524                         pending_transmission += " traffic pattern ";
 525                         if(circuitsToFly) {
 526                                 pending_transmission += ConvertNumToSpokenDigits(circuitsToFly + 1);
 527                                 pending_transmission += " circuits touch and go";
 528                         } else {
 529                                 pending_transmission += " one circuit to full stop";
 530                         }
 531                         Transmit(2);
 532                         break;
 533                 case GROUND:
 534                         if(!tower) {
 535                                 SG_LOG(SG_ATC, SG_ALERT, "ERROR: Trying to change frequency to ground in FGAILocalTraffic, but tower is NULL!!!");
 536                                 break;
 537                         }
 538                         if(!ground) {
 539                                 SG_LOG(SG_ATC, SG_ALERT, "ERROR: Trying to change frequency to ground in FGAILocalTraffic, but ground is NULL!!!");
 540                                 break;
 541                         }
 542                         tower->DeregisterAIPlane(plane.callsign);
 543                         tuned_station = ground;
 544                         freq = (double)ground->get_freq() / 100.0;
 545                         break;
 546                 // And to avoid compiler warnings...
 547                 case APPROACH:  break;
 548                 case ATIS:      break;
 549     case AWOS:      break;
 550                 case ENROUTE:   break;
 551                 case DEPARTURE: break;
 552                 case INVALID:   break;
 553                 }
 554                 changeFreq = false;
 555         }
 556
 557         //cout << "," << flush;
 558
 559         switch(operatingState) {
 560         case IN_PATTERN:
 561                 //cout << "In IN_PATTERN\n";
 562                 if(!inAir) {
 563                         DoGroundElev();
 564                         if(!elevInitGood) {
 565                                 if(_ground_elevation_m > -9990.0) {
 566                                         _pos.setElevationM(_ground_elevation_m + wheelOffset);
 567                                         //cout << "TAKEOFF_ROLL, POS = " << pos.lon() << ", " << pos.lat() << ", " << pos.elev() << '\n';
 568                                         //Transform();
 569                                         _aip.setVisible(true);
 570                                         //cout << "Making plane visible!\n";
 571                                         elevInitGood = true;
 572                                 }
 573                         }
 574                 }
 575                 FlyTrafficPattern(dt);
 576                 Transform();
 577                 break;
 578         case TAXIING:
 579                 //cout << "In TAXIING\n";
 580                 //cout << "*" << flush;
 581                 if(!elevInitGood) {
 582                         //DoGroundElev();
 583                         if(_ground_elevation_m > -9990.0) {
 584                                 _pos.setElevationM(_ground_elevation_m + wheelOffset);
 585                                 //Transform();
 586                                 _aip.setVisible(true);
 587                                 //Transform();
 588                                 //cout << "Making plane visible!\n";
 589                                 elevInitGood = true;
 590                         }
 591                 }
 592                 DoGroundElev();
 593                 //cout << "~" << flush;
 594                 if(!((holdingShort) && (!clearedToLineUp))) {
 595                         //cout << "|" << flush;
 596                         Taxi(dt);
 597                 }
 598                 //cout << ";" << flush;
 599                 if((clearedToTakeOff) && (responseCounter >= 8.0)) {
 600                         // possible assumption that we're at the hold short here - may not always hold
 601                         // TODO - sort out the case where we're cleared to line-up first and then cleared to take-off on the rwy.
 602                         taxiState = TD_LINING_UP;
 603                         //cout << "A" << endl;
 604                         path = ground->GetPath(holdShortNode, rwy.rwyID);
 605                         //cout << "B" << endl;
 606                         if(!path.size()) {      // Assume no facility file so we'll just taxi to a point on the runway near the threshold
 607                                 //cout << "C" << endl;
 608                                 node* np = new node;
 609                                 np->struct_type = NODE;
 610                                 np->pos = ortho.ConvertFromLocal(SGVec3d(0.0, 10.0, 0.0));
 611                                 path.push_back(np);
 612                         } else {
 613                                 //cout << "D" << endl;
 614                         }
 615                         /*
 616                         cout << "path returned was:" << endl;
 617                         for(unsigned int i=0; i<path.size(); ++i) {
 618                                 switch(path[i]->struct_type) {
 619                                         case NODE:
 620                                         cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
 621                                         break;
 622                                         case ARC:
 623                                         cout << "ARC\n";
 624                                         break;
 625                                 }
 626                         }
 627                         */
 628                         clearedToTakeOff = false;       // We *are* still cleared - this simply stops the response recurring!!
 629                         holdingShort = false;
 630                         string trns = "Cleared for take-off ";
 631                         trns += plane.callsign;
 632                         pending_transmission = trns;
 633                         Transmit();
 634                         StartTaxi();
 635                 }
 636                 //cout << "^" << flush;
 637                 Transform();
 638                 break;
 639         case PARKED:
 640                 //cout << "In PARKED\n";
 641                 if(!elevInitGood) {
 642                         DoGroundElev();
 643                         if(_ground_elevation_m > -9990.0) {
 644                                 _pos.setElevationM(_ground_elevation_m + wheelOffset);
 645                                 //Transform();
 646                                 _aip.setVisible(true);
 647                                 //Transform();
 648                                 //cout << "Making plane visible!\n";
 649                                 elevInitGood = true;
 650                         }
 651                 }
 652
 653                 if(circuitsToFly) {
 654                         if((taxiRequestPending) && (taxiRequestCleared)) {
 655                                 //cout << "&" << flush;
 656                                 // Get the active runway details (in case they've changed since init)
 657                                 GetRwyDetails(airportID);
 658
 659                                 // Get the takeoff node for the active runway, get a path to it and start taxiing
 660                                 path = ground->GetPathToHoldShort(ourGate, rwy.rwyID);
 661                                 if(path.size() < 2) {
 662                                         // something has gone wrong
 663                                         SG_LOG(SG_ATC, SG_ALERT, "Invalid path from gate to theshold in FGAILocalTraffic::FlyCircuits\n");
 664                                         return;
 665                                 }
 666                                 /*
 667                                 cout << "path returned was:\n";
 668                                 for(unsigned int i=0; i<path.size(); ++i) {
 669                                         switch(path[i]->struct_type) {
 670                                                 case NODE:
 671                                                 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
 672                                                 break;
 673                                                 case ARC:
 674                                                 cout << "ARC\n";
 675                                                 break;
 676                                         }
 677                                 }
 678                                 */
 679                                 path.erase(path.begin());       // pop the gate - we're here already!
 680                                 taxiState = TD_OUTBOUND;
 681                                 taxiRequestPending = false;
 682                                 holdShortNode = (node*)(*(path.begin() + path.size()));
 683                                 StartTaxi();
 684                         } else if(!taxiRequestPending) {
 685                                 //cout << "(" << flush;
 686                                 // Do some communication
 687                                 // airport name + tower + airplane callsign + location + request taxi for + operation type + ?
 688                                 string trns = "";
 689                                 if(_controlled) {
 690                                         trns += tower->get_name();
 691                                         trns += " tower ";
 692                                 } else {
 693                                         trns += "Traffic ";
 694                                         // TODO - get the airport name somehow if uncontrolled
 695                                 }
 696                                 trns += plane.callsign;
 697                                 trns += " on apron parking request taxi for traffic pattern";
 698                                 //cout << "trns = " << trns << endl;
 699                                 pending_transmission = trns;
 700                                 Transmit(1);
 701                                 taxiRequestCleared = false;
 702                                 taxiRequestPending = true;
 703                         }
 704                 }
 705
 706                 //cout << "!" << flush;
 707
 708                 // Maybe the below should be set when we get to the threshold and prepare for TO?
 709                 // FIXME TODO - pattern direction is still hardwired
 710                 patternDirection = -1;          // Left
 711                 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
 712                 if(rwy.rwyID.size() == 3) {
 713                         patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
 714                 }
 715                 // Do nothing
 716                 Transform();
 717                 //cout << ")" << flush;
 718                 break;
 719         default:
 720                 break;
 721         }
 722         //cout << "I " << flush;
 723
 724         //cout << "Update _pos = " << _pos << ", vis = " << _aip.getVisible() << '\n';
 725
 726         // Convienience output for AI debugging using the property logger
 727         //fgSetDouble("/AI/Local1/ortho-x", (ortho.ConvertToLocal(_pos)).x());
 728         //fgSetDouble("/AI/Local1/ortho-y", (ortho.ConvertToLocal(_pos)).y());
 729         //fgSetDouble("/AI/Local1/elev", _pos.elev() * SG_METER_TO_FEET);
 730
 731         // And finally, call parent.
 732         FGAIPlane::Update(dt);
 733 }
 734
 735 void FGAILocalTraffic::RegisterTransmission(int code) {
 736         switch(code) {
 737         case 1: // taxi request cleared
 738                 taxiRequestCleared = true;
 739                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to taxi...");
 740                 break;
 741         case 2: // contact tower
 742                 responseCounter = 0;
 743                 contactTower = true;
 744                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " told to contact tower...");
 745                 break;
 746         case 3: // Cleared to line up
 747                 responseCounter = 0;
 748                 clearedToLineUp = true;
 749                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to line-up...");
 750                 break;
 751         case 4: // cleared to take-off
 752                 responseCounter = 0;
 753                 clearedToTakeOff = true;
 754                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to take-off...");
 755                 break;
 756         case 5: // contact ground
 757                 responseCounter = 0;
 758                 contactGround = true;
 759                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " told to contact ground...");
 760                 break;
 761         // case 6 is a temporary mega-hack for controlled airports without separate ground control
 762         case 6: // taxi to the GA parking
 763                 responseCounter = 0;
 764                 _taxiToGA = true;
 765                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " told to taxi to the GA parking...");
 766                 break;
 767         case 7:  // Cleared to land (also implies cleared for the option
 768                 _clearedToLand = true;
 769                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to land...");
 770                 break;
 771         case 13: // Go around!
 772                 responseCounter = 0;
 773                 goAround = true;
 774                 _clearedToLand = false;
 775                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " told to go-around!!");
 776                 break;
 777         default:
 778                 break;
 779         }
 780 }
 781
 782 // Fly a traffic pattern
 783 // FIXME - far too much of the mechanics of turning, rolling, accellerating, descending etc is in here.
 784 //         Move it out to FGAIPlane and have FlyTrafficPattern just specify what to do, not the implementation.
 785 void FGAILocalTraffic::FlyTrafficPattern(double dt) {
 786         // Need to differentiate between in-air (IAS governed) and on-ground (vel governed)
 787         // Take-off is an interesting case - we are on the ground but takeoff speed is IAS governed.
 788
 789         // WIND
 790         // Wind has two effects - a mechanical one in that IAS translates to a different vel, and the hdg != track,
 791         // but also a piloting effect, in that the AI must be able to descend at a different rate in order to hit the threshold.
 792
 793         //cout << "dt = " << dt << '\n';
 794         double dist = 0;
 795         // ack - I can't remember how long a rate 1 turn is meant to take.
 796         double turn_time = 60.0;        // seconds - TODO - check this guess
 797         double turn_circumference;
 798         double turn_radius;
 799         SGVec3d orthopos = ortho.ConvertToLocal(_pos);  // ortho position of the plane
 800         //cout << "runway elev = " << rwy.threshold_pos.getElevationM() << ' ' << rwy.threshold_pos.getElevationM() * SG_METER_TO_FEET << '\n';
 801         //cout << "elev = " << _pos.elev() << ' ' << _pos.elev() * SG_METER_TO_FEET << '\n';
 802
 803         // HACK FOR TESTING - REMOVE
 804         //cout << "Calling ExitRunway..." << endl;
 805         //ExitRunway(orthopos);
 806         //return;
 807         // END HACK
 808
 809         //wind
 810         double wind_from = wind_from_hdg->getDoubleValue();
 811         double wind_speed = wind_speed_knots->getDoubleValue();
 812
 813         double dveldt;
 814
 815         switch(leg) {
 816         case TAKEOFF_ROLL:
 817                 //inAir = false;
 818                 track = rwy.hdg;
 819                 if(vel < 80.0) {
 820                         double dveldt = 5.0;
 821                         vel += dveldt * dt;
 822                 }
 823                 if(_ground_elevation_m > -9990.0) {
 824                         _pos.setElevationM(_ground_elevation_m + wheelOffset);
 825                 }
 826                 IAS = vel + (cos((_hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
 827                 if(IAS >= 70) {
 828                         leg = CLIMBOUT;
 829                         SetTrack(rwy.hdg);      // Hands over control of turning to AIPlane
 830                         _pitch = 10.0;
 831                         IAS = best_rate_of_climb_speed;
 832                         //slope = 7.0;
 833                         slope = 6.0;    // Reduced it slightly since it's climbing a lot steeper than I can in the JSBSim C172.
 834                         inAir = true;
 835                 }
 836                 break;
 837         case CLIMBOUT:
 838                 // Turn to crosswind if above 700ft AND if other traffic allows
 839                 // (decided in FGTower and accessed through GetCrosswindConstraint(...)).
 840                 // According to AIM, traffic should climb to within 300ft of pattern altitude before commencing crosswind turn.
 841                 // TODO - At hot 'n high airports this may be 500ft AGL though - need to make this a variable.
 842                 if((_pos.getElevationM() - rwy.threshold_pos.getElevationM()) * SG_METER_TO_FEET > 700) {
 843                         double cc = 0.0;
 844                         if(tower->GetCrosswindConstraint(cc)) {
 845                                 if(orthopos.y() > cc) {
 846                                         //cout << "Turning to crosswind, distance from threshold = " << orthopos.y() << '\n';
 847                                         leg = TURN1;
 848                                 }
 849                         } else if(orthopos.y() > 1500.0) {   // Added this constraint as a hack to prevent turning too early when going around.
 850                                 // TODO - We should be doing it as a distance from takeoff end, not theshold end though.
 851                                 //cout << "Turning to crosswind, distance from threshold = " << orthopos.y() << '\n';
 852                                 leg = TURN1;
 853                         }
 854                 }
 855                 // Need to check for levelling off in case we can't turn crosswind as soon
 856                 // as we would like due to other traffic.
 857                 if((_pos.getElevationM() - rwy.threshold_pos.getElevationM()) * SG_METER_TO_FEET > 1000) {
 858                         slope = 0.0;
 859                         _pitch = 0.0;
 860                         IAS = 80.0;             // FIXME - use smooth transistion to new speed and attitude.
 861                 }
 862                 if(goAround && !goAroundCalled) {
 863                         if(responseCounter > 5.5) {
 864                                 pending_transmission = plane.callsign;
 865                                 pending_transmission += " going around";
 866                                 Transmit();
 867                                 goAroundCalled = true;
 868                         }
 869                 }
 870                 break;
 871         case TURN1:
 872                 SetTrack(rwy.hdg + (90.0 * patternDirection));
 873                 if((track < (rwy.hdg - 89.0)) || (track > (rwy.hdg + 89.0))) {
 874                         leg = CROSSWIND;
 875                 }
 876                 break;
 877         case CROSSWIND:
 878                 goAround = false;
 879                 if((_pos.getElevationM() - rwy.threshold_pos.getElevationM()) * SG_METER_TO_FEET > 1000) {
 880                         slope = 0.0;
 881                         _pitch = 0.0;
 882                         IAS = 80.0;             // FIXME - use smooth transistion to new speed
 883                 }
 884                 // turn 1000m out for now, taking other traffic into accout
 885                 if(fabs(orthopos.x()) > 900) {
 886                         double dd = 0.0;
 887                         if(tower->GetDownwindConstraint(dd)) {
 888                                 if(fabs(orthopos.x()) > fabs(dd)) {
 889                                         //cout << "Turning to downwind, distance from centerline = " << fabs(orthopos.x()) << '\n';
 890                                         leg = TURN2;
 891                                 }
 892                         } else {
 893                                 //cout << "Turning to downwind, distance from centerline = " << fabs(orthopos.x()) << '\n';
 894                                 leg = TURN2;
 895                         }
 896                 }
 897                 break;
 898         case TURN2:
 899                 SetTrack(rwy.hdg - (180 * patternDirection));
 900                 // just in case we didn't make height on crosswind
 901                 if((_pos.getElevationM() - rwy.threshold_pos.getElevationM()) * SG_METER_TO_FEET > 1000) {
 902                         slope = 0.0;
 903                         _pitch = 0.0;
 904                         IAS = 80.0;             // FIXME - use smooth transistion to new speed
 905                 }
 906                 if((track < (rwy.hdg - 179.0)) || (track > (rwy.hdg + 179.0))) {
 907                         leg = DOWNWIND;
 908                         transmitted = false;
 909                 }
 910                 break;
 911         case DOWNWIND:
 912                 // just in case we didn't make height on crosswind
 913                 if(((_pos.getElevationM() - rwy.threshold_pos.getElevationM()) * SG_METER_TO_FEET > 995) && ((_pos.getElevationM() - rwy.threshold_pos.getElevationM()) * SG_METER_TO_FEET < 1015)) {
 914                         slope = 0.0;
 915                         _pitch = 0.0;
 916                         IAS = 90.0;             // FIXME - use smooth transistion to new speed
 917                 }
 918                 if((_pos.getElevationM() - rwy.threshold_pos.getElevationM()) * SG_METER_TO_FEET >= 1015) {
 919                         slope = -1.0;
 920                         _pitch = -1.0;
 921                         IAS = 90.0;             // FIXME - use smooth transistion to new speed
 922                 }
 923                 if((orthopos.y() < 0) && (!transmitted)) {
 924                         TransmitPatternPositionReport();
 925                         transmitted = true;
 926                 }
 927                 if((orthopos.y() < -100) && (!descending)) {
 928                         //cout << "DDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDdddd\n";
 929                         // Maybe we should think about when to start descending.
 930                         // For now we're assuming that we aim to follow the same glidepath regardless of wind.
 931                         double d1;
 932                         double d2;
 933                         CalculateSoD((tower->GetBaseConstraint(d1) ? d1 : -1000.0), (tower->GetDownwindConstraint(d2) ? d2 : 1000.0 * patternDirection), (patternDirection ? true : false));
 934                         if(SoD.leg == DOWNWIND) {
 935                                 descending = (orthopos.y() < SoD.y ? true : false);
 936                         }
 937
 938                 }
 939                 if(descending) {
 940                         slope = -5.5;   // FIXME - calculate to descent at 500fpm and hit the desired point on the runway (taking wind into account as well!!)
 941                         _pitch = -3.0;
 942                         IAS = 85.0;
 943                 }
 944
 945                 // Try and arrange to turn nicely onto base
 946                 turn_circumference = IAS * 0.514444 * turn_time;
 947                 //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius
 948                 //We'll leave it as a hack with IAS for now but it needs revisiting.
 949                 turn_radius = turn_circumference / (2.0 * DCL_PI);
 950                 if(orthopos.y() < -1000.0 + turn_radius) {
 951                 //if(orthopos.y() < -980) {
 952                         double bb = 0.0;
 953                         if(tower->GetBaseConstraint(bb)) {
 954                                 if(fabs(orthopos.y()) > fabs(bb)) {
 955                                         //cout << "Turning to base, distance from threshold = " << fabs(orthopos.y()) << '\n';
 956                                         leg = TURN3;
 957                                         transmitted = false;
 958                                         IAS = 80.0;
 959                                 }
 960                         } else {
 961                                 //cout << "Turning to base, distance from threshold = " << fabs(orthopos.y()) << '\n';
 962                                 leg = TURN3;
 963                                 transmitted = false;
 964                                 IAS = 80.0;
 965                         }
 966                 }
 967                 break;
 968         case TURN3:
 969                 SetTrack(rwy.hdg - (90 * patternDirection));
 970                 if(fabs(rwy.hdg - track) < 91.0) {
 971                         leg = BASE;
 972                 }
 973                 break;
 974         case BASE:
 975                 if(!transmitted) {
 976                         // Base report should only be transmitted at uncontrolled airport - not towered.
 977                         if(!_controlled) TransmitPatternPositionReport();
 978                         transmitted = true;
 979                 }
 980
 981                 if(!descending) {
 982                         double d1;
 983                         // Make downwind leg position artifically large to avoid any chance of SoD being returned as
 984                         // on downwind when we are already on base.
 985                         CalculateSoD((tower->GetBaseConstraint(d1) ? d1 : -1000.0), (10000.0 * patternDirection), (patternDirection ? true : false));
 986                         if(SoD.leg == BASE) {
 987                                 descending = (fabs(orthopos.y()) < fabs(SoD.y) ? true : false);
 988                         }
 989
 990                 }
 991                 if(descending) {
 992                         slope = -5.5;   // FIXME - calculate to descent at 500fpm and hit the threshold (taking wind into account as well!!)
 993                         _pitch = -4.0;
 994                         IAS = 70.0;
 995                 }
 996
 997                 // Try and arrange to turn nicely onto final
 998                 turn_circumference = IAS * 0.514444 * turn_time;
 999                 //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius
1000                 //We'll leave it as a hack with IAS for now but it needs revisiting.
1001                 turn_radius = turn_circumference / (2.0 * DCL_PI);
1002                 if(fabs(orthopos.x()) < (turn_radius + 50)) {
1003                         leg = TURN4;
1004                         transmitted = false;
1005                         //roll = -20;
1006                 }
1007                 break;
1008         case TURN4:
1009                 SetTrack(rwy.hdg);
1010                 if(fabs(track - rwy.hdg) < 0.6) {
1011                         leg = FINAL;
1012                         vel = nominal_final_speed;
1013                 }
1014                 break;
1015         case FINAL:
1016                 if(goAround && responseCounter > 2.0) {
1017                         leg = CLIMBOUT;
1018                         _pitch = 8.0;
1019                         IAS = best_rate_of_climb_speed;
1020                         slope = 5.0;    // A bit less steep than the initial climbout.
1021                         inAir = true;
1022                         goAroundCalled = false;
1023                         descending = false;
1024                         break;
1025                 }
1026                 LevelWings();
1027                 if(!transmitted) {
1028                         if((!_controlled) || (!_clearedToLand)) TransmitPatternPositionReport();
1029                         transmitted = true;
1030                 }
1031                 if(!descending) {
1032                         // Make base leg position artifically large to avoid any chance of SoD being returned as
1033                         // on base or downwind when we are already on final.
1034                         CalculateSoD(-10000.0, (1000.0 * patternDirection), (patternDirection ? true : false));
1035                         if(SoD.leg == FINAL) {
1036                                 descending = (fabs(orthopos.y()) < fabs(SoD.y) ? true : false);
1037                         }
1038
1039                 }
1040                 if(descending) {
1041                         if(orthopos.y() < -50.0) {
1042                                 double thesh_offset = 30.0;
1043                                 slope = atan((_pos.getElevationM() - fgGetAirportElev(airportID)) / (orthopos.y() - thesh_offset)) * DCL_RADIANS_TO_DEGREES;
1044                                 //cout << "slope = " << slope << ", elev = " << _pos.elev() << ", apt_elev = " << fgGetAirportElev(airportID) << ", op.y = " << orthopos.y() << '\n';
1045                                 if(slope < -10.0) slope = -10.0;
1046                                 _savedSlope = slope;
1047                                 _pitch = -4.0;
1048                                 IAS = 70.0;
1049                         } else {
1050                                 if(_pos.getElevationM() < (rwy.threshold_pos.getElevationM()+10.0+wheelOffset)) {
1051                                         if(_ground_elevation_m > -9990.0) {
1052                                                 if(_pos.getElevationM() < (_ground_elevation_m + wheelOffset + 1.0)) {
1053                                                         slope = -2.0;
1054                                                         _pitch = 1.0;
1055                                                         IAS = 55.0;
1056                                                 } else if(_pos.getElevationM() < (_ground_elevation_m + wheelOffset + 5.0)) {
1057                                                         slope = -4.0;
1058                                                         _pitch = -2.0;
1059                                                         IAS = 60.0;
1060                                                 } else {
1061                                                         slope = _savedSlope;
1062                                                         _pitch = -3.0;
1063                                                         IAS = 65.0;
1064                                                 }
1065                                         } else {
1066                                                 // Elev not determined
1067                                                 slope = _savedSlope;
1068                                                 _pitch = -3.0;
1069                                                 IAS = 65.0;
1070                                         }
1071                                 } else {
1072                                         slope = _savedSlope;
1073                                         _pitch = -3.0;
1074                                         IAS = 65.0;
1075                                 }
1076                         }
1077                 }
1078                 // Try and track the extended centreline
1079                 SetTrack(rwy.hdg - (0.2 * orthopos.x()));
1080                 //cout << "orthopos.x() = " << orthopos.x() << " hdg = " << hdg << '\n';
1081                 if(_pos.getElevationM() < (rwy.threshold_pos.getElevationM()+20.0+wheelOffset)) {
1082                         DoGroundElev(); // Need to call it here expicitly on final since it's only called
1083                                         // for us in update(...) when the inAir flag is false.
1084                 }
1085                 if(_pos.getElevationM() < (rwy.threshold_pos.getElevationM()+10.0+wheelOffset)) {
1086                         //slope = -1.0;
1087                         //_pitch = 1.0;
1088                         if(_ground_elevation_m > -9990.0) {
1089                                 if((_ground_elevation_m + wheelOffset) > _pos.getElevationM()) {
1090                                         slope = 0.0;
1091                                         _pitch = 0.0;
1092                                         leg = LANDING_ROLL;
1093                                         inAir = false;
1094                                         LevelWings();
1095                                         ClearTrack();   // Take over explicit track handling since AIPlane currently always banks when changing course
1096                                 }
1097                         }       // else need a fallback position based on arpt elev in case ground elev determination fails?
1098                 } else {
1099                         // TODO
1100                 }
1101                 break;
1102         case LANDING_ROLL:
1103                 //inAir = false;
1104                 descending = false;
1105                 if(_ground_elevation_m > -9990.0) {
1106                         _pos.setElevationM(_ground_elevation_m + wheelOffset);
1107                 }
1108                 track = rwy.hdg;
1109                 dveldt = -5.0;
1110                 vel += dveldt * dt;
1111                 // FIXME - differentiate between touch and go and full stops
1112                 if(vel <= 15.0) {
1113                         //cout << "Vel <= 15.0, circuitsToFly = " << circuitsToFly << endl;
1114                         if(circuitsToFly <= 0) {
1115                                 //cout << "Calling ExitRunway..." << endl;
1116                                 ExitRunway(orthopos);
1117                                 return;
1118                         } else {
1119                                 //cout << "Taking off again..." << endl;
1120                                 leg = TAKEOFF_ROLL;
1121                                 --circuitsToFly;
1122                         }
1123                 }
1124                 break;
1125         case LEG_UNKNOWN:
1126                 break;
1127     }
1128
1129         if(inAir) {
1130                 // FIXME - at the moment this is a bit screwy
1131                 // The velocity correction is applied based on the relative headings.
1132                 // Then the heading is changed based on the velocity.
1133                 // Which comes first, the chicken or the egg?
1134                 // Does it really matter?
1135
1136                 // Apply wind to ground-relative velocity if in the air
1137                 vel = IAS - (cos((_hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
1138                 //crab = f(track, wind, vel);
1139                 // The vector we need to fly is our desired vector minus the wind vector
1140                 // TODO - we probably ought to use plib's built in vector types and operations for this
1141                 // ie.  There's almost *certainly* a better way to do this!
1142                 double gxx = vel * sin(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity x component wrt ground
1143                 double gyy = vel * cos(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity y component wrt ground
1144                 double wxx = wind_speed * sin((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS);    // Wind velocity x component
1145                 double wyy = wind_speed * cos((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS);    // Wind velocity y component
1146                 double axx = gxx - wxx; // Plane in-air velocity x component
1147                 double ayy = gyy - wyy; // Plane in-air velocity y component
1148                 // Now we want the angle between gxx and axx (which is the crab)
1149                 double maga = sqrt(axx*axx + ayy*ayy);
1150                 double magg = sqrt(gxx*gxx + gyy*gyy);
1151                 crab = acos((axx*gxx + ayy*gyy) / (maga * magg));
1152                 // At this point this works except we're getting the modulus of the angle
1153                 //cout << "crab = " << crab << '\n';
1154
1155                 // Make sure both headings are in the 0->360 circle in order to get sane differences
1156                 dclBoundHeading(wind_from);
1157                 dclBoundHeading(track);
1158                 if(track > wind_from) {
1159                         if((track - wind_from) <= 180) {
1160                                 crab *= -1.0;
1161                         }
1162                 } else {
1163                         if((wind_from - track) >= 180) {
1164                                 crab *= -1.0;
1165                         }
1166                 }
1167         } else {        // on the ground - crab dosen't apply
1168                 crab = 0.0;
1169         }
1170
1171         //cout << "X " << orthopos.x() << "  Y " << orthopos.y() << "  SLOPE " << slope << "  elev " << _pos.elev() * SG_METER_TO_FEET << '\n';
1172
1173         _hdg = track + crab;
1174         dist = vel * 0.514444 * dt;
1175         _pos = dclUpdatePosition(_pos, track, slope, dist);
1176 }
1177
1178 // Pattern direction is true for right, false for left
1179 void FGAILocalTraffic::CalculateSoD(double base_leg_pos, double downwind_leg_pos, bool pattern_direction) {
1180         // For now we'll ignore wind and hardwire the glide angle.
1181         double ga = 5.5;        //degrees
1182         double pa = 1000.0 * SG_FEET_TO_METER;  // pattern altitude in meters
1183         // FIXME - get glideslope angle and pattern altitude agl from airport details if available
1184
1185         // For convienience, we'll have +ve versions of the input distances
1186         double blp = fabs(base_leg_pos);
1187         double dlp = fabs(downwind_leg_pos);
1188
1189         //double turn_allowance = 150.0;        // Approximate distance in meters that a 90deg corner is shortened by turned in a light plane.
1190
1191         double stod = pa / tan(ga * DCL_DEGREES_TO_RADIANS);    // distance in meters from touchdown point to start descent
1192         //cout << "Descent to start = " << stod << " meters out\n";
1193         if(stod < blp) {        // Start descending on final
1194                 SoD.leg = FINAL;
1195                 SoD.y = stod * -1.0;
1196                 SoD.x = 0.0;
1197         } else if(stod < (blp + dlp)) { // Start descending on base leg
1198                 SoD.leg = BASE;
1199                 SoD.y = blp * -1.0;
1200                 SoD.x = (pattern_direction ? (stod - dlp) : (stod - dlp) * -1.0);
1201         } else {        // Start descending on downwind leg
1202                 SoD.leg = DOWNWIND;
1203                 SoD.x = (pattern_direction ? dlp : dlp * -1.0);
1204                 SoD.y = (blp - (stod - (blp + dlp))) * -1.0;
1205         }
1206 }
1207
1208 void FGAILocalTraffic::TransmitPatternPositionReport(void) {
1209         // airport name + "traffic" + airplane callsign + pattern direction + pattern leg + rwy + ?
1210         string trns = "";
1211         int code = 0;
1212
1213         trns += tower->get_name();
1214         trns += " Traffic ";
1215         trns += plane.callsign;
1216         if(patternDirection == 1) {
1217                 trns += " right ";
1218         } else {
1219                 trns += " left ";
1220         }
1221
1222         // We could probably get rid of this whole switch statement and just pass a string containing the leg from the FlyPattern function.
1223         switch(leg) {   // We'll assume that transmissions in turns are intended for next leg - do pilots ever call out that they are in the turn?
1224         case TURN1:
1225                 // Fall through to CROSSWIND
1226         case CROSSWIND: // I don't think this case will be used here but it can't hurt to leave it in
1227                 trns += "crosswind ";
1228                 break;
1229         case TURN2:
1230                 // Fall through to DOWNWIND
1231         case DOWNWIND:
1232                 trns += "downwind ";
1233                 code = 11;
1234                 break;
1235         case TURN3:
1236                 // Fall through to BASE
1237         case BASE:
1238                 trns += "base ";
1239                 break;
1240         case TURN4:
1241                 // Fall through to FINAL
1242         case FINAL:             // maybe this should include long/short final if appropriate?
1243                 trns += "final ";
1244                 code = 13;
1245                 break;
1246         default:                // Hopefully this won't be used
1247                 trns += "pattern ";
1248                 break;
1249         }
1250         trns += ConvertRwyNumToSpokenString(rwy.rwyID);
1251
1252         trns += " ";
1253
1254         // And add the airport name again
1255         trns += tower->get_name();
1256
1257         pending_transmission = trns;
1258         ConditionalTransmit(60.0, code);                // Assume a report of this leg will be invalid if we can't transmit within a minute.
1259 }
1260
1261 // Callback handler
1262 // TODO - Really should enumerate these coded values.
1263 void FGAILocalTraffic::ProcessCallback(int code) {
1264         // 1 - Request Departure from ground
1265         // 2 - Report at hold short
1266         // 3 - Report runway vacated
1267         // 10 - report crosswind
1268         // 11 - report downwind
1269         // 12 - report base
1270         // 13 - report final
1271         if(code == 1) {
1272                 ground->RequestDeparture(plane, this);
1273         } else if(code == 2) {
1274                 tower->ContactAtHoldShort(plane, this, CIRCUIT);
1275         } else if(code == 3) {
1276                 tower->ReportRunwayVacated(plane.callsign);
1277         } else if(code == 11) {
1278                 tower->ReportDownwind(plane.callsign);
1279         } else if(code == 13) {
1280                 tower->ReportFinal(plane.callsign);
1281         } else if(code == 99) { // Flag this instance for deletion
1282                 responseCounter = 0;
1283                 _removeSelf = true;
1284                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " delete instance callback called.");
1285         }
1286 }
1287
1288 void FGAILocalTraffic::ExitRunway(const SGVec3d& orthopos) {
1289         //cout << "In ExitRunway" << endl;
1290         //cout << "Runway ID is " << rwy.ID << endl;
1291
1292         _clearedToLand = false;
1293
1294         node_array_type exitNodes = ground->GetExits(rwy.rwyID);        //I suppose we ought to have some fallback for rwy with no defined exits?
1295         /*
1296         cout << "Node ID's of exits are ";
1297         for(unsigned int i=0; i<exitNodes.size(); ++i) {
1298                 cout << exitNodes[i]->nodeID << ' ';
1299         }
1300         cout << endl;
1301         */
1302         if(exitNodes.size()) {
1303                 //Find the next exit from orthopos.y
1304                 double d;
1305                 double dist = 100000;   //ie. longer than any runway in existance
1306                 double backdist = 100000;
1307                 node_array_iterator nItr = exitNodes.begin();
1308                 node* rwyExit = *(exitNodes.begin());
1309                 //int gateID;           //This might want to be more persistant at some point
1310                 while(nItr != exitNodes.end()) {
1311                         d = ortho.ConvertToLocal((*nItr)->pos).y() - ortho.ConvertToLocal(_pos).y();    //FIXME - consider making orthopos a class variable
1312                         if(d > 0.0) {
1313                                 if(d < dist) {
1314                                         dist = d;
1315                                         rwyExit = *nItr;
1316                                 }
1317                         } else {
1318                                 if(fabs(d) < backdist) {
1319                                         backdist = d;
1320                                         //TODO - need some logic here that if we don't get a forward exit we turn round and store the backwards one
1321                                 }
1322                         }
1323                         ++nItr;
1324                 }
1325                 ourGate = ground->GetGateNode();
1326                 if(ourGate == NULL) {
1327                         // Implies no available gates - what shall we do?
1328                         // For now just vanish the plane - possibly we can make this more elegant in the future
1329                         SG_LOG(SG_ATC, SG_ALERT, "No gate found by FGAILocalTraffic whilst landing at " << airportID << '\n');
1330                         //_aip.setVisible(false);
1331                         //cout << "Setting visible false\n";
1332                         operatingState = PARKED;
1333                         return;
1334                 }
1335                 path = ground->GetPath(rwyExit, ourGate);
1336                 /*
1337                 cout << "path returned was:" << endl;
1338                 for(unsigned int i=0; i<path.size(); ++i) {
1339                         switch(path[i]->struct_type) {
1340                         case NODE:
1341                                 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
1342                                 break;
1343                         case ARC:
1344                                 cout << "ARC\n";
1345                                 break;
1346                         }
1347                 }
1348                 */
1349                 taxiState = TD_INBOUND;
1350                 StartTaxi();
1351         } else {
1352                 // Something must have gone wrong with the ground network file - or there is only a rwy here and no exits defined
1353                 SG_LOG(SG_ATC, SG_INFO, "No exits found by FGAILocalTraffic from runway " << rwy.rwyID << " at " << airportID << '\n');
1354                 //if(airportID == "KRHV") cout << "No exits found by " << plane.callsign << " from runway " << rwy.rwyID << " at " << airportID << '\n';
1355                 // What shall we do - just remove the plane from sight?
1356                 _aip.setVisible(false);
1357                 _invisible = true;
1358                 //cout << "Setting visible false\n";
1359                 //tower->ReportRunwayVacated(plane.callsign);
1360                 string trns = "Clear of the runway ";
1361                 trns += plane.callsign;
1362                 pending_transmission = trns;
1363                 Transmit(3);
1364                 operatingState = PARKED;
1365         }
1366 }
1367
1368 // Set the class variable nextTaxiNode to the next node in the path
1369 // and update taxiPathPos, the class variable path iterator position
1370 // TODO - maybe should return error codes to the calling function if we fail here
1371 void FGAILocalTraffic::GetNextTaxiNode() {
1372         //cout << "GetNextTaxiNode called " << endl;
1373         //cout << "taxiPathPos = " << taxiPathPos << endl;
1374         ground_network_path_iterator pathItr = path.begin() + taxiPathPos;
1375         if(pathItr == path.end()) {
1376                 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - no more nodes in path\n");
1377         } else {
1378                 if((*pathItr)->struct_type == NODE) {
1379                         //cout << "ITS A NODE" << endl;
1380                         //*pathItr = new node;
1381                         nextTaxiNode = (node*)*pathItr;
1382                         ++taxiPathPos;
1383                         //delete pathItr;
1384                 } else {
1385                         //cout << "ITS NOT A NODE" << endl;
1386                         //The first item in found must have been an arc
1387                         //Assume for now that it was straight
1388                         pathItr++;
1389                         taxiPathPos++;
1390                         if(pathItr == path.end()) {
1391                                 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - path ended with an arc\n");
1392                         } else if((*pathItr)->struct_type == NODE) {
1393                                 nextTaxiNode = (node*)*pathItr;
1394                                 ++taxiPathPos;
1395                         } else {
1396                                 //OOPS - two non-nodes in a row - that shouldn't happen ATM
1397                                 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - two non-nodes in sequence\n");
1398                         }
1399                 }
1400         }
1401 }
1402
1403 // StartTaxi - set up the taxiing state - call only at the start of taxiing
1404 void FGAILocalTraffic::StartTaxi() {
1405         //cout << "StartTaxi called" << endl;
1406         operatingState = TAXIING;
1407
1408         taxiPathPos = 0;
1409
1410         //Set the desired heading
1411         //Assume we are aiming for first node on path
1412         //Eventually we may need to consider the fact that we might start on a curved arc and
1413         //not be able to head directly for the first node.
1414         GetNextTaxiNode();      // sets the class variable nextTaxiNode to the next taxi node!
1415         desiredTaxiHeading = GetHeadingFromTo(_pos, nextTaxiNode->pos);
1416         //cout << "First taxi heading is " << desiredTaxiHeading << endl;
1417 }
1418
1419 // speed in knots, headings in degrees, radius in meters.
1420 static double TaxiTurnTowardsHeading(double current_hdg, double desired_hdg, double speed, double radius, double dt) {
1421         // wrap heading - this prevents a logic bug where the plane would just go round in circles!!
1422         while(current_hdg < 0.0) {
1423                 current_hdg += 360.0;
1424         }
1425         while(current_hdg > 360.0) {
1426                 current_hdg -= 360.0;
1427         }
1428         if(fabs(current_hdg - desired_hdg) > 0.1) {
1429                 // Which is the quickest direction to turn onto heading?
1430                 if(desired_hdg > current_hdg) {
1431                         if((desired_hdg - current_hdg) <= 180) {
1432                                 // turn right
1433                                 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1434                                 // TODO - check that increments are less than the delta that we check for the right direction
1435                                 // Probably need to reduce convergence speed as convergence is reached
1436                         } else {
1437                                 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1438                         }
1439                 } else {
1440                         if((current_hdg - desired_hdg) <= 180) {
1441                                 // turn left
1442                                 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1443                                 // TODO - check that increments are less than the delta that we check for the right direction
1444                                 // Probably need to reduce convergence speed as convergence is reached
1445                         } else {
1446                                 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1447                         }
1448                 }
1449         }
1450         return(current_hdg);
1451 }
1452
1453 void FGAILocalTraffic::Taxi(double dt) {
1454         //cout << "Taxi called" << endl;
1455         // Logic - if we are further away from next point than turn radius then head for it
1456         // If we have reached turning point then get next point and turn onto that heading
1457         // Look out for the finish!!
1458
1459         desiredTaxiHeading = GetHeadingFromTo(_pos, nextTaxiNode->pos);
1460
1461         bool lastNode = (taxiPathPos == path.size() ? true : false);
1462         if(lastNode) {
1463                 //cout << "LAST NODE\n";
1464         }
1465
1466         // HACK ALERT! - for now we will taxi at constant speed for straights and turns
1467
1468         // Remember that hdg is always equal to track when taxiing so we don't have to consider them both
1469         double dist_to_go = dclGetHorizontalSeparation(_pos, nextTaxiNode->pos);        // we may be able to do this more cheaply using orthopos
1470         //cout << "dist_to_go = " << dist_to_go << endl;
1471         if((nextTaxiNode->type == GATE) && (dist_to_go <= 0.1)) {
1472                 // This might be more robust to outward paths starting with a gate if we check for either
1473                 // last node or TD_INBOUND ?
1474                 // park up
1475                 operatingState = PARKED;
1476         } else if(((dist_to_go > taxiTurnRadius) || (nextTaxiNode->type == GATE)) && (!liningUp)){
1477                 // if the turn radius is r, and speed is s, then in a time dt we turn through
1478                 // ((s.dt)/(PI.r)) x 180 degrees
1479                 // or alternatively (s.dt)/r radians
1480                 //cout << "hdg = " << hdg << " desired taxi heading = " << desiredTaxiHeading << '\n';
1481                 _hdg = TaxiTurnTowardsHeading(_hdg, desiredTaxiHeading, nominalTaxiSpeed, taxiTurnRadius, dt);
1482                 double vel = nominalTaxiSpeed;
1483                 //cout << "vel = " << vel << endl;
1484                 double dist = vel * 0.514444 * dt;
1485                 //cout << "dist = " << dist << endl;
1486                 double track = _hdg;
1487                 //cout << "track = " << track << endl;
1488                 double slope = 0.0;
1489                 _pos = dclUpdatePosition(_pos, track, slope, dist);
1490                 //cout << "Updated position...\n";
1491                 if(_ground_elevation_m > -9990) {
1492                         _pos.setElevationM(_ground_elevation_m + wheelOffset);
1493                 } // else don't change the elev until we get a valid ground elev again!
1494         } else if(lastNode) {
1495                 if(taxiState == TD_LINING_UP) {
1496                         if((!liningUp) && (dist_to_go <= taxiTurnRadius)) {
1497                                 liningUp = true;
1498                         }
1499                         if(liningUp) {
1500                                 _hdg = TaxiTurnTowardsHeading(_hdg, rwy.hdg, nominalTaxiSpeed, taxiTurnRadius, dt);
1501                                 double vel = nominalTaxiSpeed;
1502                                 //cout << "vel = " << vel << endl;
1503                                 double dist = vel * 0.514444 * dt;
1504                                 //cout << "dist = " << dist << endl;
1505                                 double track = _hdg;
1506                                 //cout << "track = " << track << endl;
1507                                 double slope = 0.0;
1508                                 _pos = dclUpdatePosition(_pos, track, slope, dist);
1509                                 //cout << "Updated position...\n";
1510                                 if(_ground_elevation_m > -9990) {
1511                                         _pos.setElevationM(_ground_elevation_m + wheelOffset);
1512                                 } // else don't change the elev until we get a valid ground elev again!
1513                                 if(fabs(_hdg - rwy.hdg) <= 1.0) {
1514                                         operatingState = IN_PATTERN;
1515                                         leg = TAKEOFF_ROLL;
1516                                         inAir = false;
1517                                         liningUp = false;
1518                                 }
1519                         }
1520                 } else if(taxiState == TD_OUTBOUND) {
1521                         // Pause awaiting further instructions
1522                         // and for now assume we've reached the hold-short node
1523                         holdingShort = true;
1524                 } // else at the moment assume TD_INBOUND always ends in a gate in which case we can ignore it
1525         } else {
1526                 // Time to turn (we've already checked it's not the end we're heading for).
1527                 // set the target node to be the next node which will prompt automatically turning onto
1528                 // the right heading in the stuff above, with the usual provisos applied.
1529                 GetNextTaxiNode();
1530                 // For now why not just recursively call this function?
1531                 Taxi(dt);
1532         }
1533 }
1534
1535
1536 // Warning - ground elev determination is CPU intensive
1537 // Either this function or the logic of how often it is called
1538 // will almost certainly change.
1539 void FGAILocalTraffic::DoGroundElev() {
1540         // Only do the proper hitlist stuff if we are within visible range of the viewer.
1541         double visibility_meters = fgGetDouble("/environment/visibility-m");
1542         FGViewer* vw = globals->get_current_view();
1543         if(dclGetHorizontalSeparation(_pos, SGGeod::fromGeodM(vw->getPosition(), 0.0)) > visibility_meters) {
1544                 _ground_elevation_m = aptElev;
1545                 return;
1546         }
1547
1548   // FIXME: make shure the pos.lat/pos.lon values are in degrees ...
1549   double range = 500.0;
1550   if (!globals->get_tile_mgr()->scenery_available(_aip.getPosition(), range)) {
1551     // Try to shedule tiles for that position.
1552     globals->get_tile_mgr()->update( _aip.getPosition(), range );
1553   }
1554
1555   // FIXME: make shure the pos.lat/pos.lon values are in degrees ...
1556   double alt;
1557   if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(_aip.getPosition(), 20000), alt, 0, _aip.getSceneGraph()))
1558     _ground_elevation_m = alt;
1559 }
1560