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