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