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