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 <Main/globals.hxx>
  27 #include <Main/location.hxx>
  28 #include <Scenery/scenery.hxx>
  29 #include <Scenery/tilemgr.hxx>
  30 #include <simgear/math/point3d.hxx>
  31 #include <simgear/math/sg_geodesy.hxx>
  32 #include <simgear/misc/sg_path.hxx>
  33 #include <string>
  34 #include <math.h>
  35
  36 SG_USING_STD(string);
  37
  38 #include "ATCmgr.hxx"
  39 #include "AILocalTraffic.hxx"
  40 #include "ATCutils.hxx"
  41
  42 FGAILocalTraffic::FGAILocalTraffic() {
  43         //Hardwire initialisation for now - a lot of this should be read in from config eventually
  44         Vr = 70.0;
  45         best_rate_of_climb_speed = 70.0;
  46         //best_rate_of_climb;
  47         //nominal_climb_speed;
  48         //nominal_climb_rate;
  49         //nominal_circuit_speed;
  50         //min_circuit_speed;
  51         //max_circuit_speed;
  52         nominal_descent_rate = 500.0;
  53         nominal_final_speed = 65.0;
  54         //nominal_approach_speed;
  55         //stall_speed_landing_config;
  56         nominalTaxiSpeed = 8.0;
  57         taxiTurnRadius = 8.0;
  58         wheelOffset = 1.45;     // Warning - hardwired to the C172 - we need to read this in from file.
  59         // Init the property nodes
  60         wind_from_hdg = fgGetNode("/environment/wind-from-heading-deg", true);
  61         wind_speed_knots = fgGetNode("/environment/wind-speed-kts", true);
  62         circuitsToFly = 0;
  63 }
  64
  65 FGAILocalTraffic::~FGAILocalTraffic() {
  66 }
  67
  68 void FGAILocalTraffic::Init() {
  69         // Hack alert - Hardwired path!!
  70         string planepath = "Aircraft/c172/Models/c172-dpm.ac";
  71         SGPath path = globals->get_fg_root();
  72         path.append(planepath);
  73         aip.init(planepath.c_str());
  74         aip.setVisible(true);
  75         globals->get_scenery()->get_scene_graph()->addKid(aip.getSceneGraph());
  76         // is it OK to leave it like this until the first time transform is called?
  77         // Really ought to be started in a parking space unless otherwise specified?
  78
  79         // Find the tower frequency - this is dependent on the ATC system being initialised before the AI system
  80         // FIXME - ATM this is hardwired.
  81         airportID = "KEMT";
  82         AirportATC a;
  83         if(globals->get_ATC_mgr()->GetAirportATCDetails((string)airportID, &a)) {
  84                 if(a.tower_freq) {      // Has a tower
  85                         tower = (FGTower*)globals->get_ATC_mgr()->GetATCPointer((string)airportID, TOWER);      // Maybe need some error checking here
  86                         freq = (double)tower->get_freq() / 100.0;
  87                         //cout << "***********************************AILocalTraffic freq = " << freq << '\n';
  88                 } else {
  89                         // Check CTAF, unicom etc
  90                 }
  91         } else {
  92                 //cout << "Unable to find airport details in FGAILocalTraffic::Init()\n";
  93         }
  94
  95         // Initiallise the FGAirportData structure
  96         // This needs a complete overhaul soon - what happens if we have 2 AI planes at same airport - they don't both need a structure
  97         // This needs to be handled by the ATC manager or similar so only one set of physical data per airport is instantiated
  98         // ie. TODO TODO FIXME FIXME
  99         airport.Init();
 100
 101 }
 102
 103 // Commands to do something from higher level logic
 104 void FGAILocalTraffic::FlyCircuits(int numCircuits, bool tag) {
 105         circuitsToFly += numCircuits - 1;       // Hack (-1) because we only test and decrement circuitsToFly after landing
 106                                                                                 // thus flying one to many circuits.  TODO - Need to sort this out better!
 107         touchAndGo = tag;
 108
 109         //At the moment we'll assume that we are always finished previous circuits when called,
 110         //And just teleport to the threshold to start.
 111         //This is a hack though, we need to check where we are and taxi out if appropriate.
 112         operatingState = IN_PATTERN;
 113
 114         // Hardwire to KEMT for now
 115         // Hardwired points at each end of KEMT runway
 116         Point3D P010(-118.037483, 34.081358, 296 * SG_FEET_TO_METER);
 117         Point3D P190(-118.032308, 34.090456, 299.395263 * SG_FEET_TO_METER);
 118         Point3D takeoff_end;
 119         bool d010 = true;       // use this to change the hardwired runway direction
 120         if(d010) {
 121                 rwy.threshold_pos = P010;
 122                 takeoff_end = P190;
 123                 rwy.hdg = 25.32;        //from default.apt
 124                 rwy.ID = 1;
 125                 patternDirection = -1;  // Left
 126                 pos.setelev(rwy.threshold_pos.elev() + (-8.5 * SG_FEET_TO_METER));  // This is a complete hack - the rendered runway takes the underlying scenery elev rather than the published runway elev so I should use height above terrain or something.
 127         } else {
 128                 rwy.threshold_pos = P190;
 129                 takeoff_end = P010;
 130                 rwy.hdg = 205.32;
 131                 rwy.ID = 19;
 132                 patternDirection = 1;   // Right
 133                 pos.setelev(rwy.threshold_pos.elev() + (-0.0 * SG_FEET_TO_METER));  // This is a complete hack - the rendered runway takes the underlying scenery elev rather than the published runway elev so I should use height above terrain or something.
 134         }
 135
 136         //rwy.threshold_pos.setlat(34.081358);
 137         //rwy.threshold_pos.setlon(-118.037483);
 138         //rwy.mag_hdg = 12.0;
 139         //rwy.mag_var = 14.0;
 140         //rwy.hdg = rwy.mag_hdg + rwy.mag_var;
 141         //rwy.threshold_pos.setelev(296 * SG_FEET_TO_METER);
 142
 143         // Initial position on threshold for now
 144         // TODO - check wind / default runway
 145         pos.setlat(rwy.threshold_pos.lat());
 146         pos.setlon(rwy.threshold_pos.lon());
 147         hdg = rwy.hdg;
 148
 149         pitch = 0.0;
 150         roll = 0.0;
 151         leg = TAKEOFF_ROLL;
 152         vel = 0.0;
 153         slope = 0.0;
 154
 155         // Set the projection for the local area
 156         ortho.Init(rwy.threshold_pos, rwy.hdg);
 157         rwy.end1ortho = ortho.ConvertToLocal(rwy.threshold_pos);        // should come out as zero
 158         // Hardwire to KEMT for now
 159         rwy.end2ortho = ortho.ConvertToLocal(takeoff_end);
 160         //cout << "*********************************************************************************\n";
 161         //cout << "*********************************************************************************\n";
 162         //cout << "*********************************************************************************\n";
 163         //cout << "end1ortho = " << rwy.end1ortho << '\n';
 164         //cout << "end2ortho = " << rwy.end2ortho << '\n';      // end2ortho.x() should be zero or thereabouts
 165
 166         Transform();
 167 }
 168
 169 // Run the internal calculations
 170 void FGAILocalTraffic::Update(double dt) {
 171         switch(operatingState) {
 172         case IN_PATTERN:
 173                 FlyTrafficPattern(dt);
 174                 Transform();
 175                 break;
 176         case TAXIING:
 177                 DoGroundElev();
 178                 Taxi(dt);
 179                 Transform();
 180                 break;
 181         case PARKED:
 182                 // Do nothing
 183                 break;
 184         default:
 185                 break;
 186         }
 187 }
 188
 189 // Fly a traffic pattern
 190 // FIXME - far too much of the mechanics of turning, rolling, accellerating, descending etc is in here.
 191 //         Move it out to FGAIPlane and have FlyTrafficPattern just specify what to do, not the implementation.
 192 void FGAILocalTraffic::FlyTrafficPattern(double dt) {
 193         // Need to differentiate between in-air (IAS governed) and on-ground (vel governed)
 194         // Take-off is an interesting case - we are on the ground but takeoff speed is IAS governed.
 195         bool inAir = true;      // FIXME - possibly make into a class variable
 196
 197         static bool transmitted = false;        // FIXME - this is a hack
 198
 199         // WIND
 200         // Wind has two effects - a mechanical one in that IAS translates to a different vel, and the hdg != track,
 201         // but also a piloting effect, in that the AI must be able to descend at a different rate in order to hit the threshold.
 202
 203         //cout << "dt = " << dt << '\n';
 204         double dist = 0;
 205         // ack - I can't remember how long a rate 1 turn is meant to take.
 206         double turn_time = 60.0;        // seconds - TODO - check this guess
 207         double turn_circumference;
 208         double turn_radius;
 209         Point3D orthopos = ortho.ConvertToLocal(pos);   // ortho position of the plane
 210         //cout << "runway elev = " << rwy.threshold_pos.elev() << ' ' << rwy.threshold_pos.elev() * SG_METER_TO_FEET << '\n';
 211         //cout << "elev = " << pos.elev() << ' ' << pos.elev() * SG_METER_TO_FEET << '\n';
 212
 213         // HACK FOR TESTING - REMOVE
 214         //cout << "Calling ExitRunway..." << endl;
 215         //ExitRunway(orthopos);
 216         //return;
 217         // END HACK
 218
 219         //wind
 220         double wind_from = wind_from_hdg->getDoubleValue();
 221         double wind_speed = wind_speed_knots->getDoubleValue();
 222
 223         switch(leg) {
 224         case TAKEOFF_ROLL:
 225                 inAir = false;
 226                 track = rwy.hdg;
 227                 if(vel < 80.0) {
 228                         double dveldt = 5.0;
 229                         vel += dveldt * dt;
 230                 }
 231                 IAS = vel + (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
 232                 if(IAS >= 70) {
 233                         leg = CLIMBOUT;
 234                         pitch = 10.0;
 235                         IAS = best_rate_of_climb_speed;
 236                         slope = 7.0;
 237                 }
 238                 break;
 239         case CLIMBOUT:
 240                 track = rwy.hdg;
 241                 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 600) {
 242                         leg = TURN1;
 243                 }
 244                 break;
 245         case TURN1:
 246                 track += (360.0 / turn_time) * dt * patternDirection;
 247                 Bank(25.0 * patternDirection);
 248                 if((track < (rwy.hdg - 89.0)) || (track > (rwy.hdg + 89.0))) {
 249                         leg = CROSSWIND;
 250                 }
 251                 break;
 252         case CROSSWIND:
 253                 LevelWings();
 254                 track = rwy.hdg + (90.0 * patternDirection);
 255                 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
 256                         slope = 0.0;
 257                         pitch = 0.0;
 258                         IAS = 80.0;             // FIXME - use smooth transistion to new speed
 259                 }
 260                 // turn 1000m out for now
 261                 if(fabs(orthopos.x()) > 980) {
 262                         leg = TURN2;
 263                 }
 264                 break;
 265         case TURN2:
 266                 track += (360.0 / turn_time) * dt * patternDirection;
 267                 Bank(25.0 * patternDirection);
 268                 // just in case we didn't make height on crosswind
 269                 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
 270                         slope = 0.0;
 271                         pitch = 0.0;
 272                         IAS = 80.0;             // FIXME - use smooth transistion to new speed
 273                 }
 274                 if((track < (rwy.hdg - 179.0)) || (track > (rwy.hdg + 179.0))) {
 275                         leg = DOWNWIND;
 276                         transmitted = false;
 277                         //roll = 0.0;
 278                 }
 279                 break;
 280         case DOWNWIND:
 281                 LevelWings();
 282                 track = rwy.hdg - (180 * patternDirection);     //should tend to bring track back into the 0->360 range
 283                 // just in case we didn't make height on crosswind
 284                 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
 285                         slope = 0.0;
 286                         pitch = 0.0;
 287                         IAS = 90.0;             // FIXME - use smooth transistion to new speed
 288                 }
 289                 if((orthopos.y() < 0) && (!transmitted)) {
 290                         TransmitPatternPositionReport();
 291                         transmitted = true;
 292                 }
 293                 if(orthopos.y() < -480) {
 294                         slope = -4.0;   // FIXME - calculate to descent at 500fpm and hit the threshold (taking wind into account as well!!)
 295                         pitch = -3.0;
 296                         IAS = 85.0;
 297                 }
 298                 if(orthopos.y() < -980) {
 299                         //roll = -20;
 300                         leg = TURN3;
 301                         transmitted = false;
 302                         IAS = 80.0;
 303                 }
 304                 break;
 305         case TURN3:
 306                 track += (360.0 / turn_time) * dt * patternDirection;
 307                 Bank(25.0 * patternDirection);
 308                 if(fabs(rwy.hdg - track) < 91.0) {
 309                         leg = BASE;
 310                 }
 311                 break;
 312         case BASE:
 313                 LevelWings();
 314                 if(!transmitted) {
 315                         TransmitPatternPositionReport();
 316                         transmitted = true;
 317                 }
 318                 track = rwy.hdg - (90 * patternDirection);
 319                 slope = -6.0;   // FIXME - calculate to descent at 500fpm and hit the threshold
 320                 pitch = -4.0;
 321                 IAS = 70.0;     // FIXME - slowdown gradually
 322                 // Try and arrange to turn nicely onto base
 323                 turn_circumference = IAS * 0.514444 * turn_time;
 324                 //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius
 325                 //We'll leave it as a hack with IAS for now but it needs revisiting.
 326
 327                 turn_radius = turn_circumference / (2.0 * DCL_PI);
 328                 if(fabs(orthopos.x()) < (turn_radius + 50)) {
 329                         leg = TURN4;
 330                         transmitted = false;
 331                         //roll = -20;
 332                 }
 333                 break;
 334         case TURN4:
 335                 track += (360.0 / turn_time) * dt * patternDirection;
 336                 Bank(25.0 * patternDirection);
 337                 if(fabs(track - rwy.hdg) < 0.6) {
 338                         leg = FINAL;
 339                         vel = nominal_final_speed;
 340                 }
 341                 break;
 342         case FINAL:
 343                 LevelWings();
 344                 if(!transmitted) {
 345                         TransmitPatternPositionReport();
 346                         transmitted = true;
 347                 }
 348                 // Try and track the extended centreline
 349                 track = rwy.hdg - (0.2 * orthopos.x());
 350                 //cout << "orthopos.x() = " << orthopos.x() << " hdg = " << hdg << '\n';
 351                 if(pos.elev() <= rwy.threshold_pos.elev()) {
 352                         pos.setelev(rwy.threshold_pos.elev());// + (-8.5 * SG_FEET_TO_METER));  // This is a complete hack - the rendered runway takes the underlying scenery elev rather than the published runway elev so I should use height above terrain or something.
 353                         slope = 0.0;
 354                         pitch = 0.0;
 355                         leg = LANDING_ROLL;
 356                 }
 357                 break;
 358         case LANDING_ROLL:
 359                 inAir = false;
 360                 track = rwy.hdg;
 361                 double dveldt = -5.0;
 362                 vel += dveldt * dt;
 363                 // FIXME - differentiate between touch and go and full stops
 364                 if(vel <= 15.0) {
 365                         //cout << "Vel <= 15.0, circuitsToFly = " << circuitsToFly << endl;
 366                         if(circuitsToFly <= 0) {
 367                                 //cout << "Calling ExitRunway..." << endl;
 368                                 ExitRunway(orthopos);
 369                                 return;
 370                         } else {
 371                                 //cout << "Taking off again..." << endl;
 372                                 leg = TAKEOFF_ROLL;
 373                                 --circuitsToFly;
 374                         }
 375                 }
 376                 break;
 377     }
 378
 379         if(inAir) {
 380                 // FIXME - at the moment this is a bit screwy
 381                 // The velocity correction is applied based on the relative headings.
 382                 // Then the heading is changed based on the velocity.
 383                 // Which comes first, the chicken or the egg?
 384                 // Does it really matter?
 385
 386                 // Apply wind to ground-relative velocity if in the air
 387                 vel = IAS - (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
 388                 //crab = f(track, wind, vel);
 389                 // The vector we need to fly is our desired vector minus the wind vector
 390                 // TODO - we probably ought to use plib's built in vector types and operations for this
 391                 // ie.  There's almost *certainly* a better way to do this!
 392                 double gxx = vel * sin(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity x component wrt ground
 393                 double gyy = vel * cos(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity y component wrt ground
 394                 double wxx = wind_speed * sin((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS);    // Wind velocity x component
 395                 double wyy = wind_speed * cos((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS);    // Wind velocity y component
 396                 double axx = gxx - wxx; // Plane in-air velocity x component
 397                 double ayy = gyy - wyy; // Plane in-air velocity y component
 398                 // Now we want the angle between gxx and axx (which is the crab)
 399                 double maga = sqrt(axx*axx + ayy*ayy);
 400                 double magg = sqrt(gxx*gxx + gyy*gyy);
 401                 crab = acos((axx*gxx + ayy*gyy) / (maga * magg));
 402                 // At this point this works except we're getting the modulus of the angle
 403                 //cout << "crab = " << crab << '\n';
 404
 405                 // Make sure both headings are in the 0->360 circle in order to get sane differences
 406                 dclBoundHeading(wind_from);
 407                 dclBoundHeading(track);
 408                 if(track > wind_from) {
 409                         if((track - wind_from) <= 180) {
 410                                 crab *= -1.0;
 411                         }
 412                 } else {
 413                         if((wind_from - track) >= 180) {
 414                                 crab *= -1.0;
 415                         }
 416                 }
 417         } else {        // on the ground - crab dosen't apply
 418                 crab = 0.0;
 419         }
 420
 421         hdg = track + crab;
 422         dist = vel * 0.514444 * dt;
 423         pos = dclUpdatePosition(pos, track, slope, dist);
 424 }
 425
 426 void FGAILocalTraffic::TransmitPatternPositionReport(void) {
 427         // airport name + "traffic" + airplane callsign + pattern direction + pattern leg + rwy + ?
 428         string trns = "";
 429
 430         trns += tower->get_name();
 431         trns += " Traffic ";
 432         // FIXME - add the callsign to the class variables
 433         trns += "Trainer-two-five-charlie ";
 434         if(patternDirection == 1) {
 435                 trns += "right ";
 436         } else {
 437                 trns += "left ";
 438         }
 439
 440         // We could probably get rid of this whole switch statement and just pass a string containing the leg from the FlyPattern function.
 441         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?
 442         case TURN1:
 443                 // Fall through to CROSSWIND
 444         case CROSSWIND: // I don't think this case will be used here but it can't hurt to leave it in
 445                 trns += "crosswind ";
 446                 break;
 447         case TURN2:
 448                 // Fall through to DOWNWIND
 449         case DOWNWIND:
 450                 trns += "downwind ";
 451                 break;
 452         case TURN3:
 453                 // Fall through to BASE
 454         case BASE:
 455                 trns += "base ";
 456                 break;
 457         case TURN4:
 458                 // Fall through to FINAL
 459         case FINAL:             // maybe this should include long/short final if appropriate?
 460                 trns += "final ";
 461                 break;
 462         default:                // Hopefully this won't be used
 463                 trns += "pattern ";
 464                 break;
 465         }
 466         // FIXME - I've hardwired the runway call as well!! (We could work this out from rwy heading and mag deviation)
 467         trns += ConvertRwyNumToSpokenString(1);
 468
 469         // And add the airport name again
 470         trns += tower->get_name();
 471
 472         Transmit(trns);
 473 }
 474
 475 void FGAILocalTraffic::ExitRunway(Point3D orthopos) {
 476         //cout << "In ExitRunway" << endl;
 477         //cout << "Runway ID is " << rwy.ID << endl;
 478         node_array_type exitNodes = airport.GetExits(rwy.ID);   //I suppose we ought to have some fallback for rwy with no defined exits?
 479         /*
 480         cout << "Node ID's of exits are ";
 481         for(unsigned int i=0; i<exitNodes.size(); ++i) {
 482                 cout << exitNodes[i]->nodeID << ' ';
 483         }
 484         cout << endl;
 485         */
 486         if(exitNodes.size()) {
 487                 //Find the next exit from orthopos.y
 488                 double d;
 489                 double dist = 100000;   //ie. longer than any runway in existance
 490                 double backdist = 100000;
 491                 node_array_iterator nItr = exitNodes.begin();
 492                 node* rwyExit = *(exitNodes.begin());
 493                 //int gateID;           //This might want to be more persistant at some point
 494                 while(nItr != exitNodes.end()) {
 495                         d = ortho.ConvertToLocal((*nItr)->pos).y() - ortho.ConvertToLocal(pos).y();     //FIXME - consider making orthopos a class variable
 496                         if(d > 0.0) {
 497                                 if(d < dist) {
 498                                         dist = d;
 499                                         rwyExit = *nItr;
 500                                 }
 501                         } else {
 502                                 if(fabs(d) < backdist) {
 503                                         backdist = d;
 504                                         //TODO - need some logic here that if we don't get a forward exit we turn round and store the backwards one
 505                                 }
 506                         }
 507                         ++nItr;
 508                 }
 509                 in_dest = airport.GetGateNode();
 510                 if(in_dest == NULL) {
 511                         // Implies no available gates - what shall we do?
 512                         // For now just vanish the plane - possibly we can make this more elegant in the future
 513                         SG_LOG(SG_GENERAL, SG_ALERT, "No gate found by FGAILocalTraffic whilst landing at " << airportID << '\n');
 514                         aip.setVisible(false);
 515                         operatingState = PARKED;
 516                         return;
 517                 }
 518                 path = airport.GetPath(rwyExit, in_dest);
 519                 /*
 520                 cout << "path returned was:" << endl;
 521                 for(unsigned int i=0; i<path.size(); ++i) {
 522                         switch(path[i]->struct_type) {
 523                         case NODE:
 524                                 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
 525                                 break;
 526                         case ARC:
 527                                 cout << "ARC\n";
 528                                 break;
 529                         }
 530                 }
 531                 */
 532                 taxiState = TD_INBOUND;
 533                 StartTaxi();
 534         } else {
 535                 // Something must have gone wrong with the ground network file - or there is only a rwy here and no exits defined
 536                 SG_LOG(SG_GENERAL, SG_ALERT, "No exits found by FGAILocalTraffic from runway " << rwy.ID << " at " << airportID << '\n');
 537                 // What shall we do - just remove the plane from sight?
 538                 aip.setVisible(false);
 539                 operatingState = PARKED;
 540         }
 541 }
 542
 543 // Set the class variable nextTaxiNode to the next node in the path
 544 // and update taxiPathPos, the class variable path iterator position
 545 // TODO - maybe should return error codes to the calling function if we fail here
 546 void FGAILocalTraffic::GetNextTaxiNode() {
 547         //cout << "GetNextTaxiNode called " << endl;
 548         //cout << "taxiPathPos = " << taxiPathPos << endl;
 549         ground_network_path_iterator pathItr = path.begin() + taxiPathPos;
 550         if(pathItr == path.end()) {
 551                 SG_LOG(SG_GENERAL, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - no more nodes in path\n");
 552         } else {
 553                 if((*pathItr)->struct_type == NODE) {
 554                         //cout << "ITS A NODE" << endl;
 555                         //*pathItr = new node;
 556                         nextTaxiNode = (node*)*pathItr;
 557                         ++taxiPathPos;
 558                         //delete pathItr;
 559                 } else {
 560                         //cout << "ITS NOT A NODE" << endl;
 561                         //The first item in found must have been an arc
 562                         //Assume for now that it was straight
 563                         pathItr++;
 564                         taxiPathPos++;
 565                         if(pathItr == path.end()) {
 566                                 SG_LOG(SG_GENERAL, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - path ended with an arc\n");
 567                         } else if((*pathItr)->struct_type == NODE) {
 568                                 nextTaxiNode = (node*)*pathItr;
 569                                 ++taxiPathPos;
 570                         } else {
 571                                 //OOPS - two non-nodes in a row - that shouldn't happen ATM
 572                                 SG_LOG(SG_GENERAL, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - two non-nodes in sequence\n");
 573                         }
 574                 }
 575         }
 576 }
 577
 578 // StartTaxi - set up the taxiing state - call only at the start of taxiing
 579 void FGAILocalTraffic::StartTaxi() {
 580         //cout << "StartTaxi called" << endl;
 581         operatingState = TAXIING;
 582         taxiPathPos = 0;
 583
 584         //Set the desired heading
 585         //Assume we are aiming for first node on path
 586         //Eventually we may need to consider the fact that we might start on a curved arc and
 587         //not be able to head directly for the first node.
 588         GetNextTaxiNode();      // sets the class variable nextTaxiNode to the next taxi node!
 589         desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
 590         //cout << "First taxi heading is " << desiredTaxiHeading << endl;
 591 }
 592
 593 void FGAILocalTraffic::Taxi(double dt) {
 594         //cout << "Taxi called" << endl;
 595         // Logic - if we are further away from next point than turn radius then head for it
 596         // If we have reached turning point then get next point and turn onto that heading
 597         // Look out for the finish!!
 598
 599         //Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane
 600         desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
 601
 602         // HACK ALERT! - for now we will taxi at constant speed for straights and turns
 603
 604         // Remember that hdg is always equal to track when taxiing so we don't have to consider them both
 605         double dist_to_go = dclGetHorizontalSeparation(pos, nextTaxiNode->pos); // we may be able to do this more cheaply using orthopos
 606         //cout << "dist_to_go = " << dist_to_go << endl;
 607         if((nextTaxiNode->type == GATE) && (dist_to_go <= 0.1)) {
 608                 // park up
 609                 //taxiing = false;
 610                 //parked = true;
 611                 operatingState = PARKED;
 612         } else if((dist_to_go > taxiTurnRadius) || (nextTaxiNode->type == GATE)) {
 613                 // if the turn radius is r, and speed is s, then in a time dt we turn through
 614                 // ((s.dt)/(PI.r)) x 180 degrees
 615                 // or alternatively (s.dt)/r radians
 616                 //cout << "hdg = " << hdg << " desired taxi heading = " << desiredTaxiHeading << '\n';
 617                 if(fabs(hdg - desiredTaxiHeading) > 0.1) {
 618                         // Which is the quickest direction to turn onto heading?
 619                         if(desiredTaxiHeading > hdg) {
 620                                 if((desiredTaxiHeading - hdg) <= 180) {
 621                                         // turn right
 622                                         hdg += ((nominalTaxiSpeed * 0.514444 * dt) / (taxiTurnRadius * DCL_PI)) * 180.0;
 623                                         // TODO - check that increments are less than the delta that we check for the right direction
 624                                         // Probably need to reduce convergence speed as convergence is reached
 625                                 } else {
 626                                         hdg -= ((nominalTaxiSpeed * 0.514444 * dt) / (taxiTurnRadius * DCL_PI)) * 180.0;
 627                                 }
 628                         } else {
 629                                 if((hdg - desiredTaxiHeading) <= 180) {
 630                                         // turn left
 631                                         hdg -= ((nominalTaxiSpeed * 0.514444 * dt) / (taxiTurnRadius * DCL_PI)) * 180.0;
 632                                         // TODO - check that increments are less than the delta that we check for the right direction
 633                                         // Probably need to reduce convergence speed as convergence is reached
 634                                 } else {
 635                                         hdg += ((nominalTaxiSpeed * 0.514444 * dt) / (taxiTurnRadius * DCL_PI)) * 180.0;
 636                                 }
 637                         }
 638                 }
 639                 double vel = nominalTaxiSpeed;
 640                 //cout << "vel = " << vel << endl;
 641                 double dist = vel * 0.514444 * dt;
 642                 //cout << "dist = " << dist << endl;
 643                 double track = hdg;
 644                 //cout << "track = " << track << endl;
 645                 double slope = 0.0;
 646                 pos = dclUpdatePosition(pos, track, slope, dist);
 647                 //cout << "Updated position...\n";
 648                 if(aip.getFGLocation()->get_cur_elev_m() > -9990) {
 649                         pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
 650                 } // else don't change the elev until we get a valid ground elev again!
 651         } else {
 652                 // Time to turn (we've already checked it's not the end we're heading for).
 653                 // set the target node to be the next node which will prompt automatically turning onto
 654                 // the right heading in the stuff above, with the usual provisos applied.
 655                 GetNextTaxiNode();
 656                 // For now why not just recursively call this function?
 657                 Taxi(dt);
 658         }
 659 }
 660
 661
 662 // Warning - ground elev determination is CPU intensive
 663 // Either this function or the logic of how often it is called
 664 // will almost certainly change.
 665 void FGAILocalTraffic::DoGroundElev() {
 666
 667         // It would be nice if we could set the correct tile center here in order to get a correct
 668         // answer with one call to the function, but what I tried in the two commented-out lines
 669         // below only intermittently worked, and I haven't quite groked why yet.
 670         //SGBucket buck(pos.lon(), pos.lat());
 671         //aip.getFGLocation()->set_tile_center(Point3D(buck.get_center_lon(), buck.get_center_lat(), 0.0));
 672
 673         double visibility_meters = fgGetDouble("/environment/visibility-m");
 674         //globals->get_tile_mgr()->prep_ssg_nodes( acmodel_location,
 675         globals->get_tile_mgr()->prep_ssg_nodes( aip.getFGLocation(),   visibility_meters );
 676         globals->get_tile_mgr()->update( aip.getFGLocation(), visibility_meters, (aip.getFGLocation())->get_absolute_view_pos() );
 677         // save results of update in FGLocation for fdm...
 678
 679         //if ( globals->get_scenery()->get_cur_elev() > -9990 ) {
 680         //      acmodel_location->
 681         //      set_cur_elev_m( globals->get_scenery()->get_cur_elev() );
 682         //}
 683
 684         // The need for this here means that at least 2 consecutive passes are needed :-(
 685         aip.getFGLocation()->set_tile_center( globals->get_scenery()->get_next_center() );
 686
 687         //cout << "Transform Elev is " << globals->get_scenery()->get_cur_elev() << '\n';
 688         aip.getFGLocation()->set_cur_elev_m(globals->get_scenery()->get_cur_elev());
 689         //return(globals->get_scenery()->get_cur_elev());
 690 }