src/ATC/AILocalTraffic.cxx

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