1 // groundnet.cxx - Implimentation of the FlightGear airport ground handling code
3 // Written by Durk Talsma, started June 2005.
5 // Copyright (C) 2004 Durk Talsma.
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.
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.
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.
30 #include <boost/foreach.hpp>
33 #include <osg/Geometry>
34 #include <osg/MatrixTransform>
37 #include <simgear/debug/logstream.hxx>
38 #include <simgear/scene/material/EffectGeode.hxx>
39 #include <simgear/scene/material/matlib.hxx>
40 #include <simgear/scene/material/mat.hxx>
41 #include <simgear/scene/util/OsgMath.hxx>
42 #include <simgear/structure/exception.hxx>
44 #include <Airports/simple.hxx>
45 #include <Airports/dynamics.hxx>
47 #include <AIModel/AIAircraft.hxx>
48 #include <AIModel/performancedata.hxx>
49 #include <AIModel/AIFlightPlan.hxx>
51 #include <ATC/atc_mgr.hxx>
53 #include <Scenery/scenery.hxx>
55 #include "groundnetwork.hxx"
59 /***************************************************************************
61 **************************************************************************/
63 FGTaxiSegment::FGTaxiSegment(int aStart, int aEnd, bool isPushBack) :
69 isPushBackRoute(isPushBack),
77 bool FGTaxiSegment::bindToNodes(const IndexTaxiNodeMap& nodes)
79 IndexTaxiNodeMap::const_iterator it = nodes.find(startNode);
80 if (it == nodes.end()) {
86 it = nodes.find(endNode);
87 if (it == nodes.end()) {
93 start->addSegment(this);
95 SGGeodesy::inverse(start->geod(), end->geod(), heading, az2, length);
99 SGGeod FGTaxiSegment::getCenter() const
101 return SGGeodesy::direct(start->geod(), heading, length * 0.5);
104 void FGTaxiSegment::block(int id, time_t blockTime, time_t now)
106 BlockListIterator i = blockTimes.begin();
107 while (i != blockTimes.end()) {
108 if (i->getId() == id)
112 if (i == blockTimes.end()) {
113 blockTimes.push_back(Block(id, blockTime, now));
114 sort(blockTimes.begin(), blockTimes.end());
116 i->updateTimeStamps(blockTime, now);
120 // The segment has a block if any of the block times listed in the block list is
121 // smaller than the current time.
122 bool FGTaxiSegment::hasBlock(time_t now)
124 for (BlockListIterator i = blockTimes.begin(); i != blockTimes.end(); i++) {
125 if (i->getBlockTime() < now)
131 void FGTaxiSegment::unblock(time_t now)
133 if (blockTimes.size()) {
134 BlockListIterator i = blockTimes.begin();
135 if (i->getTimeStamp() < (now - 30)) {
143 /***************************************************************************
145 **************************************************************************/
146 bool FGTaxiRoute::next(int *nde)
148 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
149 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
150 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
151 //if (currNode != nodes.end())
152 // cerr << "true" << endl;
154 // cerr << "false" << endl;
155 //if (nodes.size() != (routes.size()) +1)
156 // cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
158 if (currNode == nodes.end())
161 if (currNode != nodes.begin()) // make sure route corresponds to the end node
167 bool FGTaxiRoute::next(int *nde, int *rte)
169 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
170 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
171 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
172 //if (currNode != nodes.end())
173 // cerr << "true" << endl;
175 // cerr << "false" << endl;
176 if (nodes.size() != (routes.size()) + 1) {
177 SG_LOG(SG_GENERAL, SG_ALERT,
178 "ALERT: Misconfigured TaxiRoute : " << nodes.
179 size() << " " << routes.size());
180 throw sg_range_exception("misconfigured taxi route");
182 if (currNode == nodes.end())
185 //*rte = *(currRoute);
186 if (currNode != nodes.begin()) // Make sure route corresponds to the end node
191 // If currNode points to the first node, this means the aircraft is not on the taxi node
192 // yet. Make sure to return a unique identifyer in this situation though, because otherwise
193 // the speed adjust AI code may be unable to resolve whether two aircraft are on the same
194 // taxi route or not. the negative of the preceding route seems a logical choice, as it is
195 // unique for any starting location.
196 // Note that this is probably just a temporary fix until I get Parking / tower control working.
197 *rte = -1 * *(currRoute);
204 void FGTaxiRoute::rewind(int route)
210 if (!(next(&currPoint, &currRoute))) {
211 SG_LOG(SG_GENERAL, SG_ALERT,
212 "Error in rewinding TaxiRoute: current" << currRoute <<
215 } while (currRoute != route);
221 /***************************************************************************
223 **************************************************************************/
225 bool compare_trafficrecords(FGTrafficRecord a, FGTrafficRecord b)
227 return (a.getIntentions().size() < b.getIntentions().size());
230 FGGroundNetwork::FGGroundNetwork() :
239 currTraffic = activeTraffic.begin();
242 networkInitialized = false;
246 FGGroundNetwork::~FGGroundNetwork()
248 // JMT 2012-09-8 - disabling the groundnet-caching as part of enabling the
249 // navcache. The problem isn't the NavCache - it's that for the past few years,
250 // we have not being running destructors on FGPositioned classes, and hence,
251 // not running this code.
252 // When I fix FGPositioned lifetimes (unloading-at-runtime support), this
253 // will need to be re-visited so it can run safely during shutdown.
255 //cerr << "Running Groundnetwork Destructor " << endl;
256 bool saveData = false;
258 if (fgGetBool("/sim/ai/groundnet-cache")) {
259 SGPath cacheData(globals->get_fg_home());
260 cacheData.append("ai");
261 string airport = parent->getId();
263 if ((airport) != "") {
265 ::snprintf(buffer, 128, "%c/%c/%c/",
266 airport[0], airport[1], airport[2]);
267 cacheData.append(buffer);
268 if (!cacheData.exists()) {
269 cacheData.create_dir(0777);
271 cacheData.append(airport + "-groundnet-cache.txt");
272 cachefile.open(cacheData.str().c_str());
276 cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl;
277 for (FGTaxiNodeVectorIterator node = nodes.begin();
278 node != nodes.end(); node++) {
280 cachefile << (*node)->getIndex () << " "
281 << (*node)->getElevationM (parent->getElevation()*SG_FEET_TO_METER) << " "
287 pushBackNodes.clear();
288 for (FGTaxiSegmentVectorIterator seg = segments.begin();
289 seg != segments.end(); seg++) {
299 void FGGroundNetwork::saveElevationCache() {
300 //cerr << "Running Groundnetwork Destructor " << endl;
301 bool saveData = false;
303 if (fgGetBool("/sim/ai/groundnet-cache")) {
304 SGPath cacheData(globals->get_fg_home());
305 cacheData.append("ai");
306 string airport = parent->getId();
308 if ((airport) != "") {
310 ::snprintf(buffer, 128, "%c/%c/%c/",
311 airport[0], airport[1], airport[2]);
312 cacheData.append(buffer);
313 if (!cacheData.exists()) {
314 cacheData.create_dir(0777);
316 cacheData.append(airport + "-groundnet-cache.txt");
317 cachefile.open(cacheData.str().c_str());
321 cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl;
322 for (IndexTaxiNodeMap::iterator node = nodes.begin();
323 node != nodes.end(); node++) {
325 cachefile << node->second->getIndex () << " "
326 << node->second->getElevationM (parent->getElevation()*SG_FEET_TO_METER) << " "
335 void FGGroundNetwork::addSegment(FGTaxiSegment* seg)
337 segments.push_back(seg);
340 void FGGroundNetwork::addNode(FGTaxiNode* node)
343 IndexTaxiNodeMap::iterator it = nodes.find(node->getIndex());
344 if (it != nodes.end()) {
345 throw sg_range_exception();
348 nodes.insert(it, std::make_pair(node->getIndex(), node));
351 void FGGroundNetwork::addNodes(FGParkingVec * parkings)
353 BOOST_FOREACH(FGParking* parking, *parkings) {
358 void FGGroundNetwork::init()
360 if (networkInitialized) {
361 FGATCController::init();
362 //cerr << "FGground network already initialized" << endl;
369 // bind segments to nodes
370 BOOST_FOREACH(FGTaxiSegment* segment, segments) {
371 if (!segment->bindToNodes(nodes)) {
372 SG_LOG(SG_GENERAL, SG_ALERT, "unable to bind taxiway segment");
375 segment->setIndex(index++);
376 if (segment->isPushBack()) {
377 pushBackNodes.push_back(segment->getEnd());
381 // establish pairing of segments
382 BOOST_FOREACH(FGTaxiSegment* segment, segments) {
383 FGTaxiSegment* opp = segment->getEnd()->getArcTo(segment->getStart());
385 segment->setOpposite(opp);
389 if (fgGetBool("/sim/ai/groundnet-cache")) {
393 networkInitialized = true;
396 void FGGroundNetwork::parseCache()
398 SGPath cacheData(globals->get_fg_home());
399 cacheData.append("ai");
400 string airport = parent->getId();
402 if (airport.empty()) {
407 ::snprintf(buffer, 128, "%c/%c/%c/",
408 airport[0], airport[1], airport[2]);
409 cacheData.append(buffer);
410 if (!cacheData.exists()) {
411 cacheData.create_dir(0777);
415 cacheData.append(airport + "-groundnet-cache.txt");
416 if (cacheData.exists()) {
417 ifstream data(cacheData.c_str());
420 if (revisionStr != "[GroundNetcachedata:ref:2011:09:04]") {
421 SG_LOG(SG_GENERAL, SG_ALERT,"GroundNetwork Warning: discarding outdated cachefile " <<
422 cacheData.c_str() << " for Airport " << airport);
424 for (IndexTaxiNodeMap::iterator i = nodes.begin();
427 i->second->setElevation(parent->elevation() * SG_FEET_TO_METER);
428 data >> index >> elev;
431 if (index != i->second->getIndex()) {
432 SG_LOG(SG_GENERAL, SG_ALERT, "Index read from ground network cache at airport " << airport << " does not match index in the network itself");
434 i->second->setElevation(elev);
441 int FGGroundNetwork::findNearestNode(const SGGeod & aGeod)
443 double minDist = HUGE_VAL;
446 IndexTaxiNodeMap::iterator i;
447 for (i = nodes.begin(); i != nodes.end(); i++) {
448 double d = SGGeodesy::distanceM(aGeod, i->second->geod());
458 int FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod)
460 double minDist = HUGE_VAL;
463 IndexTaxiNodeMap::iterator i;
464 for (i = nodes.begin(); i != nodes.end(); i++) {
465 if (!i->second->getIsOnRunway()) {
469 double d = SGGeodesy::distanceM(aGeod, i->second->geod());
479 FGTaxiNode* FGGroundNetwork::findNode(unsigned int idx)
481 IndexTaxiNodeMap::iterator i = nodes.find(idx);
482 if (i == nodes.end()) {
489 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx)
491 for (FGTaxiSegmentVectorIterator
492 itr = segments.begin();
493 itr != segments.end(); itr++)
495 if (itr->getIndex() == idx)
496 return itr->getAddress();
499 if ((idx > 0) && (idx <= segments.size()))
500 return segments[idx - 1];
502 //cerr << "Alert: trying to find invalid segment " << idx << endl;
508 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end,
511 //implements Dijkstra's algorithm to find shortest distance route from start to end
512 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
514 //double INFINITE = 100000000000.0;
515 // initialize scoring values
516 int nParkings = parent->getDynamics()->getNrOfParkings();
517 FGTaxiNodeVector unvisited;
520 // create vector from map values
521 IndexTaxiNodeMap::iterator i;
522 for (i = nodes.begin(); i != nodes.end(); i++) {
523 unvisited.push_back(i->second);
526 unvisited = pushBackNodes;
529 BOOST_FOREACH(FGTaxiNode* node, unvisited) {
530 node->setPathScore(HUGE_VAL); //infinity by all practical means
531 node->setPreviousNode(0); //
532 node->setPreviousSeg(0); //
535 FGTaxiNode *firstNode = findNode(start);
538 SG_LOG(SG_GENERAL, SG_ALERT,
539 "Error in ground network. Failed to find first waypoint: " << start
540 << " at " << ((parent) ? parent->getId() : "<unknown>"));
541 return FGTaxiRoute();
543 firstNode->setPathScore(0);
545 FGTaxiNode *lastNode = findNode(end);
548 SG_LOG(SG_GENERAL, SG_ALERT,
549 "Error in ground network. Failed to find last waypoint: " << end
550 << " at " << ((parent) ? parent->getId() : "<unknown>"));
551 return FGTaxiRoute();
554 while (!unvisited.empty()) {
555 FGTaxiNode *best = unvisited.front();
556 BOOST_FOREACH(FGTaxiNode* i, unvisited) {
557 if (i->getPathScore() < best->getPathScore()) {
562 // remove 'best' from the unvisited set
563 FGTaxiNodeVectorIterator newend =
564 remove(unvisited.begin(), unvisited.end(), best);
565 unvisited.erase(newend, unvisited.end());
567 if (best == lastNode) { // found route or best not connected
571 BOOST_FOREACH(FGTaxiSegment* seg, best->arcs()) {
572 if (!fullSearch && !seg->isPushBack()) {
573 continue; // inelligible!
576 FGTaxiNode *tgt = seg->getEnd();
577 double alt = best->getPathScore() + seg->getLength() +
578 seg->getPenalty(nParkings);
579 if (alt < tgt->getPathScore()) { // Relax (u,v)
580 tgt->setPathScore(alt);
581 tgt->setPreviousNode(best);
582 tgt->setPreviousSeg(seg);
584 } // of outgoing arcs/segments from current best node iteration
585 } // of unvisited nodes remaining
587 if (lastNode->getPathScore() == HUGE_VAL) {
588 // no valid route found
590 SG_LOG(SG_GENERAL, SG_ALERT,
591 "Failed to find route from waypoint " << start << " to "
592 << end << " at " << parent->getId());
596 //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
598 // assemble route from backtrace information
599 intVec nodes, routes;
600 FGTaxiNode *bt = lastNode;
601 while (bt->getPreviousNode() != 0) {
602 nodes.push_back(bt->getIndex());
603 routes.push_back(bt->getPreviousSegment()->getIndex());
604 bt = bt->getPreviousNode();
606 nodes.push_back(start);
607 reverse(nodes.begin(), nodes.end());
608 reverse(routes.begin(), routes.end());
610 return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0);
614 int FGTaxiSegment::getPenalty(int nGates)
617 if (end->getIndex() < nGates) {
620 if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active.
626 /* ATC Related Functions */
628 void FGGroundNetwork::announcePosition(int id,
629 FGAIFlightPlan * intendedRoute,
630 int currentPosition, double lat,
631 double lon, double heading,
632 double speed, double alt,
633 double radius, int leg,
634 FGAIAircraft * aircraft)
637 TrafficVectorIterator i = activeTraffic.begin();
638 // Search search if the current id alread has an entry
639 // This might be faster using a map instead of a vector, but let's start by taking a safe route
640 if (activeTraffic.size()) {
641 //while ((i->getId() != id) && i != activeTraffic.end()) {
642 while (i != activeTraffic.end()) {
643 if (i->getId() == id) {
649 // Add a new TrafficRecord if no one exsists for this aircraft.
650 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
654 rec.setPositionAndIntentions(currentPosition, intendedRoute);
655 rec.setPositionAndHeading(lat, lon, heading, speed, alt);
656 rec.setRadius(radius); // only need to do this when creating the record.
657 rec.setAircraft(aircraft);
659 activeTraffic.push_front(rec);
661 activeTraffic.push_back(rec);
665 i->setPositionAndIntentions(currentPosition, intendedRoute);
666 i->setPositionAndHeading(lat, lon, heading, speed, alt);
671 void FGGroundNetwork::signOff(int id)
673 TrafficVectorIterator i = activeTraffic.begin();
674 // Search search if the current id alread has an entry
675 // This might be faster using a map instead of a vector, but let's start by taking a safe route
676 if (activeTraffic.size()) {
677 //while ((i->getId() != id) && i != activeTraffic.end()) {
678 while (i != activeTraffic.end()) {
679 if (i->getId() == id) {
685 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
686 SG_LOG(SG_GENERAL, SG_ALERT,
687 "AI error: Aircraft without traffic record is signing off at " << SG_ORIGIN);
689 i = activeTraffic.erase(i);
693 * The ground network can deal with the following states:
694 * 0 = Normal; no action required
695 * 1 = "Acknowledge "Hold position
696 * 2 = "Acknowledge "Resume taxi".
697 * 3 = "Issue TaxiClearance"
698 * 4 = Acknowledge Taxi Clearance"
699 * 5 = Post acknowlegde taxiclearance: Start taxiing
701 * 7 = Acknowledge report runway
702 * 8 = Switch tower frequency
703 * 9 = Acknowledge switch tower frequency
704 *************************************************************************************************************************/
705 bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId,
708 int state = i->getState();
709 if ((state >= minState) && (state <= maxState) && available) {
710 if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) {
711 //cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl;
712 static SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true);
713 int n = trans_num->getIntValue();
715 trans_num->setIntValue(-1);
717 //cerr << "Selected transmission message " << n << endl;
718 //FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc");
719 FGATCDialogNew::instance()->removeEntry(1);
721 //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl;
722 transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, false);
726 transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, true);
728 lastTransmission = now;
735 void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
736 double heading, double speed, double alt,
739 time_t currentTime = time(NULL);
740 if (nextSave < currentTime) {
741 saveElevationCache();
742 nextSave = currentTime + 100 + rand() % 200;
744 // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
745 // Transmit air-to-ground "Ready to taxi request:
746 // Transmit ground to air approval / hold
747 // Transmit confirmation ...
748 // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
751 TrafficVectorIterator i = activeTraffic.begin();
752 // Search search if the current id has an entry
753 // This might be faster using a map instead of a vector, but let's start by taking a safe route
754 TrafficVectorIterator current, closest;
755 if (activeTraffic.size()) {
756 //while ((i->getId() != id) && i != activeTraffic.end()) {
757 while (i != activeTraffic.end()) {
758 if (i->getId() == id) {
764 // update position of the current aircraft
765 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
766 SG_LOG(SG_GENERAL, SG_ALERT,
767 "AI error: updating aircraft without traffic record at " << SG_ORIGIN);
769 i->setPositionAndHeading(lat, lon, heading, speed, alt);
775 // Update every three secs, but add some randomness
776 // to prevent all IA objects doing this in synchrony
777 //if (getDt() < (3.0) + (rand() % 10))
781 current->clearResolveCircularWait();
782 current->setWaitsForId(0);
783 checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
784 bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
785 if (!needsTaxiClearance) {
786 checkHoldPosition(id, lat, lon, heading, speed, alt);
787 //if (checkForCircularWaits(id)) {
788 // i->setResolveCircularWait();
791 current->setHoldPosition(true);
792 int state = current->getState();
793 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
794 if ((now - lastTransmission) > 15) {
797 if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
798 current->setState(3);
800 if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) {
801 current->setState(4);
803 if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
804 current->setState(5);
806 if ((state == 5) && available) {
807 current->setState(0);
808 current->getAircraft()->setTaxiClearanceRequest(false);
809 current->setHoldPosition(false);
817 Scan for a speed adjustment change. Find the nearest aircraft that is in front
818 and adjust speed when we get too close. Only do this when current position and/or
819 intentions of the current aircraft match current taxiroute position of the proximate
820 aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
821 instruction. See below for the hold position instruction.
823 Note that there currently still is one flaw in the logic that needs to be addressed.
824 There can be situations where one aircraft is in front of the current aircraft, on a separate
825 route, but really close after an intersection coming off the current route. This
826 aircraft is still close enough to block the current aircraft. This situation is currently
827 not addressed yet, but should be.
830 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
831 double lon, double heading,
832 double speed, double alt)
835 TrafficVectorIterator current, closest, closestOnNetwork;
836 TrafficVectorIterator i = activeTraffic.begin();
837 bool otherReasonToSlowDown = false;
838 // bool previousInstruction;
839 if (activeTraffic.size()) {
840 //while ((i->getId() != id) && (i != activeTraffic.end()))
841 while (i != activeTraffic.end()) {
842 if (i->getId() == id) {
850 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
851 SG_LOG(SG_GENERAL, SG_ALERT,
852 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment at " << SG_ORIGIN);
857 // previousInstruction = current->getSpeedAdjustment();
858 double mindist = HUGE_VAL;
859 if (activeTraffic.size()) {
860 double course, dist, bearing, az2; // minbearing,
861 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
862 //TrafficVector iterator closest;
864 closestOnNetwork = current;
865 for (TrafficVectorIterator i = activeTraffic.begin();
866 i != activeTraffic.end(); i++) {
871 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
874 SGGeodesy::inverse(curr, other, course, az2, dist);
875 bearing = fabs(heading - course);
877 bearing = 360 - bearing;
878 if ((dist < mindist) && (bearing < 60.0)) {
881 closestOnNetwork = i;
882 // minbearing = bearing;
886 //Check traffic at the tower controller
887 if (towerController->hasActiveTraffic()) {
888 for (TrafficVectorIterator i =
889 towerController->getActiveTraffic().begin();
890 i != towerController->getActiveTraffic().end(); i++) {
891 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
892 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
895 SGGeodesy::inverse(curr, other, course, az2, dist);
896 bearing = fabs(heading - course);
898 bearing = 360 - bearing;
899 if ((dist < mindist) && (bearing < 60.0)) {
900 //cerr << "Current aircraft " << current->getAircraft()->getTrafficRef()->getCallSign()
901 // << " is closest to " << i->getAircraft()->getTrafficRef()->getCallSign()
902 // << ", which has status " << i->getAircraft()->isScheduledForTakeoff()
906 // minbearing = bearing;
907 otherReasonToSlowDown = true;
911 // Finally, check UserPosition
912 // Note, as of 2011-08-01, this should no longer be necessecary.
914 double userLatitude = fgGetDouble("/position/latitude-deg");
915 double userLongitude = fgGetDouble("/position/longitude-deg");
916 SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
917 SGGeodesy::inverse(curr, user, course, az2, dist);
919 bearing = fabs(heading - course);
921 bearing = 360 - bearing;
922 if ((dist < mindist) && (bearing < 60.0)) {
925 minbearing = bearing;
926 otherReasonToSlowDown = true;
929 current->clearSpeedAdjustment();
930 bool needBraking = false;
931 if (current->checkPositionAndIntentions(*closest)
932 || otherReasonToSlowDown) {
933 double maxAllowableDistance =
934 (1.1 * current->getRadius()) +
935 (1.1 * closest->getRadius());
936 if (mindist < 2 * maxAllowableDistance) {
937 if (current->getId() == closest->getWaitsForId())
940 current->setWaitsForId(closest->getId());
941 if (closest->getId() != current->getId()) {
942 current->setSpeedAdjustment(closest->getSpeed() *
947 // closest->getAircraft()->getTakeOffStatus() &&
948 // (current->getAircraft()->getTrafficRef()->getDepartureAirport() == closest->getAircraft()->getTrafficRef()->getDepartureAirport()) &&
949 // (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway())
951 // current->getAircraft()->scheduleForATCTowerDepartureControl(1);
953 current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
955 if (mindist < maxAllowableDistance) {
956 //double newSpeed = (maxAllowableDistance-mindist);
957 //current->setSpeedAdjustment(newSpeed);
958 //if (mindist < 0.5* maxAllowableDistance)
960 current->setSpeedAdjustment(0);
965 if ((closest->getId() == closestOnNetwork->getId()) && (current->getPriority() < closest->getPriority()) && needBraking) {
966 swap(current, closest);
972 Check for "Hold position instruction".
973 The hold position should be issued under the following conditions:
974 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
975 2) For taxiing aircraft that use one taxiway in opposite directions
976 3) For crossing or merging taxiroutes.
979 void FGGroundNetwork::checkHoldPosition(int id, double lat,
980 double lon, double heading,
981 double speed, double alt)
983 TrafficVectorIterator current;
984 TrafficVectorIterator i = activeTraffic.begin();
985 if (activeTraffic.size()) {
986 //while ((i->getId() != id) && i != activeTraffic.end())
987 while (i != activeTraffic.end()) {
988 if (i->getId() == id) {
996 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
997 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
998 SG_LOG(SG_GENERAL, SG_ALERT,
999 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition at " << SG_ORIGIN);
1003 if (current->getAircraft()->getTakeOffStatus() == 1) {
1004 current->setHoldPosition(true);
1007 if (current->getAircraft()->getTakeOffStatus() == 2) {
1008 //cerr << current->getAircraft()->getCallSign() << ". Taxi in position and hold" << endl;
1009 current->setHoldPosition(false);
1010 current->clearSpeedAdjustment();
1013 bool origStatus = current->hasHoldPosition();
1014 current->setHoldPosition(false);
1015 //SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
1016 int currentRoute = i->getCurrentPosition();
1018 if (i->getIntentions().size()) {
1019 nextRoute = (*(i->getIntentions().begin()));
1023 if (currentRoute > 0) {
1024 FGTaxiSegment *tx = findSegment(currentRoute);
1027 nx = findSegment(nextRoute);
1031 //if (tx->hasBlock(now) || nx->hasBlock(now) ) {
1032 // current->setHoldPosition(true);
1034 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1035 SGGeod end (nx->getStart()->geod());
1037 double distance = SGGeodesy::distanceM(start, end);
1038 if (nx->hasBlock(now) && (distance < i->getRadius() * 4)) {
1039 current->setHoldPosition(true);
1041 intVecIterator ivi = i->getIntentions().begin();
1042 while (ivi != i->getIntentions().end()) {
1044 distance += segments[(*ivi)-1]->getLength();
1045 if ((segments[(*ivi)-1]->hasBlock(now)) && (distance < i->getRadius() * 4)) {
1046 current->setHoldPosition(true);
1054 bool currStatus = current->hasHoldPosition();
1055 current->setHoldPosition(origStatus);
1056 // Either a Hold Position or a resume taxi transmission has been issued
1057 if ((now - lastTransmission) > 2) {
1060 if (current->getState() == 0) {
1061 if ((origStatus != currStatus) && available) {
1062 //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
1063 if (currStatus == true) { // No has a hold short instruction
1064 transmit(&(*current), &(*parent->getDynamics()), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
1065 //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
1066 current->setState(1);
1068 transmit(&(*current), &(*parent->getDynamics()), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
1069 //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
1070 current->setState(2);
1072 lastTransmission = now;
1074 // Don't act on the changed instruction until the transmission is confirmed
1075 // So set back to original status
1076 //cerr << "Current state " << current->getState() << endl;
1080 // 6 = Report runway
1081 // 7 = Acknowledge report runway
1082 // 8 = Switch tower frequency
1083 //9 = Acknowledge switch tower frequency
1085 //int state = current->getState();
1086 if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
1087 current->setState(0);
1088 current->setHoldPosition(true);
1090 if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
1091 current->setState(0);
1092 current->setHoldPosition(false);
1094 if (current->getAircraft()->getTakeOffStatus() && (current->getState() == 0)) {
1095 //cerr << "Scheduling " << current->getAircraft()->getCallSign() << " for hold short" << endl;
1096 current->setState(6);
1098 if (checkTransmissionState(6,6, current, now, MSG_REPORT_RUNWAY_HOLD_SHORT, ATC_AIR_TO_GROUND)) {
1100 if (checkTransmissionState(7,7, current, now, MSG_ACKNOWLEDGE_REPORT_RUNWAY_HOLD_SHORT, ATC_GROUND_TO_AIR)) {
1102 if (checkTransmissionState(8,8, current, now, MSG_SWITCH_TOWER_FREQUENCY, ATC_GROUND_TO_AIR)) {
1104 if (checkTransmissionState(9,9, current, now, MSG_ACKNOWLEDGE_SWITCH_TOWER_FREQUENCY, ATC_AIR_TO_GROUND)) {
1109 //current->setState(0);
1113 * Check whether situations occur where the current aircraft is waiting for itself
1114 * due to higher order interactions.
1115 * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
1116 * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
1117 * through this list of waiting aircraft, we can check if we'd eventually end back
1118 * at the current aircraft.
1120 * Note that we should consider the situation where we are actually checking aircraft
1121 * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
1122 * the looping aircraft. If we don't check for that, this function will get stuck into
1126 bool FGGroundNetwork::checkForCircularWaits(int id)
1128 //cerr << "Performing Wait check " << id << endl;
1130 TrafficVectorIterator current, other;
1131 TrafficVectorIterator i = activeTraffic.begin();
1132 int trafficSize = activeTraffic.size();
1134 while (i != activeTraffic.end()) {
1135 if (i->getId() == id) {
1143 if (i == activeTraffic.end() || (trafficSize == 0)) {
1144 SG_LOG(SG_GENERAL, SG_ALERT,
1145 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits at " << SG_ORIGIN);
1149 target = current->getWaitsForId();
1150 //bool printed = false; // Note that this variable is for debugging purposes only.
1154 //cerr << "aircraft waits for user" << endl;
1159 while ((target > 0) && (target != id) && counter++ < trafficSize) {
1161 TrafficVectorIterator i = activeTraffic.begin();
1163 //while ((i->getId() != id) && i != activeTraffic.end())
1164 while (i != activeTraffic.end()) {
1165 if (i->getId() == target) {
1173 if (i == activeTraffic.end() || (trafficSize == 0)) {
1174 //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
1175 // The target id is not found on the current network, which means it's at the tower
1176 //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1180 target = other->getWaitsForId();
1182 // actually this trap isn't as impossible as it first seemed:
1183 // the setWaitsForID(id) is set to current when the aircraft
1184 // is waiting for the user controlled aircraft.
1185 //if (current->getId() == other->getId()) {
1186 // cerr << "Caught the impossible trap" << endl;
1187 // cerr << "Current = " << current->getId() << endl;
1188 // cerr << "Other = " << other ->getId() << endl;
1189 // for (TrafficVectorIterator at = activeTraffic.begin();
1190 // at != activeTraffic.end();
1192 // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1195 if (current->getId() == other->getId())
1198 //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign()
1199 // << " (" << other->getId() << "); " << endl;;
1209 // cerr << "[done] " << endl << endl;;
1211 SG_LOG(SG_GENERAL, SG_WARN,
1212 "Detected circular wait condition: Id = " << id <<
1213 "target = " << target);
1220 // Note that this function is probably obsolete...
1221 bool FGGroundNetwork::hasInstruction(int id)
1223 TrafficVectorIterator i = activeTraffic.begin();
1224 // Search search if the current id has an entry
1225 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1226 if (activeTraffic.size()) {
1227 //while ((i->getId() != id) && i != activeTraffic.end()) {
1228 while (i != activeTraffic.end()) {
1229 if (i->getId() == id) {
1235 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1236 SG_LOG(SG_GENERAL, SG_ALERT,
1237 "AI error: checking ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1239 return i->hasInstruction();
1244 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1246 TrafficVectorIterator i = activeTraffic.begin();
1247 // Search search if the current id has an entry
1248 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1249 if (activeTraffic.size()) {
1250 //while ((i->getId() != id) && i != activeTraffic.end()) {
1251 while (i != activeTraffic.end()) {
1252 if (i->getId() == id) {
1258 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1259 SG_LOG(SG_GENERAL, SG_ALERT,
1260 "AI error: requesting ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1262 return i->getInstruction();
1264 return FGATCInstruction();
1267 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
1268 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
1269 double lon, double elev, double hdg, double slope)
1271 SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
1272 obj_pos = makeZUpFrame(geod);
1273 // hdg is not a compass heading, but a counter-clockwise rotation
1274 // around the Z axis
1275 obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
1277 obj_pos.preMult(osg::Matrix::rotate(slope * SGD_DEGREES_TO_RADIANS,
1284 void FGGroundNetwork::render(bool visible)
1287 SGMaterialLib *matlib = globals->get_matlib();
1290 globals->get_scenery()->get_scene_graph()->removeChild(group);
1291 //while (group->getNumChildren()) {
1292 // cerr << "Number of children: " << group->getNumChildren() << endl;
1293 //simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
1294 //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
1295 //geode->releaseGLObjects();
1296 //group->removeChild(geode);
1301 group = new osg::Group;
1302 FGScenery * local_scenery = globals->get_scenery();
1303 // double elevation_meters = 0.0;
1304 // double elevation_feet = 0.0;
1305 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1306 //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
1308 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1309 // Handle start point
1310 int pos = i->getCurrentPosition() - 1;
1313 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1314 SGGeod end (segments[pos]->getEnd()->geod());
1316 double length = SGGeodesy::distanceM(start, end);
1317 //heading = SGGeodesy::headingDeg(start->geod(), end->geod());
1319 double az2, heading; //, distanceM;
1320 SGGeodesy::inverse(start, end, heading, az2, length);
1321 double coveredDistance = length * 0.5;
1323 SGGeodesy::direct(start, heading, coveredDistance, center, az2);
1324 //cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
1325 ///////////////////////////////////////////////////////////////////////////////
1326 // Make a helper function out of this
1327 osg::Matrix obj_pos;
1328 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1329 obj_trans->setDataVariance(osg::Object::STATIC);
1330 // Experimental: Calculate slope here, based on length, and the individual elevations
1331 double elevationStart;
1332 if (isUserAircraft((i)->getAircraft())) {
1333 elevationStart = fgGetDouble("/position/ground-elev-m");
1335 elevationStart = ((i)->getAircraft()->_getAltitude());
1337 double elevationEnd = segments[pos]->getEnd()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1338 //cerr << "Using elevation " << elevationEnd << endl;
1340 if ((elevationEnd == 0) || (elevationEnd = parent->getElevation())) {
1341 SGGeod center2 = end;
1342 center2.setElevationM(SG_MAX_ELEVATION_M);
1343 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1344 // elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1345 //elevation_meters += 0.5;
1348 elevationEnd = parent->getElevation();
1350 segments[pos]->getEnd()->setElevation(elevationEnd);
1352 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1353 double elevDiff = elevationEnd - elevationStart;
1355 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1357 //cerr << "1. Using mean elevation : " << elevationMean << " and " << slope << endl;
1359 WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), elevationMean+ 0.5, -(heading), slope );
1361 obj_trans->setMatrix( obj_pos );
1362 //osg::Vec3 center(0, 0, 0)
1364 float width = length /2.0;
1365 osg::Vec3 corner(-width, 0, 0.25f);
1366 osg::Vec3 widthVec(2*width + 1, 0, 0);
1367 osg::Vec3 heightVec(0, 1, 0);
1368 osg::Geometry* geometry;
1369 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1370 simgear::EffectGeode* geode = new simgear::EffectGeode;
1371 geode->setName("test");
1372 geode->addDrawable(geometry);
1373 //osg::Node *custom_obj;
1375 if (segments[pos]->hasBlock(now)) {
1376 mat = matlib->find("UnidirectionalTaperRed");
1378 mat = matlib->find("UnidirectionalTaperGreen");
1381 geode->setEffect(mat->get_effect());
1382 obj_trans->addChild(geode);
1383 // wire as much of the scene graph together as we can
1384 //->addChild( obj_trans );
1385 group->addChild( obj_trans );
1386 /////////////////////////////////////////////////////////////////////
1388 //cerr << "BIG FAT WARNING: current position is here : " << pos << endl;
1390 for (intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
1391 osg::Matrix obj_pos;
1394 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1395 obj_trans->setDataVariance(osg::Object::STATIC);
1397 // Experimental: Calculate slope here, based on length, and the individual elevations
1398 double elevationStart = segments[k]->getStart()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1399 double elevationEnd = segments[k]->getEnd ()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1400 if ((elevationStart == 0) || (elevationStart == parent->getElevation())) {
1401 SGGeod center2 = segments[k]->getStart()->geod();
1402 center2.setElevationM(SG_MAX_ELEVATION_M);
1403 if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) {
1404 // elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5;
1405 //elevation_meters += 0.5;
1408 elevationStart = parent->getElevation();
1410 segments[k]->getStart()->setElevation(elevationStart);
1412 if ((elevationEnd == 0) || (elevationEnd == parent->getElevation())) {
1413 SGGeod center2 = segments[k]->getEnd()->geod();
1414 center2.setElevationM(SG_MAX_ELEVATION_M);
1415 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1416 // elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1417 //elevation_meters += 0.5;
1420 elevationEnd = parent->getElevation();
1422 segments[k]->getEnd()->setElevation(elevationEnd);
1425 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1426 double elevDiff = elevationEnd - elevationStart;
1427 double length = segments[k]->getLength();
1428 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1430 // cerr << "2. Using mean elevation : " << elevationMean << " and " << slope << endl;
1432 SGGeod segCenter = segments[k]->getCenter();
1433 WorldCoordinate( obj_pos, segCenter.getLatitudeDeg(), segCenter.getLongitudeDeg(), elevationMean+ 0.5, -(segments[k]->getHeading()), slope );
1435 obj_trans->setMatrix( obj_pos );
1436 //osg::Vec3 center(0, 0, 0)
1438 float width = segments[k]->getLength() /2.0;
1439 osg::Vec3 corner(-width, 0, 0.25f);
1440 osg::Vec3 widthVec(2*width + 1, 0, 0);
1441 osg::Vec3 heightVec(0, 1, 0);
1442 osg::Geometry* geometry;
1443 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1444 simgear::EffectGeode* geode = new simgear::EffectGeode;
1445 geode->setName("test");
1446 geode->addDrawable(geometry);
1447 //osg::Node *custom_obj;
1449 if (segments[k]->hasBlock(now)) {
1450 mat = matlib->find("UnidirectionalTaperRed");
1452 mat = matlib->find("UnidirectionalTaperGreen");
1455 geode->setEffect(mat->get_effect());
1456 obj_trans->addChild(geode);
1457 // wire as much of the scene graph together as we can
1458 //->addChild( obj_trans );
1459 group->addChild( obj_trans );
1464 globals->get_scenery()->get_scene_graph()->addChild(group);
1468 string FGGroundNetwork::getName() {
1469 return string(parent->getId() + "-ground");
1472 void FGGroundNetwork::update(double dt)
1474 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1475 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1476 (*tsi)->unblock(now);
1479 //sort(activeTraffic.begin(), activeTraffic.end(), compare_trafficrecords);
1480 // Handle traffic that is under ground control first; this way we'll prevent clutter at the gate areas.
1481 // Don't allow an aircraft to pushback when a taxiing aircraft is currently using part of the intended route.
1482 for (TrafficVectorIterator i = parent->getDynamics()->getStartupController()->getActiveTraffic().begin();
1483 i != parent->getDynamics()->getStartupController()->getActiveTraffic().end(); i++) {
1485 i->setPriority(priority++);
1486 // in meters per second;
1487 double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1488 if (i->isActive(0)) {
1490 // Check for all active aircraft whether it's current pos segment is
1491 // an opposite of one of the departing aircraft's intentions
1492 for (TrafficVectorIterator j = activeTraffic.begin(); j != activeTraffic.end(); j++) {
1493 int pos = j->getCurrentPosition();
1495 FGTaxiSegment *seg = segments[pos-1]->opposite();
1497 int posReverse = seg->getIndex();
1498 for (intVecIterator k = i->getIntentions().begin(); k != i->getIntentions().end(); k++) {
1499 if ((*k) == posReverse) {
1501 segments[posReverse-1]->block(i->getId(), now, now);
1507 // if the current aircraft is still allowed to pushback, we can start reserving a route for if by blocking all the entry taxiways.
1508 if (i->pushBackAllowed()) {
1510 int pos = i->getCurrentPosition();
1512 FGTaxiSegment *seg = segments[pos-1];
1513 FGTaxiNode *node = seg->getEnd();
1514 length = seg->getLength();
1515 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1516 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1517 (*tsi)->block(i->getId(), now, now);
1521 for (intVecIterator j = i->getIntentions().begin(); j != i->getIntentions().end(); j++) {
1524 FGTaxiSegment *seg = segments[pos-1];
1525 FGTaxiNode *node = seg->getEnd();
1526 length += seg->getLength();
1527 time_t blockTime = now + (length / vTaxi);
1528 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1529 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1530 (*tsi)->block(i->getId(), blockTime-30, now);
1538 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1540 double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1541 i->setPriority(priority++);
1542 int pos = i->getCurrentPosition();
1544 length = segments[pos-1]->getLength();
1545 if (segments[pos-1]->hasBlock(now)) {
1546 //SG_LOG(SG_GENERAL, SG_ALERT, "Taxiway incursion for AI aircraft" << i->getAircraft()->getCallSign());
1551 for (ivi = i->getIntentions().begin(); ivi != i->getIntentions().end(); ivi++) {
1552 int segIndex = (*ivi);
1554 if (segments[segIndex-1]->hasBlock(now))
1558 //after this, ivi points just behind the last valid unblocked taxi segment.
1559 for (intVecIterator j = i->getIntentions().begin(); j != ivi; j++) {
1562 FGTaxiSegment *seg = segments[pos-1];
1563 FGTaxiNode *node = seg->getEnd();
1564 length += seg->getLength();
1565 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1566 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1567 time_t blockTime = now + (length / vTaxi);
1568 (*tsi)->block(i->getId(), blockTime - 30, now);