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