src/AIModel/AIFlightPlanCreate.cxx

   1 /******************************************************************************
   2  * AIFlightPlanCreate.cxx
   3  * Written by Durk Talsma, started May, 2004.
   4  *
   5  * This program is free software; you can redistribute it and/or
   6  * modify it under the terms of the GNU General Public License as
   7  * published by the Free Software Foundation; either version 2 of the
   8  * License, or (at your option) any later version.
   9  *
  10  * This program is distributed in the hope that it will be useful, but
  11  * WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13  * General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this program; if not, write to the Free Software
  17  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  18  *
  19  **************************************************************************/
  20
  21 #ifdef HAVE_CONFIG_H
  22 #  include <config.h>
  23 #endif
  24
  25 #include "AIFlightPlan.hxx"
  26 #include <simgear/math/sg_geodesy.hxx>
  27 #include <simgear/props/props.hxx>
  28 #include <simgear/props/props_io.hxx>
  29
  30 #include <Airports/runways.hxx>
  31 #include <Airports/dynamics.hxx>
  32 #include "AIAircraft.hxx"
  33 #include "performancedata.hxx"
  34
  35 #include <Environment/environment_mgr.hxx>
  36 #include <Environment/environment.hxx>
  37
  38
  39 /* FGAIFlightPlan::create()
  40  * dynamically create a flight plan for AI traffic, based on data provided by the
  41  * Traffic Manager, when reading a filed flightplan failes. (DT, 2004/07/10)
  42  *
  43  * This is the top-level function, and the only one that is publicly available.
  44  *
  45  */
  46
  47
  48 // Check lat/lon values during initialization;
  49 void FGAIFlightPlan::create(FGAIAircraft * ac, FGAirport * dep,
  50                             FGAirport * arr, int legNr, double alt,
  51                             double speed, double latitude,
  52                             double longitude, bool firstFlight,
  53                             double radius, const string & fltType,
  54                             const string & aircraftType,
  55                             const string & airline, double distance)
  56 {
  57     int currWpt = wpt_iterator - waypoints.begin();
  58     switch (legNr) {
  59     case 1:
  60         createPushBack(ac, firstFlight, dep, latitude, longitude,
  61                        radius, fltType, aircraftType, airline);
  62         break;
  63     case 2:
  64         createTakeoffTaxi(ac, firstFlight, dep, radius, fltType,
  65                           aircraftType, airline);
  66         break;
  67     case 3:
  68         createTakeOff(ac, firstFlight, dep, speed, fltType);
  69         break;
  70     case 4:
  71         createClimb(ac, firstFlight, dep, speed, alt, fltType);
  72         break;
  73     case 5:
  74         createCruise(ac, firstFlight, dep, arr, latitude, longitude, speed,
  75                      alt, fltType);
  76         break;
  77     case 6:
  78         createDescent(ac, arr, latitude, longitude, speed, alt, fltType,
  79                       distance);
  80         break;
  81     case 7:
  82         createLanding(ac, arr, fltType);
  83         break;
  84     case 8:
  85         createLandingTaxi(ac, arr, radius, fltType, aircraftType, airline);
  86         break;
  87     case 9:
  88         createParking(ac, arr, radius);
  89         break;
  90     default:
  91         //exit(1);
  92         SG_LOG(SG_INPUT, SG_ALERT,
  93                "AIFlightPlan::create() attempting to create unknown leg"
  94                " this is probably an internal program error");
  95     }
  96     wpt_iterator = waypoints.begin() + currWpt;
  97     leg++;
  98 }
  99
 100 FGAIFlightPlan::waypoint *
 101     FGAIFlightPlan::createOnGround(FGAIAircraft * ac,
 102                                    const std::string & aName,
 103                                    const SGGeod & aPos, double aElev,
 104                                    double aSpeed)
 105 {
 106     waypoint *wpt = new waypoint;
 107     wpt->name = aName;
 108     wpt->longitude = aPos.getLongitudeDeg();
 109     wpt->latitude = aPos.getLatitudeDeg();
 110     wpt->altitude = aElev;
 111     wpt->speed = aSpeed;
 112     wpt->crossat = -10000.1;
 113     wpt->gear_down = true;
 114     wpt->flaps_down = true;
 115     wpt->finished = false;
 116     wpt->on_ground = true;
 117     wpt->routeIndex = 0;
 118     return wpt;
 119 }
 120
 121 FGAIFlightPlan::waypoint *
 122     FGAIFlightPlan::createInAir(FGAIAircraft * ac,
 123                                 const std::string & aName,
 124                                 const SGGeod & aPos, double aElev,
 125                                 double aSpeed)
 126 {
 127     waypoint *wpt = new waypoint;
 128     wpt->name = aName;
 129     wpt->longitude = aPos.getLongitudeDeg();
 130     wpt->latitude = aPos.getLatitudeDeg();
 131     wpt->altitude = aElev;
 132     wpt->speed = aSpeed;
 133     wpt->crossat = -10000.1;
 134     wpt->gear_down = false;
 135     wpt->flaps_down = false;
 136     wpt->finished = false;
 137     wpt->on_ground = false;
 138     wpt->routeIndex = 0;
 139     return wpt;
 140 }
 141
 142 FGAIFlightPlan::waypoint *
 143     FGAIFlightPlan::cloneWithPos(FGAIAircraft * ac, waypoint * aWpt,
 144                                  const std::string & aName,
 145                                  const SGGeod & aPos)
 146 {
 147     waypoint *wpt = new waypoint;
 148     wpt->name = aName;
 149     wpt->longitude = aPos.getLongitudeDeg();
 150     wpt->latitude = aPos.getLatitudeDeg();
 151
 152     wpt->altitude = aWpt->altitude;
 153     wpt->speed = aWpt->speed;
 154     wpt->crossat = aWpt->crossat;
 155     wpt->gear_down = aWpt->gear_down;
 156     wpt->flaps_down = aWpt->flaps_down;
 157     wpt->finished = aWpt->finished;
 158     wpt->on_ground = aWpt->on_ground;
 159     wpt->routeIndex = 0;
 160
 161     return wpt;
 162 }
 163
 164 FGAIFlightPlan::waypoint * FGAIFlightPlan::clone(waypoint * aWpt)
 165 {
 166     waypoint *wpt = new waypoint;
 167     wpt->name = aWpt->name;
 168     wpt->longitude = aWpt->longitude;
 169     wpt->latitude = aWpt->latitude;
 170
 171     wpt->altitude = aWpt->altitude;
 172     wpt->speed = aWpt->speed;
 173     wpt->crossat = aWpt->crossat;
 174     wpt->gear_down = aWpt->gear_down;
 175     wpt->flaps_down = aWpt->flaps_down;
 176     wpt->finished = aWpt->finished;
 177     wpt->on_ground = aWpt->on_ground;
 178     wpt->routeIndex = 0;
 179
 180     return wpt;
 181 }
 182
 183
 184 void FGAIFlightPlan::createDefaultTakeoffTaxi(FGAIAircraft * ac,
 185                                               FGAirport * aAirport,
 186                                               FGRunway * aRunway)
 187 {
 188     SGGeod runwayTakeoff = aRunway->pointOnCenterline(5.0);
 189     double airportElev = aAirport->getElevation();
 190
 191     waypoint *wpt;
 192     wpt =
 193         createOnGround(ac, "Airport Center", aAirport->geod(), airportElev,
 194                        ac->getPerformance()->vTaxi());
 195     waypoints.push_back(wpt);
 196     wpt =
 197         createOnGround(ac, "Runway Takeoff", runwayTakeoff, airportElev,
 198                        ac->getPerformance()->vTaxi());
 199     waypoints.push_back(wpt);
 200 }
 201
 202 void FGAIFlightPlan::createTakeoffTaxi(FGAIAircraft * ac, bool firstFlight,
 203                                        FGAirport * apt,
 204                                        double radius,
 205                                        const string & fltType,
 206                                        const string & acType,
 207                                        const string & airline)
 208 {
 209     double heading, lat, lon;
 210
 211     // If this function is called during initialization,
 212     // make sure we obtain a valid gate ID first
 213     // and place the model at the location of the gate.
 214     if (firstFlight) {
 215         if (!(apt->getDynamics()->getAvailableParking(&lat, &lon,
 216                                                       &heading, &gateId,
 217                                                       radius, fltType,
 218                                                       acType, airline))) {
 219             SG_LOG(SG_INPUT, SG_WARN, "Could not find parking for a " <<
 220                    acType <<
 221                    " of flight type " << fltType <<
 222                    " of airline     " << airline <<
 223                    " at airport     " << apt->getId());
 224         }
 225     }
 226
 227     string rwyClass = getRunwayClassFromTrafficType(fltType);
 228
 229     // Only set this if it hasn't been set by ATC already.
 230     if (activeRunway.empty()) {
 231         //cerr << "Getting runway for " << ac->getTrafficRef()->getCallSign() << " at " << apt->getId() << endl;
 232         double depHeading = ac->getTrafficRef()->getCourse();
 233         apt->getDynamics()->getActiveRunway(rwyClass, 1, activeRunway,
 234                                             depHeading);
 235     }
 236     rwy = apt->getRunwayByIdent(activeRunway);
 237     SGGeod runwayTakeoff = rwy->pointOnCenterline(5.0);
 238
 239     FGGroundNetwork *gn = apt->getDynamics()->getGroundNetwork();
 240     if (!gn->exists()) {
 241         createDefaultTakeoffTaxi(ac, apt, rwy);
 242         return;
 243     }
 244
 245     intVec ids;
 246     int runwayId = gn->findNearestNode(runwayTakeoff);
 247
 248     // A negative gateId indicates an overflow parking, use a
 249     // fallback mechanism for this.
 250     // Starting from gate 0 in this case is a bit of a hack
 251     // which requires a more proper solution later on.
 252     delete taxiRoute;
 253     taxiRoute = new FGTaxiRoute;
 254
 255     // Determine which node to start from.
 256     int node = 0;
 257     // Find out which node to start from
 258     FGParking *park = apt->getDynamics()->getParking(gateId);
 259     if (park) {
 260         node = park->getPushBackPoint();
 261     }
 262
 263     if (node == -1) {
 264         node = gateId;
 265     }
 266     // HAndle case where parking doens't have a node
 267     if ((node == 0) && park) {
 268         if (firstFlight) {
 269             node = gateId;
 270         } else {
 271             node = lastNodeVisited;
 272         }
 273     }
 274
 275     *taxiRoute = gn->findShortestRoute(node, runwayId);
 276     intVecIterator i;
 277
 278     if (taxiRoute->empty()) {
 279         createDefaultTakeoffTaxi(ac, apt, rwy);
 280         return;
 281     }
 282
 283     taxiRoute->first();
 284     //bool isPushBackPoint = false;
 285     if (firstFlight) {
 286         // If this is called during initialization, randomly
 287         // skip a number of waypoints to get a more realistic
 288         // taxi situation.
 289         int nrWaypointsToSkip = rand() % taxiRoute->size();
 290         // but make sure we always keep two active waypoints
 291         // to prevent a segmentation fault
 292         for (int i = 0; i < nrWaypointsToSkip - 3; i++) {
 293             taxiRoute->next(&node);
 294         }
 295         apt->getDynamics()->releaseParking(gateId);
 296     } else {
 297         if (taxiRoute->size() > 1) {
 298             taxiRoute->next(&node);     // chop off the first waypoint, because that is already the last of the pushback route
 299         }
 300     }
 301
 302     // push each node on the taxi route as a waypoint
 303     int route;
 304     while (taxiRoute->next(&node, &route)) {
 305         char buffer[10];
 306         snprintf(buffer, 10, "%d", node);
 307         FGTaxiNode *tn =
 308             apt->getDynamics()->getGroundNetwork()->findNode(node);
 309         waypoint *wpt =
 310             createOnGround(ac, buffer, tn->getGeod(), apt->getElevation(),
 311                            ac->getPerformance()->vTaxi());
 312         wpt->routeIndex = route;
 313         waypoints.push_back(wpt);
 314     }
 315 }
 316
 317 void FGAIFlightPlan::createDefaultLandingTaxi(FGAIAircraft * ac,
 318                                               FGAirport * aAirport)
 319 {
 320     SGGeod lastWptPos =
 321         SGGeod::fromDeg(waypoints.back()->longitude,
 322                         waypoints.back()->latitude);
 323     double airportElev = aAirport->getElevation();
 324
 325     waypoint *wpt;
 326     wpt =
 327         createOnGround(ac, "Runway Exit", lastWptPos, airportElev,
 328                        ac->getPerformance()->vTaxi());
 329     waypoints.push_back(wpt);
 330     wpt =
 331         createOnGround(ac, "Airport Center", aAirport->geod(), airportElev,
 332                        ac->getPerformance()->vTaxi());
 333     waypoints.push_back(wpt);
 334
 335     double heading, lat, lon;
 336     aAirport->getDynamics()->getParking(gateId, &lat, &lon, &heading);
 337     wpt =
 338         createOnGround(ac, "END", SGGeod::fromDeg(lon, lat), airportElev,
 339                        ac->getPerformance()->vTaxi());
 340     waypoints.push_back(wpt);
 341 }
 342
 343 void FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac, FGAirport * apt,
 344                                        double radius,
 345                                        const string & fltType,
 346                                        const string & acType,
 347                                        const string & airline)
 348 {
 349     double heading, lat, lon;
 350     apt->getDynamics()->getAvailableParking(&lat, &lon, &heading,
 351                                             &gateId, radius, fltType,
 352                                             acType, airline);
 353
 354     SGGeod lastWptPos =
 355         SGGeod::fromDeg(waypoints.back()->longitude,
 356                         waypoints.back()->latitude);
 357     FGGroundNetwork *gn = apt->getDynamics()->getGroundNetwork();
 358
 359     // Find a route from runway end to parking/gate.
 360     if (!gn->exists()) {
 361         createDefaultLandingTaxi(ac, apt);
 362         return;
 363     }
 364
 365     intVec ids;
 366     int runwayId = gn->findNearestNode(lastWptPos);
 367     // A negative gateId indicates an overflow parking, use a
 368     // fallback mechanism for this.
 369     // Starting from gate 0 is a bit of a hack...
 370     //FGTaxiRoute route;
 371     delete taxiRoute;
 372     taxiRoute = new FGTaxiRoute;
 373     if (gateId >= 0)
 374         *taxiRoute = gn->findShortestRoute(runwayId, gateId);
 375     else
 376         *taxiRoute = gn->findShortestRoute(runwayId, 0);
 377     intVecIterator i;
 378
 379     if (taxiRoute->empty()) {
 380         createDefaultLandingTaxi(ac, apt);
 381         return;
 382     }
 383
 384     int node;
 385     taxiRoute->first();
 386     int size = taxiRoute->size();
 387     // Omit the last two waypoints, as
 388     // those are created by createParking()
 389     int route;
 390     for (int i = 0; i < size - 2; i++) {
 391         taxiRoute->next(&node, &route);
 392         char buffer[10];
 393         snprintf(buffer, 10, "%d", node);
 394         FGTaxiNode *tn = gn->findNode(node);
 395         waypoint *wpt =
 396             createOnGround(ac, buffer, tn->getGeod(), apt->getElevation(),
 397                            ac->getPerformance()->vTaxi());
 398         wpt->routeIndex = route;
 399         waypoints.push_back(wpt);
 400     }
 401 }
 402
 403 /*******************************************************************
 404  * CreateTakeOff
 405  * A note on units:
 406  *  - Speed -> knots -> nm/hour
 407  *  - distance along runway =-> meters
 408  *  - accel / decel -> is given as knots/hour, but this is highly questionable:
 409  *  for a jet_transport performance class, a accel / decel rate of 5 / 2 is
 410  *  given respectively. According to performance data.cxx, a value of kts / second seems
 411  *  more likely however.
 412  *
 413  ******************************************************************/
 414 void FGAIFlightPlan::createTakeOff(FGAIAircraft * ac, bool firstFlight,
 415                                    FGAirport * apt, double speed,
 416                                    const string & fltType)
 417 {
 418     double accel = ac->getPerformance()->acceleration();
 419     double vTaxi = ac->getPerformance()->vTaxi();
 420     double vRotate = ac->getPerformance()->vRotate();
 421     double vTakeoff = ac->getPerformance()->vTakeoff();
 422     double vClimb = ac->getPerformance()->vClimb();
 423
 424     double accelMetric = (accel * SG_NM_TO_METER) / 3600;
 425     double vTaxiMetric = (vTaxi * SG_NM_TO_METER) / 3600;
 426     double vRotateMetric = (vRotate * SG_NM_TO_METER) / 3600;
 427     double vTakeoffMetric = (vTakeoff * SG_NM_TO_METER) / 3600;
 428     double vClimbMetric = (vClimb * SG_NM_TO_METER) / 3600;
 429     // Acceleration = dV / dT
 430     // Acceleration X dT = dV
 431     // dT = dT / Acceleration
 432     //d = (Vf^2 - Vo^2) / (2*a)
 433     //double accelTime = (vRotate - vTaxi) / accel;
 434     //cerr << "Using " << accelTime << " as total acceleration time" << endl;
 435     double accelDistance =
 436         (vRotateMetric * vRotateMetric -
 437          vTaxiMetric * vTaxiMetric) / (2 * accelMetric);
 438     //cerr << "Using " << accelDistance << " " << accelMetric << " " << vRotateMetric << endl;
 439     waypoint *wpt;
 440     // Get the current active runway, based on code from David Luff
 441     // This should actually be unified and extended to include
 442     // Preferential runway use schema's
 443     // NOTE: DT (2009-01-18: IIRC, this is currently already the case,
 444     // because the getActive runway function takes care of that.
 445     if (firstFlight) {
 446         string rwyClass = getRunwayClassFromTrafficType(fltType);
 447         double heading = ac->getTrafficRef()->getCourse();
 448         apt->getDynamics()->getActiveRunway(rwyClass, 1, activeRunway,
 449                                             heading);
 450         rwy = apt->getRunwayByIdent(activeRunway);
 451     }
 452
 453     double airportElev = apt->getElevation();
 454     // Acceleration point, 105 meters into the runway,
 455     SGGeod accelPoint = rwy->pointOnCenterline(105.0);
 456     wpt = createOnGround(ac, "accel", accelPoint, airportElev, vRotate);
 457     waypoints.push_back(wpt);
 458
 459
 460     accelDistance =
 461         (vTakeoffMetric * vTakeoffMetric -
 462          vTaxiMetric * vTaxiMetric) / (2 * accelMetric);
 463     //cerr << "Using " << accelDistance << " " << accelMetric << " " << vTakeoffMetric << endl;
 464     accelPoint = rwy->pointOnCenterline(105.0 + accelDistance);
 465     wpt = createOnGround(ac, "rotate", accelPoint, airportElev, vTakeoff);
 466     waypoints.push_back(wpt);
 467
 468     accelDistance =
 469         ((vTakeoffMetric * 1.1) * (vTakeoffMetric * 1.1) -
 470          vTaxiMetric * vTaxiMetric) / (2 * accelMetric);
 471     //cerr << "Using " << accelDistance << " " << accelMetric << " " << vTakeoffMetric << endl;
 472     accelPoint = rwy->pointOnCenterline(105.0 + accelDistance);
 473     wpt =
 474         createOnGround(ac, "rotate", accelPoint, airportElev + 1000,
 475                        vTakeoff * 1.1);
 476     wpt->on_ground = false;
 477     waypoints.push_back(wpt);
 478
 479     wpt = cloneWithPos(ac, wpt, "3000 ft", rwy->end());
 480     wpt->altitude = airportElev + 3000;
 481     waypoints.push_back(wpt);
 482
 483     // Finally, add two more waypoints, so that aircraft will remain under
 484     // Tower control until they have reached the 3000 ft climb point
 485     SGGeod pt = rwy->pointOnCenterline(5000 + rwy->lengthM() * 0.5);
 486     wpt = cloneWithPos(ac, wpt, "5000 ft", pt);
 487     wpt->altitude = airportElev + 5000;
 488     waypoints.push_back(wpt);
 489 }
 490
 491 /*******************************************************************
 492  * CreateClimb
 493  * initialize the Aircraft at the parking location
 494  ******************************************************************/
 495 void FGAIFlightPlan::createClimb(FGAIAircraft * ac, bool firstFlight,
 496                                  FGAirport * apt, double speed, double alt,
 497                                  const string & fltType)
 498 {
 499     waypoint *wpt;
 500 //  bool planLoaded = false;
 501     string fPLName;
 502     double vClimb = ac->getPerformance()->vClimb();
 503
 504     if (firstFlight) {
 505         string rwyClass = getRunwayClassFromTrafficType(fltType);
 506         double heading = ac->getTrafficRef()->getCourse();
 507         apt->getDynamics()->getActiveRunway(rwyClass, 1, activeRunway,
 508                                             heading);
 509         rwy = apt->getRunwayByIdent(activeRunway);
 510     }
 511     if (sid) {
 512         for (wpt_vector_iterator i = sid->getFirstWayPoint();
 513              i != sid->getLastWayPoint(); i++) {
 514             waypoints.push_back(clone(*(i)));
 515             //cerr << " Cloning waypoint " << endl;
 516         }
 517     } else {
 518         SGGeod climb1 = rwy->pointOnCenterline(10 * SG_NM_TO_METER);
 519         wpt = createInAir(ac, "10000ft climb", climb1, vClimb, 10000);
 520         wpt->gear_down = true;
 521         wpt->flaps_down = true;
 522         waypoints.push_back(wpt);
 523
 524         SGGeod climb2 = rwy->pointOnCenterline(20 * SG_NM_TO_METER);
 525         wpt = cloneWithPos(ac, wpt, "18000ft climb", climb2);
 526         wpt->altitude = 18000;
 527         waypoints.push_back(wpt);
 528     }
 529 }
 530
 531
 532
 533 /*******************************************************************
 534  * CreateDescent
 535  * Generate a flight path from the last waypoint of the cruise to
 536  * the permission to land point
 537  ******************************************************************/
 538 void FGAIFlightPlan::createDescent(FGAIAircraft * ac, FGAirport * apt,
 539                                    double latitude, double longitude,
 540                                    double speed, double alt,
 541                                    const string & fltType,
 542                                    double requiredDistance)
 543 {
 544     bool reposition = false;
 545     waypoint *wpt;
 546     double vDescent = ac->getPerformance()->vDescent();
 547     double vApproach = ac->getPerformance()->vApproach();
 548
 549
 550     //Beginning of Descent
 551     string rwyClass = getRunwayClassFromTrafficType(fltType);
 552     double heading = ac->getTrafficRef()->getCourse();
 553     apt->getDynamics()->getActiveRunway(rwyClass, 2, activeRunway,
 554                                         heading);
 555     rwy = apt->getRunwayByIdent(activeRunway);
 556
 557
 558
 559     // Create a slow descent path that ends 250 lateral to the runway.
 560     double initialTurnRadius = getTurnRadius(vDescent, true);
 561     double finalTurnRadius = getTurnRadius(vApproach, true);
 562
 563 // get length of the downwind leg for the intended runway
 564     double distanceOut = apt->getDynamics()->getApproachController()->getRunway(rwy->name())->getApproachDistance();    //12 * SG_NM_TO_METER;
 565     //time_t previousArrivalTime=  apt->getDynamics()->getApproachController()->getRunway(rwy->name())->getEstApproachTime();
 566
 567
 568     SGGeod current = SGGeod::fromDegM(longitude, latitude, 0);
 569     SGGeod initialTarget = rwy->pointOnCenterline(-distanceOut);
 570     SGGeod refPoint = rwy->pointOnCenterline(0);
 571     double distance = SGGeodesy::distanceM(current, initialTarget);
 572     double azimuth = SGGeodesy::courseDeg(current, initialTarget);
 573     double dummyAz2;
 574
 575     // To prevent absurdly steep approaches, compute the origin from where the approach should have started
 576     SGGeod origin;
 577
 578     if (ac->getTrafficRef()->getCallSign() ==
 579         fgGetString("/ai/track-callsign")) {
 580         //cerr << "Reposition information: Actual distance " << distance << ". required distance " << requiredDistance << endl;
 581         //exit(1);
 582     }
 583
 584     if (distance < requiredDistance * 0.8) {
 585         reposition = true;
 586         SGGeodesy::direct(initialTarget, azimuth,
 587                           -requiredDistance, origin, dummyAz2);
 588
 589         distance = SGGeodesy::distanceM(current, initialTarget);
 590         azimuth = SGGeodesy::courseDeg(current, initialTarget);
 591     } else {
 592         origin = current;
 593     }
 594
 595
 596     double dAlt = alt - (apt->getElevation() + 2000);
 597
 598     double nPoints = 100;
 599
 600     char buffer[16];
 601
 602     // The descent path contains the following phases:
 603     // 1) a linear glide path from the initial position to
 604     // 2) a semi circle turn to final
 605     // 3) approach
 606
 607     //cerr << "Phase 1: Linear Descent path to runway" << rwy->name() << endl;
 608     // Create an initial destination point on a semicircle
 609     //cerr << "lateral offset : " << lateralOffset << endl;
 610     //cerr << "Distance       : " << distance      << endl;
 611     //cerr << "Azimuth        : " << azimuth       << endl;
 612     //cerr << "Initial Lateral point: " << lateralOffset << endl;
 613     double lat = refPoint.getLatitudeDeg();
 614     double lon = refPoint.getLongitudeDeg();
 615     //cerr << "Reference point (" << lat << ", " << lon << ")." << endl;
 616     lat = initialTarget.getLatitudeDeg();
 617     lon = initialTarget.getLongitudeDeg();
 618     //cerr << "Initial Target point (" << lat << ", " << lon << ")." << endl;
 619
 620     double ratio = initialTurnRadius / distance;
 621     if (ratio > 1.0)
 622         ratio = 1.0;
 623     if (ratio < -1.0)
 624         ratio = -1.0;
 625
 626     double newHeading = asin(ratio) * SG_RADIANS_TO_DEGREES;
 627     double newDistance =
 628         cos(newHeading * SG_DEGREES_TO_RADIANS) * distance;
 629     //cerr << "new distance " << newDistance << ". additional Heading " << newHeading << endl;
 630     double side = azimuth - rwy->headingDeg();
 631     double lateralOffset = initialTurnRadius;
 632     if (side < 0)
 633         side += 360;
 634     if (side < 180) {
 635         lateralOffset *= -1;
 636     }
 637     // Calculate the ETA at final, based on remaining distance, and approach speed.
 638     // distance should really consist of flying time to terniary target, plus circle
 639     // but the distance to secondary target should work as a reasonable approximation
 640     // aditionally add the amount of distance covered by making a turn of "side"
 641     double turnDistance = (2 * M_PI * initialTurnRadius) * (side / 360.0);
 642     time_t remaining =
 643         (turnDistance + distance) / ((vDescent * SG_NM_TO_METER) / 3600.0);
 644     time_t now = time(NULL) + fgGetLong("/sim/time/warp");
 645     //if (ac->getTrafficRef()->getCallSign() == fgGetString("/ai/track-callsign")) {
 646     //     cerr << "   Arrival time estimation: turn angle " <<  side << ". Turn distance " << turnDistance << ". Linear distance " << distance << ". Time to go " << remaining << endl;
 647     //     //exit(1);
 648     //}
 649
 650     time_t eta = now + remaining;
 651     //choose a distance to the runway such that it will take at least 60 seconds more
 652     // time to get there than the previous aircraft.
 653     // Don't bother when aircraft need to be repositioned, because that marks the initialization phased...
 654
 655     time_t newEta;
 656
 657     if (reposition == false) {
 658         newEta =
 659             apt->getDynamics()->getApproachController()->getRunway(rwy->
 660                                                                    name
 661                                                                    ())->
 662             requestTimeSlot(eta);
 663     } else {
 664         newEta = eta;
 665     }
 666     //if ((eta < (previousArrivalTime+60)) && (reposition == false)) {
 667     arrivalTime = newEta;
 668     time_t additionalTimeNeeded = newEta - eta;
 669     double distanceCovered =
 670         ((vApproach * SG_NM_TO_METER) / 3600.0) * additionalTimeNeeded;
 671     distanceOut += distanceCovered;
 672     //apt->getDynamics()->getApproachController()->getRunway(rwy->name())->setEstApproachTime(eta+additionalTimeNeeded);
 673     //cerr << "Adding additional distance: " << distanceCovered << " to allow " << additionalTimeNeeded << " seconds of flying time" << endl << endl;
 674     //} else {
 675     //apt->getDynamics()->getApproachController()->getRunway(rwy->name())->setEstApproachTime(eta);
 676     //}
 677     //cerr << "Timing information : Previous eta: " << previousArrivalTime << ". Current ETA : " << eta << endl;
 678
 679     SGGeod secondaryTarget =
 680         rwy->pointOffCenterline(-distanceOut, lateralOffset);
 681     initialTarget = rwy->pointOnCenterline(-distanceOut);
 682     distance = SGGeodesy::distanceM(origin, secondaryTarget);
 683     azimuth = SGGeodesy::courseDeg(origin, secondaryTarget);
 684
 685
 686     lat = secondaryTarget.getLatitudeDeg();
 687     lon = secondaryTarget.getLongitudeDeg();
 688     //cerr << "Secondary Target point (" << lat << ", " << lon << ")." << endl;
 689     //cerr << "Distance       : " << distance      << endl;
 690     //cerr << "Azimuth        : " << azimuth       << endl;
 691
 692
 693     ratio = initialTurnRadius / distance;
 694     if (ratio > 1.0)
 695         ratio = 1.0;
 696     if (ratio < -1.0)
 697         ratio = -1.0;
 698     newHeading = asin(ratio) * SG_RADIANS_TO_DEGREES;
 699     newDistance = cos(newHeading * SG_DEGREES_TO_RADIANS) * distance;
 700     //cerr << "new distance realative to secondary target: " << newDistance << ". additional Heading " << newHeading << endl;
 701     if (side < 180) {
 702         azimuth += newHeading;
 703     } else {
 704         azimuth -= newHeading;
 705     }
 706
 707     SGGeod tertiaryTarget;
 708     SGGeodesy::direct(origin, azimuth,
 709                       newDistance, tertiaryTarget, dummyAz2);
 710
 711     lat = tertiaryTarget.getLatitudeDeg();
 712     lon = tertiaryTarget.getLongitudeDeg();
 713     //cerr << "tertiary Target point (" << lat << ", " << lon << ")." << endl;
 714
 715
 716     for (int i = 1; i < nPoints; i++) {
 717         SGGeod result;
 718         double currentDist = i * (newDistance / nPoints);
 719         double currentAltitude = alt - (i * (dAlt / nPoints));
 720         SGGeodesy::direct(origin, azimuth, currentDist, result, dummyAz2);
 721         snprintf(buffer, 16, "descent%03d", i);
 722         wpt = createInAir(ac, buffer, result, currentAltitude, vDescent);
 723         wpt->crossat = currentAltitude;
 724         wpt->trackLength = (newDistance / nPoints);
 725         waypoints.push_back(wpt);
 726         //cerr << "Track Length : " << wpt->trackLength;
 727         //cerr << "  Position : " << result.getLatitudeDeg() << " " << result.getLongitudeDeg() << " " << currentAltitude << endl;
 728     }
 729
 730     //cerr << "Phase 2: Circle " << endl;
 731     double initialAzimuth =
 732         SGGeodesy::courseDeg(secondaryTarget, tertiaryTarget);
 733     double finalAzimuth =
 734         SGGeodesy::courseDeg(secondaryTarget, initialTarget);
 735
 736     //cerr << "Angles from secondary target: " << initialAzimuth << " " << finalAzimuth << endl;
 737     int increment, startval, endval;
 738     // circle right around secondary target if orig of position is to the right of the runway
 739     // i.e. use negative angles; else circle leftward and use postivi
 740     if (side < 180) {
 741         increment = -1;
 742         startval = floor(initialAzimuth);
 743         endval = ceil(finalAzimuth);
 744         if (endval > startval) {
 745             endval -= 360;
 746         }
 747     } else {
 748         increment = 1;
 749         startval = ceil(initialAzimuth);
 750         endval = floor(finalAzimuth);
 751         if (endval < startval) {
 752             endval += 360;
 753         }
 754
 755     }
 756
 757     //cerr << "creating circle between " << startval << " and " << endval << " using " << increment << endl;
 758     double trackLength = (2 * M_PI * initialTurnRadius) / 360.0;
 759     for (int i = startval; i != endval; i += increment) {
 760         SGGeod result;
 761         double currentAltitude = apt->getElevation() + 2000;
 762         SGGeodesy::direct(secondaryTarget, i,
 763                           initialTurnRadius, result, dummyAz2);
 764         snprintf(buffer, 16, "turn%03d", i);
 765         wpt = createInAir(ac, buffer, result, currentAltitude, vDescent);
 766         wpt->crossat = currentAltitude;
 767         wpt->trackLength = trackLength;
 768         //cerr << "Track Length : " << wpt->trackLength;
 769         waypoints.push_back(wpt);
 770         //cerr << "  Position : " << result.getLatitudeDeg() << " " << result.getLongitudeDeg() << " " << currentAltitude << endl;
 771     }
 772
 773
 774     // The approach leg should bring the aircraft to approximately 4-6 out, after which the landing phase should take over.
 775     //cerr << "Phase 3: Approach" << endl;
 776     distanceOut -= distanceCovered;
 777     for (int i = 1; i < nPoints; i++) {
 778         SGGeod result;
 779         double currentDist = i * (distanceOut / nPoints);
 780         double currentAltitude =
 781             apt->getElevation() + 2000 - (i * 2000 / nPoints);
 782         snprintf(buffer, 16, "final%03d", i);
 783         result = rwy->pointOnCenterline((-distanceOut) + currentDist);
 784         wpt = createInAir(ac, buffer, result, currentAltitude, vApproach);
 785         wpt->crossat = currentAltitude;
 786         wpt->trackLength = (distanceOut / nPoints);
 787         // account for the extra distance due to an extended downwind leg
 788         if (i == 1) {
 789             wpt->trackLength += distanceCovered;
 790         }
 791         //cerr << "Track Length : " << wpt->trackLength;
 792         waypoints.push_back(wpt);
 793         //cerr << "  Position : " << result.getLatitudeDeg() << " " << result.getLongitudeDeg() << " " << currentAltitude << endl;
 794     }
 795
 796     //cerr << "Done" << endl;
 797
 798     // Erase the two bogus BOD points: Note check for conflicts with scripted AI flightPlans
 799     IncrementWaypoint(true);
 800     IncrementWaypoint(true);
 801
 802     if (reposition) {
 803         double tempDistance;
 804         double minDistance = HUGE_VAL;
 805         string wptName;
 806         tempDistance = SGGeodesy::distanceM(current, initialTarget);
 807         time_t eta =
 808             tempDistance / ((vDescent * SG_NM_TO_METER) / 3600.0) + now;
 809         time_t newEta =
 810             apt->getDynamics()->getApproachController()->getRunway(rwy->
 811                                                                    name
 812                                                                    ())->
 813             requestTimeSlot(eta);
 814         arrivalTime = newEta;
 815         double newDistance =
 816             ((vDescent * SG_NM_TO_METER) / 3600.0) * (newEta - now);
 817         //cerr << "Repositioning information : eta" << eta << ". New ETA " << newEta << ". Diff = " << (newEta - eta) << ". Distance = " << tempDistance << ". New distance = " << newDistance << endl;
 818         IncrementWaypoint(true);        // remove waypoint BOD2
 819         while (checkTrackLength("final001") > newDistance) {
 820             IncrementWaypoint(true);
 821         }
 822         //cerr << "Repositioning to waypoint " << (*waypoints.begin())->name << endl;
 823         ac->resetPositionFromFlightPlan();
 824     }
 825
 826
 827 }
 828
 829 /*******************************************************************
 830  * CreateLanding
 831  * Create a flight path from the "permision to land" point (currently
 832    hardcoded at 5000 meters from the threshold) to the threshold, at
 833    a standard glide slope angle of 3 degrees.
 834  ******************************************************************/
 835 void FGAIFlightPlan::createLanding(FGAIAircraft * ac, FGAirport * apt,
 836                                    const string & fltType)
 837 {
 838     double vTouchdown = ac->getPerformance()->vTouchdown();
 839     double vTaxi = ac->getPerformance()->vTaxi();
 840
 841     //string rwyClass = getRunwayClassFromTrafficType(fltType);
 842     //double heading = ac->getTrafficRef()->getCourse();
 843     //apt->getDynamics()->getActiveRunway(rwyClass, 2, activeRunway, heading);
 844     //rwy = apt->getRunwayByIdent(activeRunway);
 845
 846
 847     waypoint *wpt;
 848     double aptElev = apt->getElevation();
 849
 850     SGGeod coord;
 851     char buffer[12];
 852     for (int i = 1; i < 10; i++) {
 853         snprintf(buffer, 12, "wpt%d", i);
 854         coord = rwy->pointOnCenterline(rwy->lengthM() * (i / 10.0));
 855         wpt = createOnGround(ac, buffer, coord, aptElev, (vTouchdown / i));
 856         wpt->crossat = apt->getElevation();
 857         waypoints.push_back(wpt);
 858     }
 859
 860     /*
 861        //Runway Threshold
 862        wpt = createOnGround(ac, "Threshold", rwy->threshold(), aptElev, vTouchdown);
 863        wpt->crossat = apt->getElevation();
 864        waypoints.push_back(wpt);
 865
 866        // Roll-out
 867        wpt = createOnGround(ac, "Center", rwy->geod(), aptElev, vTaxi*2);
 868        waypoints.push_back(wpt);
 869
 870        SGGeod rollOut = rwy->pointOnCenterline(rwy->lengthM() * 0.9);
 871        wpt = createOnGround(ac, "Roll Out", rollOut, aptElev, vTaxi);
 872        wpt->crossat   = apt->getElevation();
 873        waypoints.push_back(wpt);
 874      */
 875 }
 876
 877 /*******************************************************************
 878  * CreateParking
 879  * initialize the Aircraft at the parking location
 880  ******************************************************************/
 881 void FGAIFlightPlan::createParking(FGAIAircraft * ac, FGAirport * apt,
 882                                    double radius)
 883 {
 884     waypoint *wpt;
 885     double aptElev = apt->getElevation();
 886     double lat = 0.0, lat2 = 0.0;
 887     double lon = 0.0, lon2 = 0.0;
 888     double az2 = 0.0;
 889     double heading = 0.0;
 890
 891     double vTaxi = ac->getPerformance()->vTaxi();
 892     double vTaxiReduced = vTaxi * (2.0 / 3.0);
 893     apt->getDynamics()->getParking(gateId, &lat, &lon, &heading);
 894     heading += 180.0;
 895     if (heading > 360)
 896         heading -= 360;
 897     geo_direct_wgs_84(0, lat, lon, heading,
 898                       2.2 * radius, &lat2, &lon2, &az2);
 899     wpt =
 900         createOnGround(ac, "taxiStart", SGGeod::fromDeg(lon2, lat2),
 901                        aptElev, vTaxiReduced);
 902     waypoints.push_back(wpt);
 903
 904     geo_direct_wgs_84(0, lat, lon, heading,
 905                       0.1 * radius, &lat2, &lon2, &az2);
 906
 907     wpt =
 908         createOnGround(ac, "taxiStart2", SGGeod::fromDeg(lon2, lat2),
 909                        aptElev, vTaxiReduced);
 910     waypoints.push_back(wpt);
 911
 912     wpt =
 913         createOnGround(ac, "END", SGGeod::fromDeg(lon, lat), aptElev,
 914                        vTaxiReduced);
 915     waypoints.push_back(wpt);
 916 }
 917
 918 /**
 919  *
 920  * @param fltType a string describing the type of
 921  * traffic, normally used for gate assignments
 922  * @return a converted string that gives the runway
 923  * preference schedule to be used at aircraft having
 924  * a preferential runway schedule implemented (i.e.
 925  * having a rwyprefs.xml file
 926  *
 927  * Currently valid traffic types for gate assignment:
 928  * - gate (commercial gate)
 929  * - cargo (commercial gargo),
 930  * - ga (general aviation) ,
 931  * - ul (ultralight),
 932  * - mil-fighter (military - fighter),
 933  * - mil-transport (military - transport)
 934  *
 935  * Valid runway classes:
 936  * - com (commercial traffic: jetliners, passenger and cargo)
 937  * - gen (general aviation)
 938  * - ul (ultralight: I can imagine that these may share a runway with ga on some airports)
 939  * - mil (all military traffic)
 940  */
 941 string FGAIFlightPlan::getRunwayClassFromTrafficType(string fltType)
 942 {
 943     if ((fltType == "gate") || (fltType == "cargo")) {
 944         return string("com");
 945     }
 946     if (fltType == "ga") {
 947         return string("gen");
 948     }
 949     if (fltType == "ul") {
 950         return string("ul");
 951     }
 952     if ((fltType == "mil-fighter") || (fltType == "mil-transport")) {
 953         return string("mil");
 954     }
 955     return string("com");
 956 }
 957
 958
 959 double FGAIFlightPlan::getTurnRadius(double speed, bool inAir)
 960 {
 961     double turn_radius;
 962     if (inAir == false) {
 963         turn_radius = ((360 / 30) * fabs(speed)) / (2 * M_PI);
 964     } else {
 965         turn_radius = 0.1911 * speed * speed;   // an estimate for 25 degrees bank
 966     }
 967     return turn_radius;
 968 }