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