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