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 <Airports/runways.hxx>
  27 #include <Main/globals.hxx>
  28 #include <Main/location.hxx>
  29 #include <Scenery/scenery.hxx>
  30 #include <Scenery/tilemgr.hxx>
  31 #include <simgear/math/point3d.hxx>
  32 #include <simgear/math/sg_geodesy.hxx>
  33 #include <simgear/misc/sg_path.hxx>
  34 #include <string>
  35 #include <math.h>
  36
  37 SG_USING_STD(string);
  38
  39 #include "ATCmgr.hxx"
  40 #include "AILocalTraffic.hxx"
  41 #include "ATCutils.hxx"
  42
  43 FGAILocalTraffic::FGAILocalTraffic() {
  44         roll = 0.0;
  45         pitch = 0.0;
  46         hdg = 270.0;
  47
  48         //Hardwire initialisation for now - a lot of this should be read in from config eventually
  49         Vr = 70.0;
  50         best_rate_of_climb_speed = 70.0;
  51         //best_rate_of_climb;
  52         //nominal_climb_speed;
  53         //nominal_climb_rate;
  54         //nominal_circuit_speed;
  55         //min_circuit_speed;
  56         //max_circuit_speed;
  57         nominal_descent_rate = 500.0;
  58         nominal_final_speed = 65.0;
  59         //nominal_approach_speed;
  60         //stall_speed_landing_config;
  61         nominalTaxiSpeed = 8.0;
  62         taxiTurnRadius = 8.0;
  63         wheelOffset = 1.45;     // Warning - hardwired to the C172 - we need to read this in from file.
  64         elevInitGood = false;
  65         // Init the property nodes
  66         wind_from_hdg = fgGetNode("/environment/wind-from-heading-deg", true);
  67         wind_speed_knots = fgGetNode("/environment/wind-speed-kts", true);
  68         circuitsToFly = 0;
  69         liningUp = false;
  70 }
  71
  72 FGAILocalTraffic::~FGAILocalTraffic() {
  73 }
  74
  75
  76 // Get details of the active runway
  77 // This is a private internal function and it is assumed that by the
  78 // time it is called the tower control and airport code will have been set up.
  79 void FGAILocalTraffic::GetRwyDetails() {
  80         //cout << "GetRwyDetails called" << endl;
  81
  82         // Based on the airport-id and wind get the active runway
  83         SGPath path( globals->get_fg_root() );
  84         path.append( "Airports" );
  85         path.append( "runways.mk4" );
  86         FGRunways runways( path.c_str() );
  87
  88         //wind
  89         double hdg = wind_from_hdg->getDoubleValue();
  90         double speed = wind_speed_knots->getDoubleValue();
  91         hdg = (speed == 0.0 ? 270.0 : hdg);
  92         //cout << "Heading = " << hdg << '\n';
  93
  94         FGRunway runway;
  95         bool rwyGood = runways.search(airportID, int(hdg), &runway);
  96         if(rwyGood) {
  97                 // Get the threshold position
  98         hdg = runway.heading;
  99                 //cout << "hdg reset to " << hdg << '\n';
 100                 double other_way = hdg - 180.0;
 101                 while(other_way <= 0.0) {
 102                         other_way += 360.0;
 103                 }
 104
 105         // move to the +l end/center of the runway
 106                 //cout << "Runway center is at " << runway.lon << ", " << runway.lat << '\n';
 107         Point3D origin = Point3D(runway.lon, runway.lat, aptElev);
 108                 Point3D ref = origin;
 109         double tshlon, tshlat, tshr;
 110                 double tolon, tolat, tor;
 111                 rwy.length = runway.length * SG_FEET_TO_METER;
 112         geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), other_way,
 113                                 rwy.length / 2.0 - 25.0, &tshlat, &tshlon, &tshr );
 114         geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), hdg,
 115                                 rwy.length / 2.0 - 25.0, &tolat, &tolon, &tor );
 116                 // Note - 25 meters in from the runway end is a bit of a hack to put the plane ahead of the user.
 117                 // now copy what we need out of runway into rwy
 118         rwy.threshold_pos = Point3D(tshlon, tshlat, aptElev);
 119                 Point3D takeoff_end = Point3D(tolon, tolat, aptElev);
 120                 //cout << "Threshold position = " << tshlon << ", " << tshlat << ", " << aptElev << '\n';
 121                 //cout << "Takeoff position = " << tolon << ", " << tolat << ", " << aptElev << '\n';
 122                 rwy.hdg = hdg;
 123                 rwy.rwyID = runway.rwy_no;
 124                 // Set the projection for the local area
 125                 ortho.Init(rwy.threshold_pos, rwy.hdg);
 126                 rwy.end1ortho = ortho.ConvertToLocal(rwy.threshold_pos);        // should come out as zero
 127                 rwy.end2ortho = ortho.ConvertToLocal(takeoff_end);
 128                 rwy.mag_var = 14.0;             // TODO - remove this last hardwired bit!!
 129                 //(Although I don't think we even use the magvar any more?)
 130                 rwy.mag_hdg = rwy.hdg - rwy.mag_var;
 131                 rwy.ID = (int)rwy.mag_hdg / 10;
 132                 //cout << "rwy.ID = " << rwy.ID << '\n';
 133         } else {
 134                 SG_LOG(SG_GENERAL, SG_ALERT, "Help  - can't get good runway in FGAILocalTraffic!!\n");
 135         }
 136 }
 137
 138 /*
 139 There are two possible scenarios during initialisation:
 140 The first is that the user is flying towards the airport, and hence the traffic
 141 could be initialised anywhere, as long as the AI planes are consistent with
 142 each other.
 143 The second is that the user has started the sim at or close to the airport, and
 144 hence the traffic must be initialised with respect to the user as well as each other.
 145 To a certain extent it's FGAIMgr that has to worry about this, but we need to provide
 146 sufficient initialisation functionality within the plane classes to allow the manager
 147 to initialy position them where and how required.
 148 */
 149 bool FGAILocalTraffic::Init(string ICAO, OperatingState initialState, PatternLeg initialLeg) {
 150         //cout << "FGAILocalTraffic.Init(...) called" << endl;
 151         // Hack alert - Hardwired path!!
 152         string planepath = "Aircraft/c172/Models/c172-dpm.ac";
 153         SGPath path = globals->get_fg_root();
 154         path.append(planepath);
 155         aip.init(planepath.c_str());
 156         aip.setVisible(false);          // This will be set to true once a valid ground elevation has been determined
 157         globals->get_scenery()->get_scene_graph()->addKid(aip.getSceneGraph());
 158
 159         // Find the tower frequency - this is dependent on the ATC system being initialised before the AI system
 160         airportID = ICAO;
 161         AirportATC a;
 162         if(globals->get_ATC_mgr()->GetAirportATCDetails(airportID, &a)) {
 163                 if(a.tower_freq) {      // Has a tower
 164                         tower = (FGTower*)globals->get_ATC_mgr()->GetATCPointer((string)airportID, TOWER);      // Maybe need some error checking here
 165                         freq = (double)tower->get_freq() / 100.0;
 166                         //cout << "***********************************AILocalTraffic freq = " << freq << '\n';
 167                 } else {
 168                         // Check CTAF, unicom etc
 169                 }
 170         } else {
 171                 //cout << "Unable to find airport details in FGAILocalTraffic::Init()\n";
 172         }
 173
 174         // Initialise the relevant FGGround
 175         // This needs a complete overhaul soon - what happens if we have 2 AI planes at same airport - they don't both need a structure
 176         // This needs to be handled by the ATC manager or similar so only one set of physical data per airport is instantiated
 177         // ie. TODO TODO FIXME FIXME
 178         airport.Init();
 179
 180         // Get the airport elevation
 181         aptElev = dclGetAirportElev(airportID.c_str()) * SG_FEET_TO_METER;
 182         //cout << "Airport elev in AILocalTraffic = " << aptElev << '\n';
 183         // WARNING - we use this elev for the whole airport - some assumptions in the code
 184         // might fall down with very slopey airports.
 185
 186         //cout << "In Init(), initialState = " << initialState << '\n';
 187         operatingState = initialState;
 188         switch(operatingState) {
 189         case PARKED:
 190                 ourGate = airport.GetGateNode();
 191                 if(ourGate == NULL) {
 192                         // Implies no available gates - what shall we do?
 193                         // For now just vanish the plane - possibly we can make this more elegant in the future
 194                         SG_LOG(SG_GENERAL, SG_ALERT, "No gate found by FGAILocalTraffic whilst attempting Init at " << airportID << '\n');
 195                         return(false);
 196                 }
 197                 pitch = 0.0;
 198                 roll = 0.0;
 199                 vel = 0.0;
 200                 slope = 0.0;
 201                 pos = ourGate->pos;
 202                 pos.setelev(aptElev);
 203                 hdg = ourGate->heading;
 204
 205                 // Now we've set the position we can do the ground elev
 206                 elevInitGood = false;
 207                 inAir = false;
 208                 DoGroundElev();
 209
 210                 Transform();
 211                 break;
 212         case TAXIING:
 213                 // FIXME - implement this case properly
 214                 return(false);  // remove this line when fixed!
 215                 break;
 216         case IN_PATTERN:
 217                 // For now we'll always start the in_pattern case on the threshold ready to take-off
 218                 // since we've got the implementation for this case already.
 219                 // TODO - implement proper generic in_pattern startup.
 220
 221                 // Get the active runway details (and copy them into rwy)
 222                 GetRwyDetails();
 223
 224                 // Initial position on threshold for now
 225                 pos.setlat(rwy.threshold_pos.lat());
 226                 pos.setlon(rwy.threshold_pos.lon());
 227                 pos.setelev(rwy.threshold_pos.elev());
 228                 hdg = rwy.hdg;
 229
 230                 // Now we've set the position we can do the ground elev
 231                 // This might not always be necessary if we implement in-air start
 232                 elevInitGood = false;
 233                 inAir = false;
 234                 DoGroundElev();
 235
 236                 pitch = 0.0;
 237                 roll = 0.0;
 238                 leg = TAKEOFF_ROLL;
 239                 vel = 0.0;
 240                 slope = 0.0;
 241
 242                 circuitsToFly = 0;              // ie just fly this circuit and then stop
 243                 touchAndGo = false;
 244                 // FIXME TODO - pattern direction is still hardwired
 245                 patternDirection = -1;          // Left
 246                 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
 247                 if(rwy.rwyID.size() == 3) {
 248                         patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
 249                 }
 250
 251                 operatingState = IN_PATTERN;
 252
 253                 Transform();
 254                 break;
 255         default:
 256                 SG_LOG(SG_GENERAL, SG_ALERT, "Attempt to set unknown operating state in FGAILocalTraffic.Init(...)\n");
 257                 return(false);
 258         }
 259
 260
 261         return(true);
 262 }
 263
 264 // Commands to do something from higher level logic
 265 void FGAILocalTraffic::FlyCircuits(int numCircuits, bool tag) {
 266         //cout << "FlyCircuits called" << endl;
 267
 268         switch(operatingState) {
 269         case IN_PATTERN:
 270                 circuitsToFly += numCircuits;
 271                 return;
 272                 break;
 273         case TAXIING:
 274                 // For now we'll punt this and do nothing
 275                 break;
 276         case PARKED:
 277                 circuitsToFly = numCircuits - 1;        // Hack (-1) because we only test and decrement circuitsToFly after landing
 278                                                                                 // thus flying one too many circuits.  TODO - Need to sort this out better!
 279                 touchAndGo = tag;
 280
 281                 // Get the active runway details (and copy them into rwy)
 282                 GetRwyDetails();
 283
 284                 // Get the takeoff node for the active runway, get a path to it and start taxiing
 285                 path = airport.GetPath(ourGate, rwy.rwyID);
 286                 if(path.size() < 2) {
 287                         // something has gone wrong
 288                         SG_LOG(SG_GENERAL, SG_ALERT, "Invalid path from gate to theshold in FGAILocalTraffic::FlyCircuits\n");
 289                         return;
 290                 }
 291                 /*
 292                 cout << "path returned was:" << endl;
 293                 for(unsigned int i=0; i<path.size(); ++i) {
 294                         switch(path[i]->struct_type) {
 295                         case NODE:
 296                                 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
 297                                 break;
 298                         case ARC:
 299                                 cout << "ARC\n";
 300                                 break;
 301                         }
 302                 }
 303                 */
 304                 // pop the gate - we're here already!
 305                 path.erase(path.begin());
 306                 //path.erase(path.begin());
 307                 /*
 308                 cout << "path after popping front is:" << endl;
 309                 for(unsigned int i=0; i<path.size(); ++i) {
 310                         switch(path[i]->struct_type) {
 311                         case NODE:
 312                                 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
 313                                 break;
 314                         case ARC:
 315                                 cout << "ARC\n";
 316                                 break;
 317                         }
 318                 }
 319                 */
 320
 321                 taxiState = TD_OUTBOUND;
 322                 StartTaxi();
 323
 324                 // Maybe the below should be set when we get to the threshold and prepare for TO?
 325                 // FIXME TODO - pattern direction is still hardwired
 326                 patternDirection = -1;          // Left
 327                 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
 328                 if(rwy.rwyID.size() == 3) {
 329                         patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
 330                 }
 331
 332                 Transform();
 333                 break;
 334         }
 335 }
 336
 337 // Run the internal calculations
 338 void FGAILocalTraffic::Update(double dt) {
 339         switch(operatingState) {
 340         case IN_PATTERN:
 341                 //cout << "In IN_PATTERN\n";
 342                 if(!inAir) DoGroundElev();
 343                 if(!elevInitGood) {
 344                         if(aip.getFGLocation()->get_cur_elev_m() > -9990.0) {
 345                                 pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
 346                                 //cout << "TAKEOFF_ROLL, POS = " << pos.lon() << ", " << pos.lat() << ", " << pos.elev() << '\n';
 347                                 //Transform();
 348                                 aip.setVisible(true);
 349                                 //cout << "Making plane visible!\n";
 350                                 elevInitGood = true;
 351                         }
 352                 }
 353                 FlyTrafficPattern(dt);
 354                 Transform();
 355                 break;
 356         case TAXIING:
 357                 //cout << "In TAXIING\n";
 358                 if(!elevInitGood) {
 359                         //DoGroundElev();
 360                         if(aip.getFGLocation()->get_cur_elev_m() > -9990.0) {
 361                                 pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
 362                                 //Transform();
 363                                 aip.setVisible(true);
 364                                 //Transform();
 365                                 //cout << "Making plane visible!\n";
 366                                 elevInitGood = true;
 367                         }
 368                 }
 369                 DoGroundElev();
 370                 Taxi(dt);
 371                 Transform();
 372                 break;
 373         case PARKED:
 374                 //cout << "In PARKED\n";
 375                 if(!elevInitGood) {
 376                         DoGroundElev();
 377                         if(aip.getFGLocation()->get_cur_elev_m() > -9990.0) {
 378                                 pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
 379                                 //Transform();
 380                                 aip.setVisible(true);
 381                                 //Transform();
 382                                 //cout << "Making plane visible!\n";
 383                                 elevInitGood = true;
 384                         }
 385                 }
 386                 // Do nothing
 387                 Transform();
 388                 break;
 389         default:
 390                 break;
 391         }
 392 }
 393
 394 // Fly a traffic pattern
 395 // FIXME - far too much of the mechanics of turning, rolling, accellerating, descending etc is in here.
 396 //         Move it out to FGAIPlane and have FlyTrafficPattern just specify what to do, not the implementation.
 397 void FGAILocalTraffic::FlyTrafficPattern(double dt) {
 398         // Need to differentiate between in-air (IAS governed) and on-ground (vel governed)
 399         // Take-off is an interesting case - we are on the ground but takeoff speed is IAS governed.
 400
 401         static bool transmitted = false;        // FIXME - this is a hack
 402
 403         // WIND
 404         // Wind has two effects - a mechanical one in that IAS translates to a different vel, and the hdg != track,
 405         // but also a piloting effect, in that the AI must be able to descend at a different rate in order to hit the threshold.
 406
 407         //cout << "dt = " << dt << '\n';
 408         double dist = 0;
 409         // ack - I can't remember how long a rate 1 turn is meant to take.
 410         double turn_time = 60.0;        // seconds - TODO - check this guess
 411         double turn_circumference;
 412         double turn_radius;
 413         Point3D orthopos = ortho.ConvertToLocal(pos);   // ortho position of the plane
 414         //cout << "runway elev = " << rwy.threshold_pos.elev() << ' ' << rwy.threshold_pos.elev() * SG_METER_TO_FEET << '\n';
 415         //cout << "elev = " << pos.elev() << ' ' << pos.elev() * SG_METER_TO_FEET << '\n';
 416
 417         // HACK FOR TESTING - REMOVE
 418         //cout << "Calling ExitRunway..." << endl;
 419         //ExitRunway(orthopos);
 420         //return;
 421         // END HACK
 422
 423         //wind
 424         double wind_from = wind_from_hdg->getDoubleValue();
 425         double wind_speed = wind_speed_knots->getDoubleValue();
 426
 427         switch(leg) {
 428         case TAKEOFF_ROLL:
 429                 //inAir = false;
 430                 track = rwy.hdg;
 431                 if(vel < 80.0) {
 432                         double dveldt = 5.0;
 433                         vel += dveldt * dt;
 434                 }
 435                 if(aip.getFGLocation()->get_cur_elev_m() > -9990.0) {
 436                         pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
 437                 }
 438                 IAS = vel + (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
 439                 if(IAS >= 70) {
 440                         leg = CLIMBOUT;
 441                         pitch = 10.0;
 442                         IAS = best_rate_of_climb_speed;
 443                         slope = 7.0;
 444                         inAir = true;
 445                 }
 446                 break;
 447         case CLIMBOUT:
 448                 track = rwy.hdg;
 449                 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 600) {
 450                         leg = TURN1;
 451                 }
 452                 break;
 453         case TURN1:
 454                 track += (360.0 / turn_time) * dt * patternDirection;
 455                 Bank(25.0 * patternDirection);
 456                 if((track < (rwy.hdg - 89.0)) || (track > (rwy.hdg + 89.0))) {
 457                         leg = CROSSWIND;
 458                 }
 459                 break;
 460         case CROSSWIND:
 461                 LevelWings();
 462                 track = rwy.hdg + (90.0 * patternDirection);
 463                 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
 464                         slope = 0.0;
 465                         pitch = 0.0;
 466                         IAS = 80.0;             // FIXME - use smooth transistion to new speed
 467                 }
 468                 // turn 1000m out for now
 469                 if(fabs(orthopos.x()) > 980) {
 470                         leg = TURN2;
 471                 }
 472                 break;
 473         case TURN2:
 474                 track += (360.0 / turn_time) * dt * patternDirection;
 475                 Bank(25.0 * patternDirection);
 476                 // just in case we didn't make height on crosswind
 477                 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
 478                         slope = 0.0;
 479                         pitch = 0.0;
 480                         IAS = 80.0;             // FIXME - use smooth transistion to new speed
 481                 }
 482                 if((track < (rwy.hdg - 179.0)) || (track > (rwy.hdg + 179.0))) {
 483                         leg = DOWNWIND;
 484                         transmitted = false;
 485                         //roll = 0.0;
 486                 }
 487                 break;
 488         case DOWNWIND:
 489                 LevelWings();
 490                 track = rwy.hdg - (180 * patternDirection);     //should tend to bring track back into the 0->360 range
 491                 // just in case we didn't make height on crosswind
 492                 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
 493                         slope = 0.0;
 494                         pitch = 0.0;
 495                         IAS = 90.0;             // FIXME - use smooth transistion to new speed
 496                 }
 497                 if((orthopos.y() < 0) && (!transmitted)) {
 498                         TransmitPatternPositionReport();
 499                         transmitted = true;
 500                 }
 501                 if(orthopos.y() < -480) {
 502                         slope = -4.0;   // FIXME - calculate to descent at 500fpm and hit the threshold (taking wind into account as well!!)
 503                         pitch = -3.0;
 504                         IAS = 85.0;
 505                 }
 506                 if(orthopos.y() < -980) {
 507                         //roll = -20;
 508                         leg = TURN3;
 509                         transmitted = false;
 510                         IAS = 80.0;
 511                 }
 512                 break;
 513         case TURN3:
 514                 track += (360.0 / turn_time) * dt * patternDirection;
 515                 Bank(25.0 * patternDirection);
 516                 if(fabs(rwy.hdg - track) < 91.0) {
 517                         leg = BASE;
 518                 }
 519                 break;
 520         case BASE:
 521                 LevelWings();
 522                 if(!transmitted) {
 523                         TransmitPatternPositionReport();
 524                         transmitted = true;
 525                 }
 526                 track = rwy.hdg - (90 * patternDirection);
 527                 slope = -6.0;   // FIXME - calculate to descent at 500fpm and hit the threshold
 528                 pitch = -4.0;
 529                 IAS = 70.0;     // FIXME - slowdown gradually
 530                 // Try and arrange to turn nicely onto base
 531                 turn_circumference = IAS * 0.514444 * turn_time;
 532                 //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius
 533                 //We'll leave it as a hack with IAS for now but it needs revisiting.
 534
 535                 turn_radius = turn_circumference / (2.0 * DCL_PI);
 536                 if(fabs(orthopos.x()) < (turn_radius + 50)) {
 537                         leg = TURN4;
 538                         transmitted = false;
 539                         //roll = -20;
 540                 }
 541                 break;
 542         case TURN4:
 543                 track += (360.0 / turn_time) * dt * patternDirection;
 544                 Bank(25.0 * patternDirection);
 545                 if(fabs(track - rwy.hdg) < 0.6) {
 546                         leg = FINAL;
 547                         vel = nominal_final_speed;
 548                 }
 549                 break;
 550         case FINAL:
 551                 LevelWings();
 552                 if(!transmitted) {
 553                         TransmitPatternPositionReport();
 554                         transmitted = true;
 555                 }
 556                 // Try and track the extended centreline
 557                 track = rwy.hdg - (0.2 * orthopos.x());
 558                 //cout << "orthopos.x() = " << orthopos.x() << " hdg = " << hdg << '\n';
 559                 if(pos.elev() < (rwy.threshold_pos.elev()+20.0+wheelOffset)) {
 560                         DoGroundElev(); // Need to call it here expicitly on final since it's only called
 561                                         // for us in update(...) when the inAir flag is false.
 562                 }
 563                 if(pos.elev() < (rwy.threshold_pos.elev()+10.0+wheelOffset)) {
 564                         if(aip.getFGLocation()->get_cur_elev_m() > -9990.0) {
 565                                 if((aip.getFGLocation()->get_cur_elev_m() + wheelOffset) > pos.elev()) {
 566                                         slope = 0.0;
 567                                         pitch = 0.0;
 568                                         leg = LANDING_ROLL;
 569                                         inAir = false;
 570                                 }
 571                         }       // else need a fallback position based on arpt elev in case ground elev determination fails?
 572                 }
 573                 break;
 574         case LANDING_ROLL:
 575                 //inAir = false;
 576                 if(aip.getFGLocation()->get_cur_elev_m() > -9990.0) {
 577                         pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
 578                 }
 579                 track = rwy.hdg;
 580                 double dveldt = -5.0;
 581                 vel += dveldt * dt;
 582                 // FIXME - differentiate between touch and go and full stops
 583                 if(vel <= 15.0) {
 584                         //cout << "Vel <= 15.0, circuitsToFly = " << circuitsToFly << endl;
 585                         if(circuitsToFly <= 0) {
 586                                 //cout << "Calling ExitRunway..." << endl;
 587                                 ExitRunway(orthopos);
 588                                 return;
 589                         } else {
 590                                 //cout << "Taking off again..." << endl;
 591                                 leg = TAKEOFF_ROLL;
 592                                 --circuitsToFly;
 593                         }
 594                 }
 595                 break;
 596     }
 597
 598         if(inAir) {
 599                 // FIXME - at the moment this is a bit screwy
 600                 // The velocity correction is applied based on the relative headings.
 601                 // Then the heading is changed based on the velocity.
 602                 // Which comes first, the chicken or the egg?
 603                 // Does it really matter?
 604
 605                 // Apply wind to ground-relative velocity if in the air
 606                 vel = IAS - (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
 607                 //crab = f(track, wind, vel);
 608                 // The vector we need to fly is our desired vector minus the wind vector
 609                 // TODO - we probably ought to use plib's built in vector types and operations for this
 610                 // ie.  There's almost *certainly* a better way to do this!
 611                 double gxx = vel * sin(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity x component wrt ground
 612                 double gyy = vel * cos(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity y component wrt ground
 613                 double wxx = wind_speed * sin((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS);    // Wind velocity x component
 614                 double wyy = wind_speed * cos((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS);    // Wind velocity y component
 615                 double axx = gxx - wxx; // Plane in-air velocity x component
 616                 double ayy = gyy - wyy; // Plane in-air velocity y component
 617                 // Now we want the angle between gxx and axx (which is the crab)
 618                 double maga = sqrt(axx*axx + ayy*ayy);
 619                 double magg = sqrt(gxx*gxx + gyy*gyy);
 620                 crab = acos((axx*gxx + ayy*gyy) / (maga * magg));
 621                 // At this point this works except we're getting the modulus of the angle
 622                 //cout << "crab = " << crab << '\n';
 623
 624                 // Make sure both headings are in the 0->360 circle in order to get sane differences
 625                 dclBoundHeading(wind_from);
 626                 dclBoundHeading(track);
 627                 if(track > wind_from) {
 628                         if((track - wind_from) <= 180) {
 629                                 crab *= -1.0;
 630                         }
 631                 } else {
 632                         if((wind_from - track) >= 180) {
 633                                 crab *= -1.0;
 634                         }
 635                 }
 636         } else {        // on the ground - crab dosen't apply
 637                 crab = 0.0;
 638         }
 639
 640         hdg = track + crab;
 641         dist = vel * 0.514444 * dt;
 642         pos = dclUpdatePosition(pos, track, slope, dist);
 643 }
 644
 645 void FGAILocalTraffic::TransmitPatternPositionReport(void) {
 646         // airport name + "traffic" + airplane callsign + pattern direction + pattern leg + rwy + ?
 647         string trns = "";
 648
 649         trns += tower->get_name();
 650         trns += " Traffic ";
 651         // FIXME - add the callsign to the class variables
 652         trns += "Trainer-two-five-charlie ";
 653         if(patternDirection == 1) {
 654                 trns += "right ";
 655         } else {
 656                 trns += "left ";
 657         }
 658
 659         // We could probably get rid of this whole switch statement and just pass a string containing the leg from the FlyPattern function.
 660         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?
 661         case TURN1:
 662                 // Fall through to CROSSWIND
 663         case CROSSWIND: // I don't think this case will be used here but it can't hurt to leave it in
 664                 trns += "crosswind ";
 665                 break;
 666         case TURN2:
 667                 // Fall through to DOWNWIND
 668         case DOWNWIND:
 669                 trns += "downwind ";
 670                 break;
 671         case TURN3:
 672                 // Fall through to BASE
 673         case BASE:
 674                 trns += "base ";
 675                 break;
 676         case TURN4:
 677                 // Fall through to FINAL
 678         case FINAL:             // maybe this should include long/short final if appropriate?
 679                 trns += "final ";
 680                 break;
 681         default:                // Hopefully this won't be used
 682                 trns += "pattern ";
 683                 break;
 684         }
 685         // FIXME - I've hardwired the runway call as well!! (We could work this out from rwy heading and mag deviation)
 686         trns += ConvertRwyNumToSpokenString(1);
 687
 688         // And add the airport name again
 689         trns += tower->get_name();
 690
 691         Transmit(trns);
 692 }
 693
 694 void FGAILocalTraffic::ExitRunway(Point3D orthopos) {
 695         //cout << "In ExitRunway" << endl;
 696         //cout << "Runway ID is " << rwy.ID << endl;
 697         node_array_type exitNodes = airport.GetExits(rwy.ID);   //I suppose we ought to have some fallback for rwy with no defined exits?
 698         /*
 699         cout << "Node ID's of exits are ";
 700         for(unsigned int i=0; i<exitNodes.size(); ++i) {
 701                 cout << exitNodes[i]->nodeID << ' ';
 702         }
 703         cout << endl;
 704         */
 705         if(exitNodes.size()) {
 706                 //Find the next exit from orthopos.y
 707                 double d;
 708                 double dist = 100000;   //ie. longer than any runway in existance
 709                 double backdist = 100000;
 710                 node_array_iterator nItr = exitNodes.begin();
 711                 node* rwyExit = *(exitNodes.begin());
 712                 //int gateID;           //This might want to be more persistant at some point
 713                 while(nItr != exitNodes.end()) {
 714                         d = ortho.ConvertToLocal((*nItr)->pos).y() - ortho.ConvertToLocal(pos).y();     //FIXME - consider making orthopos a class variable
 715                         if(d > 0.0) {
 716                                 if(d < dist) {
 717                                         dist = d;
 718                                         rwyExit = *nItr;
 719                                 }
 720                         } else {
 721                                 if(fabs(d) < backdist) {
 722                                         backdist = d;
 723                                         //TODO - need some logic here that if we don't get a forward exit we turn round and store the backwards one
 724                                 }
 725                         }
 726                         ++nItr;
 727                 }
 728                 ourGate = airport.GetGateNode();
 729                 if(ourGate == NULL) {
 730                         // Implies no available gates - what shall we do?
 731                         // For now just vanish the plane - possibly we can make this more elegant in the future
 732                         SG_LOG(SG_GENERAL, SG_ALERT, "No gate found by FGAILocalTraffic whilst landing at " << airportID << '\n');
 733                         aip.setVisible(false);
 734                         operatingState = PARKED;
 735                         return;
 736                 }
 737                 path = airport.GetPath(rwyExit, ourGate);
 738                 /*
 739                 cout << "path returned was:" << endl;
 740                 for(unsigned int i=0; i<path.size(); ++i) {
 741                         switch(path[i]->struct_type) {
 742                         case NODE:
 743                                 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
 744                                 break;
 745                         case ARC:
 746                                 cout << "ARC\n";
 747                                 break;
 748                         }
 749                 }
 750                 */
 751                 taxiState = TD_INBOUND;
 752                 StartTaxi();
 753         } else {
 754                 // Something must have gone wrong with the ground network file - or there is only a rwy here and no exits defined
 755                 SG_LOG(SG_GENERAL, SG_ALERT, "No exits found by FGAILocalTraffic from runway " << rwy.ID << " at " << airportID << '\n');
 756                 // What shall we do - just remove the plane from sight?
 757                 aip.setVisible(false);
 758                 operatingState = PARKED;
 759         }
 760 }
 761
 762 // Set the class variable nextTaxiNode to the next node in the path
 763 // and update taxiPathPos, the class variable path iterator position
 764 // TODO - maybe should return error codes to the calling function if we fail here
 765 void FGAILocalTraffic::GetNextTaxiNode() {
 766         //cout << "GetNextTaxiNode called " << endl;
 767         //cout << "taxiPathPos = " << taxiPathPos << endl;
 768         ground_network_path_iterator pathItr = path.begin() + taxiPathPos;
 769         if(pathItr == path.end()) {
 770                 SG_LOG(SG_GENERAL, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - no more nodes in path\n");
 771         } else {
 772                 if((*pathItr)->struct_type == NODE) {
 773                         //cout << "ITS A NODE" << endl;
 774                         //*pathItr = new node;
 775                         nextTaxiNode = (node*)*pathItr;
 776                         ++taxiPathPos;
 777                         //delete pathItr;
 778                 } else {
 779                         //cout << "ITS NOT A NODE" << endl;
 780                         //The first item in found must have been an arc
 781                         //Assume for now that it was straight
 782                         pathItr++;
 783                         taxiPathPos++;
 784                         if(pathItr == path.end()) {
 785                                 SG_LOG(SG_GENERAL, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - path ended with an arc\n");
 786                         } else if((*pathItr)->struct_type == NODE) {
 787                                 nextTaxiNode = (node*)*pathItr;
 788                                 ++taxiPathPos;
 789                         } else {
 790                                 //OOPS - two non-nodes in a row - that shouldn't happen ATM
 791                                 SG_LOG(SG_GENERAL, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - two non-nodes in sequence\n");
 792                         }
 793                 }
 794         }
 795 }
 796
 797 // StartTaxi - set up the taxiing state - call only at the start of taxiing
 798 void FGAILocalTraffic::StartTaxi() {
 799         //cout << "StartTaxi called" << endl;
 800         operatingState = TAXIING;
 801         taxiPathPos = 0;
 802
 803         //Set the desired heading
 804         //Assume we are aiming for first node on path
 805         //Eventually we may need to consider the fact that we might start on a curved arc and
 806         //not be able to head directly for the first node.
 807         GetNextTaxiNode();      // sets the class variable nextTaxiNode to the next taxi node!
 808         desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
 809         //cout << "First taxi heading is " << desiredTaxiHeading << endl;
 810 }
 811
 812 // speed in knots, headings in degrees, radius in meters.
 813 static double TaxiTurnTowardsHeading(double current_hdg, double desired_hdg, double speed, double radius, double dt) {
 814         // wrap heading - this prevents a logic bug where the plane would just go round in circles!!
 815         while(current_hdg < 0.0) {
 816                 current_hdg += 360.0;
 817         }
 818         while(current_hdg > 360.0) {
 819                 current_hdg -= 360.0;
 820         }
 821         if(fabs(current_hdg - desired_hdg) > 0.1) {
 822                 // Which is the quickest direction to turn onto heading?
 823                 if(desired_hdg > current_hdg) {
 824                         if((desired_hdg - current_hdg) <= 180) {
 825                                 // turn right
 826                                 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
 827                                 // TODO - check that increments are less than the delta that we check for the right direction
 828                                 // Probably need to reduce convergence speed as convergence is reached
 829                         } else {
 830                                 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
 831                         }
 832                 } else {
 833                         if((current_hdg - desired_hdg) <= 180) {
 834                                 // turn left
 835                                 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
 836                                 // TODO - check that increments are less than the delta that we check for the right direction
 837                                 // Probably need to reduce convergence speed as convergence is reached
 838                         } else {
 839                                 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
 840                         }
 841                 }
 842         }
 843         return(current_hdg);
 844 }
 845
 846 void FGAILocalTraffic::Taxi(double dt) {
 847         //cout << "Taxi called" << endl;
 848         // Logic - if we are further away from next point than turn radius then head for it
 849         // If we have reached turning point then get next point and turn onto that heading
 850         // Look out for the finish!!
 851
 852         //Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane
 853         desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
 854
 855         bool lastNode = (taxiPathPos == path.size() ? true : false);
 856         if(lastNode) {
 857                 //cout << "LAST NODE\n";
 858         }
 859
 860         // HACK ALERT! - for now we will taxi at constant speed for straights and turns
 861
 862         // Remember that hdg is always equal to track when taxiing so we don't have to consider them both
 863         double dist_to_go = dclGetHorizontalSeparation(pos, nextTaxiNode->pos); // we may be able to do this more cheaply using orthopos
 864         //cout << "dist_to_go = " << dist_to_go << endl;
 865         if((nextTaxiNode->type == GATE) && (dist_to_go <= 0.1)) {
 866                 // This might be more robust to outward paths starting with a gate if we check for either
 867                 // last node or TD_INBOUND ?
 868                 // park up
 869                 operatingState = PARKED;
 870         } else if(((dist_to_go > taxiTurnRadius) || (nextTaxiNode->type == GATE)) && (!liningUp)){
 871                 // if the turn radius is r, and speed is s, then in a time dt we turn through
 872                 // ((s.dt)/(PI.r)) x 180 degrees
 873                 // or alternatively (s.dt)/r radians
 874                 //cout << "hdg = " << hdg << " desired taxi heading = " << desiredTaxiHeading << '\n';
 875                 hdg = TaxiTurnTowardsHeading(hdg, desiredTaxiHeading, nominalTaxiSpeed, taxiTurnRadius, dt);
 876                 double vel = nominalTaxiSpeed;
 877                 //cout << "vel = " << vel << endl;
 878                 double dist = vel * 0.514444 * dt;
 879                 //cout << "dist = " << dist << endl;
 880                 double track = hdg;
 881                 //cout << "track = " << track << endl;
 882                 double slope = 0.0;
 883                 pos = dclUpdatePosition(pos, track, slope, dist);
 884                 //cout << "Updated position...\n";
 885                 if(aip.getFGLocation()->get_cur_elev_m() > -9990) {
 886                         pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
 887                 } // else don't change the elev until we get a valid ground elev again!
 888         } else if(lastNode) {
 889                 if(taxiState == TD_OUTBOUND) {
 890                         if((!liningUp) && (dist_to_go <= taxiTurnRadius)) {
 891                                 liningUp = true;
 892                         }
 893                         if(liningUp) {
 894                                 hdg = TaxiTurnTowardsHeading(hdg, rwy.hdg, nominalTaxiSpeed, taxiTurnRadius, dt);
 895                                 double vel = nominalTaxiSpeed;
 896                                 //cout << "vel = " << vel << endl;
 897                                 double dist = vel * 0.514444 * dt;
 898                                 //cout << "dist = " << dist << endl;
 899                                 double track = hdg;
 900                                 //cout << "track = " << track << endl;
 901                                 double slope = 0.0;
 902                                 pos = dclUpdatePosition(pos, track, slope, dist);
 903                                 //cout << "Updated position...\n";
 904                                 if(aip.getFGLocation()->get_cur_elev_m() > -9990) {
 905                                         pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
 906                                 } // else don't change the elev until we get a valid ground elev again!
 907                                 if(fabs(hdg - rwy.hdg) <= 1.0) {
 908                                         operatingState = IN_PATTERN;
 909                                         leg = TAKEOFF_ROLL;
 910                                         inAir = false;
 911                                         liningUp = false;
 912                                 }
 913                         }
 914                 } // else at the moment assume TD_INBOUND always ends in a gate in which case we can ignore it
 915         } else {
 916                 // Time to turn (we've already checked it's not the end we're heading for).
 917                 // set the target node to be the next node which will prompt automatically turning onto
 918                 // the right heading in the stuff above, with the usual provisos applied.
 919                 GetNextTaxiNode();
 920                 // For now why not just recursively call this function?
 921                 Taxi(dt);
 922         }
 923 }
 924
 925
 926 // Warning - ground elev determination is CPU intensive
 927 // Either this function or the logic of how often it is called
 928 // will almost certainly change.
 929 void FGAILocalTraffic::DoGroundElev() {
 930
 931         // It would be nice if we could set the correct tile center here in order to get a correct
 932         // answer with one call to the function, but what I tried in the two commented-out lines
 933         // below only intermittently worked, and I haven't quite groked why yet.
 934         //SGBucket buck(pos.lon(), pos.lat());
 935         //aip.getFGLocation()->set_tile_center(Point3D(buck.get_center_lon(), buck.get_center_lat(), 0.0));
 936
 937         double visibility_meters = fgGetDouble("/environment/visibility-m");
 938         //globals->get_tile_mgr()->prep_ssg_nodes( acmodel_location,
 939         globals->get_tile_mgr()->prep_ssg_nodes( aip.getFGLocation(),   visibility_meters );
 940         globals->get_tile_mgr()->update( aip.getFGLocation(), visibility_meters, (aip.getFGLocation())->get_absolute_view_pos() );
 941         // save results of update in FGLocation for fdm...
 942
 943         //if ( globals->get_scenery()->get_cur_elev() > -9990 ) {
 944         //      acmodel_location->
 945         //      set_cur_elev_m( globals->get_scenery()->get_cur_elev() );
 946         //}
 947
 948         // The need for this here means that at least 2 consecutive passes are needed :-(
 949         aip.getFGLocation()->set_tile_center( globals->get_scenery()->get_next_center() );
 950
 951         //cout << "Transform Elev is " << globals->get_scenery()->get_cur_elev() << '\n';
 952         aip.getFGLocation()->set_cur_elev_m(globals->get_scenery()->get_cur_elev());
 953         //return(globals->get_scenery()->get_cur_elev());
 954 }