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                 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
 804                         pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
 805                 }
 806                 track = rwy.hdg;
 807                 dveldt = -5.0;
 808                 vel += dveldt * dt;
 809                 // FIXME - differentiate between touch and go and full stops
 810                 if(vel <= 15.0) {
 811                         //cout << "Vel <= 15.0, circuitsToFly = " << circuitsToFly << endl;
 812                         if(circuitsToFly <= 0) {
 813                                 //cout << "Calling ExitRunway..." << endl;
 814                                 ExitRunway(orthopos);
 815                                 return;
 816                         } else {
 817                                 //cout << "Taking off again..." << endl;
 818                                 leg = TAKEOFF_ROLL;
 819                                 --circuitsToFly;
 820                         }
 821                 }
 822                 break;
 823         case LEG_UNKNOWN:
 824                 break;
 825     }
 826
 827         if(inAir) {
 828                 // FIXME - at the moment this is a bit screwy
 829                 // The velocity correction is applied based on the relative headings.
 830                 // Then the heading is changed based on the velocity.
 831                 // Which comes first, the chicken or the egg?
 832                 // Does it really matter?
 833
 834                 // Apply wind to ground-relative velocity if in the air
 835                 vel = IAS - (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
 836                 //crab = f(track, wind, vel);
 837                 // The vector we need to fly is our desired vector minus the wind vector
 838                 // TODO - we probably ought to use plib's built in vector types and operations for this
 839                 // ie.  There's almost *certainly* a better way to do this!
 840                 double gxx = vel * sin(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity x component wrt ground
 841                 double gyy = vel * cos(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity y component wrt ground
 842                 double wxx = wind_speed * sin((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS);    // Wind velocity x component
 843                 double wyy = wind_speed * cos((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS);    // Wind velocity y component
 844                 double axx = gxx - wxx; // Plane in-air velocity x component
 845                 double ayy = gyy - wyy; // Plane in-air velocity y component
 846                 // Now we want the angle between gxx and axx (which is the crab)
 847                 double maga = sqrt(axx*axx + ayy*ayy);
 848                 double magg = sqrt(gxx*gxx + gyy*gyy);
 849                 crab = acos((axx*gxx + ayy*gyy) / (maga * magg));
 850                 // At this point this works except we're getting the modulus of the angle
 851                 //cout << "crab = " << crab << '\n';
 852
 853                 // Make sure both headings are in the 0->360 circle in order to get sane differences
 854                 dclBoundHeading(wind_from);
 855                 dclBoundHeading(track);
 856                 if(track > wind_from) {
 857                         if((track - wind_from) <= 180) {
 858                                 crab *= -1.0;
 859                         }
 860                 } else {
 861                         if((wind_from - track) >= 180) {
 862                                 crab *= -1.0;
 863                         }
 864                 }
 865         } else {        // on the ground - crab dosen't apply
 866                 crab = 0.0;
 867         }
 868
 869         hdg = track + crab;
 870         dist = vel * 0.514444 * dt;
 871         pos = dclUpdatePosition(pos, track, slope, dist);
 872 }
 873
 874 // Pattern direction is true for right, false for left
 875 void FGAILocalTraffic::CalculateSoD(double base_leg_pos, double downwind_leg_pos, bool pattern_direction) {
 876         // For now we'll ignore wind and hardwire the glide angle.
 877         double ga = 5.5;        //degrees
 878         double pa = 1000.0 * SG_FEET_TO_METER;  // pattern altitude in meters
 879         // FIXME - get glideslope angle and pattern altitude agl from airport details if available
 880
 881         // For convienience, we'll have +ve versions of the input distances
 882         double blp = fabs(base_leg_pos);
 883         double dlp = fabs(downwind_leg_pos);
 884
 885         //double turn_allowance = 150.0;        // Approximate distance in meters that a 90deg corner is shortened by turned in a light plane.
 886
 887         double stod = pa / tan(ga * DCL_DEGREES_TO_RADIANS);    // distance in meters from touchdown point to start descent
 888         cout << "Descent to start = " << stod << " meters out\n";
 889         if(stod < blp) {        // Start descending on final
 890                 SoD.leg = FINAL;
 891                 SoD.y = stod * -1.0;
 892                 SoD.x = 0.0;
 893         } else if(stod < (blp + dlp)) { // Start descending on base leg
 894                 SoD.leg = BASE;
 895                 SoD.y = blp * -1.0;
 896                 SoD.x = (pattern_direction ? (stod - dlp) : (stod - dlp) * -1.0);
 897         } else {        // Start descending on downwind leg
 898                 SoD.leg = DOWNWIND;
 899                 SoD.x = (pattern_direction ? dlp : dlp * -1.0);
 900                 SoD.y = (blp - (stod - (blp + dlp))) * -1.0;
 901         }
 902 }
 903
 904 void FGAILocalTraffic::TransmitPatternPositionReport(void) {
 905         // airport name + "traffic" + airplane callsign + pattern direction + pattern leg + rwy + ?
 906         string trns = "";
 907
 908         trns += tower->get_name();
 909         trns += " Traffic ";
 910         trns += plane.callsign;
 911         if(patternDirection == 1) {
 912                 trns += " right ";
 913         } else {
 914                 trns += " left ";
 915         }
 916
 917         // We could probably get rid of this whole switch statement and just pass a string containing the leg from the FlyPattern function.
 918         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?
 919         case TURN1:
 920                 // Fall through to CROSSWIND
 921         case CROSSWIND: // I don't think this case will be used here but it can't hurt to leave it in
 922                 trns += "crosswind ";
 923                 break;
 924         case TURN2:
 925                 // Fall through to DOWNWIND
 926         case DOWNWIND:
 927                 trns += "downwind ";
 928                 break;
 929         case TURN3:
 930                 // Fall through to BASE
 931         case BASE:
 932                 trns += "base ";
 933                 break;
 934         case TURN4:
 935                 // Fall through to FINAL
 936         case FINAL:             // maybe this should include long/short final if appropriate?
 937                 trns += "final ";
 938                 break;
 939         default:                // Hopefully this won't be used
 940                 trns += "pattern ";
 941                 break;
 942         }
 943         // FIXME - I've hardwired the runway call as well!! (We could work this out from rwy heading and mag deviation)
 944         trns += ConvertRwyNumToSpokenString(1);
 945
 946         // And add the airport name again
 947         trns += tower->get_name();
 948
 949         Transmit(trns);
 950 }
 951
 952 void FGAILocalTraffic::ExitRunway(Point3D orthopos) {
 953         //cout << "In ExitRunway" << endl;
 954         //cout << "Runway ID is " << rwy.ID << endl;
 955         node_array_type exitNodes = ground->GetExits(rwy.rwyID);        //I suppose we ought to have some fallback for rwy with no defined exits?
 956         /*
 957         cout << "Node ID's of exits are ";
 958         for(unsigned int i=0; i<exitNodes.size(); ++i) {
 959                 cout << exitNodes[i]->nodeID << ' ';
 960         }
 961         cout << endl;
 962         */
 963         if(exitNodes.size()) {
 964                 //Find the next exit from orthopos.y
 965                 double d;
 966                 double dist = 100000;   //ie. longer than any runway in existance
 967                 double backdist = 100000;
 968                 node_array_iterator nItr = exitNodes.begin();
 969                 node* rwyExit = *(exitNodes.begin());
 970                 //int gateID;           //This might want to be more persistant at some point
 971                 while(nItr != exitNodes.end()) {
 972                         d = ortho.ConvertToLocal((*nItr)->pos).y() - ortho.ConvertToLocal(pos).y();     //FIXME - consider making orthopos a class variable
 973                         if(d > 0.0) {
 974                                 if(d < dist) {
 975                                         dist = d;
 976                                         rwyExit = *nItr;
 977                                 }
 978                         } else {
 979                                 if(fabs(d) < backdist) {
 980                                         backdist = d;
 981                                         //TODO - need some logic here that if we don't get a forward exit we turn round and store the backwards one
 982                                 }
 983                         }
 984                         ++nItr;
 985                 }
 986                 ourGate = ground->GetGateNode();
 987                 if(ourGate == NULL) {
 988                         // Implies no available gates - what shall we do?
 989                         // For now just vanish the plane - possibly we can make this more elegant in the future
 990                         SG_LOG(SG_ATC, SG_ALERT, "No gate found by FGAILocalTraffic whilst landing at " << airportID << '\n');
 991                         aip.setVisible(false);
 992                         operatingState = PARKED;
 993                         return;
 994                 }
 995                 path = ground->GetPath(rwyExit, ourGate);
 996                 /*
 997                 cout << "path returned was:" << endl;
 998                 for(unsigned int i=0; i<path.size(); ++i) {
 999                         switch(path[i]->struct_type) {
1000                         case NODE:
1001                                 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
1002                                 break;
1003                         case ARC:
1004                                 cout << "ARC\n";
1005                                 break;
1006                         }
1007                 }
1008                 */
1009                 taxiState = TD_INBOUND;
1010                 StartTaxi();
1011         } else {
1012                 // Something must have gone wrong with the ground network file - or there is only a rwy here and no exits defined
1013                 SG_LOG(SG_ATC, SG_ALERT, "No exits found by FGAILocalTraffic from runway " << rwy.rwyID << " at " << airportID << '\n');
1014                 // What shall we do - just remove the plane from sight?
1015                 aip.setVisible(false);
1016                 operatingState = PARKED;
1017         }
1018 }
1019
1020 // Set the class variable nextTaxiNode to the next node in the path
1021 // and update taxiPathPos, the class variable path iterator position
1022 // TODO - maybe should return error codes to the calling function if we fail here
1023 void FGAILocalTraffic::GetNextTaxiNode() {
1024         //cout << "GetNextTaxiNode called " << endl;
1025         //cout << "taxiPathPos = " << taxiPathPos << endl;
1026         ground_network_path_iterator pathItr = path.begin() + taxiPathPos;
1027         if(pathItr == path.end()) {
1028                 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - no more nodes in path\n");
1029         } else {
1030                 if((*pathItr)->struct_type == NODE) {
1031                         //cout << "ITS A NODE" << endl;
1032                         //*pathItr = new node;
1033                         nextTaxiNode = (node*)*pathItr;
1034                         ++taxiPathPos;
1035                         //delete pathItr;
1036                 } else {
1037                         //cout << "ITS NOT A NODE" << endl;
1038                         //The first item in found must have been an arc
1039                         //Assume for now that it was straight
1040                         pathItr++;
1041                         taxiPathPos++;
1042                         if(pathItr == path.end()) {
1043                                 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - path ended with an arc\n");
1044                         } else if((*pathItr)->struct_type == NODE) {
1045                                 nextTaxiNode = (node*)*pathItr;
1046                                 ++taxiPathPos;
1047                         } else {
1048                                 //OOPS - two non-nodes in a row - that shouldn't happen ATM
1049                                 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - two non-nodes in sequence\n");
1050                         }
1051                 }
1052         }
1053 }
1054
1055 // StartTaxi - set up the taxiing state - call only at the start of taxiing
1056 void FGAILocalTraffic::StartTaxi() {
1057         //cout << "StartTaxi called" << endl;
1058         operatingState = TAXIING;
1059         taxiPathPos = 0;
1060
1061         //Set the desired heading
1062         //Assume we are aiming for first node on path
1063         //Eventually we may need to consider the fact that we might start on a curved arc and
1064         //not be able to head directly for the first node.
1065         GetNextTaxiNode();      // sets the class variable nextTaxiNode to the next taxi node!
1066         desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
1067         //cout << "First taxi heading is " << desiredTaxiHeading << endl;
1068 }
1069
1070 // speed in knots, headings in degrees, radius in meters.
1071 static double TaxiTurnTowardsHeading(double current_hdg, double desired_hdg, double speed, double radius, double dt) {
1072         // wrap heading - this prevents a logic bug where the plane would just go round in circles!!
1073         while(current_hdg < 0.0) {
1074                 current_hdg += 360.0;
1075         }
1076         while(current_hdg > 360.0) {
1077                 current_hdg -= 360.0;
1078         }
1079         if(fabs(current_hdg - desired_hdg) > 0.1) {
1080                 // Which is the quickest direction to turn onto heading?
1081                 if(desired_hdg > current_hdg) {
1082                         if((desired_hdg - current_hdg) <= 180) {
1083                                 // turn right
1084                                 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1085                                 // TODO - check that increments are less than the delta that we check for the right direction
1086                                 // Probably need to reduce convergence speed as convergence is reached
1087                         } else {
1088                                 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1089                         }
1090                 } else {
1091                         if((current_hdg - desired_hdg) <= 180) {
1092                                 // turn left
1093                                 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1094                                 // TODO - check that increments are less than the delta that we check for the right direction
1095                                 // Probably need to reduce convergence speed as convergence is reached
1096                         } else {
1097                                 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1098                         }
1099                 }
1100         }
1101         return(current_hdg);
1102 }
1103
1104 void FGAILocalTraffic::Taxi(double dt) {
1105         //cout << "Taxi called" << endl;
1106         // Logic - if we are further away from next point than turn radius then head for it
1107         // If we have reached turning point then get next point and turn onto that heading
1108         // Look out for the finish!!
1109
1110         //Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane
1111         desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
1112
1113         bool lastNode = (taxiPathPos == path.size() ? true : false);
1114         if(lastNode) {
1115                 //cout << "LAST NODE\n";
1116         }
1117
1118         // HACK ALERT! - for now we will taxi at constant speed for straights and turns
1119
1120         // Remember that hdg is always equal to track when taxiing so we don't have to consider them both
1121         double dist_to_go = dclGetHorizontalSeparation(pos, nextTaxiNode->pos); // we may be able to do this more cheaply using orthopos
1122         //cout << "dist_to_go = " << dist_to_go << endl;
1123         if((nextTaxiNode->type == GATE) && (dist_to_go <= 0.1)) {
1124                 // This might be more robust to outward paths starting with a gate if we check for either
1125                 // last node or TD_INBOUND ?
1126                 // park up
1127                 operatingState = PARKED;
1128         } else if(((dist_to_go > taxiTurnRadius) || (nextTaxiNode->type == GATE)) && (!liningUp)){
1129                 // if the turn radius is r, and speed is s, then in a time dt we turn through
1130                 // ((s.dt)/(PI.r)) x 180 degrees
1131                 // or alternatively (s.dt)/r radians
1132                 //cout << "hdg = " << hdg << " desired taxi heading = " << desiredTaxiHeading << '\n';
1133                 hdg = TaxiTurnTowardsHeading(hdg, desiredTaxiHeading, nominalTaxiSpeed, taxiTurnRadius, dt);
1134                 double vel = nominalTaxiSpeed;
1135                 //cout << "vel = " << vel << endl;
1136                 double dist = vel * 0.514444 * dt;
1137                 //cout << "dist = " << dist << endl;
1138                 double track = hdg;
1139                 //cout << "track = " << track << endl;
1140                 double slope = 0.0;
1141                 pos = dclUpdatePosition(pos, track, slope, dist);
1142                 //cout << "Updated position...\n";
1143                 if(aip.getSGLocation()->get_cur_elev_m() > -9990) {
1144                         pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
1145                 } // else don't change the elev until we get a valid ground elev again!
1146         } else if(lastNode) {
1147                 if(taxiState == TD_LINING_UP) {
1148                         if((!liningUp) && (dist_to_go <= taxiTurnRadius)) {
1149                                 liningUp = true;
1150                         }
1151                         if(liningUp) {
1152                                 hdg = TaxiTurnTowardsHeading(hdg, rwy.hdg, nominalTaxiSpeed, taxiTurnRadius, dt);
1153                                 double vel = nominalTaxiSpeed;
1154                                 //cout << "vel = " << vel << endl;
1155                                 double dist = vel * 0.514444 * dt;
1156                                 //cout << "dist = " << dist << endl;
1157                                 double track = hdg;
1158                                 //cout << "track = " << track << endl;
1159                                 double slope = 0.0;
1160                                 pos = dclUpdatePosition(pos, track, slope, dist);
1161                                 //cout << "Updated position...\n";
1162                                 if(aip.getSGLocation()->get_cur_elev_m() > -9990) {
1163                                         pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
1164                                 } // else don't change the elev until we get a valid ground elev again!
1165                                 if(fabs(hdg - rwy.hdg) <= 1.0) {
1166                                         operatingState = IN_PATTERN;
1167                                         leg = TAKEOFF_ROLL;
1168                                         inAir = false;
1169                                         liningUp = false;
1170                                 }
1171                         }
1172                 } else if(taxiState == TD_OUTBOUND) {
1173                         // Pause awaiting further instructions
1174                         // and for now assume we've reached the hold-short node
1175                         holdingShort = true;
1176                 } // else at the moment assume TD_INBOUND always ends in a gate in which case we can ignore it
1177         } else {
1178                 // Time to turn (we've already checked it's not the end we're heading for).
1179                 // set the target node to be the next node which will prompt automatically turning onto
1180                 // the right heading in the stuff above, with the usual provisos applied.
1181                 GetNextTaxiNode();
1182                 // For now why not just recursively call this function?
1183                 Taxi(dt);
1184         }
1185 }
1186
1187
1188 // Warning - ground elev determination is CPU intensive
1189 // Either this function or the logic of how often it is called
1190 // will almost certainly change.
1191 void FGAILocalTraffic::DoGroundElev() {
1192
1193         // It would be nice if we could set the correct tile center here in order to get a correct
1194         // answer with one call to the function, but what I tried in the two commented-out lines
1195         // below only intermittently worked, and I haven't quite groked why yet.
1196         //SGBucket buck(pos.lon(), pos.lat());
1197         //aip.getSGLocation()->set_tile_center(Point3D(buck.get_center_lon(), buck.get_center_lat(), 0.0));
1198
1199         double visibility_meters = fgGetDouble("/environment/visibility-m");
1200         //globals->get_tile_mgr()->prep_ssg_nodes( acmodel_location,
1201         globals->get_tile_mgr()->prep_ssg_nodes( aip.getSGLocation(),   visibility_meters );
1202         Point3D scenery_center = globals->get_scenery()->get_center();
1203         globals->get_tile_mgr()->update( aip.getSGLocation(), visibility_meters, (aip.getSGLocation())->get_absolute_view_pos( scenery_center ) );
1204         // save results of update in SGLocation for fdm...
1205
1206         //if ( globals->get_scenery()->get_cur_elev() > -9990 ) {
1207         //      acmodel_location->
1208         //      set_cur_elev_m( globals->get_scenery()->get_cur_elev() );
1209         //}
1210
1211         // The need for this here means that at least 2 consecutive passes are needed :-(
1212         aip.getSGLocation()->set_tile_center( globals->get_scenery()->get_next_center() );
1213
1214         //cout << "Transform Elev is " << globals->get_scenery()->get_cur_elev() << '\n';
1215         aip.getSGLocation()->set_cur_elev_m(globals->get_scenery()->get_cur_elev());
1216         //return(globals->get_scenery()->get_cur_elev());
1217 }
1218