src/ATC/AILocalTraffic.cxx

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