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