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