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