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 ENROUTE:   break;
 550                 case DEPARTURE: break;
 551                 case INVALID:   break;
 552                 }
 553                 changeFreq = false;
 554         }
 555
 556         //cout << "," << flush;
 557
 558         switch(operatingState) {
 559         case IN_PATTERN:
 560                 //cout << "In IN_PATTERN\n";
 561                 if(!inAir) {
 562                         DoGroundElev();
 563                         if(!elevInitGood) {
 564                                 if(_ground_elevation_m > -9990.0) {
 565                                         _pos.setElevationM(_ground_elevation_m + wheelOffset);
 566                                         //cout << "TAKEOFF_ROLL, POS = " << pos.lon() << ", " << pos.lat() << ", " << pos.elev() << '\n';
 567                                         //Transform();
 568                                         _aip.setVisible(true);
 569                                         //cout << "Making plane visible!\n";
 570                                         elevInitGood = true;
 571                                 }
 572                         }
 573                 }
 574                 FlyTrafficPattern(dt);
 575                 Transform();
 576                 break;
 577         case TAXIING:
 578                 //cout << "In TAXIING\n";
 579                 //cout << "*" << flush;
 580                 if(!elevInitGood) {
 581                         //DoGroundElev();
 582                         if(_ground_elevation_m > -9990.0) {
 583                                 _pos.setElevationM(_ground_elevation_m + wheelOffset);
 584                                 //Transform();
 585                                 _aip.setVisible(true);
 586                                 //Transform();
 587                                 //cout << "Making plane visible!\n";
 588                                 elevInitGood = true;
 589                         }
 590                 }
 591                 DoGroundElev();
 592                 //cout << "~" << flush;
 593                 if(!((holdingShort) && (!clearedToLineUp))) {
 594                         //cout << "|" << flush;
 595                         Taxi(dt);
 596                 }
 597                 //cout << ";" << flush;
 598                 if((clearedToTakeOff) && (responseCounter >= 8.0)) {
 599                         // possible assumption that we're at the hold short here - may not always hold
 600                         // TODO - sort out the case where we're cleared to line-up first and then cleared to take-off on the rwy.
 601                         taxiState = TD_LINING_UP;
 602                         //cout << "A" << endl;
 603                         path = ground->GetPath(holdShortNode, rwy.rwyID);
 604                         //cout << "B" << endl;
 605                         if(!path.size()) {      // Assume no facility file so we'll just taxi to a point on the runway near the threshold
 606                                 //cout << "C" << endl;
 607                                 node* np = new node;
 608                                 np->struct_type = NODE;
 609                                 np->pos = ortho.ConvertFromLocal(SGVec3d(0.0, 10.0, 0.0));
 610                                 path.push_back(np);
 611                         } else {
 612                                 //cout << "D" << endl;
 613                         }
 614                         /*
 615                         cout << "path returned was:" << endl;
 616                         for(unsigned int i=0; i<path.size(); ++i) {
 617                                 switch(path[i]->struct_type) {
 618                                         case NODE:
 619                                         cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
 620                                         break;
 621                                         case ARC:
 622                                         cout << "ARC\n";
 623                                         break;
 624                                 }
 625                         }
 626                         */
 627                         clearedToTakeOff = false;       // We *are* still cleared - this simply stops the response recurring!!
 628                         holdingShort = false;
 629                         string trns = "Cleared for take-off ";
 630                         trns += plane.callsign;
 631                         pending_transmission = trns;
 632                         Transmit();
 633                         StartTaxi();
 634                 }
 635                 //cout << "^" << flush;
 636                 Transform();
 637                 break;
 638         case PARKED:
 639                 //cout << "In PARKED\n";
 640                 if(!elevInitGood) {
 641                         DoGroundElev();
 642                         if(_ground_elevation_m > -9990.0) {
 643                                 _pos.setElevationM(_ground_elevation_m + wheelOffset);
 644                                 //Transform();
 645                                 _aip.setVisible(true);
 646                                 //Transform();
 647                                 //cout << "Making plane visible!\n";
 648                                 elevInitGood = true;
 649                         }
 650                 }
 651
 652                 if(circuitsToFly) {
 653                         if((taxiRequestPending) && (taxiRequestCleared)) {
 654                                 //cout << "&" << flush;
 655                                 // Get the active runway details (in case they've changed since init)
 656                                 GetRwyDetails(airportID);
 657
 658                                 // Get the takeoff node for the active runway, get a path to it and start taxiing
 659                                 path = ground->GetPathToHoldShort(ourGate, rwy.rwyID);
 660                                 if(path.size() < 2) {
 661                                         // something has gone wrong
 662                                         SG_LOG(SG_ATC, SG_ALERT, "Invalid path from gate to theshold in FGAILocalTraffic::FlyCircuits\n");
 663                                         return;
 664                                 }
 665                                 /*
 666                                 cout << "path returned was:\n";
 667                                 for(unsigned int i=0; i<path.size(); ++i) {
 668                                         switch(path[i]->struct_type) {
 669                                                 case NODE:
 670                                                 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
 671                                                 break;
 672                                                 case ARC:
 673                                                 cout << "ARC\n";
 674                                                 break;
 675                                         }
 676                                 }
 677                                 */
 678                                 path.erase(path.begin());       // pop the gate - we're here already!
 679                                 taxiState = TD_OUTBOUND;
 680                                 taxiRequestPending = false;
 681                                 holdShortNode = (node*)(*(path.begin() + path.size()));
 682                                 StartTaxi();
 683                         } else if(!taxiRequestPending) {
 684                                 //cout << "(" << flush;
 685                                 // Do some communication
 686                                 // airport name + tower + airplane callsign + location + request taxi for + operation type + ?
 687                                 string trns = "";
 688                                 if(_controlled) {
 689                                         trns += tower->get_name();
 690                                         trns += " tower ";
 691                                 } else {
 692                                         trns += "Traffic ";
 693                                         // TODO - get the airport name somehow if uncontrolled
 694                                 }
 695                                 trns += plane.callsign;
 696                                 trns += " on apron parking request taxi for traffic pattern";
 697                                 //cout << "trns = " << trns << endl;
 698                                 pending_transmission = trns;
 699                                 Transmit(1);
 700                                 taxiRequestCleared = false;
 701                                 taxiRequestPending = true;
 702                         }
 703                 }
 704
 705                 //cout << "!" << flush;
 706
 707                 // Maybe the below should be set when we get to the threshold and prepare for TO?
 708                 // FIXME TODO - pattern direction is still hardwired
 709                 patternDirection = -1;          // Left
 710                 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
 711                 if(rwy.rwyID.size() == 3) {
 712                         patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
 713                 }
 714                 // Do nothing
 715                 Transform();
 716                 //cout << ")" << flush;
 717                 break;
 718         default:
 719                 break;
 720         }
 721         //cout << "I " << flush;
 722
 723         //cout << "Update _pos = " << _pos << ", vis = " << _aip.getVisible() << '\n';
 724
 725         // Convienience output for AI debugging using the property logger
 726         //fgSetDouble("/AI/Local1/ortho-x", (ortho.ConvertToLocal(_pos)).x());
 727         //fgSetDouble("/AI/Local1/ortho-y", (ortho.ConvertToLocal(_pos)).y());
 728         //fgSetDouble("/AI/Local1/elev", _pos.elev() * SG_METER_TO_FEET);
 729
 730         // And finally, call parent.
 731         FGAIPlane::Update(dt);
 732 }
 733
 734 void FGAILocalTraffic::RegisterTransmission(int code) {
 735         switch(code) {
 736         case 1: // taxi request cleared
 737                 taxiRequestCleared = true;
 738                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to taxi...");
 739                 break;
 740         case 2: // contact tower
 741                 responseCounter = 0;
 742                 contactTower = true;
 743                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " told to contact tower...");
 744                 break;
 745         case 3: // Cleared to line up
 746                 responseCounter = 0;
 747                 clearedToLineUp = true;
 748                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to line-up...");
 749                 break;
 750         case 4: // cleared to take-off
 751                 responseCounter = 0;
 752                 clearedToTakeOff = true;
 753                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to take-off...");
 754                 break;
 755         case 5: // contact ground
 756                 responseCounter = 0;
 757                 contactGround = true;
 758                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " told to contact ground...");
 759                 break;
 760         // case 6 is a temporary mega-hack for controlled airports without separate ground control
 761         case 6: // taxi to the GA parking
 762                 responseCounter = 0;
 763                 _taxiToGA = true;
 764                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " told to taxi to the GA parking...");
 765                 break;
 766         case 7:  // Cleared to land (also implies cleared for the option
 767                 _clearedToLand = true;
 768                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to land...");
 769                 break;
 770         case 13: // Go around!
 771                 responseCounter = 0;
 772                 goAround = true;
 773                 _clearedToLand = false;
 774                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " told to go-around!!");
 775                 break;
 776         default:
 777                 break;
 778         }
 779 }
 780
 781 // Fly a traffic pattern
 782 // FIXME - far too much of the mechanics of turning, rolling, accellerating, descending etc is in here.
 783 //         Move it out to FGAIPlane and have FlyTrafficPattern just specify what to do, not the implementation.
 784 void FGAILocalTraffic::FlyTrafficPattern(double dt) {
 785         // Need to differentiate between in-air (IAS governed) and on-ground (vel governed)
 786         // Take-off is an interesting case - we are on the ground but takeoff speed is IAS governed.
 787
 788         // WIND
 789         // Wind has two effects - a mechanical one in that IAS translates to a different vel, and the hdg != track,
 790         // but also a piloting effect, in that the AI must be able to descend at a different rate in order to hit the threshold.
 791
 792         //cout << "dt = " << dt << '\n';
 793         double dist = 0;
 794         // ack - I can't remember how long a rate 1 turn is meant to take.
 795         double turn_time = 60.0;        // seconds - TODO - check this guess
 796         double turn_circumference;
 797         double turn_radius;
 798         SGVec3d orthopos = ortho.ConvertToLocal(_pos);  // ortho position of the plane
 799         //cout << "runway elev = " << rwy.threshold_pos.getElevationM() << ' ' << rwy.threshold_pos.getElevationM() * SG_METER_TO_FEET << '\n';
 800         //cout << "elev = " << _pos.elev() << ' ' << _pos.elev() * SG_METER_TO_FEET << '\n';
 801
 802         // HACK FOR TESTING - REMOVE
 803         //cout << "Calling ExitRunway..." << endl;
 804         //ExitRunway(orthopos);
 805         //return;
 806         // END HACK
 807
 808         //wind
 809         double wind_from = wind_from_hdg->getDoubleValue();
 810         double wind_speed = wind_speed_knots->getDoubleValue();
 811
 812         double dveldt;
 813
 814         switch(leg) {
 815         case TAKEOFF_ROLL:
 816                 //inAir = false;
 817                 track = rwy.hdg;
 818                 if(vel < 80.0) {
 819                         double dveldt = 5.0;
 820                         vel += dveldt * dt;
 821                 }
 822                 if(_ground_elevation_m > -9990.0) {
 823                         _pos.setElevationM(_ground_elevation_m + wheelOffset);
 824                 }
 825                 IAS = vel + (cos((_hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
 826                 if(IAS >= 70) {
 827                         leg = CLIMBOUT;
 828                         SetTrack(rwy.hdg);      // Hands over control of turning to AIPlane
 829                         _pitch = 10.0;
 830                         IAS = best_rate_of_climb_speed;
 831                         //slope = 7.0;
 832                         slope = 6.0;    // Reduced it slightly since it's climbing a lot steeper than I can in the JSBSim C172.
 833                         inAir = true;
 834                 }
 835                 break;
 836         case CLIMBOUT:
 837                 // Turn to crosswind if above 700ft AND if other traffic allows
 838                 // (decided in FGTower and accessed through GetCrosswindConstraint(...)).
 839                 // According to AIM, traffic should climb to within 300ft of pattern altitude before commencing crosswind turn.
 840                 // TODO - At hot 'n high airports this may be 500ft AGL though - need to make this a variable.
 841                 if((_pos.getElevationM() - rwy.threshold_pos.getElevationM()) * SG_METER_TO_FEET > 700) {
 842                         double cc = 0.0;
 843                         if(tower->GetCrosswindConstraint(cc)) {
 844                                 if(orthopos.y() > cc) {
 845                                         //cout << "Turning to crosswind, distance from threshold = " << orthopos.y() << '\n';
 846                                         leg = TURN1;
 847                                 }
 848                         } else if(orthopos.y() > 1500.0) {   // Added this constraint as a hack to prevent turning too early when going around.
 849                                 // TODO - We should be doing it as a distance from takeoff end, not theshold end though.
 850                                 //cout << "Turning to crosswind, distance from threshold = " << orthopos.y() << '\n';
 851                                 leg = TURN1;
 852                         }
 853                 }
 854                 // Need to check for levelling off in case we can't turn crosswind as soon
 855                 // as we would like due to other traffic.
 856                 if((_pos.getElevationM() - rwy.threshold_pos.getElevationM()) * SG_METER_TO_FEET > 1000) {
 857                         slope = 0.0;
 858                         _pitch = 0.0;
 859                         IAS = 80.0;             // FIXME - use smooth transistion to new speed and attitude.
 860                 }
 861                 if(goAround && !goAroundCalled) {
 862                         if(responseCounter > 5.5) {
 863                                 pending_transmission = plane.callsign;
 864                                 pending_transmission += " going around";
 865                                 Transmit();
 866                                 goAroundCalled = true;
 867                         }
 868                 }
 869                 break;
 870         case TURN1:
 871                 SetTrack(rwy.hdg + (90.0 * patternDirection));
 872                 if((track < (rwy.hdg - 89.0)) || (track > (rwy.hdg + 89.0))) {
 873                         leg = CROSSWIND;
 874                 }
 875                 break;
 876         case CROSSWIND:
 877                 goAround = false;
 878                 if((_pos.getElevationM() - rwy.threshold_pos.getElevationM()) * SG_METER_TO_FEET > 1000) {
 879                         slope = 0.0;
 880                         _pitch = 0.0;
 881                         IAS = 80.0;             // FIXME - use smooth transistion to new speed
 882                 }
 883                 // turn 1000m out for now, taking other traffic into accout
 884                 if(fabs(orthopos.x()) > 900) {
 885                         double dd = 0.0;
 886                         if(tower->GetDownwindConstraint(dd)) {
 887                                 if(fabs(orthopos.x()) > fabs(dd)) {
 888                                         //cout << "Turning to downwind, distance from centerline = " << fabs(orthopos.x()) << '\n';
 889                                         leg = TURN2;
 890                                 }
 891                         } else {
 892                                 //cout << "Turning to downwind, distance from centerline = " << fabs(orthopos.x()) << '\n';
 893                                 leg = TURN2;
 894                         }
 895                 }
 896                 break;
 897         case TURN2:
 898                 SetTrack(rwy.hdg - (180 * patternDirection));
 899                 // just in case we didn't make height on crosswind
 900                 if((_pos.getElevationM() - rwy.threshold_pos.getElevationM()) * SG_METER_TO_FEET > 1000) {
 901                         slope = 0.0;
 902                         _pitch = 0.0;
 903                         IAS = 80.0;             // FIXME - use smooth transistion to new speed
 904                 }
 905                 if((track < (rwy.hdg - 179.0)) || (track > (rwy.hdg + 179.0))) {
 906                         leg = DOWNWIND;
 907                         transmitted = false;
 908                 }
 909                 break;
 910         case DOWNWIND:
 911                 // just in case we didn't make height on crosswind
 912                 if(((_pos.getElevationM() - rwy.threshold_pos.getElevationM()) * SG_METER_TO_FEET > 995) && ((_pos.getElevationM() - rwy.threshold_pos.getElevationM()) * SG_METER_TO_FEET < 1015)) {
 913                         slope = 0.0;
 914                         _pitch = 0.0;
 915                         IAS = 90.0;             // FIXME - use smooth transistion to new speed
 916                 }
 917                 if((_pos.getElevationM() - rwy.threshold_pos.getElevationM()) * SG_METER_TO_FEET >= 1015) {
 918                         slope = -1.0;
 919                         _pitch = -1.0;
 920                         IAS = 90.0;             // FIXME - use smooth transistion to new speed
 921                 }
 922                 if((orthopos.y() < 0) && (!transmitted)) {
 923                         TransmitPatternPositionReport();
 924                         transmitted = true;
 925                 }
 926                 if((orthopos.y() < -100) && (!descending)) {
 927                         //cout << "DDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDdddd\n";
 928                         // Maybe we should think about when to start descending.
 929                         // For now we're assuming that we aim to follow the same glidepath regardless of wind.
 930                         double d1;
 931                         double d2;
 932                         CalculateSoD((tower->GetBaseConstraint(d1) ? d1 : -1000.0), (tower->GetDownwindConstraint(d2) ? d2 : 1000.0 * patternDirection), (patternDirection ? true : false));
 933                         if(SoD.leg == DOWNWIND) {
 934                                 descending = (orthopos.y() < SoD.y ? true : false);
 935                         }
 936
 937                 }
 938                 if(descending) {
 939                         slope = -5.5;   // FIXME - calculate to descent at 500fpm and hit the desired point on the runway (taking wind into account as well!!)
 940                         _pitch = -3.0;
 941                         IAS = 85.0;
 942                 }
 943
 944                 // Try and arrange to turn nicely onto base
 945                 turn_circumference = IAS * 0.514444 * turn_time;
 946                 //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius
 947                 //We'll leave it as a hack with IAS for now but it needs revisiting.
 948                 turn_radius = turn_circumference / (2.0 * DCL_PI);
 949                 if(orthopos.y() < -1000.0 + turn_radius) {
 950                 //if(orthopos.y() < -980) {
 951                         double bb = 0.0;
 952                         if(tower->GetBaseConstraint(bb)) {
 953                                 if(fabs(orthopos.y()) > fabs(bb)) {
 954                                         //cout << "Turning to base, distance from threshold = " << fabs(orthopos.y()) << '\n';
 955                                         leg = TURN3;
 956                                         transmitted = false;
 957                                         IAS = 80.0;
 958                                 }
 959                         } else {
 960                                 //cout << "Turning to base, distance from threshold = " << fabs(orthopos.y()) << '\n';
 961                                 leg = TURN3;
 962                                 transmitted = false;
 963                                 IAS = 80.0;
 964                         }
 965                 }
 966                 break;
 967         case TURN3:
 968                 SetTrack(rwy.hdg - (90 * patternDirection));
 969                 if(fabs(rwy.hdg - track) < 91.0) {
 970                         leg = BASE;
 971                 }
 972                 break;
 973         case BASE:
 974                 if(!transmitted) {
 975                         // Base report should only be transmitted at uncontrolled airport - not towered.
 976                         if(!_controlled) TransmitPatternPositionReport();
 977                         transmitted = true;
 978                 }
 979
 980                 if(!descending) {
 981                         double d1;
 982                         // Make downwind leg position artifically large to avoid any chance of SoD being returned as
 983                         // on downwind when we are already on base.
 984                         CalculateSoD((tower->GetBaseConstraint(d1) ? d1 : -1000.0), (10000.0 * patternDirection), (patternDirection ? true : false));
 985                         if(SoD.leg == BASE) {
 986                                 descending = (fabs(orthopos.y()) < fabs(SoD.y) ? true : false);
 987                         }
 988
 989                 }
 990                 if(descending) {
 991                         slope = -5.5;   // FIXME - calculate to descent at 500fpm and hit the threshold (taking wind into account as well!!)
 992                         _pitch = -4.0;
 993                         IAS = 70.0;
 994                 }
 995
 996                 // Try and arrange to turn nicely onto final
 997                 turn_circumference = IAS * 0.514444 * turn_time;
 998                 //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius
 999                 //We'll leave it as a hack with IAS for now but it needs revisiting.
1000                 turn_radius = turn_circumference / (2.0 * DCL_PI);
1001                 if(fabs(orthopos.x()) < (turn_radius + 50)) {
1002                         leg = TURN4;
1003                         transmitted = false;
1004                         //roll = -20;
1005                 }
1006                 break;
1007         case TURN4:
1008                 SetTrack(rwy.hdg);
1009                 if(fabs(track - rwy.hdg) < 0.6) {
1010                         leg = FINAL;
1011                         vel = nominal_final_speed;
1012                 }
1013                 break;
1014         case FINAL:
1015                 if(goAround && responseCounter > 2.0) {
1016                         leg = CLIMBOUT;
1017                         _pitch = 8.0;
1018                         IAS = best_rate_of_climb_speed;
1019                         slope = 5.0;    // A bit less steep than the initial climbout.
1020                         inAir = true;
1021                         goAroundCalled = false;
1022                         descending = false;
1023                         break;
1024                 }
1025                 LevelWings();
1026                 if(!transmitted) {
1027                         if((!_controlled) || (!_clearedToLand)) TransmitPatternPositionReport();
1028                         transmitted = true;
1029                 }
1030                 if(!descending) {
1031                         // Make base leg position artifically large to avoid any chance of SoD being returned as
1032                         // on base or downwind when we are already on final.
1033                         CalculateSoD(-10000.0, (1000.0 * patternDirection), (patternDirection ? true : false));
1034                         if(SoD.leg == FINAL) {
1035                                 descending = (fabs(orthopos.y()) < fabs(SoD.y) ? true : false);
1036                         }
1037
1038                 }
1039                 if(descending) {
1040                         if(orthopos.y() < -50.0) {
1041                                 double thesh_offset = 30.0;
1042                                 slope = atan((_pos.getElevationM() - fgGetAirportElev(airportID)) / (orthopos.y() - thesh_offset)) * DCL_RADIANS_TO_DEGREES;
1043                                 //cout << "slope = " << slope << ", elev = " << _pos.elev() << ", apt_elev = " << fgGetAirportElev(airportID) << ", op.y = " << orthopos.y() << '\n';
1044                                 if(slope < -10.0) slope = -10.0;
1045                                 _savedSlope = slope;
1046                                 _pitch = -4.0;
1047                                 IAS = 70.0;
1048                         } else {
1049                                 if(_pos.getElevationM() < (rwy.threshold_pos.getElevationM()+10.0+wheelOffset)) {
1050                                         if(_ground_elevation_m > -9990.0) {
1051                                                 if(_pos.getElevationM() < (_ground_elevation_m + wheelOffset + 1.0)) {
1052                                                         slope = -2.0;
1053                                                         _pitch = 1.0;
1054                                                         IAS = 55.0;
1055                                                 } else if(_pos.getElevationM() < (_ground_elevation_m + wheelOffset + 5.0)) {
1056                                                         slope = -4.0;
1057                                                         _pitch = -2.0;
1058                                                         IAS = 60.0;
1059                                                 } else {
1060                                                         slope = _savedSlope;
1061                                                         _pitch = -3.0;
1062                                                         IAS = 65.0;
1063                                                 }
1064                                         } else {
1065                                                 // Elev not determined
1066                                                 slope = _savedSlope;
1067                                                 _pitch = -3.0;
1068                                                 IAS = 65.0;
1069                                         }
1070                                 } else {
1071                                         slope = _savedSlope;
1072                                         _pitch = -3.0;
1073                                         IAS = 65.0;
1074                                 }
1075                         }
1076                 }
1077                 // Try and track the extended centreline
1078                 SetTrack(rwy.hdg - (0.2 * orthopos.x()));
1079                 //cout << "orthopos.x() = " << orthopos.x() << " hdg = " << hdg << '\n';
1080                 if(_pos.getElevationM() < (rwy.threshold_pos.getElevationM()+20.0+wheelOffset)) {
1081                         DoGroundElev(); // Need to call it here expicitly on final since it's only called
1082                                         // for us in update(...) when the inAir flag is false.
1083                 }
1084                 if(_pos.getElevationM() < (rwy.threshold_pos.getElevationM()+10.0+wheelOffset)) {
1085                         //slope = -1.0;
1086                         //_pitch = 1.0;
1087                         if(_ground_elevation_m > -9990.0) {
1088                                 if((_ground_elevation_m + wheelOffset) > _pos.getElevationM()) {
1089                                         slope = 0.0;
1090                                         _pitch = 0.0;
1091                                         leg = LANDING_ROLL;
1092                                         inAir = false;
1093                                         LevelWings();
1094                                         ClearTrack();   // Take over explicit track handling since AIPlane currently always banks when changing course
1095                                 }
1096                         }       // else need a fallback position based on arpt elev in case ground elev determination fails?
1097                 } else {
1098                         // TODO
1099                 }
1100                 break;
1101         case LANDING_ROLL:
1102                 //inAir = false;
1103                 descending = false;
1104                 if(_ground_elevation_m > -9990.0) {
1105                         _pos.setElevationM(_ground_elevation_m + wheelOffset);
1106                 }
1107                 track = rwy.hdg;
1108                 dveldt = -5.0;
1109                 vel += dveldt * dt;
1110                 // FIXME - differentiate between touch and go and full stops
1111                 if(vel <= 15.0) {
1112                         //cout << "Vel <= 15.0, circuitsToFly = " << circuitsToFly << endl;
1113                         if(circuitsToFly <= 0) {
1114                                 //cout << "Calling ExitRunway..." << endl;
1115                                 ExitRunway(orthopos);
1116                                 return;
1117                         } else {
1118                                 //cout << "Taking off again..." << endl;
1119                                 leg = TAKEOFF_ROLL;
1120                                 --circuitsToFly;
1121                         }
1122                 }
1123                 break;
1124         case LEG_UNKNOWN:
1125                 break;
1126     }
1127
1128         if(inAir) {
1129                 // FIXME - at the moment this is a bit screwy
1130                 // The velocity correction is applied based on the relative headings.
1131                 // Then the heading is changed based on the velocity.
1132                 // Which comes first, the chicken or the egg?
1133                 // Does it really matter?
1134
1135                 // Apply wind to ground-relative velocity if in the air
1136                 vel = IAS - (cos((_hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
1137                 //crab = f(track, wind, vel);
1138                 // The vector we need to fly is our desired vector minus the wind vector
1139                 // TODO - we probably ought to use plib's built in vector types and operations for this
1140                 // ie.  There's almost *certainly* a better way to do this!
1141                 double gxx = vel * sin(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity x component wrt ground
1142                 double gyy = vel * cos(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity y component wrt ground
1143                 double wxx = wind_speed * sin((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS);    // Wind velocity x component
1144                 double wyy = wind_speed * cos((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS);    // Wind velocity y component
1145                 double axx = gxx - wxx; // Plane in-air velocity x component
1146                 double ayy = gyy - wyy; // Plane in-air velocity y component
1147                 // Now we want the angle between gxx and axx (which is the crab)
1148                 double maga = sqrt(axx*axx + ayy*ayy);
1149                 double magg = sqrt(gxx*gxx + gyy*gyy);
1150                 crab = acos((axx*gxx + ayy*gyy) / (maga * magg));
1151                 // At this point this works except we're getting the modulus of the angle
1152                 //cout << "crab = " << crab << '\n';
1153
1154                 // Make sure both headings are in the 0->360 circle in order to get sane differences
1155                 dclBoundHeading(wind_from);
1156                 dclBoundHeading(track);
1157                 if(track > wind_from) {
1158                         if((track - wind_from) <= 180) {
1159                                 crab *= -1.0;
1160                         }
1161                 } else {
1162                         if((wind_from - track) >= 180) {
1163                                 crab *= -1.0;
1164                         }
1165                 }
1166         } else {        // on the ground - crab dosen't apply
1167                 crab = 0.0;
1168         }
1169
1170         //cout << "X " << orthopos.x() << "  Y " << orthopos.y() << "  SLOPE " << slope << "  elev " << _pos.elev() * SG_METER_TO_FEET << '\n';
1171
1172         _hdg = track + crab;
1173         dist = vel * 0.514444 * dt;
1174         _pos = dclUpdatePosition(_pos, track, slope, dist);
1175 }
1176
1177 // Pattern direction is true for right, false for left
1178 void FGAILocalTraffic::CalculateSoD(double base_leg_pos, double downwind_leg_pos, bool pattern_direction) {
1179         // For now we'll ignore wind and hardwire the glide angle.
1180         double ga = 5.5;        //degrees
1181         double pa = 1000.0 * SG_FEET_TO_METER;  // pattern altitude in meters
1182         // FIXME - get glideslope angle and pattern altitude agl from airport details if available
1183
1184         // For convienience, we'll have +ve versions of the input distances
1185         double blp = fabs(base_leg_pos);
1186         double dlp = fabs(downwind_leg_pos);
1187
1188         //double turn_allowance = 150.0;        // Approximate distance in meters that a 90deg corner is shortened by turned in a light plane.
1189
1190         double stod = pa / tan(ga * DCL_DEGREES_TO_RADIANS);    // distance in meters from touchdown point to start descent
1191         //cout << "Descent to start = " << stod << " meters out\n";
1192         if(stod < blp) {        // Start descending on final
1193                 SoD.leg = FINAL;
1194                 SoD.y = stod * -1.0;
1195                 SoD.x = 0.0;
1196         } else if(stod < (blp + dlp)) { // Start descending on base leg
1197                 SoD.leg = BASE;
1198                 SoD.y = blp * -1.0;
1199                 SoD.x = (pattern_direction ? (stod - dlp) : (stod - dlp) * -1.0);
1200         } else {        // Start descending on downwind leg
1201                 SoD.leg = DOWNWIND;
1202                 SoD.x = (pattern_direction ? dlp : dlp * -1.0);
1203                 SoD.y = (blp - (stod - (blp + dlp))) * -1.0;
1204         }
1205 }
1206
1207 void FGAILocalTraffic::TransmitPatternPositionReport(void) {
1208         // airport name + "traffic" + airplane callsign + pattern direction + pattern leg + rwy + ?
1209         string trns = "";
1210         int code = 0;
1211
1212         trns += tower->get_name();
1213         trns += " Traffic ";
1214         trns += plane.callsign;
1215         if(patternDirection == 1) {
1216                 trns += " right ";
1217         } else {
1218                 trns += " left ";
1219         }
1220
1221         // We could probably get rid of this whole switch statement and just pass a string containing the leg from the FlyPattern function.
1222         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?
1223         case TURN1:
1224                 // Fall through to CROSSWIND
1225         case CROSSWIND: // I don't think this case will be used here but it can't hurt to leave it in
1226                 trns += "crosswind ";
1227                 break;
1228         case TURN2:
1229                 // Fall through to DOWNWIND
1230         case DOWNWIND:
1231                 trns += "downwind ";
1232                 code = 11;
1233                 break;
1234         case TURN3:
1235                 // Fall through to BASE
1236         case BASE:
1237                 trns += "base ";
1238                 break;
1239         case TURN4:
1240                 // Fall through to FINAL
1241         case FINAL:             // maybe this should include long/short final if appropriate?
1242                 trns += "final ";
1243                 code = 13;
1244                 break;
1245         default:                // Hopefully this won't be used
1246                 trns += "pattern ";
1247                 break;
1248         }
1249         trns += ConvertRwyNumToSpokenString(rwy.rwyID);
1250
1251         trns += " ";
1252
1253         // And add the airport name again
1254         trns += tower->get_name();
1255
1256         pending_transmission = trns;
1257         ConditionalTransmit(60.0, code);                // Assume a report of this leg will be invalid if we can't transmit within a minute.
1258 }
1259
1260 // Callback handler
1261 // TODO - Really should enumerate these coded values.
1262 void FGAILocalTraffic::ProcessCallback(int code) {
1263         // 1 - Request Departure from ground
1264         // 2 - Report at hold short
1265         // 3 - Report runway vacated
1266         // 10 - report crosswind
1267         // 11 - report downwind
1268         // 12 - report base
1269         // 13 - report final
1270         if(code == 1) {
1271                 ground->RequestDeparture(plane, this);
1272         } else if(code == 2) {
1273                 tower->ContactAtHoldShort(plane, this, CIRCUIT);
1274         } else if(code == 3) {
1275                 tower->ReportRunwayVacated(plane.callsign);
1276         } else if(code == 11) {
1277                 tower->ReportDownwind(plane.callsign);
1278         } else if(code == 13) {
1279                 tower->ReportFinal(plane.callsign);
1280         } else if(code == 99) { // Flag this instance for deletion
1281                 responseCounter = 0;
1282                 _removeSelf = true;
1283                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " delete instance callback called.");
1284         }
1285 }
1286
1287 void FGAILocalTraffic::ExitRunway(const SGVec3d& orthopos) {
1288         //cout << "In ExitRunway" << endl;
1289         //cout << "Runway ID is " << rwy.ID << endl;
1290
1291         _clearedToLand = false;
1292
1293         node_array_type exitNodes = ground->GetExits(rwy.rwyID);        //I suppose we ought to have some fallback for rwy with no defined exits?
1294         /*
1295         cout << "Node ID's of exits are ";
1296         for(unsigned int i=0; i<exitNodes.size(); ++i) {
1297                 cout << exitNodes[i]->nodeID << ' ';
1298         }
1299         cout << endl;
1300         */
1301         if(exitNodes.size()) {
1302                 //Find the next exit from orthopos.y
1303                 double d;
1304                 double dist = 100000;   //ie. longer than any runway in existance
1305                 double backdist = 100000;
1306                 node_array_iterator nItr = exitNodes.begin();
1307                 node* rwyExit = *(exitNodes.begin());
1308                 //int gateID;           //This might want to be more persistant at some point
1309                 while(nItr != exitNodes.end()) {
1310                         d = ortho.ConvertToLocal((*nItr)->pos).y() - ortho.ConvertToLocal(_pos).y();    //FIXME - consider making orthopos a class variable
1311                         if(d > 0.0) {
1312                                 if(d < dist) {
1313                                         dist = d;
1314                                         rwyExit = *nItr;
1315                                 }
1316                         } else {
1317                                 if(fabs(d) < backdist) {
1318                                         backdist = d;
1319                                         //TODO - need some logic here that if we don't get a forward exit we turn round and store the backwards one
1320                                 }
1321                         }
1322                         ++nItr;
1323                 }
1324                 ourGate = ground->GetGateNode();
1325                 if(ourGate == NULL) {
1326                         // Implies no available gates - what shall we do?
1327                         // For now just vanish the plane - possibly we can make this more elegant in the future
1328                         SG_LOG(SG_ATC, SG_ALERT, "No gate found by FGAILocalTraffic whilst landing at " << airportID << '\n');
1329                         //_aip.setVisible(false);
1330                         //cout << "Setting visible false\n";
1331                         operatingState = PARKED;
1332                         return;
1333                 }
1334                 path = ground->GetPath(rwyExit, ourGate);
1335                 /*
1336                 cout << "path returned was:" << endl;
1337                 for(unsigned int i=0; i<path.size(); ++i) {
1338                         switch(path[i]->struct_type) {
1339                         case NODE:
1340                                 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
1341                                 break;
1342                         case ARC:
1343                                 cout << "ARC\n";
1344                                 break;
1345                         }
1346                 }
1347                 */
1348                 taxiState = TD_INBOUND;
1349                 StartTaxi();
1350         } else {
1351                 // Something must have gone wrong with the ground network file - or there is only a rwy here and no exits defined
1352                 SG_LOG(SG_ATC, SG_INFO, "No exits found by FGAILocalTraffic from runway " << rwy.rwyID << " at " << airportID << '\n');
1353                 //if(airportID == "KRHV") cout << "No exits found by " << plane.callsign << " from runway " << rwy.rwyID << " at " << airportID << '\n';
1354                 // What shall we do - just remove the plane from sight?
1355                 _aip.setVisible(false);
1356                 _invisible = true;
1357                 //cout << "Setting visible false\n";
1358                 //tower->ReportRunwayVacated(plane.callsign);
1359                 string trns = "Clear of the runway ";
1360                 trns += plane.callsign;
1361                 pending_transmission = trns;
1362                 Transmit(3);
1363                 operatingState = PARKED;
1364         }
1365 }
1366
1367 // Set the class variable nextTaxiNode to the next node in the path
1368 // and update taxiPathPos, the class variable path iterator position
1369 // TODO - maybe should return error codes to the calling function if we fail here
1370 void FGAILocalTraffic::GetNextTaxiNode() {
1371         //cout << "GetNextTaxiNode called " << endl;
1372         //cout << "taxiPathPos = " << taxiPathPos << endl;
1373         ground_network_path_iterator pathItr = path.begin() + taxiPathPos;
1374         if(pathItr == path.end()) {
1375                 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - no more nodes in path\n");
1376         } else {
1377                 if((*pathItr)->struct_type == NODE) {
1378                         //cout << "ITS A NODE" << endl;
1379                         //*pathItr = new node;
1380                         nextTaxiNode = (node*)*pathItr;
1381                         ++taxiPathPos;
1382                         //delete pathItr;
1383                 } else {
1384                         //cout << "ITS NOT A NODE" << endl;
1385                         //The first item in found must have been an arc
1386                         //Assume for now that it was straight
1387                         pathItr++;
1388                         taxiPathPos++;
1389                         if(pathItr == path.end()) {
1390                                 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - path ended with an arc\n");
1391                         } else if((*pathItr)->struct_type == NODE) {
1392                                 nextTaxiNode = (node*)*pathItr;
1393                                 ++taxiPathPos;
1394                         } else {
1395                                 //OOPS - two non-nodes in a row - that shouldn't happen ATM
1396                                 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - two non-nodes in sequence\n");
1397                         }
1398                 }
1399         }
1400 }
1401
1402 // StartTaxi - set up the taxiing state - call only at the start of taxiing
1403 void FGAILocalTraffic::StartTaxi() {
1404         //cout << "StartTaxi called" << endl;
1405         operatingState = TAXIING;
1406
1407         taxiPathPos = 0;
1408
1409         //Set the desired heading
1410         //Assume we are aiming for first node on path
1411         //Eventually we may need to consider the fact that we might start on a curved arc and
1412         //not be able to head directly for the first node.
1413         GetNextTaxiNode();      // sets the class variable nextTaxiNode to the next taxi node!
1414         desiredTaxiHeading = GetHeadingFromTo(_pos, nextTaxiNode->pos);
1415         //cout << "First taxi heading is " << desiredTaxiHeading << endl;
1416 }
1417
1418 // speed in knots, headings in degrees, radius in meters.
1419 static double TaxiTurnTowardsHeading(double current_hdg, double desired_hdg, double speed, double radius, double dt) {
1420         // wrap heading - this prevents a logic bug where the plane would just go round in circles!!
1421         while(current_hdg < 0.0) {
1422                 current_hdg += 360.0;
1423         }
1424         while(current_hdg > 360.0) {
1425                 current_hdg -= 360.0;
1426         }
1427         if(fabs(current_hdg - desired_hdg) > 0.1) {
1428                 // Which is the quickest direction to turn onto heading?
1429                 if(desired_hdg > current_hdg) {
1430                         if((desired_hdg - current_hdg) <= 180) {
1431                                 // turn right
1432                                 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1433                                 // TODO - check that increments are less than the delta that we check for the right direction
1434                                 // Probably need to reduce convergence speed as convergence is reached
1435                         } else {
1436                                 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1437                         }
1438                 } else {
1439                         if((current_hdg - desired_hdg) <= 180) {
1440                                 // turn left
1441                                 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1442                                 // TODO - check that increments are less than the delta that we check for the right direction
1443                                 // Probably need to reduce convergence speed as convergence is reached
1444                         } else {
1445                                 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1446                         }
1447                 }
1448         }
1449         return(current_hdg);
1450 }
1451
1452 void FGAILocalTraffic::Taxi(double dt) {
1453         //cout << "Taxi called" << endl;
1454         // Logic - if we are further away from next point than turn radius then head for it
1455         // If we have reached turning point then get next point and turn onto that heading
1456         // Look out for the finish!!
1457
1458         desiredTaxiHeading = GetHeadingFromTo(_pos, nextTaxiNode->pos);
1459
1460         bool lastNode = (taxiPathPos == path.size() ? true : false);
1461         if(lastNode) {
1462                 //cout << "LAST NODE\n";
1463         }
1464
1465         // HACK ALERT! - for now we will taxi at constant speed for straights and turns
1466
1467         // Remember that hdg is always equal to track when taxiing so we don't have to consider them both
1468         double dist_to_go = dclGetHorizontalSeparation(_pos, nextTaxiNode->pos);        // we may be able to do this more cheaply using orthopos
1469         //cout << "dist_to_go = " << dist_to_go << endl;
1470         if((nextTaxiNode->type == GATE) && (dist_to_go <= 0.1)) {
1471                 // This might be more robust to outward paths starting with a gate if we check for either
1472                 // last node or TD_INBOUND ?
1473                 // park up
1474                 operatingState = PARKED;
1475         } else if(((dist_to_go > taxiTurnRadius) || (nextTaxiNode->type == GATE)) && (!liningUp)){
1476                 // if the turn radius is r, and speed is s, then in a time dt we turn through
1477                 // ((s.dt)/(PI.r)) x 180 degrees
1478                 // or alternatively (s.dt)/r radians
1479                 //cout << "hdg = " << hdg << " desired taxi heading = " << desiredTaxiHeading << '\n';
1480                 _hdg = TaxiTurnTowardsHeading(_hdg, desiredTaxiHeading, nominalTaxiSpeed, taxiTurnRadius, dt);
1481                 double vel = nominalTaxiSpeed;
1482                 //cout << "vel = " << vel << endl;
1483                 double dist = vel * 0.514444 * dt;
1484                 //cout << "dist = " << dist << endl;
1485                 double track = _hdg;
1486                 //cout << "track = " << track << endl;
1487                 double slope = 0.0;
1488                 _pos = dclUpdatePosition(_pos, track, slope, dist);
1489                 //cout << "Updated position...\n";
1490                 if(_ground_elevation_m > -9990) {
1491                         _pos.setElevationM(_ground_elevation_m + wheelOffset);
1492                 } // else don't change the elev until we get a valid ground elev again!
1493         } else if(lastNode) {
1494                 if(taxiState == TD_LINING_UP) {
1495                         if((!liningUp) && (dist_to_go <= taxiTurnRadius)) {
1496                                 liningUp = true;
1497                         }
1498                         if(liningUp) {
1499                                 _hdg = TaxiTurnTowardsHeading(_hdg, rwy.hdg, nominalTaxiSpeed, taxiTurnRadius, dt);
1500                                 double vel = nominalTaxiSpeed;
1501                                 //cout << "vel = " << vel << endl;
1502                                 double dist = vel * 0.514444 * dt;
1503                                 //cout << "dist = " << dist << endl;
1504                                 double track = _hdg;
1505                                 //cout << "track = " << track << endl;
1506                                 double slope = 0.0;
1507                                 _pos = dclUpdatePosition(_pos, track, slope, dist);
1508                                 //cout << "Updated position...\n";
1509                                 if(_ground_elevation_m > -9990) {
1510                                         _pos.setElevationM(_ground_elevation_m + wheelOffset);
1511                                 } // else don't change the elev until we get a valid ground elev again!
1512                                 if(fabs(_hdg - rwy.hdg) <= 1.0) {
1513                                         operatingState = IN_PATTERN;
1514                                         leg = TAKEOFF_ROLL;
1515                                         inAir = false;
1516                                         liningUp = false;
1517                                 }
1518                         }
1519                 } else if(taxiState == TD_OUTBOUND) {
1520                         // Pause awaiting further instructions
1521                         // and for now assume we've reached the hold-short node
1522                         holdingShort = true;
1523                 } // else at the moment assume TD_INBOUND always ends in a gate in which case we can ignore it
1524         } else {
1525                 // Time to turn (we've already checked it's not the end we're heading for).
1526                 // set the target node to be the next node which will prompt automatically turning onto
1527                 // the right heading in the stuff above, with the usual provisos applied.
1528                 GetNextTaxiNode();
1529                 // For now why not just recursively call this function?
1530                 Taxi(dt);
1531         }
1532 }
1533
1534
1535 // Warning - ground elev determination is CPU intensive
1536 // Either this function or the logic of how often it is called
1537 // will almost certainly change.
1538 void FGAILocalTraffic::DoGroundElev() {
1539         // Only do the proper hitlist stuff if we are within visible range of the viewer.
1540         double visibility_meters = fgGetDouble("/environment/visibility-m");
1541         FGViewer* vw = globals->get_current_view();
1542         if(dclGetHorizontalSeparation(_pos, SGGeod::fromGeodM(vw->getPosition(), 0.0)) > visibility_meters) {
1543                 _ground_elevation_m = aptElev;
1544                 return;
1545         }
1546
1547   // FIXME: make shure the pos.lat/pos.lon values are in degrees ...
1548   double range = 500.0;
1549   if (!globals->get_tile_mgr()->scenery_available(_aip.getPosition(), range)) {
1550     // Try to shedule tiles for that position.
1551     globals->get_tile_mgr()->update( _aip.getPosition(), range );
1552   }
1553
1554   // FIXME: make shure the pos.lat/pos.lon values are in degrees ...
1555   double alt;
1556   if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(_aip.getPosition(), 20000), alt, 0, _aip.getSceneGraph()))
1557     _ground_elevation_m = alt;
1558 }
1559