src/ATC/GroundController.cxx

   1 // GroundController.hxx - forked from groundnetwork.cxx
   2
   3 // Written by Durk Talsma, started June 2005.
   4 //
   5 // Copyright (C) 2004 Durk Talsma.
   6 //
   7 // This program is free software; you can redistribute it and/or
   8 // modify it under the terms of the GNU General Public License as
   9 // published by the Free Software Foundation; either version 2 of the
  10 // License, or (at your option) any later version.
  11 //
  12 // This program is distributed in the hope that it will be useful, but
  13 // WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15 // General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU General Public License
  18 // along with this program; if not, write to the Free Software
  19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  20 //
  21 // $Id$
  22
  23 #ifdef HAVE_CONFIG_H
  24 #  include <config.h>
  25 #endif
  26
  27 #include <cmath>
  28 #include <algorithm>
  29 #include <fstream>
  30 #include <map>
  31 #include <boost/foreach.hpp>
  32
  33 #include <osg/Geode>
  34 #include <osg/Geometry>
  35 #include <osg/MatrixTransform>
  36 #include <osg/Shape>
  37
  38 #include <simgear/debug/logstream.hxx>
  39 #include <simgear/scene/material/EffectGeode.hxx>
  40 #include <simgear/scene/material/matlib.hxx>
  41 #include <simgear/scene/material/mat.hxx>
  42 #include <simgear/scene/util/OsgMath.hxx>
  43 #include <simgear/structure/exception.hxx>
  44 #include <simgear/timing/timestamp.hxx>
  45 #include <simgear/timing/sg_time.hxx>
  46
  47 #include <Airports/airport.hxx>
  48 #include <Airports/dynamics.hxx>
  49 #include <Airports/runways.hxx>
  50 #include <Airports/groundnetwork.hxx>
  51
  52 #include <Main/globals.hxx>
  53 #include <Main/fg_props.hxx>
  54 #include <AIModel/AIAircraft.hxx>
  55 #include <AIModel/performancedata.hxx>
  56 #include <AIModel/AIFlightPlan.hxx>
  57 #include <Navaids/NavDataCache.hxx>
  58
  59 #include <ATC/atc_mgr.hxx>
  60
  61 #include <Scenery/scenery.hxx>
  62
  63 #include "GroundController.hxx"
  64
  65 using std::string;
  66
  67
  68 /***************************************************************************
  69  * FGGroundController()
  70  **************************************************************************/
  71
  72 bool compare_trafficrecords(FGTrafficRecord a, FGTrafficRecord b)
  73 {
  74     return (a.getIntentions().size() < b.getIntentions().size());
  75 }
  76
  77 FGGroundController::FGGroundController() :
  78     parent(NULL)
  79 {
  80     hasNetwork = false;
  81     count = 0;
  82     currTraffic = activeTraffic.begin();
  83     group = 0;
  84     version = 0;
  85     networkInitialized = false;
  86
  87 }
  88
  89 FGGroundController::~FGGroundController()
  90 {
  91 }
  92
  93 void FGGroundController::init(FGAirportDynamics* aDynamics)
  94 {
  95     FGATCController::init();
  96
  97     dynamics = aDynamics;
  98     parent = dynamics->parent();
  99     hasNetwork = true;
 100     networkInitialized = true;
 101 }
 102
 103 void FGGroundController::announcePosition(int id,
 104                                        FGAIFlightPlan * intendedRoute,
 105                                        int currentPosition, double lat,
 106                                        double lon, double heading,
 107                                        double speed, double alt,
 108                                        double radius, int leg,
 109                                        FGAIAircraft * aircraft)
 110 {
 111     if (!aircraft || !aircraft->getPerformance()) {
 112         SG_LOG(SG_ATC, SG_ALERT, "announcePosition: missing aircraft performance");
 113         return;
 114     }
 115
 116     TrafficVectorIterator i = activeTraffic.begin();
 117     // Search search if the current id alread has an entry
 118     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 119     if (activeTraffic.size()) {
 120         //while ((i->getId() != id) && i != activeTraffic.end()) {
 121         while (i != activeTraffic.end()) {
 122             if (i->getId() == id) {
 123                 break;
 124             }
 125             i++;
 126         }
 127     }
 128     // Add a new TrafficRecord if no one exsists for this aircraft.
 129     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 130         FGTrafficRecord rec;
 131         rec.setId(id);
 132         rec.setLeg(leg);
 133         rec.setPositionAndIntentions(currentPosition, intendedRoute);
 134         rec.setPositionAndHeading(lat, lon, heading, speed, alt);
 135         rec.setRadius(radius);  // only need to do this when creating the record.
 136         rec.setAircraft(aircraft);
 137         if (leg == 2) {
 138             activeTraffic.push_front(rec);
 139         } else {
 140             activeTraffic.push_back(rec);
 141         }
 142
 143     } else {
 144         i->setPositionAndIntentions(currentPosition, intendedRoute);
 145         i->setPositionAndHeading(lat, lon, heading, speed, alt);
 146     }
 147 }
 148
 149
 150 void FGGroundController::signOff(int id)
 151 {
 152     TrafficVectorIterator i = activeTraffic.begin();
 153     // Search search if the current id alread has an entry
 154     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 155     if (activeTraffic.size()) {
 156         //while ((i->getId() != id) && i != activeTraffic.end()) {
 157         while (i != activeTraffic.end()) {
 158             if (i->getId() == id) {
 159                 break;
 160             }
 161             i++;
 162         }
 163     }
 164     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 165         SG_LOG(SG_GENERAL, SG_ALERT,
 166                "AI error: Aircraft without traffic record is signing off at " << SG_ORIGIN);
 167     } else {
 168         i = activeTraffic.erase(i);
 169     }
 170 }
 171 /**
 172  * The ground network can deal with the following states:
 173  * 0 =  Normal; no action required
 174  * 1 = "Acknowledge "Hold position
 175  * 2 = "Acknowledge "Resume taxi".
 176  * 3 = "Issue TaxiClearance"
 177  * 4 = Acknowledge Taxi Clearance"
 178  * 5 = Post acknowlegde taxiclearance: Start taxiing
 179  * 6 = Report runway
 180  * 7 = Acknowledge report runway
 181  * 8 = Switch tower frequency
 182  * 9 = Acknowledge switch tower frequency
 183  *************************************************************************************************************************/
 184 bool FGGroundController::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId,
 185         AtcMsgDir msgDir)
 186 {
 187     int state = i->getState();
 188     if ((state >= minState) && (state <= maxState) && available) {
 189         if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) {
 190             //cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl;
 191             SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true);
 192             int n = trans_num->getIntValue();
 193             if (n == 0) {
 194                 trans_num->setIntValue(-1);
 195                 // PopupCallback(n);
 196                 //cerr << "Selected transmission message " << n << endl;
 197                 //FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc");
 198                 FGATCDialogNew::instance()->removeEntry(1);
 199             } else {
 200                 //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl;
 201                 transmit(&(*i), dynamics, msgId, msgDir, false);
 202                 return false;
 203             }
 204         }
 205         transmit(&(*i), dynamics, msgId, msgDir, true);
 206         i->updateState();
 207         lastTransmission = now;
 208         available = false;
 209         return true;
 210     }
 211     return false;
 212 }
 213
 214 void FGGroundController::updateAircraftInformation(int id, double lat, double lon,
 215         double heading, double speed, double alt,
 216         double dt)
 217 {
 218     // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
 219     // Transmit air-to-ground "Ready to taxi request:
 220     // Transmit ground to air approval / hold
 221     // Transmit confirmation ...
 222     // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
 223
 224
 225     TrafficVectorIterator i = activeTraffic.begin();
 226     // Search search if the current id has an entry
 227     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 228     TrafficVectorIterator current, closest;
 229     if (activeTraffic.size()) {
 230         //while ((i->getId() != id) && i != activeTraffic.end()) {
 231         while (i != activeTraffic.end()) {
 232             if (i->getId() == id) {
 233                 break;
 234             }
 235             i++;
 236         }
 237     }
 238     // update position of the current aircraft
 239     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 240         SG_LOG(SG_GENERAL, SG_ALERT,
 241                "AI error: updating aircraft without traffic record at " << SG_ORIGIN);
 242     } else {
 243         i->setPositionAndHeading(lat, lon, heading, speed, alt);
 244         current = i;
 245     }
 246
 247     setDt(getDt() + dt);
 248
 249     // Update every three secs, but add some randomness
 250     // to prevent all IA objects doing this in synchrony
 251     //if (getDt() < (3.0) + (rand() % 10))
 252     //  return;
 253     //else
 254     //  setDt(0);
 255     current->clearResolveCircularWait();
 256     current->setWaitsForId(0);
 257     checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
 258     bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
 259     if (!needsTaxiClearance) {
 260         checkHoldPosition(id, lat, lon, heading, speed, alt);
 261         //if (checkForCircularWaits(id)) {
 262         //    i->setResolveCircularWait();
 263         //}
 264     } else {
 265         current->setHoldPosition(true);
 266         int state = current->getState();
 267         time_t now = globals->get_time_params()->get_cur_time();
 268
 269         if ((now - lastTransmission) > 15) {
 270             available = true;
 271         }
 272         if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
 273             current->setState(3);
 274         }
 275         if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) {
 276             current->setState(4);
 277         }
 278         if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
 279             current->setState(5);
 280         }
 281         if ((state == 5) && available) {
 282             current->setState(0);
 283             current->getAircraft()->setTaxiClearanceRequest(false);
 284             current->setHoldPosition(false);
 285             available = false;
 286         }
 287
 288     }
 289 }
 290
 291 /**
 292    Scan for a speed adjustment change. Find the nearest aircraft that is in front
 293    and adjust speed when we get too close. Only do this when current position and/or
 294    intentions of the current aircraft match current taxiroute position of the proximate
 295    aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
 296    instruction. See below for the hold position instruction.
 297
 298    Note that there currently still is one flaw in the logic that needs to be addressed.
 299    There can be situations where one aircraft is in front of the current aircraft, on a separate
 300    route, but really close after an intersection coming off the current route. This
 301    aircraft is still close enough to block the current aircraft. This situation is currently
 302    not addressed yet, but should be.
 303 */
 304
 305 void FGGroundController::checkSpeedAdjustment(int id, double lat,
 306         double lon, double heading,
 307         double speed, double alt)
 308 {
 309
 310     TrafficVectorIterator current, closest, closestOnNetwork;
 311     TrafficVectorIterator i = activeTraffic.begin();
 312     bool otherReasonToSlowDown = false;
 313 //    bool previousInstruction;
 314     if (activeTraffic.size()) {
 315         //while ((i->getId() != id) && (i != activeTraffic.end()))
 316         while (i != activeTraffic.end()) {
 317             if (i->getId() == id) {
 318                 break;
 319             }
 320             i++;
 321         }
 322     } else {
 323         return;
 324     }
 325     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 326         SG_LOG(SG_GENERAL, SG_ALERT,
 327                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment at " << SG_ORIGIN);
 328     }
 329     current = i;
 330     //closest = current;
 331
 332 //    previousInstruction = current->getSpeedAdjustment();
 333     double mindist = HUGE_VAL;
 334     if (activeTraffic.size()) {
 335         double course, dist, bearing, az2; // minbearing,
 336         SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
 337         //TrafficVector iterator closest;
 338         closest = current;
 339         closestOnNetwork = current;
 340         for (TrafficVectorIterator i = activeTraffic.begin();
 341                 i != activeTraffic.end(); i++) {
 342             if (i == current) {
 343                 continue;
 344             }
 345
 346             SGGeod other(SGGeod::fromDegM(i->getLongitude(),
 347                                           i->getLatitude(),
 348                                           i->getAltitude()));
 349             SGGeodesy::inverse(curr, other, course, az2, dist);
 350             bearing = fabs(heading - course);
 351             if (bearing > 180)
 352                 bearing = 360 - bearing;
 353             if ((dist < mindist) && (bearing < 60.0)) {
 354                 mindist = dist;
 355                 closest = i;
 356                 closestOnNetwork = i;
 357 //                minbearing = bearing;
 358
 359             }
 360         }
 361         //Check traffic at the tower controller
 362         if (towerController->hasActiveTraffic()) {
 363             for (TrafficVectorIterator i =
 364                         towerController->getActiveTraffic().begin();
 365                     i != towerController->getActiveTraffic().end(); i++) {
 366                 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
 367                 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
 368                                               i->getLatitude(),
 369                                               i->getAltitude()));
 370                 SGGeodesy::inverse(curr, other, course, az2, dist);
 371                 bearing = fabs(heading - course);
 372                 if (bearing > 180)
 373                     bearing = 360 - bearing;
 374                 if ((dist < mindist) && (bearing < 60.0)) {
 375                     //cerr << "Current aircraft " << current->getAircraft()->getTrafficRef()->getCallSign()
 376                     //     << " is closest to " << i->getAircraft()->getTrafficRef()->getCallSign()
 377                     //     << ", which has status " << i->getAircraft()->isScheduledForTakeoff()
 378                     //     << endl;
 379                     mindist = dist;
 380                     closest = i;
 381 //                    minbearing = bearing;
 382                     otherReasonToSlowDown = true;
 383                 }
 384             }
 385         }
 386         // Finally, check UserPosition
 387         // Note, as of 2011-08-01, this should no longer be necessecary.
 388         /*
 389         double userLatitude = fgGetDouble("/position/latitude-deg");
 390         double userLongitude = fgGetDouble("/position/longitude-deg");
 391         SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
 392         SGGeodesy::inverse(curr, user, course, az2, dist);
 393
 394         bearing = fabs(heading - course);
 395         if (bearing > 180)
 396             bearing = 360 - bearing;
 397         if ((dist < mindist) && (bearing < 60.0)) {
 398             mindist = dist;
 399             //closest = i;
 400             minbearing = bearing;
 401             otherReasonToSlowDown = true;
 402         }
 403         */
 404         current->clearSpeedAdjustment();
 405         bool needBraking = false;
 406         if (current->checkPositionAndIntentions(*closest)
 407                 || otherReasonToSlowDown) {
 408             double maxAllowableDistance =
 409                 (1.1 * current->getRadius()) +
 410                 (1.1 * closest->getRadius());
 411             if (mindist < 2 * maxAllowableDistance) {
 412                 if (current->getId() == closest->getWaitsForId())
 413                     return;
 414                 else
 415                     current->setWaitsForId(closest->getId());
 416                 if (closest->getId() != current->getId()) {
 417                     current->setSpeedAdjustment(closest->getSpeed() *
 418                                                 (mindist / 100));
 419                     needBraking = true;
 420
 421 //                     if (
 422 //                         closest->getAircraft()->getTakeOffStatus() &&
 423 //                         (current->getAircraft()->getTrafficRef()->getDepartureAirport() ==  closest->getAircraft()->getTrafficRef()->getDepartureAirport()) &&
 424 //                         (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway())
 425 //                     )
 426 //                         current->getAircraft()->scheduleForATCTowerDepartureControl(1);
 427                 } else {
 428                     current->setSpeedAdjustment(0);     // This can only happen when the user aircraft is the one closest
 429                 }
 430                 if (mindist < maxAllowableDistance) {
 431                     //double newSpeed = (maxAllowableDistance-mindist);
 432                     //current->setSpeedAdjustment(newSpeed);
 433                     //if (mindist < 0.5* maxAllowableDistance)
 434                     //  {
 435                     current->setSpeedAdjustment(0);
 436                     //  }
 437                 }
 438             }
 439         }
 440         if ((closest->getId() == closestOnNetwork->getId()) && (current->getPriority() < closest->getPriority()) && needBraking) {
 441             swap(current, closest);
 442         }
 443     }
 444 }
 445
 446 /**
 447    Check for "Hold position instruction".
 448    The hold position should be issued under the following conditions:
 449    1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
 450    2) For taxiing aircraft that use one taxiway in opposite directions
 451    3) For crossing or merging taxiroutes.
 452 */
 453
 454 void FGGroundController::checkHoldPosition(int id, double lat,
 455                                         double lon, double heading,
 456                                         double speed, double alt)
 457 {
 458     FGGroundNetwork* network = dynamics->getGroundNetwork();
 459     TrafficVectorIterator current;
 460     TrafficVectorIterator i = activeTraffic.begin();
 461     if (activeTraffic.size()) {
 462         //while ((i->getId() != id) && i != activeTraffic.end())
 463         while (i != activeTraffic.end()) {
 464             if (i->getId() == id) {
 465                 break;
 466             }
 467             i++;
 468         }
 469     } else {
 470         return;
 471     }
 472
 473     time_t now = globals->get_time_params()->get_cur_time();
 474     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 475         SG_LOG(SG_GENERAL, SG_ALERT,
 476                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition at " << SG_ORIGIN);
 477     }
 478     current = i;
 479     //
 480     if (current->getAircraft()->getTakeOffStatus() == 1) {
 481         current->setHoldPosition(true);
 482         return;
 483     }
 484     if (current->getAircraft()->getTakeOffStatus() == 2) {
 485         //cerr << current->getAircraft()->getCallSign() << ". Taxi in position and hold" << endl;
 486         current->setHoldPosition(false);
 487         current->clearSpeedAdjustment();
 488         return;
 489     }
 490     bool origStatus = current->hasHoldPosition();
 491     current->setHoldPosition(false);
 492     //SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
 493     int currentRoute = i->getCurrentPosition();
 494     int nextRoute;
 495     if (i->getIntentions().size()) {
 496         nextRoute    = (*(i->getIntentions().begin()));
 497     } else {
 498         nextRoute = 0;
 499     }
 500     if (currentRoute > 0) {
 501         FGTaxiSegment *tx = network->findSegment(currentRoute);
 502         FGTaxiSegment *nx;
 503         if (nextRoute) {
 504             nx = network->findSegment(nextRoute);
 505         } else {
 506             nx = tx;
 507         }
 508         //if (tx->hasBlock(now) || nx->hasBlock(now) ) {
 509         //   current->setHoldPosition(true);
 510         //}
 511         SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
 512         SGGeod end  (nx->getStart()->geod());
 513
 514         double distance = SGGeodesy::distanceM(start, end);
 515         if (nx->hasBlock(now) && (distance < i->getRadius() * 4)) {
 516             current->setHoldPosition(true);
 517         } else {
 518             intVecIterator ivi = i->getIntentions().begin();
 519             while (ivi != i->getIntentions().end()) {
 520                 if ((*ivi) > 0) {
 521                     FGTaxiSegment* seg = network->findSegment(*ivi);
 522                     distance += seg->getLength();
 523                     if ((seg->hasBlock(now)) && (distance < i->getRadius() * 4)) {
 524                         current->setHoldPosition(true);
 525                         break;
 526                     }
 527                 }
 528                 ivi++;
 529             }
 530         }
 531     }
 532     bool currStatus = current->hasHoldPosition();
 533     current->setHoldPosition(origStatus);
 534     // Either a Hold Position or a resume taxi transmission has been issued
 535     if ((now - lastTransmission) > 2) {
 536         available = true;
 537     }
 538     if (current->getState() == 0) {
 539         if ((origStatus != currStatus) && available) {
 540             //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
 541             if (currStatus == true) { // No has a hold short instruction
 542                 transmit(&(*current), dynamics, MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
 543                 //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
 544                 current->setState(1);
 545             } else {
 546                 transmit(&(*current), dynamics, MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
 547                 //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
 548                 current->setState(2);
 549             }
 550             lastTransmission = now;
 551             available = false;
 552             // Don't act on the changed instruction until the transmission is confirmed
 553             // So set back to original status
 554             //cerr << "Current state " << current->getState() << endl;
 555         }
 556
 557     }
 558     // 6 = Report runway
 559     // 7 = Acknowledge report runway
 560     // 8 = Switch tower frequency
 561     //9 = Acknowledge switch tower frequency
 562
 563     //int state = current->getState();
 564     if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
 565         current->setState(0);
 566         current->setHoldPosition(true);
 567     }
 568     if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
 569         current->setState(0);
 570         current->setHoldPosition(false);
 571     }
 572     if (current->getAircraft()->getTakeOffStatus() && (current->getState() == 0)) {
 573         //cerr << "Scheduling " << current->getAircraft()->getCallSign() << " for hold short" << endl;
 574         current->setState(6);
 575     }
 576     if (checkTransmissionState(6,6, current, now, MSG_REPORT_RUNWAY_HOLD_SHORT, ATC_AIR_TO_GROUND)) {
 577     }
 578     if (checkTransmissionState(7,7, current, now, MSG_ACKNOWLEDGE_REPORT_RUNWAY_HOLD_SHORT, ATC_GROUND_TO_AIR)) {
 579     }
 580     if (checkTransmissionState(8,8, current, now, MSG_SWITCH_TOWER_FREQUENCY, ATC_GROUND_TO_AIR)) {
 581     }
 582     if (checkTransmissionState(9,9, current, now, MSG_ACKNOWLEDGE_SWITCH_TOWER_FREQUENCY, ATC_AIR_TO_GROUND)) {
 583     }
 584
 585
 586
 587     //current->setState(0);
 588 }
 589
 590 /**
 591  * Check whether situations occur where the current aircraft is waiting for itself
 592  * due to higher order interactions.
 593  * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
 594  * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
 595  * through this list of waiting aircraft, we can check if we'd eventually end back
 596  * at the current aircraft.
 597  *
 598  * Note that we should consider the situation where we are actually checking aircraft
 599  * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
 600  * the looping aircraft. If we don't check for that, this function will get stuck into
 601  * endless loop.
 602  */
 603
 604 bool FGGroundController::checkForCircularWaits(int id)
 605 {
 606     //cerr << "Performing Wait check " << id << endl;
 607     int target = 0;
 608     TrafficVectorIterator current, other;
 609     TrafficVectorIterator i = activeTraffic.begin();
 610     int trafficSize = activeTraffic.size();
 611     if (trafficSize) {
 612         while (i != activeTraffic.end()) {
 613             if (i->getId() == id) {
 614                 break;
 615             }
 616             i++;
 617         }
 618     } else {
 619         return false;
 620     }
 621     if (i == activeTraffic.end() || (trafficSize == 0)) {
 622         SG_LOG(SG_GENERAL, SG_ALERT,
 623                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits at " << SG_ORIGIN);
 624     }
 625
 626     current = i;
 627     target = current->getWaitsForId();
 628     //bool printed = false; // Note that this variable is for debugging purposes only.
 629     int counter = 0;
 630
 631     if (id == target) {
 632         //cerr << "aircraft waits for user" << endl;
 633         return false;
 634     }
 635
 636
 637     while ((target > 0) && (target != id) && counter++ < trafficSize) {
 638         //printed = true;
 639         TrafficVectorIterator i = activeTraffic.begin();
 640         if (trafficSize) {
 641             //while ((i->getId() != id) && i != activeTraffic.end())
 642             while (i != activeTraffic.end()) {
 643                 if (i->getId() == target) {
 644                     break;
 645                 }
 646                 i++;
 647             }
 648         } else {
 649             return false;
 650         }
 651         if (i == activeTraffic.end() || (trafficSize == 0)) {
 652             //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
 653             // The target id is not found on the current network, which means it's at the tower
 654             //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
 655             return false;
 656         }
 657         other = i;
 658         target = other->getWaitsForId();
 659
 660         // actually this trap isn't as impossible as it first seemed:
 661         // the setWaitsForID(id) is set to current when the aircraft
 662         // is waiting for the user controlled aircraft.
 663         //if (current->getId() == other->getId()) {
 664         //    cerr << "Caught the impossible trap" << endl;
 665         //    cerr << "Current = " << current->getId() << endl;
 666         //    cerr << "Other   = " << other  ->getId() << endl;
 667         //    for (TrafficVectorIterator at = activeTraffic.begin();
 668         //          at != activeTraffic.end();
 669         //          at++) {
 670         //        cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
 671         //    }
 672         //    exit(1);
 673         if (current->getId() == other->getId())
 674             return false;
 675         //}
 676         //cerr << current->getCallSign() << " (" << current->getId()  << ") " << " -> " << other->getCallSign()
 677         //     << " (" << other->getId()  << "); " << endl;;
 678         //current = other;
 679     }
 680
 681
 682
 683
 684
 685
 686     //if (printed)
 687     //   cerr << "[done] " << endl << endl;;
 688     if (id == target) {
 689         SG_LOG(SG_GENERAL, SG_WARN,
 690                "Detected circular wait condition: Id = " << id <<
 691                "target = " << target);
 692         return true;
 693     } else {
 694         return false;
 695     }
 696 }
 697
 698 // Note that this function is probably obsolete...
 699 bool FGGroundController::hasInstruction(int id)
 700 {
 701     TrafficVectorIterator i = activeTraffic.begin();
 702     // Search search if the current id has an entry
 703     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 704     if (activeTraffic.size()) {
 705         //while ((i->getId() != id) && i != activeTraffic.end()) {
 706         while (i != activeTraffic.end()) {
 707             if (i->getId() == id) {
 708                 break;
 709             }
 710             i++;
 711         }
 712     }
 713     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 714         SG_LOG(SG_GENERAL, SG_ALERT,
 715                "AI error: checking ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
 716     } else {
 717         return i->hasInstruction();
 718     }
 719     return false;
 720 }
 721
 722 FGATCInstruction FGGroundController::getInstruction(int id)
 723 {
 724     TrafficVectorIterator i = activeTraffic.begin();
 725     // Search search if the current id has an entry
 726     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 727     if (activeTraffic.size()) {
 728         //while ((i->getId() != id) && i != activeTraffic.end()) {
 729         while (i != activeTraffic.end()) {
 730             if (i->getId() == id) {
 731                 break;
 732             }
 733             i++;
 734         }
 735     }
 736     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 737         SG_LOG(SG_GENERAL, SG_ALERT,
 738                "AI error: requesting ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
 739     } else {
 740         return i->getInstruction();
 741     }
 742     return FGATCInstruction();
 743 }
 744
 745 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
 746 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
 747                             double lon, double elev, double hdg, double slope)
 748 {
 749     SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
 750     obj_pos = makeZUpFrame(geod);
 751     // hdg is not a compass heading, but a counter-clockwise rotation
 752     // around the Z axis
 753     obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
 754                                         0.0, 0.0, 1.0));
 755     obj_pos.preMult(osg::Matrix::rotate(slope * SGD_DEGREES_TO_RADIANS,
 756                                         0.0, 1.0, 0.0));
 757 }
 758
 759
 760
 761
 762 void FGGroundController::render(bool visible)
 763 {
 764     SGMaterialLib *matlib = globals->get_matlib();
 765     FGGroundNetwork* network = dynamics->getGroundNetwork();
 766
 767     if (group) {
 768         //int nr = ;
 769         globals->get_scenery()->get_scene_graph()->removeChild(group);
 770         //while (group->getNumChildren()) {
 771         //  cerr << "Number of children: " << group->getNumChildren() << endl;
 772         //simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
 773         //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
 774         //geode->releaseGLObjects();
 775         //group->removeChild(geode);
 776         //delete geode;
 777         group = 0;
 778     }
 779     if (visible) {
 780         group = new osg::Group;
 781         FGScenery * local_scenery = globals->get_scenery();
 782         // double elevation_meters = 0.0;
 783 //        double elevation_feet = 0.0;
 784         time_t now = globals->get_time_params()->get_cur_time();
 785
 786         //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
 787         //double dx = 0;
 788
 789         for   (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
 790             // Handle start point i.e. the segment that is connected to the aircraft itself on the starting end
 791             // and to the the first "real" taxi segment on the other end.
 792             const int pos = i->getCurrentPosition();
 793             if (pos > 0) {
 794                 FGTaxiSegment* segment = network->findSegment(pos);
 795                 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
 796                 SGGeod end  (segment->getEnd()->geod());
 797
 798                 double length = SGGeodesy::distanceM(start, end);
 799                 //heading = SGGeodesy::headingDeg(start->geod(), end->geod());
 800
 801                 double az2, heading; //, distanceM;
 802                 SGGeodesy::inverse(start, end, heading, az2, length);
 803                 double coveredDistance = length * 0.5;
 804                 SGGeod center;
 805                 SGGeodesy::direct(start, heading, coveredDistance, center, az2);
 806                 //std::cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << std::endl;
 807                 ///////////////////////////////////////////////////////////////////////////////
 808                 // Make a helper function out of this
 809                 osg::Matrix obj_pos;
 810                 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
 811                 obj_trans->setDataVariance(osg::Object::STATIC);
 812                 // Experimental: Calculate slope here, based on length, and the individual elevations
 813                 double elevationStart;
 814                 if (isUserAircraft((i)->getAircraft())) {
 815                     elevationStart = fgGetDouble("/position/ground-elev-m");
 816                 } else {
 817                     elevationStart = ((i)->getAircraft()->_getAltitude());
 818                 }
 819                 double elevationEnd   = segment->getEnd()->getElevationM();
 820                 //cerr << "Using elevation " << elevationEnd << endl;
 821
 822                 if ((elevationEnd == 0) || (elevationEnd = parent->getElevation())) {
 823                     SGGeod center2 = end;
 824                     center2.setElevationM(SG_MAX_ELEVATION_M);
 825                     if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
 826 //                        elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
 827                         //elevation_meters += 0.5;
 828                     }
 829                     else {
 830                         elevationEnd = parent->getElevation();
 831                     }
 832                     segment->getEnd()->setElevation(elevationEnd);
 833                 }
 834                 double elevationMean  = (elevationStart + elevationEnd) / 2.0;
 835                 double elevDiff       = elevationEnd - elevationStart;
 836
 837                 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
 838
 839                 //cerr << "1. Using mean elevation : " << elevationMean << " and " << slope << endl;
 840
 841                 WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), elevationMean+ 0.5, -(heading), slope );
 842
 843                 obj_trans->setMatrix( obj_pos );
 844                 //osg::Vec3 center(0, 0, 0)
 845
 846                 float width = length /2.0;
 847                 osg::Vec3 corner(-width, 0, 0.25f);
 848                 osg::Vec3 widthVec(2*width + 1, 0, 0);
 849                 osg::Vec3 heightVec(0, 1, 0);
 850                 osg::Geometry* geometry;
 851                 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
 852                 simgear::EffectGeode* geode = new simgear::EffectGeode;
 853                 geode->setName("test");
 854                 geode->addDrawable(geometry);
 855                 //osg::Node *custom_obj;
 856                 SGMaterial *mat;
 857                 if (segment->hasBlock(now)) {
 858                     mat = matlib->find("UnidirectionalTaperRed", center);
 859                 } else {
 860                     mat = matlib->find("UnidirectionalTaperGreen", center);
 861                 }
 862                 if (mat)
 863                     geode->setEffect(mat->get_effect());
 864                 obj_trans->addChild(geode);
 865                 // wire as much of the scene graph together as we can
 866                 //->addChild( obj_trans );
 867                 group->addChild( obj_trans );
 868                 /////////////////////////////////////////////////////////////////////
 869             } else {
 870                 //std::cerr << "BIG FAT WARNING: current position is here : " << pos << std::endl;
 871             }
 872             // Next: Draw the other taxi segments.
 873             for (intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
 874                 osg::Matrix obj_pos;
 875                 const int k = (*j);
 876                 if (k > 0) {
 877                     osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
 878                     obj_trans->setDataVariance(osg::Object::STATIC);
 879                     FGTaxiSegment* segmentK = network->findSegment(k);
 880                     // Experimental: Calculate slope here, based on length, and the individual elevations
 881                     double elevationStart = segmentK->getStart()->getElevationM();
 882                     double elevationEnd   = segmentK->getEnd  ()->getElevationM();
 883                     if ((elevationStart == 0)  || (elevationStart == parent->getElevation())) {
 884                         SGGeod center2 = segmentK->getStart()->geod();
 885                         center2.setElevationM(SG_MAX_ELEVATION_M);
 886                         if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) {
 887 //                            elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5;
 888                             //elevation_meters += 0.5;
 889                         }
 890                         else {
 891                             elevationStart = parent->getElevation();
 892                         }
 893                         segmentK->getStart()->setElevation(elevationStart);
 894                     }
 895                     if ((elevationEnd == 0) || (elevationEnd == parent->getElevation())) {
 896                         SGGeod center2 = segmentK->getEnd()->geod();
 897                         center2.setElevationM(SG_MAX_ELEVATION_M);
 898                         if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
 899 //                            elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
 900                             //elevation_meters += 0.5;
 901                         }
 902                         else {
 903                             elevationEnd = parent->getElevation();
 904                         }
 905                         segmentK->getEnd()->setElevation(elevationEnd);
 906                     }
 907
 908                     double elevationMean  = (elevationStart + elevationEnd) / 2.0;
 909                     double elevDiff       = elevationEnd - elevationStart;
 910                     double length         = segmentK->getLength();
 911                     double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
 912
 913                     // cerr << "2. Using mean elevation : " << elevationMean << " and " << slope << endl;
 914
 915                     SGGeod segCenter = segmentK->getCenter();
 916                     WorldCoordinate( obj_pos, segCenter.getLatitudeDeg(), segCenter.getLongitudeDeg(),
 917                                      elevationMean+ 0.5, -(segmentK->getHeading()), slope );
 918
 919                     obj_trans->setMatrix( obj_pos );
 920                     //osg::Vec3 center(0, 0, 0)
 921
 922                     float width = segmentK->getLength() /2.0;
 923                     osg::Vec3 corner(-width, 0, 0.25f);
 924                     osg::Vec3 widthVec(2*width + 1, 0, 0);
 925                     osg::Vec3 heightVec(0, 1, 0);
 926                     osg::Geometry* geometry;
 927                     geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
 928                     simgear::EffectGeode* geode = new simgear::EffectGeode;
 929                     geode->setName("test");
 930                     geode->addDrawable(geometry);
 931                     //osg::Node *custom_obj;
 932                     SGMaterial *mat;
 933                     if (segmentK->hasBlock(now)) {
 934                         mat = matlib->find("UnidirectionalTaperRed", segCenter);
 935                     } else {
 936                         mat = matlib->find("UnidirectionalTaperGreen", segCenter);
 937                     }
 938                     if (mat)
 939                         geode->setEffect(mat->get_effect());
 940                     obj_trans->addChild(geode);
 941                     // wire as much of the scene graph together as we can
 942                     //->addChild( obj_trans );
 943                     group->addChild( obj_trans );
 944                 }
 945             }
 946             //dx += 0.1;
 947         }
 948         globals->get_scenery()->get_scene_graph()->addChild(group);
 949     }
 950 }
 951
 952 string FGGroundController::getName() {
 953     return string(parent->getId() + "-ground");
 954 }
 955
 956 void FGGroundController::update(double dt)
 957 {
 958     time_t now = globals->get_time_params()->get_cur_time();
 959     FGGroundNetwork* network = dynamics->getGroundNetwork();
 960     network->unblockAllSegments(now);
 961     int priority = 1;
 962
 963     TrafficVector& startupTraffic(dynamics->getStartupController()->getActiveTraffic());
 964     TrafficVectorIterator i;
 965
 966     //sort(activeTraffic.begin(), activeTraffic.end(), compare_trafficrecords);
 967     // Handle traffic that is under ground control first; this way we'll prevent clutter at the gate areas.
 968     // Don't allow an aircraft to pushback when a taxiing aircraft is currently using part of the intended route.
 969     for (i = startupTraffic.begin(); i != startupTraffic.end(); ++i) {
 970         updateStartupTraffic(i, priority, now);
 971     }
 972
 973     for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
 974         updateActiveTraffic(i, priority, now);
 975     }
 976 }
 977
 978 void FGGroundController::updateStartupTraffic(TrafficVectorIterator i,
 979                                               int& priority,
 980                                               time_t now)
 981 {
 982     if (!i->getAircraft()) {
 983         SG_LOG(SG_ATC, SG_ALERT, "updateStartupTraffic: missing aircraft");
 984         return;
 985     }
 986
 987     if (!i->getAircraft()->getPerformance()) {
 988         SG_LOG(SG_ATC, SG_ALERT, "updateStartupTraffic: missing aircraft performance");
 989         return;
 990     }
 991
 992     i->allowPushBack();
 993     i->setPriority(priority++);
 994     // in meters per second;
 995     double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
 996     if (!i->isActive(0)) {
 997         return;
 998     }
 999
1000     FGGroundNetwork* network = dynamics->getGroundNetwork();
1001
1002     if (!network) {
1003         SG_LOG(SG_ATC, SG_ALERT, "updateStartupTraffic: missing ground network");
1004         return;
1005     }
1006
1007     // Check for all active aircraft whether it's current pos segment is
1008     // an opposite of one of the departing aircraft's intentions
1009     for (TrafficVectorIterator j = activeTraffic.begin(); j != activeTraffic.end(); j++) {
1010         int pos = j->getCurrentPosition();
1011         if (pos > 0) {
1012             FGTaxiSegment *seg = network->findOppositeSegment(pos-1);
1013             if (seg) {
1014                 int posReverse = seg->getIndex();
1015                 for (intVecIterator k = i->getIntentions().begin(); k != i->getIntentions().end(); k++) {
1016                     if ((*k) == posReverse) {
1017                         i->denyPushBack();
1018                         network->findSegment(posReverse)->block(i->getId(), now, now);
1019                     }
1020                 }
1021             }
1022         }
1023     }
1024     // if the current aircraft is still allowed to pushback, we can start reserving a route for if by blocking all the entry taxiways.
1025     if (!i->pushBackAllowed()) {
1026         return;
1027     }
1028
1029     double length = 0;
1030     int pos = i->getCurrentPosition();
1031     if (pos > 0) {
1032         FGTaxiSegment *seg = network->findSegment(pos);
1033         length = seg->getLength();
1034         network->blockSegmentsEndingAt(seg, i->getId(), now, now);
1035     }
1036     for (intVecIterator j = i->getIntentions().begin(); j != i->getIntentions().end(); j++) {
1037         int pos = (*j);
1038         if (pos > 0) {
1039             FGTaxiSegment *seg = network->findSegment(pos);
1040             length += seg->getLength();
1041             time_t blockTime = now + (length / vTaxi);
1042             network->blockSegmentsEndingAt(seg, i->getId(), blockTime - 30, now);
1043         }
1044     }
1045 }
1046
1047 void FGGroundController::updateActiveTraffic(TrafficVectorIterator i,
1048                                              int& priority,
1049                                              time_t now)
1050 {
1051     if (!i->getAircraft()) {
1052         SG_LOG(SG_ATC, SG_ALERT, "updateActiveTraffic: missing aircraft");
1053         return;
1054     }
1055
1056     if (!i->getAircraft()->getPerformance()) {
1057         SG_LOG(SG_ATC, SG_ALERT, "updateActiveTraffic: missing aircraft performance");
1058         return;
1059     }
1060
1061     double length = 0;
1062     double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1063     FGGroundNetwork* network = dynamics->getGroundNetwork();
1064
1065     if (!network) {
1066         SG_LOG(SG_ATC, SG_ALERT, "updateActiveTraffic: missing ground network");
1067         return;
1068     }
1069
1070     i->setPriority(priority++);
1071     int pos = i->getCurrentPosition();
1072     if (pos > 0) {
1073         FGTaxiSegment* segment = network->findSegment(pos);
1074         length = segment->getLength();
1075         if (segment->hasBlock(now)) {
1076             //SG_LOG(SG_GENERAL, SG_ALERT, "Taxiway incursion for AI aircraft" << i->getAircraft()->getCallSign());
1077         }
1078
1079     }
1080     intVecIterator ivi;
1081     for (ivi = i->getIntentions().begin(); ivi != i->getIntentions().end(); ivi++) {
1082         int segIndex = (*ivi);
1083         if (segIndex > 0) {
1084             FGTaxiSegment* seg = network->findSegment(segIndex);
1085             if (seg->hasBlock(now)) {
1086                 break;
1087             }
1088         }
1089     }
1090     //after this, ivi points just behind the last valid unblocked taxi segment.
1091     for (intVecIterator j = i->getIntentions().begin(); j != ivi; j++) {
1092         int pos = (*j);
1093         if (pos > 0) {
1094             FGTaxiSegment *seg = network->findSegment(pos);
1095             length += seg->getLength();
1096             time_t blockTime = now + (length / vTaxi);
1097             network->blockSegmentsEndingAt(seg, i->getId(), blockTime - 30, now);
1098         }
1099     }
1100 }