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