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