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