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