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