src/ATC/AILocalTraffic.cxx

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