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