src/ATC/AILocalTraffic.cxx

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