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