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