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