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