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