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