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((string)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         if(circuitsToFly) {
 285                 return(touchAndGo ? TOUCH_AND_GO : STOP_AND_GO);
 286         } else {
 287                 return(FULL_STOP);
 288         }
 289 }
 290
 291
 292 // Commands to do something from higher level logic
 293 void FGAILocalTraffic::FlyCircuits(int numCircuits, bool tag) {
 294         //cout << "FlyCircuits called" << endl;
 295
 296         switch(operatingState) {
 297         case IN_PATTERN:
 298                 circuitsToFly += numCircuits;
 299                 return;
 300                 break;
 301         case TAXIING:
 302                 // TODO - For now we'll punt this and do nothing
 303                 break;
 304         case PARKED:
 305                 circuitsToFly = numCircuits;    // Note that one too many circuits gets flown because we only test and decrement circuitsToFly after landing
 306                                                                                 // thus flying one too many circuits.  TODO - Need to sort this out better!
 307                 touchAndGo = tag;
 308                 break;
 309         }
 310 }
 311
 312 // Run the internal calculations
 313 void FGAILocalTraffic::Update(double dt) {
 314         //cout << "A" << flush;
 315         //double responseTime = 10.0;           // seconds - this should get more sophisticated at some point
 316         responseCounter += dt;
 317         if((contactTower) && (responseCounter >= 8.0)) {
 318                 // Acknowledge request before changing frequency so it gets rendered if the user is on the same freq
 319                 string trns = "Tower ";
 320                 double f = globals->get_ATC_mgr()->GetFrequency(airportID, TOWER) / 100.0;
 321                 char buf[10];
 322                 sprintf(buf, "%.2f", f);
 323                 trns += buf;
 324                 trns += " ";
 325                 trns += plane.callsign;
 326                 pending_transmission = trns;
 327                 ConditionalTransmit(30.0);
 328                 responseCounter = 0.0;
 329                 contactTower = false;
 330                 changeFreq = true;
 331                 changeFreqType = TOWER;
 332         }
 333
 334         if((changeFreq) && (responseCounter > 8.0)) {
 335                 switch(changeFreqType) {
 336                 case TOWER:
 337                         tuned_station = tower;
 338                         freq = (double)tower->get_freq() / 100.0;
 339                         //Transmit("DING!");
 340                         // Contact the tower, even if only virtually
 341                         changeFreq = false;
 342                         tower->ContactAtHoldShort(plane, this, CIRCUIT);
 343                         pending_transmission = "";      // Transmit an empty string until we do it properly to activate the ATC response timer mechanism
 344                         Transmit();
 345                         break;
 346                 case GROUND:
 347                         tuned_station = ground;
 348                         freq = (double)ground->get_freq() / 100.0;
 349                         break;
 350                 // And to avoid compiler warnings...
 351                 case APPROACH:  break;
 352                 case ATIS:      break;
 353                 case ENROUTE:   break;
 354                 case DEPARTURE: break;
 355                 case INVALID:   break;
 356                 }
 357         }
 358
 359         //cout << "." << flush;
 360
 361         switch(operatingState) {
 362         case IN_PATTERN:
 363                 //cout << "In IN_PATTERN\n";
 364                 if(!inAir) DoGroundElev();
 365                 if(!elevInitGood) {
 366                         if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
 367                                 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
 368                                 //cout << "TAKEOFF_ROLL, POS = " << pos.lon() << ", " << pos.lat() << ", " << pos.elev() << '\n';
 369                                 //Transform();
 370                                 aip.setVisible(true);
 371                                 //cout << "Making plane visible!\n";
 372                                 elevInitGood = true;
 373                         }
 374                 }
 375                 FlyTrafficPattern(dt);
 376                 Transform();
 377                 break;
 378         case TAXIING:
 379                 //cout << "In TAXIING\n";
 380                 //cout << "*" << flush;
 381                 if(!elevInitGood) {
 382                         //DoGroundElev();
 383                         if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
 384                                 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
 385                                 //Transform();
 386                                 aip.setVisible(true);
 387                                 //Transform();
 388                                 //cout << "Making plane visible!\n";
 389                                 elevInitGood = true;
 390                         }
 391                 }
 392                 DoGroundElev();
 393                 //cout << "," << flush;
 394                 if(!((holdingShort) && (!clearedToLineUp))) {
 395                         //cout << "|" << flush;
 396                         Taxi(dt);
 397                 }
 398                 //cout << ";" << flush;
 399                 if((clearedToTakeOff) && (responseCounter >= 8.0)) {
 400                         // possible assumption that we're at the hold short here - may not always hold
 401                         // TODO - sort out the case where we're cleared to line-up first and then cleared to take-off on the rwy.
 402                         taxiState = TD_LINING_UP;
 403                         path = ground->GetPath(holdShortNode, rwy.rwyID);
 404                         /*
 405                         cout << "path returned was:" << endl;
 406                         for(unsigned int i=0; i<path.size(); ++i) {
 407                                 switch(path[i]->struct_type) {
 408                                         case NODE:
 409                                         cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
 410                                         break;
 411                                         case ARC:
 412                                         cout << "ARC\n";
 413                                         break;
 414                                 }
 415                         }
 416                         */
 417                         clearedToTakeOff = false;       // We *are* still cleared - this simply stops the response recurring!!
 418                         holdingShort = false;
 419                         string trns = "Cleared for take-off ";
 420                         trns += plane.callsign;
 421                         pending_transmission = trns;
 422                         Transmit();
 423                         StartTaxi();
 424                 }
 425                 //cout << "^" << flush;
 426                 Transform();
 427                 break;
 428         case PARKED:
 429                 //cout << "In PARKED\n";
 430                 if(!elevInitGood) {
 431                         DoGroundElev();
 432                         if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
 433                                 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
 434                                 //Transform();
 435                                 aip.setVisible(true);
 436                                 //Transform();
 437                                 //cout << "Making plane visible!\n";
 438                                 elevInitGood = true;
 439                         }
 440                 }
 441
 442                 if(circuitsToFly) {
 443                         if((taxiRequestPending) && (taxiRequestCleared)) {
 444                                 //cout << "&" << flush;
 445                                 // Get the active runway details (and copy them into rwy)
 446                                 GetRwyDetails();
 447
 448                                 // Get the takeoff node for the active runway, get a path to it and start taxiing
 449                                 path = ground->GetPathToHoldShort(ourGate, rwy.rwyID);
 450                                 if(path.size() < 2) {
 451                                         // something has gone wrong
 452                                         SG_LOG(SG_ATC, SG_ALERT, "Invalid path from gate to theshold in FGAILocalTraffic::FlyCircuits\n");
 453                                         return;
 454                                 }
 455                                 /*
 456                                 cout << "path returned was:\n";
 457                                 for(unsigned int i=0; i<path.size(); ++i) {
 458                                         switch(path[i]->struct_type) {
 459                                                 case NODE:
 460                                                 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
 461                                                 break;
 462                                                 case ARC:
 463                                                 cout << "ARC\n";
 464                                                 break;
 465                                         }
 466                                 }
 467                                 */
 468                                 path.erase(path.begin());       // pop the gate - we're here already!
 469                                 taxiState = TD_OUTBOUND;
 470                                 taxiRequestPending = false;
 471                                 holdShortNode = (node*)(*(path.begin() + path.size()));
 472                                 StartTaxi();
 473                         } else if(!taxiRequestPending) {
 474                                 //cout << "(" << flush;
 475                                 // Do some communication
 476                                 // airport name + tower + airplane callsign + location + request taxi for + operation type + ?
 477                                 string trns = "";
 478                                 trns += tower->get_name();
 479                                 trns += " tower ";
 480                                 trns += plane.callsign;
 481                                 trns += " on apron parking request taxi for traffic pattern";
 482                                 //cout << "trns = " << trns << endl;
 483                                 pending_transmission = trns;
 484                                 Transmit(1);
 485                                 taxiRequestCleared = false;
 486                                 taxiRequestPending = true;
 487                         }
 488                 }
 489
 490                 //cout << "!" << flush;
 491
 492                 // Maybe the below should be set when we get to the threshold and prepare for TO?
 493                 // FIXME TODO - pattern direction is still hardwired
 494                 patternDirection = -1;          // Left
 495                 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
 496                 if(rwy.rwyID.size() == 3) {
 497                         patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
 498                 }
 499                 // Do nothing
 500                 Transform();
 501                 //cout << ")" << flush;
 502                 break;
 503         default:
 504                 break;
 505         }
 506         //cout << "I " << flush;
 507
 508         // Convienience output for AI debugging user the property logger
 509         fgSetDouble("/AI/Local1/ortho-x", (ortho.ConvertToLocal(pos)).x());
 510         fgSetDouble("/AI/Local1/ortho-y", (ortho.ConvertToLocal(pos)).y());
 511         fgSetDouble("/AI/Local1/elev", pos.elev() * SG_METER_TO_FEET);
 512
 513         // And finally, call parent for transmission rendering
 514         FGAIPlane::Update(dt);
 515 }
 516
 517 void FGAILocalTraffic::RegisterTransmission(int code) {
 518         switch(code) {
 519         case 1: // taxi request cleared
 520                 taxiRequestCleared = true;
 521                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to taxi...");
 522                 break;
 523         case 2: // contact tower
 524                 responseCounter = 0;
 525                 contactTower = true;
 526                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " told to contact tower...");
 527                 break;
 528         case 3: // Cleared to line up
 529                 responseCounter = 0;
 530                 clearedToLineUp = true;
 531                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to line-up...");
 532                 break;
 533         case 4: // cleared to take-off
 534                 responseCounter = 0;
 535                 clearedToTakeOff = true;
 536                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to take-off...");
 537                 break;
 538         default:
 539                 break;
 540         }
 541 }
 542
 543 // Fly a traffic pattern
 544 // FIXME - far too much of the mechanics of turning, rolling, accellerating, descending etc is in here.
 545 //         Move it out to FGAIPlane and have FlyTrafficPattern just specify what to do, not the implementation.
 546 void FGAILocalTraffic::FlyTrafficPattern(double dt) {
 547         // Need to differentiate between in-air (IAS governed) and on-ground (vel governed)
 548         // Take-off is an interesting case - we are on the ground but takeoff speed is IAS governed.
 549
 550         static bool transmitted = false;        // FIXME - this is a hack
 551
 552         // WIND
 553         // Wind has two effects - a mechanical one in that IAS translates to a different vel, and the hdg != track,
 554         // but also a piloting effect, in that the AI must be able to descend at a different rate in order to hit the threshold.
 555
 556         //cout << "dt = " << dt << '\n';
 557         double dist = 0;
 558         // ack - I can't remember how long a rate 1 turn is meant to take.
 559         double turn_time = 60.0;        // seconds - TODO - check this guess
 560         double turn_circumference;
 561         double turn_radius;
 562         Point3D orthopos = ortho.ConvertToLocal(pos);   // ortho position of the plane
 563         //cout << "runway elev = " << rwy.threshold_pos.elev() << ' ' << rwy.threshold_pos.elev() * SG_METER_TO_FEET << '\n';
 564         //cout << "elev = " << pos.elev() << ' ' << pos.elev() * SG_METER_TO_FEET << '\n';
 565
 566         // HACK FOR TESTING - REMOVE
 567         //cout << "Calling ExitRunway..." << endl;
 568         //ExitRunway(orthopos);
 569         //return;
 570         // END HACK
 571
 572         //wind
 573         double wind_from = wind_from_hdg->getDoubleValue();
 574         double wind_speed = wind_speed_knots->getDoubleValue();
 575
 576         double dveldt;
 577
 578         switch(leg) {
 579         case TAKEOFF_ROLL:
 580                 //inAir = false;
 581                 track = rwy.hdg;
 582                 if(vel < 80.0) {
 583                         double dveldt = 5.0;
 584                         vel += dveldt * dt;
 585                 }
 586                 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
 587                         pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
 588                 }
 589                 IAS = vel + (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
 590                 if(IAS >= 70) {
 591                         leg = CLIMBOUT;
 592                         pitch = 10.0;
 593                         IAS = best_rate_of_climb_speed;
 594                         //slope = 7.0;
 595                         slope = 6.0;    // Reduced it slightly since it's climbing a lot steeper than I can in the JSBSim C172.
 596                         inAir = true;
 597                 }
 598                 break;
 599         case CLIMBOUT:
 600                 track = rwy.hdg;
 601                 // Turn to crosswind if above 600ft AND if other traffic allows
 602                 // (decided in FGTower and accessed through GetCrosswindConstraint(...)).
 603                 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 600) {
 604                         double cc = 0.0;
 605                         if(tower->GetCrosswindConstraint(cc)) {
 606                                 if(orthopos.y() > cc) {
 607                                         cout << "Turning to crosswind, distance from threshold = " << orthopos.y() << '\n';
 608                                         leg = TURN1;
 609                                 }
 610                         } else {
 611                                 cout << "Turning to crosswind, distance from threshold = " << orthopos.y() << '\n';
 612                                 leg = TURN1;
 613                         }
 614                 }
 615                 // Need to check for levelling off in case we can't turn crosswind as soon
 616                 // as we would like due to other traffic.
 617                 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
 618                         slope = 0.0;
 619                         pitch = 0.0;
 620                         IAS = 80.0;             // FIXME - use smooth transistion to new speed and attitude.
 621                 }
 622                 break;
 623         case TURN1:
 624                 track += (360.0 / turn_time) * dt * patternDirection;
 625                 Bank(25.0 * patternDirection);
 626                 if((track < (rwy.hdg - 89.0)) || (track > (rwy.hdg + 89.0))) {
 627                         leg = CROSSWIND;
 628                 }
 629                 break;
 630         case CROSSWIND:
 631                 LevelWings();
 632                 track = rwy.hdg + (90.0 * patternDirection);
 633                 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
 634                         slope = 0.0;
 635                         pitch = 0.0;
 636                         IAS = 80.0;             // FIXME - use smooth transistion to new speed
 637                 }
 638                 // turn 1000m out for now, taking other traffic into accout
 639                 if(fabs(orthopos.x()) > 980) {
 640                         double dd = 0.0;
 641                         if(tower->GetDownwindConstraint(dd)) {
 642                                 if(fabs(orthopos.x()) > fabs(dd)) {
 643                                         cout << "Turning to downwind, distance from centerline = " << fabs(orthopos.x()) << '\n';
 644                                         leg = TURN2;
 645                                 }
 646                         } else {
 647                                 cout << "Turning to downwind, distance from centerline = " << fabs(orthopos.x()) << '\n';
 648                                 leg = TURN2;
 649                         }
 650                 }
 651                 break;
 652         case TURN2:
 653                 track += (360.0 / turn_time) * dt * patternDirection;
 654                 Bank(25.0 * patternDirection);
 655                 // just in case we didn't make height on crosswind
 656                 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
 657                         slope = 0.0;
 658                         pitch = 0.0;
 659                         IAS = 80.0;             // FIXME - use smooth transistion to new speed
 660                 }
 661                 if((track < (rwy.hdg - 179.0)) || (track > (rwy.hdg + 179.0))) {
 662                         leg = DOWNWIND;
 663                         transmitted = false;
 664                         //roll = 0.0;
 665                 }
 666                 break;
 667         case DOWNWIND:
 668                 LevelWings();
 669                 track = rwy.hdg - (180 * patternDirection);     //should tend to bring track back into the 0->360 range
 670                 // just in case we didn't make height on crosswind
 671                 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
 672                         slope = 0.0;
 673                         pitch = 0.0;
 674                         IAS = 90.0;             // FIXME - use smooth transistion to new speed
 675                 }
 676                 if((orthopos.y() < 0) && (!transmitted)) {
 677                         TransmitPatternPositionReport();
 678                         transmitted = true;
 679                 }
 680                 if((orthopos.y() < -100) && (!descending)) {
 681                         // Maybe we should think about when to start descending.
 682                         // For now we're assuming that we aim to follow the same glidepath regardless of wind.
 683                         double d1;
 684                         double d2;
 685                         CalculateSoD((tower->GetBaseConstraint(d1) ? d1 : -1000.0), (tower->GetDownwindConstraint(d2) ? d2 : 1000.0 * patternDirection), (patternDirection ? true : false));
 686                         if(SoD.leg == DOWNWIND) {
 687                                 descending = (orthopos.y() < SoD.y ? true : false);
 688                         }
 689
 690                 }
 691                 if(descending) {
 692                         slope = -5.5;   // FIXME - calculate to descent at 500fpm and hit the desired point on the runway (taking wind into account as well!!)
 693                         pitch = -3.0;
 694                         IAS = 85.0;
 695                 }
 696
 697                 // Try and arrange to turn nicely onto base
 698                 turn_circumference = IAS * 0.514444 * turn_time;
 699                 //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius
 700                 //We'll leave it as a hack with IAS for now but it needs revisiting.
 701                 turn_radius = turn_circumference / (2.0 * DCL_PI);
 702                 if(orthopos.y() < -1000.0 + turn_radius) {
 703                 //if(orthopos.y() < -980) {
 704                         double bb = 0.0;
 705                         if(tower->GetBaseConstraint(bb)) {
 706                                 if(fabs(orthopos.y()) > fabs(bb)) {
 707                                         cout << "Turning to base, distance from threshold = " << fabs(orthopos.y()) << '\n';
 708                                         leg = TURN3;
 709                                         transmitted = false;
 710                                         IAS = 80.0;
 711                                 }
 712                         } else {
 713                                 cout << "Turning to base, distance from threshold = " << fabs(orthopos.y()) << '\n';
 714                                 leg = TURN3;
 715                                 transmitted = false;
 716                                 IAS = 80.0;
 717                         }
 718                 }
 719                 break;
 720         case TURN3:
 721                 track += (360.0 / turn_time) * dt * patternDirection;
 722                 Bank(25.0 * patternDirection);
 723                 if(fabs(rwy.hdg - track) < 91.0) {
 724                         leg = BASE;
 725                 }
 726                 break;
 727         case BASE:
 728                 LevelWings();
 729                 if(!transmitted) {
 730                         TransmitPatternPositionReport();
 731                         transmitted = true;
 732                 }
 733
 734                 if(!descending) {
 735                         double d1;
 736                         // Make downwind leg position artifically large to avoid any chance of SoD being returned as
 737                         // on downwind when we are already on base.
 738                         CalculateSoD((tower->GetBaseConstraint(d1) ? d1 : -1000.0), (10000.0 * patternDirection), (patternDirection ? true : false));
 739                         if(SoD.leg == BASE) {
 740                                 descending = (fabs(orthopos.y()) < fabs(SoD.y) ? true : false);
 741                         }
 742
 743                 }
 744                 if(descending) {
 745                         slope = -5.5;   // FIXME - calculate to descent at 500fpm and hit the threshold (taking wind into account as well!!)
 746                         pitch = -4.0;
 747                         IAS = 70.0;
 748                 }
 749
 750                 track = rwy.hdg - (90 * patternDirection);
 751
 752                 // Try and arrange to turn nicely onto final
 753                 turn_circumference = IAS * 0.514444 * turn_time;
 754                 //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius
 755                 //We'll leave it as a hack with IAS for now but it needs revisiting.
 756                 turn_radius = turn_circumference / (2.0 * DCL_PI);
 757                 if(fabs(orthopos.x()) < (turn_radius + 50)) {
 758                         leg = TURN4;
 759                         transmitted = false;
 760                         //roll = -20;
 761                 }
 762                 break;
 763         case TURN4:
 764                 track += (360.0 / turn_time) * dt * patternDirection;
 765                 Bank(25.0 * patternDirection);
 766                 if(fabs(track - rwy.hdg) < 0.6) {
 767                         leg = FINAL;
 768                         vel = nominal_final_speed;
 769                 }
 770                 break;
 771         case FINAL:
 772                 LevelWings();
 773                 if(!transmitted) {
 774                         TransmitPatternPositionReport();
 775                         transmitted = true;
 776                 }
 777                 if(!descending) {
 778                         // Make base leg position artifically large to avoid any chance of SoD being returned as
 779                         // on base or downwind when we are already on final.
 780                         CalculateSoD(-10000.0, (1000.0 * patternDirection), (patternDirection ? true : false));
 781                         if(SoD.leg == FINAL) {
 782                                 descending = (fabs(orthopos.y()) < fabs(SoD.y) ? true : false);
 783                         }
 784
 785                 }
 786                 if(descending) {
 787                         slope = -5.5;   // FIXME - calculate to descent at 500fpm and hit the threshold (taking wind into account as well!!)
 788                         pitch = -4.0;
 789                         IAS = 70.0;
 790                 }
 791                 // Try and track the extended centreline
 792                 track = rwy.hdg - (0.2 * orthopos.x());
 793                 //cout << "orthopos.x() = " << orthopos.x() << " hdg = " << hdg << '\n';
 794                 if(pos.elev() < (rwy.threshold_pos.elev()+20.0+wheelOffset)) {
 795                         DoGroundElev(); // Need to call it here expicitly on final since it's only called
 796                                         // for us in update(...) when the inAir flag is false.
 797                 }
 798                 if(pos.elev() < (rwy.threshold_pos.elev()+10.0+wheelOffset)) {
 799                         if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
 800                                 if((aip.getSGLocation()->get_cur_elev_m() + wheelOffset) > pos.elev()) {
 801                                         slope = 0.0;
 802                                         pitch = 0.0;
 803                                         leg = LANDING_ROLL;
 804                                         inAir = false;
 805                                 }
 806                         }       // else need a fallback position based on arpt elev in case ground elev determination fails?
 807                 }
 808                 break;
 809         case LANDING_ROLL:
 810                 //inAir = false;
 811                 descending = false;
 812                 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
 813                         pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
 814                 }
 815                 track = rwy.hdg;
 816                 dveldt = -5.0;
 817                 vel += dveldt * dt;
 818                 // FIXME - differentiate between touch and go and full stops
 819                 if(vel <= 15.0) {
 820                         //cout << "Vel <= 15.0, circuitsToFly = " << circuitsToFly << endl;
 821                         if(circuitsToFly <= 0) {
 822                                 //cout << "Calling ExitRunway..." << endl;
 823                                 ExitRunway(orthopos);
 824                                 return;
 825                         } else {
 826                                 //cout << "Taking off again..." << endl;
 827                                 leg = TAKEOFF_ROLL;
 828                                 --circuitsToFly;
 829                         }
 830                 }
 831                 break;
 832         case LEG_UNKNOWN:
 833                 break;
 834     }
 835
 836         if(inAir) {
 837                 // FIXME - at the moment this is a bit screwy
 838                 // The velocity correction is applied based on the relative headings.
 839                 // Then the heading is changed based on the velocity.
 840                 // Which comes first, the chicken or the egg?
 841                 // Does it really matter?
 842
 843                 // Apply wind to ground-relative velocity if in the air
 844                 vel = IAS - (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
 845                 //crab = f(track, wind, vel);
 846                 // The vector we need to fly is our desired vector minus the wind vector
 847                 // TODO - we probably ought to use plib's built in vector types and operations for this
 848                 // ie.  There's almost *certainly* a better way to do this!
 849                 double gxx = vel * sin(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity x component wrt ground
 850                 double gyy = vel * cos(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity y component wrt ground
 851                 double wxx = wind_speed * sin((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS);    // Wind velocity x component
 852                 double wyy = wind_speed * cos((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS);    // Wind velocity y component
 853                 double axx = gxx - wxx; // Plane in-air velocity x component
 854                 double ayy = gyy - wyy; // Plane in-air velocity y component
 855                 // Now we want the angle between gxx and axx (which is the crab)
 856                 double maga = sqrt(axx*axx + ayy*ayy);
 857                 double magg = sqrt(gxx*gxx + gyy*gyy);
 858                 crab = acos((axx*gxx + ayy*gyy) / (maga * magg));
 859                 // At this point this works except we're getting the modulus of the angle
 860                 //cout << "crab = " << crab << '\n';
 861
 862                 // Make sure both headings are in the 0->360 circle in order to get sane differences
 863                 dclBoundHeading(wind_from);
 864                 dclBoundHeading(track);
 865                 if(track > wind_from) {
 866                         if((track - wind_from) <= 180) {
 867                                 crab *= -1.0;
 868                         }
 869                 } else {
 870                         if((wind_from - track) >= 180) {
 871                                 crab *= -1.0;
 872                         }
 873                 }
 874         } else {        // on the ground - crab dosen't apply
 875                 crab = 0.0;
 876         }
 877
 878         hdg = track + crab;
 879         dist = vel * 0.514444 * dt;
 880         pos = dclUpdatePosition(pos, track, slope, dist);
 881 }
 882
 883 // Pattern direction is true for right, false for left
 884 void FGAILocalTraffic::CalculateSoD(double base_leg_pos, double downwind_leg_pos, bool pattern_direction) {
 885         // For now we'll ignore wind and hardwire the glide angle.
 886         double ga = 5.5;        //degrees
 887         double pa = 1000.0 * SG_FEET_TO_METER;  // pattern altitude in meters
 888         // FIXME - get glideslope angle and pattern altitude agl from airport details if available
 889
 890         // For convienience, we'll have +ve versions of the input distances
 891         double blp = fabs(base_leg_pos);
 892         double dlp = fabs(downwind_leg_pos);
 893
 894         //double turn_allowance = 150.0;        // Approximate distance in meters that a 90deg corner is shortened by turned in a light plane.
 895
 896         double stod = pa / tan(ga * DCL_DEGREES_TO_RADIANS);    // distance in meters from touchdown point to start descent
 897         //cout << "Descent to start = " << stod << " meters out\n";
 898         if(stod < blp) {        // Start descending on final
 899                 SoD.leg = FINAL;
 900                 SoD.y = stod * -1.0;
 901                 SoD.x = 0.0;
 902         } else if(stod < (blp + dlp)) { // Start descending on base leg
 903                 SoD.leg = BASE;
 904                 SoD.y = blp * -1.0;
 905                 SoD.x = (pattern_direction ? (stod - dlp) : (stod - dlp) * -1.0);
 906         } else {        // Start descending on downwind leg
 907                 SoD.leg = DOWNWIND;
 908                 SoD.x = (pattern_direction ? dlp : dlp * -1.0);
 909                 SoD.y = (blp - (stod - (blp + dlp))) * -1.0;
 910         }
 911 }
 912
 913 void FGAILocalTraffic::TransmitPatternPositionReport(void) {
 914         // airport name + "traffic" + airplane callsign + pattern direction + pattern leg + rwy + ?
 915         string trns = "";
 916         int code = 0;
 917
 918         trns += tower->get_name();
 919         trns += " Traffic ";
 920         trns += plane.callsign;
 921         if(patternDirection == 1) {
 922                 trns += " right ";
 923         } else {
 924                 trns += " left ";
 925         }
 926
 927         // We could probably get rid of this whole switch statement and just pass a string containing the leg from the FlyPattern function.
 928         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?
 929         case TURN1:
 930                 // Fall through to CROSSWIND
 931         case CROSSWIND: // I don't think this case will be used here but it can't hurt to leave it in
 932                 trns += "crosswind ";
 933                 break;
 934         case TURN2:
 935                 // Fall through to DOWNWIND
 936         case DOWNWIND:
 937                 trns += "downwind ";
 938                 code = 11;
 939                 break;
 940         case TURN3:
 941                 // Fall through to BASE
 942         case BASE:
 943                 trns += "base ";
 944                 break;
 945         case TURN4:
 946                 // Fall through to FINAL
 947         case FINAL:             // maybe this should include long/short final if appropriate?
 948                 trns += "final ";
 949                 break;
 950         default:                // Hopefully this won't be used
 951                 trns += "pattern ";
 952                 break;
 953         }
 954         // FIXME - I've hardwired the runway call as well!! (We could work this out from rwy heading and mag deviation)
 955         trns += ConvertRwyNumToSpokenString(1);
 956
 957         // And add the airport name again
 958         trns += tower->get_name();
 959
 960         pending_transmission = trns;    // FIXME - make up pending_transmission natively
 961         ConditionalTransmit(90.0, code);                // Assume a report of this leg will be invalid if we can't transmit within a minute and a half.
 962 }
 963
 964 // Callback handler
 965 void FGAILocalTraffic::ProcessCallback(int code) {
 966         // 1 - Request Departure from ground
 967         // 10 - report crosswind
 968         // 11 - report downwind
 969         // 12 - report base
 970         // 13 - report final
 971         if(code == 1) {
 972                 ground->RequestDeparture(plane, this);
 973         } else if(code == 11) {
 974                 tower->ReportDownwind(plane.callsign);
 975         }
 976 }
 977
 978 void FGAILocalTraffic::ExitRunway(Point3D orthopos) {
 979         //cout << "In ExitRunway" << endl;
 980         //cout << "Runway ID is " << rwy.ID << endl;
 981         node_array_type exitNodes = ground->GetExits(rwy.rwyID);        //I suppose we ought to have some fallback for rwy with no defined exits?
 982         /*
 983         cout << "Node ID's of exits are ";
 984         for(unsigned int i=0; i<exitNodes.size(); ++i) {
 985                 cout << exitNodes[i]->nodeID << ' ';
 986         }
 987         cout << endl;
 988         */
 989         if(exitNodes.size()) {
 990                 //Find the next exit from orthopos.y
 991                 double d;
 992                 double dist = 100000;   //ie. longer than any runway in existance
 993                 double backdist = 100000;
 994                 node_array_iterator nItr = exitNodes.begin();
 995                 node* rwyExit = *(exitNodes.begin());
 996                 //int gateID;           //This might want to be more persistant at some point
 997                 while(nItr != exitNodes.end()) {
 998                         d = ortho.ConvertToLocal((*nItr)->pos).y() - ortho.ConvertToLocal(pos).y();     //FIXME - consider making orthopos a class variable
 999                         if(d > 0.0) {
1000                                 if(d < dist) {
1001                                         dist = d;
1002                                         rwyExit = *nItr;
1003                                 }
1004                         } else {
1005                                 if(fabs(d) < backdist) {
1006                                         backdist = d;
1007                                         //TODO - need some logic here that if we don't get a forward exit we turn round and store the backwards one
1008                                 }
1009                         }
1010                         ++nItr;
1011                 }
1012                 ourGate = ground->GetGateNode();
1013                 if(ourGate == NULL) {
1014                         // Implies no available gates - what shall we do?
1015                         // For now just vanish the plane - possibly we can make this more elegant in the future
1016                         SG_LOG(SG_ATC, SG_ALERT, "No gate found by FGAILocalTraffic whilst landing at " << airportID << '\n');
1017                         aip.setVisible(false);
1018                         operatingState = PARKED;
1019                         return;
1020                 }
1021                 path = ground->GetPath(rwyExit, ourGate);
1022                 /*
1023                 cout << "path returned was:" << endl;
1024                 for(unsigned int i=0; i<path.size(); ++i) {
1025                         switch(path[i]->struct_type) {
1026                         case NODE:
1027                                 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
1028                                 break;
1029                         case ARC:
1030                                 cout << "ARC\n";
1031                                 break;
1032                         }
1033                 }
1034                 */
1035                 taxiState = TD_INBOUND;
1036                 StartTaxi();
1037         } else {
1038                 // Something must have gone wrong with the ground network file - or there is only a rwy here and no exits defined
1039                 SG_LOG(SG_ATC, SG_ALERT, "No exits found by FGAILocalTraffic from runway " << rwy.rwyID << " at " << airportID << '\n');
1040                 // What shall we do - just remove the plane from sight?
1041                 aip.setVisible(false);
1042                 operatingState = PARKED;
1043         }
1044 }
1045
1046 // Set the class variable nextTaxiNode to the next node in the path
1047 // and update taxiPathPos, the class variable path iterator position
1048 // TODO - maybe should return error codes to the calling function if we fail here
1049 void FGAILocalTraffic::GetNextTaxiNode() {
1050         //cout << "GetNextTaxiNode called " << endl;
1051         //cout << "taxiPathPos = " << taxiPathPos << endl;
1052         ground_network_path_iterator pathItr = path.begin() + taxiPathPos;
1053         if(pathItr == path.end()) {
1054                 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - no more nodes in path\n");
1055         } else {
1056                 if((*pathItr)->struct_type == NODE) {
1057                         //cout << "ITS A NODE" << endl;
1058                         //*pathItr = new node;
1059                         nextTaxiNode = (node*)*pathItr;
1060                         ++taxiPathPos;
1061                         //delete pathItr;
1062                 } else {
1063                         //cout << "ITS NOT A NODE" << endl;
1064                         //The first item in found must have been an arc
1065                         //Assume for now that it was straight
1066                         pathItr++;
1067                         taxiPathPos++;
1068                         if(pathItr == path.end()) {
1069                                 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - path ended with an arc\n");
1070                         } else if((*pathItr)->struct_type == NODE) {
1071                                 nextTaxiNode = (node*)*pathItr;
1072                                 ++taxiPathPos;
1073                         } else {
1074                                 //OOPS - two non-nodes in a row - that shouldn't happen ATM
1075                                 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - two non-nodes in sequence\n");
1076                         }
1077                 }
1078         }
1079 }
1080
1081 // StartTaxi - set up the taxiing state - call only at the start of taxiing
1082 void FGAILocalTraffic::StartTaxi() {
1083         //cout << "StartTaxi called" << endl;
1084         operatingState = TAXIING;
1085         taxiPathPos = 0;
1086
1087         //Set the desired heading
1088         //Assume we are aiming for first node on path
1089         //Eventually we may need to consider the fact that we might start on a curved arc and
1090         //not be able to head directly for the first node.
1091         GetNextTaxiNode();      // sets the class variable nextTaxiNode to the next taxi node!
1092         desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
1093         //cout << "First taxi heading is " << desiredTaxiHeading << endl;
1094 }
1095
1096 // speed in knots, headings in degrees, radius in meters.
1097 static double TaxiTurnTowardsHeading(double current_hdg, double desired_hdg, double speed, double radius, double dt) {
1098         // wrap heading - this prevents a logic bug where the plane would just go round in circles!!
1099         while(current_hdg < 0.0) {
1100                 current_hdg += 360.0;
1101         }
1102         while(current_hdg > 360.0) {
1103                 current_hdg -= 360.0;
1104         }
1105         if(fabs(current_hdg - desired_hdg) > 0.1) {
1106                 // Which is the quickest direction to turn onto heading?
1107                 if(desired_hdg > current_hdg) {
1108                         if((desired_hdg - current_hdg) <= 180) {
1109                                 // turn right
1110                                 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1111                                 // TODO - check that increments are less than the delta that we check for the right direction
1112                                 // Probably need to reduce convergence speed as convergence is reached
1113                         } else {
1114                                 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1115                         }
1116                 } else {
1117                         if((current_hdg - desired_hdg) <= 180) {
1118                                 // turn left
1119                                 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1120                                 // TODO - check that increments are less than the delta that we check for the right direction
1121                                 // Probably need to reduce convergence speed as convergence is reached
1122                         } else {
1123                                 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1124                         }
1125                 }
1126         }
1127         return(current_hdg);
1128 }
1129
1130 void FGAILocalTraffic::Taxi(double dt) {
1131         //cout << "Taxi called" << endl;
1132         // Logic - if we are further away from next point than turn radius then head for it
1133         // If we have reached turning point then get next point and turn onto that heading
1134         // Look out for the finish!!
1135
1136         //Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane
1137         desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
1138
1139         bool lastNode = (taxiPathPos == path.size() ? true : false);
1140         if(lastNode) {
1141                 //cout << "LAST NODE\n";
1142         }
1143
1144         // HACK ALERT! - for now we will taxi at constant speed for straights and turns
1145
1146         // Remember that hdg is always equal to track when taxiing so we don't have to consider them both
1147         double dist_to_go = dclGetHorizontalSeparation(pos, nextTaxiNode->pos); // we may be able to do this more cheaply using orthopos
1148         //cout << "dist_to_go = " << dist_to_go << endl;
1149         if((nextTaxiNode->type == GATE) && (dist_to_go <= 0.1)) {
1150                 // This might be more robust to outward paths starting with a gate if we check for either
1151                 // last node or TD_INBOUND ?
1152                 // park up
1153                 operatingState = PARKED;
1154         } else if(((dist_to_go > taxiTurnRadius) || (nextTaxiNode->type == GATE)) && (!liningUp)){
1155                 // if the turn radius is r, and speed is s, then in a time dt we turn through
1156                 // ((s.dt)/(PI.r)) x 180 degrees
1157                 // or alternatively (s.dt)/r radians
1158                 //cout << "hdg = " << hdg << " desired taxi heading = " << desiredTaxiHeading << '\n';
1159                 hdg = TaxiTurnTowardsHeading(hdg, desiredTaxiHeading, nominalTaxiSpeed, taxiTurnRadius, dt);
1160                 double vel = nominalTaxiSpeed;
1161                 //cout << "vel = " << vel << endl;
1162                 double dist = vel * 0.514444 * dt;
1163                 //cout << "dist = " << dist << endl;
1164                 double track = hdg;
1165                 //cout << "track = " << track << endl;
1166                 double slope = 0.0;
1167                 pos = dclUpdatePosition(pos, track, slope, dist);
1168                 //cout << "Updated position...\n";
1169                 if(aip.getSGLocation()->get_cur_elev_m() > -9990) {
1170                         pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
1171                 } // else don't change the elev until we get a valid ground elev again!
1172         } else if(lastNode) {
1173                 if(taxiState == TD_LINING_UP) {
1174                         if((!liningUp) && (dist_to_go <= taxiTurnRadius)) {
1175                                 liningUp = true;
1176                         }
1177                         if(liningUp) {
1178                                 hdg = TaxiTurnTowardsHeading(hdg, rwy.hdg, nominalTaxiSpeed, taxiTurnRadius, dt);
1179                                 double vel = nominalTaxiSpeed;
1180                                 //cout << "vel = " << vel << endl;
1181                                 double dist = vel * 0.514444 * dt;
1182                                 //cout << "dist = " << dist << endl;
1183                                 double track = hdg;
1184                                 //cout << "track = " << track << endl;
1185                                 double slope = 0.0;
1186                                 pos = dclUpdatePosition(pos, track, slope, dist);
1187                                 //cout << "Updated position...\n";
1188                                 if(aip.getSGLocation()->get_cur_elev_m() > -9990) {
1189                                         pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
1190                                 } // else don't change the elev until we get a valid ground elev again!
1191                                 if(fabs(hdg - rwy.hdg) <= 1.0) {
1192                                         operatingState = IN_PATTERN;
1193                                         leg = TAKEOFF_ROLL;
1194                                         inAir = false;
1195                                         liningUp = false;
1196                                 }
1197                         }
1198                 } else if(taxiState == TD_OUTBOUND) {
1199                         // Pause awaiting further instructions
1200                         // and for now assume we've reached the hold-short node
1201                         holdingShort = true;
1202                 } // else at the moment assume TD_INBOUND always ends in a gate in which case we can ignore it
1203         } else {
1204                 // Time to turn (we've already checked it's not the end we're heading for).
1205                 // set the target node to be the next node which will prompt automatically turning onto
1206                 // the right heading in the stuff above, with the usual provisos applied.
1207                 GetNextTaxiNode();
1208                 // For now why not just recursively call this function?
1209                 Taxi(dt);
1210         }
1211 }
1212
1213
1214 // Warning - ground elev determination is CPU intensive
1215 // Either this function or the logic of how often it is called
1216 // will almost certainly change.
1217 void FGAILocalTraffic::DoGroundElev() {
1218
1219         // It would be nice if we could set the correct tile center here in order to get a correct
1220         // answer with one call to the function, but what I tried in the two commented-out lines
1221         // below only intermittently worked, and I haven't quite groked why yet.
1222         //SGBucket buck(pos.lon(), pos.lat());
1223         //aip.getSGLocation()->set_tile_center(Point3D(buck.get_center_lon(), buck.get_center_lat(), 0.0));
1224
1225         double visibility_meters = fgGetDouble("/environment/visibility-m");
1226         //globals->get_tile_mgr()->prep_ssg_nodes( acmodel_location,
1227         globals->get_tile_mgr()->prep_ssg_nodes( aip.getSGLocation(),   visibility_meters );
1228         Point3D scenery_center = globals->get_scenery()->get_center();
1229         globals->get_tile_mgr()->update( aip.getSGLocation(), visibility_meters, (aip.getSGLocation())->get_absolute_view_pos( scenery_center ) );
1230         // save results of update in SGLocation for fdm...
1231
1232         //if ( globals->get_scenery()->get_cur_elev() > -9990 ) {
1233         //      acmodel_location->
1234         //      set_cur_elev_m( globals->get_scenery()->get_cur_elev() );
1235         //}
1236
1237         // The need for this here means that at least 2 consecutive passes are needed :-(
1238         aip.getSGLocation()->set_tile_center( globals->get_scenery()->get_next_center() );
1239
1240         //cout << "Transform Elev is " << globals->get_scenery()->get_cur_elev() << '\n';
1241         aip.getSGLocation()->set_cur_elev_m(globals->get_scenery()->get_cur_elev());
1242         //return(globals->get_scenery()->get_cur_elev());
1243 }
1244