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.
31 #include <boost/foreach.hpp>
34 #include <osg/Geometry>
35 #include <osg/MatrixTransform>
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>
46 #include <Airports/airport.hxx>
47 #include <Airports/dynamics.hxx>
48 #include <Airports/runways.hxx>
50 #include <AIModel/AIAircraft.hxx>
51 #include <AIModel/performancedata.hxx>
52 #include <AIModel/AIFlightPlan.hxx>
53 #include <Navaids/NavDataCache.hxx>
55 #include <ATC/atc_mgr.hxx>
57 #include <Scenery/scenery.hxx>
59 #include "groundnetwork.hxx"
62 using flightgear::NavDataCache;
64 /***************************************************************************
66 **************************************************************************/
68 FGTaxiSegment::FGTaxiSegment(PositionedID aStart, PositionedID aEnd) :
77 SGGeod FGTaxiSegment::getCenter() const
79 FGTaxiNode* start(getStart()), *end(getEnd());
80 double heading, length, az2;
81 SGGeodesy::inverse(start->geod(), end->geod(), heading, az2, length);
82 return SGGeodesy::direct(start->geod(), heading, length * 0.5);
85 FGTaxiNodeRef FGTaxiSegment::getEnd() const
87 return FGPositioned::loadById<FGTaxiNode>(endNode);
90 FGTaxiNodeRef FGTaxiSegment::getStart() const
92 return FGPositioned::loadById<FGTaxiNode>(startNode);
95 double FGTaxiSegment::getLength() const
97 return dist(getStart()->cart(), getEnd()->cart());
100 double FGTaxiSegment::getHeading() const
102 return SGGeodesy::courseDeg(getStart()->geod(), getEnd()->geod());
106 void FGTaxiSegment::block(int id, time_t blockTime, time_t now)
108 BlockListIterator i = blockTimes.begin();
109 while (i != blockTimes.end()) {
110 if (i->getId() == id)
114 if (i == blockTimes.end()) {
115 blockTimes.push_back(Block(id, blockTime, now));
116 sort(blockTimes.begin(), blockTimes.end());
118 i->updateTimeStamps(blockTime, now);
122 // The segment has a block if any of the block times listed in the block list is
123 // smaller than the current time.
124 bool FGTaxiSegment::hasBlock(time_t now)
126 for (BlockListIterator i = blockTimes.begin(); i != blockTimes.end(); i++) {
127 if (i->getBlockTime() < now)
133 void FGTaxiSegment::unblock(time_t now)
135 if (blockTimes.size()) {
136 BlockListIterator i = blockTimes.begin();
137 if (i->getTimeStamp() < (now - 30)) {
145 /***************************************************************************
147 **************************************************************************/
148 bool FGTaxiRoute::next(PositionedID *nde)
150 if (currNode == nodes.end())
159 /***************************************************************************
161 **************************************************************************/
163 bool compare_trafficrecords(FGTrafficRecord a, FGTrafficRecord b)
165 return (a.getIntentions().size() < b.getIntentions().size());
168 FGGroundNetwork::FGGroundNetwork() :
176 currTraffic = activeTraffic.begin();
179 networkInitialized = false;
183 FGGroundNetwork::~FGGroundNetwork()
185 // JMT 2012-09-8 - disabling the groundnet-caching as part of enabling the
186 // navcache. The problem isn't the NavCache - it's that for the past few years,
187 // we have not being running destructors on FGPositioned classes, and hence,
188 // not running this code.
189 // When I fix FGPositioned lifetimes (unloading-at-runtime support), this
190 // will need to be re-visited so it can run safely during shutdown.
192 saveElevationCache();
194 BOOST_FOREACH(FGTaxiSegment* seg, segments) {
199 void FGGroundNetwork::saveElevationCache()
202 bool saveData = false;
204 if (fgGetBool("/sim/ai/groundnet-cache")) {
205 SGPath cacheData(globals->get_fg_home());
206 cacheData.append("ai");
207 string airport = parent->getId();
209 if ((airport) != "") {
211 ::snprintf(buffer, 128, "%c/%c/%c/",
212 airport[0], airport[1], airport[2]);
213 cacheData.append(buffer);
214 if (!cacheData.exists()) {
215 cacheData.create_dir(0755);
217 cacheData.append(airport + "-groundnet-cache.txt");
218 cachefile.open(cacheData.str().c_str());
222 cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl;
223 for (IndexTaxiNodeMap::iterator node = nodes.begin();
224 node != nodes.end(); node++) {
226 cachefile << node->second->getIndex () << " "
227 << node->second->getElevationM (parent->getElevation()*SG_FEET_TO_METER) << " "
237 void FGGroundNetwork::init(FGAirport* pr)
239 if (networkInitialized) {
240 FGATCController::init();
241 //cerr << "FGground network already initialized" << endl;
253 // establish pairing of segments
254 BOOST_FOREACH(FGTaxiSegment* segment, segments) {
255 segment->setIndex(index++);
257 if (segment->oppositeDirection) {
258 continue; // already establish
261 FGTaxiSegment* opp = findSegment(segment->endNode, segment->startNode);
263 assert(opp->oppositeDirection == NULL);
264 segment->oppositeDirection = opp;
265 opp->oppositeDirection = segment;
269 if (fgGetBool("/sim/ai/groundnet-cache")) {
273 networkInitialized = true;
276 void FGGroundNetwork::loadSegments()
278 flightgear::NavDataCache* cache = flightgear::NavDataCache::instance();
279 // iterate over all ground-net nodes in this airport
280 BOOST_FOREACH(PositionedID node, cache->groundNetNodes(parent->guid(), false)) {
281 // find all segments leaving the node
282 BOOST_FOREACH(PositionedID end, cache->groundNetEdgesFrom(node, false)) {
283 segments.push_back(new FGTaxiSegment(node, end));
288 void FGGroundNetwork::parseCache()
290 SGPath cacheData(globals->get_fg_home());
291 cacheData.append("ai");
292 string airport = parent->getId();
294 if (airport.empty()) {
299 ::snprintf(buffer, 128, "%c/%c/%c/",
300 airport[0], airport[1], airport[2]);
301 cacheData.append(buffer);
302 if (!cacheData.exists()) {
303 cacheData.create_dir(0755);
307 cacheData.append(airport + "-groundnet-cache.txt");
308 if (cacheData.exists()) {
309 ifstream data(cacheData.c_str());
312 if (revisionStr != "[GroundNetcachedata:ref:2011:09:04]") {
313 SG_LOG(SG_GENERAL, SG_ALERT,"GroundNetwork Warning: discarding outdated cachefile " <<
314 cacheData.c_str() << " for Airport " << airport);
316 for (IndexTaxiNodeMap::iterator i = nodes.begin();
319 i->second->setElevation(parent->elevation() * SG_FEET_TO_METER);
320 data >> index >> elev;
323 if (index != i->second->getIndex()) {
324 SG_LOG(SG_GENERAL, SG_ALERT, "Index read from ground network cache at airport " << airport << " does not match index in the network itself");
326 i->second->setElevation(elev);
334 int FGGroundNetwork::findNearestNode(const SGGeod & aGeod) const
336 const bool onRunway = false;
337 return NavDataCache::instance()->findGroundNetNode(parent->guid(), aGeod, onRunway);
340 int FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod, FGRunway* aRunway) const
342 const bool onRunway = true;
343 return NavDataCache::instance()->findGroundNetNode(parent->guid(), aGeod, onRunway, aRunway);
346 FGTaxiNodeRef FGGroundNetwork::findNode(PositionedID idx) const
348 return FGPositioned::loadById<FGTaxiNode>(idx);
351 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx) const
353 if ((idx > 0) && (idx <= segments.size()))
354 return segments[idx - 1];
356 //cerr << "Alert: trying to find invalid segment " << idx << endl;
361 FGTaxiSegment* FGGroundNetwork::findSegment(PositionedID from, PositionedID to) const
367 // completely boring linear search of segments. Can be improved if/when
368 // this ever becomes a hot-spot
369 BOOST_FOREACH(FGTaxiSegment* seg, segments) {
370 if (seg->startNode != from) {
374 if ((to == 0) || (seg->endNode == to)) {
379 return NULL; // not found
382 static int edgePenalty(FGTaxiNode* tn)
384 return (tn->type() == FGPositioned::PARKING ? 10000 : 0) +
385 (tn->getIsOnRunway() ? 1000 : 0);
388 class ShortestPathData
396 FGTaxiNodeRef previousNode;
399 FGTaxiRoute FGGroundNetwork::findShortestRoute(PositionedID start, PositionedID end,
402 //implements Dijkstra's algorithm to find shortest distance route from start to end
403 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
404 FGTaxiNodeVector unvisited;
405 flightgear::NavDataCache* cache = flightgear::NavDataCache::instance();
406 std::map<FGTaxiNode*, ShortestPathData> searchData;
408 BOOST_FOREACH(PositionedID n, cache->groundNetNodes(parent->guid(), !fullSearch)) {
409 unvisited.push_back(findNode(n));
412 FGTaxiNode *firstNode = findNode(start);
415 SG_LOG(SG_GENERAL, SG_ALERT,
416 "Error in ground network. Failed to find first waypoint: " << start
417 << " at " << ((parent) ? parent->getId() : "<unknown>"));
418 return FGTaxiRoute();
420 searchData[firstNode].score = 0.0;
422 FGTaxiNode *lastNode = findNode(end);
425 SG_LOG(SG_GENERAL, SG_ALERT,
426 "Error in ground network. Failed to find last waypoint: " << end
427 << " at " << ((parent) ? parent->getId() : "<unknown>"));
428 return FGTaxiRoute();
431 while (!unvisited.empty()) {
432 FGTaxiNode *best = unvisited.front();
433 BOOST_FOREACH(FGTaxiNode* i, unvisited) {
434 if (searchData[i].score < searchData[best].score) {
439 // remove 'best' from the unvisited set
440 FGTaxiNodeVectorIterator newend =
441 remove(unvisited.begin(), unvisited.end(), best);
442 unvisited.erase(newend, unvisited.end());
444 if (best == lastNode) { // found route or best not connected
448 BOOST_FOREACH(PositionedID targetId, cache->groundNetEdgesFrom(best->guid(), !fullSearch)) {
449 FGTaxiNodeRef tgt = FGPositioned::loadById<FGTaxiNode>(targetId);
450 double edgeLength = dist(best->cart(), tgt->cart());
451 double alt = searchData[best].score + edgeLength + edgePenalty(tgt);
452 if (alt < searchData[tgt].score) { // Relax (u,v)
453 searchData[tgt].score = alt;
454 searchData[tgt].previousNode = best;
456 } // of outgoing arcs/segments from current best node iteration
457 } // of unvisited nodes remaining
459 if (searchData[lastNode].score == HUGE_VAL) {
460 // no valid route found
462 SG_LOG(SG_GENERAL, SG_ALERT,
463 "Failed to find route from waypoint " << start << " to "
464 << end << " at " << parent->getId());
467 return FGTaxiRoute();
470 // assemble route from backtrace information
471 PositionedIDVec nodes;
472 FGTaxiNode *bt = lastNode;
473 while (searchData[bt].previousNode != 0) {
474 nodes.push_back(bt->guid());
475 bt = searchData[bt].previousNode;
477 nodes.push_back(start);
478 reverse(nodes.begin(), nodes.end());
479 return FGTaxiRoute(nodes, searchData[lastNode].score, 0);
482 /* ATC Related Functions */
484 void FGGroundNetwork::announcePosition(int id,
485 FGAIFlightPlan * intendedRoute,
486 int currentPosition, double lat,
487 double lon, double heading,
488 double speed, double alt,
489 double radius, int leg,
490 FGAIAircraft * aircraft)
494 TrafficVectorIterator i = activeTraffic.begin();
495 // Search search if the current id alread has an entry
496 // This might be faster using a map instead of a vector, but let's start by taking a safe route
497 if (activeTraffic.size()) {
498 //while ((i->getId() != id) && i != activeTraffic.end()) {
499 while (i != activeTraffic.end()) {
500 if (i->getId() == id) {
506 // Add a new TrafficRecord if no one exsists for this aircraft.
507 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
511 rec.setPositionAndIntentions(currentPosition, intendedRoute);
512 rec.setPositionAndHeading(lat, lon, heading, speed, alt);
513 rec.setRadius(radius); // only need to do this when creating the record.
514 rec.setAircraft(aircraft);
516 activeTraffic.push_front(rec);
518 activeTraffic.push_back(rec);
522 i->setPositionAndIntentions(currentPosition, intendedRoute);
523 i->setPositionAndHeading(lat, lon, heading, speed, alt);
528 void FGGroundNetwork::signOff(int id)
530 TrafficVectorIterator i = activeTraffic.begin();
531 // Search search if the current id alread has an entry
532 // This might be faster using a map instead of a vector, but let's start by taking a safe route
533 if (activeTraffic.size()) {
534 //while ((i->getId() != id) && i != activeTraffic.end()) {
535 while (i != activeTraffic.end()) {
536 if (i->getId() == id) {
542 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
543 SG_LOG(SG_GENERAL, SG_ALERT,
544 "AI error: Aircraft without traffic record is signing off at " << SG_ORIGIN);
546 i = activeTraffic.erase(i);
550 * The ground network can deal with the following states:
551 * 0 = Normal; no action required
552 * 1 = "Acknowledge "Hold position
553 * 2 = "Acknowledge "Resume taxi".
554 * 3 = "Issue TaxiClearance"
555 * 4 = Acknowledge Taxi Clearance"
556 * 5 = Post acknowlegde taxiclearance: Start taxiing
558 * 7 = Acknowledge report runway
559 * 8 = Switch tower frequency
560 * 9 = Acknowledge switch tower frequency
561 *************************************************************************************************************************/
562 bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId,
565 int state = i->getState();
566 if ((state >= minState) && (state <= maxState) && available) {
567 if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) {
568 //cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl;
569 SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true);
570 int n = trans_num->getIntValue();
572 trans_num->setIntValue(-1);
574 //cerr << "Selected transmission message " << n << endl;
575 //FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc");
576 FGATCDialogNew::instance()->removeEntry(1);
578 //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl;
579 transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, false);
583 transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, true);
585 lastTransmission = now;
592 void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
593 double heading, double speed, double alt,
596 time_t currentTime = time(NULL);
597 if (nextSave < currentTime) {
598 saveElevationCache();
599 nextSave = currentTime + 100 + rand() % 200;
601 // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
602 // Transmit air-to-ground "Ready to taxi request:
603 // Transmit ground to air approval / hold
604 // Transmit confirmation ...
605 // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
608 TrafficVectorIterator i = activeTraffic.begin();
609 // Search search if the current id has an entry
610 // This might be faster using a map instead of a vector, but let's start by taking a safe route
611 TrafficVectorIterator current, closest;
612 if (activeTraffic.size()) {
613 //while ((i->getId() != id) && i != activeTraffic.end()) {
614 while (i != activeTraffic.end()) {
615 if (i->getId() == id) {
621 // update position of the current aircraft
622 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
623 SG_LOG(SG_GENERAL, SG_ALERT,
624 "AI error: updating aircraft without traffic record at " << SG_ORIGIN);
626 i->setPositionAndHeading(lat, lon, heading, speed, alt);
632 // Update every three secs, but add some randomness
633 // to prevent all IA objects doing this in synchrony
634 //if (getDt() < (3.0) + (rand() % 10))
638 current->clearResolveCircularWait();
639 current->setWaitsForId(0);
640 checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
641 bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
642 if (!needsTaxiClearance) {
643 checkHoldPosition(id, lat, lon, heading, speed, alt);
644 //if (checkForCircularWaits(id)) {
645 // i->setResolveCircularWait();
648 current->setHoldPosition(true);
649 int state = current->getState();
650 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
651 if ((now - lastTransmission) > 15) {
654 if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
655 current->setState(3);
657 if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) {
658 current->setState(4);
660 if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
661 current->setState(5);
663 if ((state == 5) && available) {
664 current->setState(0);
665 current->getAircraft()->setTaxiClearanceRequest(false);
666 current->setHoldPosition(false);
674 Scan for a speed adjustment change. Find the nearest aircraft that is in front
675 and adjust speed when we get too close. Only do this when current position and/or
676 intentions of the current aircraft match current taxiroute position of the proximate
677 aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
678 instruction. See below for the hold position instruction.
680 Note that there currently still is one flaw in the logic that needs to be addressed.
681 There can be situations where one aircraft is in front of the current aircraft, on a separate
682 route, but really close after an intersection coming off the current route. This
683 aircraft is still close enough to block the current aircraft. This situation is currently
684 not addressed yet, but should be.
687 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
688 double lon, double heading,
689 double speed, double alt)
692 TrafficVectorIterator current, closest, closestOnNetwork;
693 TrafficVectorIterator i = activeTraffic.begin();
694 bool otherReasonToSlowDown = false;
695 // bool previousInstruction;
696 if (activeTraffic.size()) {
697 //while ((i->getId() != id) && (i != activeTraffic.end()))
698 while (i != activeTraffic.end()) {
699 if (i->getId() == id) {
707 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
708 SG_LOG(SG_GENERAL, SG_ALERT,
709 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment at " << SG_ORIGIN);
714 // previousInstruction = current->getSpeedAdjustment();
715 double mindist = HUGE_VAL;
716 if (activeTraffic.size()) {
717 double course, dist, bearing, az2; // minbearing,
718 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
719 //TrafficVector iterator closest;
721 closestOnNetwork = current;
722 for (TrafficVectorIterator i = activeTraffic.begin();
723 i != activeTraffic.end(); i++) {
728 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
731 SGGeodesy::inverse(curr, other, course, az2, dist);
732 bearing = fabs(heading - course);
734 bearing = 360 - bearing;
735 if ((dist < mindist) && (bearing < 60.0)) {
738 closestOnNetwork = i;
739 // minbearing = bearing;
743 //Check traffic at the tower controller
744 if (towerController->hasActiveTraffic()) {
745 for (TrafficVectorIterator i =
746 towerController->getActiveTraffic().begin();
747 i != towerController->getActiveTraffic().end(); i++) {
748 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
749 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
752 SGGeodesy::inverse(curr, other, course, az2, dist);
753 bearing = fabs(heading - course);
755 bearing = 360 - bearing;
756 if ((dist < mindist) && (bearing < 60.0)) {
757 //cerr << "Current aircraft " << current->getAircraft()->getTrafficRef()->getCallSign()
758 // << " is closest to " << i->getAircraft()->getTrafficRef()->getCallSign()
759 // << ", which has status " << i->getAircraft()->isScheduledForTakeoff()
763 // minbearing = bearing;
764 otherReasonToSlowDown = true;
768 // Finally, check UserPosition
769 // Note, as of 2011-08-01, this should no longer be necessecary.
771 double userLatitude = fgGetDouble("/position/latitude-deg");
772 double userLongitude = fgGetDouble("/position/longitude-deg");
773 SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
774 SGGeodesy::inverse(curr, user, course, az2, dist);
776 bearing = fabs(heading - course);
778 bearing = 360 - bearing;
779 if ((dist < mindist) && (bearing < 60.0)) {
782 minbearing = bearing;
783 otherReasonToSlowDown = true;
786 current->clearSpeedAdjustment();
787 bool needBraking = false;
788 if (current->checkPositionAndIntentions(*closest)
789 || otherReasonToSlowDown) {
790 double maxAllowableDistance =
791 (1.1 * current->getRadius()) +
792 (1.1 * closest->getRadius());
793 if (mindist < 2 * maxAllowableDistance) {
794 if (current->getId() == closest->getWaitsForId())
797 current->setWaitsForId(closest->getId());
798 if (closest->getId() != current->getId()) {
799 current->setSpeedAdjustment(closest->getSpeed() *
804 // closest->getAircraft()->getTakeOffStatus() &&
805 // (current->getAircraft()->getTrafficRef()->getDepartureAirport() == closest->getAircraft()->getTrafficRef()->getDepartureAirport()) &&
806 // (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway())
808 // current->getAircraft()->scheduleForATCTowerDepartureControl(1);
810 current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
812 if (mindist < maxAllowableDistance) {
813 //double newSpeed = (maxAllowableDistance-mindist);
814 //current->setSpeedAdjustment(newSpeed);
815 //if (mindist < 0.5* maxAllowableDistance)
817 current->setSpeedAdjustment(0);
822 if ((closest->getId() == closestOnNetwork->getId()) && (current->getPriority() < closest->getPriority()) && needBraking) {
823 swap(current, closest);
829 Check for "Hold position instruction".
830 The hold position should be issued under the following conditions:
831 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
832 2) For taxiing aircraft that use one taxiway in opposite directions
833 3) For crossing or merging taxiroutes.
836 void FGGroundNetwork::checkHoldPosition(int id, double lat,
837 double lon, double heading,
838 double speed, double alt)
840 TrafficVectorIterator current;
841 TrafficVectorIterator i = activeTraffic.begin();
842 if (activeTraffic.size()) {
843 //while ((i->getId() != id) && i != activeTraffic.end())
844 while (i != activeTraffic.end()) {
845 if (i->getId() == id) {
853 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
854 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
855 SG_LOG(SG_GENERAL, SG_ALERT,
856 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition at " << SG_ORIGIN);
860 if (current->getAircraft()->getTakeOffStatus() == 1) {
861 current->setHoldPosition(true);
864 if (current->getAircraft()->getTakeOffStatus() == 2) {
865 //cerr << current->getAircraft()->getCallSign() << ". Taxi in position and hold" << endl;
866 current->setHoldPosition(false);
867 current->clearSpeedAdjustment();
870 bool origStatus = current->hasHoldPosition();
871 current->setHoldPosition(false);
872 //SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
873 int currentRoute = i->getCurrentPosition();
875 if (i->getIntentions().size()) {
876 nextRoute = (*(i->getIntentions().begin()));
880 if (currentRoute > 0) {
881 FGTaxiSegment *tx = findSegment(currentRoute);
884 nx = findSegment(nextRoute);
888 //if (tx->hasBlock(now) || nx->hasBlock(now) ) {
889 // current->setHoldPosition(true);
891 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
892 SGGeod end (nx->getStart()->geod());
894 double distance = SGGeodesy::distanceM(start, end);
895 if (nx->hasBlock(now) && (distance < i->getRadius() * 4)) {
896 current->setHoldPosition(true);
898 intVecIterator ivi = i->getIntentions().begin();
899 while (ivi != i->getIntentions().end()) {
901 distance += segments[(*ivi)-1]->getLength();
902 if ((segments[(*ivi)-1]->hasBlock(now)) && (distance < i->getRadius() * 4)) {
903 current->setHoldPosition(true);
911 bool currStatus = current->hasHoldPosition();
912 current->setHoldPosition(origStatus);
913 // Either a Hold Position or a resume taxi transmission has been issued
914 if ((now - lastTransmission) > 2) {
917 if (current->getState() == 0) {
918 if ((origStatus != currStatus) && available) {
919 //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
920 if (currStatus == true) { // No has a hold short instruction
921 transmit(&(*current), &(*parent->getDynamics()), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
922 //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
923 current->setState(1);
925 transmit(&(*current), &(*parent->getDynamics()), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
926 //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
927 current->setState(2);
929 lastTransmission = now;
931 // Don't act on the changed instruction until the transmission is confirmed
932 // So set back to original status
933 //cerr << "Current state " << current->getState() << endl;
938 // 7 = Acknowledge report runway
939 // 8 = Switch tower frequency
940 //9 = Acknowledge switch tower frequency
942 //int state = current->getState();
943 if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
944 current->setState(0);
945 current->setHoldPosition(true);
947 if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
948 current->setState(0);
949 current->setHoldPosition(false);
951 if (current->getAircraft()->getTakeOffStatus() && (current->getState() == 0)) {
952 //cerr << "Scheduling " << current->getAircraft()->getCallSign() << " for hold short" << endl;
953 current->setState(6);
955 if (checkTransmissionState(6,6, current, now, MSG_REPORT_RUNWAY_HOLD_SHORT, ATC_AIR_TO_GROUND)) {
957 if (checkTransmissionState(7,7, current, now, MSG_ACKNOWLEDGE_REPORT_RUNWAY_HOLD_SHORT, ATC_GROUND_TO_AIR)) {
959 if (checkTransmissionState(8,8, current, now, MSG_SWITCH_TOWER_FREQUENCY, ATC_GROUND_TO_AIR)) {
961 if (checkTransmissionState(9,9, current, now, MSG_ACKNOWLEDGE_SWITCH_TOWER_FREQUENCY, ATC_AIR_TO_GROUND)) {
966 //current->setState(0);
970 * Check whether situations occur where the current aircraft is waiting for itself
971 * due to higher order interactions.
972 * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
973 * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
974 * through this list of waiting aircraft, we can check if we'd eventually end back
975 * at the current aircraft.
977 * Note that we should consider the situation where we are actually checking aircraft
978 * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
979 * the looping aircraft. If we don't check for that, this function will get stuck into
983 bool FGGroundNetwork::checkForCircularWaits(int id)
985 //cerr << "Performing Wait check " << id << endl;
987 TrafficVectorIterator current, other;
988 TrafficVectorIterator i = activeTraffic.begin();
989 int trafficSize = activeTraffic.size();
991 while (i != activeTraffic.end()) {
992 if (i->getId() == id) {
1000 if (i == activeTraffic.end() || (trafficSize == 0)) {
1001 SG_LOG(SG_GENERAL, SG_ALERT,
1002 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits at " << SG_ORIGIN);
1006 target = current->getWaitsForId();
1007 //bool printed = false; // Note that this variable is for debugging purposes only.
1011 //cerr << "aircraft waits for user" << endl;
1016 while ((target > 0) && (target != id) && counter++ < trafficSize) {
1018 TrafficVectorIterator i = activeTraffic.begin();
1020 //while ((i->getId() != id) && i != activeTraffic.end())
1021 while (i != activeTraffic.end()) {
1022 if (i->getId() == target) {
1030 if (i == activeTraffic.end() || (trafficSize == 0)) {
1031 //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
1032 // The target id is not found on the current network, which means it's at the tower
1033 //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1037 target = other->getWaitsForId();
1039 // actually this trap isn't as impossible as it first seemed:
1040 // the setWaitsForID(id) is set to current when the aircraft
1041 // is waiting for the user controlled aircraft.
1042 //if (current->getId() == other->getId()) {
1043 // cerr << "Caught the impossible trap" << endl;
1044 // cerr << "Current = " << current->getId() << endl;
1045 // cerr << "Other = " << other ->getId() << endl;
1046 // for (TrafficVectorIterator at = activeTraffic.begin();
1047 // at != activeTraffic.end();
1049 // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1052 if (current->getId() == other->getId())
1055 //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign()
1056 // << " (" << other->getId() << "); " << endl;;
1066 // cerr << "[done] " << endl << endl;;
1068 SG_LOG(SG_GENERAL, SG_WARN,
1069 "Detected circular wait condition: Id = " << id <<
1070 "target = " << target);
1077 // Note that this function is probably obsolete...
1078 bool FGGroundNetwork::hasInstruction(int id)
1080 TrafficVectorIterator i = activeTraffic.begin();
1081 // Search search if the current id has an entry
1082 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1083 if (activeTraffic.size()) {
1084 //while ((i->getId() != id) && i != activeTraffic.end()) {
1085 while (i != activeTraffic.end()) {
1086 if (i->getId() == id) {
1092 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1093 SG_LOG(SG_GENERAL, SG_ALERT,
1094 "AI error: checking ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1096 return i->hasInstruction();
1101 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1103 TrafficVectorIterator i = activeTraffic.begin();
1104 // Search search if the current id has an entry
1105 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1106 if (activeTraffic.size()) {
1107 //while ((i->getId() != id) && i != activeTraffic.end()) {
1108 while (i != activeTraffic.end()) {
1109 if (i->getId() == id) {
1115 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1116 SG_LOG(SG_GENERAL, SG_ALERT,
1117 "AI error: requesting ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1119 return i->getInstruction();
1121 return FGATCInstruction();
1124 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
1125 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
1126 double lon, double elev, double hdg, double slope)
1128 SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
1129 obj_pos = makeZUpFrame(geod);
1130 // hdg is not a compass heading, but a counter-clockwise rotation
1131 // around the Z axis
1132 obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
1134 obj_pos.preMult(osg::Matrix::rotate(slope * SGD_DEGREES_TO_RADIANS,
1141 void FGGroundNetwork::render(bool visible)
1144 SGMaterialLib *matlib = globals->get_matlib();
1147 globals->get_scenery()->get_scene_graph()->removeChild(group);
1148 //while (group->getNumChildren()) {
1149 // cerr << "Number of children: " << group->getNumChildren() << endl;
1150 //simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
1151 //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
1152 //geode->releaseGLObjects();
1153 //group->removeChild(geode);
1158 group = new osg::Group;
1159 FGScenery * local_scenery = globals->get_scenery();
1160 // double elevation_meters = 0.0;
1161 // double elevation_feet = 0.0;
1162 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1163 //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
1165 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1166 // Handle start point
1167 int pos = i->getCurrentPosition() - 1;
1170 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1171 SGGeod end (segments[pos]->getEnd()->geod());
1173 double length = SGGeodesy::distanceM(start, end);
1174 //heading = SGGeodesy::headingDeg(start->geod(), end->geod());
1176 double az2, heading; //, distanceM;
1177 SGGeodesy::inverse(start, end, heading, az2, length);
1178 double coveredDistance = length * 0.5;
1180 SGGeodesy::direct(start, heading, coveredDistance, center, az2);
1181 //cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
1182 ///////////////////////////////////////////////////////////////////////////////
1183 // Make a helper function out of this
1184 osg::Matrix obj_pos;
1185 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1186 obj_trans->setDataVariance(osg::Object::STATIC);
1187 // Experimental: Calculate slope here, based on length, and the individual elevations
1188 double elevationStart;
1189 if (isUserAircraft((i)->getAircraft())) {
1190 elevationStart = fgGetDouble("/position/ground-elev-m");
1192 elevationStart = ((i)->getAircraft()->_getAltitude());
1194 double elevationEnd = segments[pos]->getEnd()->getElevationM();
1195 //cerr << "Using elevation " << elevationEnd << endl;
1197 if ((elevationEnd == 0) || (elevationEnd = parent->getElevation())) {
1198 SGGeod center2 = end;
1199 center2.setElevationM(SG_MAX_ELEVATION_M);
1200 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1201 // elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1202 //elevation_meters += 0.5;
1205 elevationEnd = parent->getElevation();
1207 segments[pos]->getEnd()->setElevation(elevationEnd);
1209 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1210 double elevDiff = elevationEnd - elevationStart;
1212 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1214 //cerr << "1. Using mean elevation : " << elevationMean << " and " << slope << endl;
1216 WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), elevationMean+ 0.5, -(heading), slope );
1218 obj_trans->setMatrix( obj_pos );
1219 //osg::Vec3 center(0, 0, 0)
1221 float width = length /2.0;
1222 osg::Vec3 corner(-width, 0, 0.25f);
1223 osg::Vec3 widthVec(2*width + 1, 0, 0);
1224 osg::Vec3 heightVec(0, 1, 0);
1225 osg::Geometry* geometry;
1226 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1227 simgear::EffectGeode* geode = new simgear::EffectGeode;
1228 geode->setName("test");
1229 geode->addDrawable(geometry);
1230 //osg::Node *custom_obj;
1232 if (segments[pos]->hasBlock(now)) {
1233 mat = matlib->find("UnidirectionalTaperRed", center);
1235 mat = matlib->find("UnidirectionalTaperGreen", center);
1238 geode->setEffect(mat->get_effect());
1239 obj_trans->addChild(geode);
1240 // wire as much of the scene graph together as we can
1241 //->addChild( obj_trans );
1242 group->addChild( obj_trans );
1243 /////////////////////////////////////////////////////////////////////
1245 //cerr << "BIG FAT WARNING: current position is here : " << pos << endl;
1247 for (intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
1248 osg::Matrix obj_pos;
1251 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1252 obj_trans->setDataVariance(osg::Object::STATIC);
1254 // Experimental: Calculate slope here, based on length, and the individual elevations
1255 double elevationStart = segments[k]->getStart()->getElevationM();
1256 double elevationEnd = segments[k]->getEnd ()->getElevationM();
1257 if ((elevationStart == 0) || (elevationStart == parent->getElevation())) {
1258 SGGeod center2 = segments[k]->getStart()->geod();
1259 center2.setElevationM(SG_MAX_ELEVATION_M);
1260 if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) {
1261 // elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5;
1262 //elevation_meters += 0.5;
1265 elevationStart = parent->getElevation();
1267 segments[k]->getStart()->setElevation(elevationStart);
1269 if ((elevationEnd == 0) || (elevationEnd == parent->getElevation())) {
1270 SGGeod center2 = segments[k]->getEnd()->geod();
1271 center2.setElevationM(SG_MAX_ELEVATION_M);
1272 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1273 // elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1274 //elevation_meters += 0.5;
1277 elevationEnd = parent->getElevation();
1279 segments[k]->getEnd()->setElevation(elevationEnd);
1282 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1283 double elevDiff = elevationEnd - elevationStart;
1284 double length = segments[k]->getLength();
1285 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1287 // cerr << "2. Using mean elevation : " << elevationMean << " and " << slope << endl;
1289 SGGeod segCenter = segments[k]->getCenter();
1290 WorldCoordinate( obj_pos, segCenter.getLatitudeDeg(), segCenter.getLongitudeDeg(), elevationMean+ 0.5, -(segments[k]->getHeading()), slope );
1292 obj_trans->setMatrix( obj_pos );
1293 //osg::Vec3 center(0, 0, 0)
1295 float width = segments[k]->getLength() /2.0;
1296 osg::Vec3 corner(-width, 0, 0.25f);
1297 osg::Vec3 widthVec(2*width + 1, 0, 0);
1298 osg::Vec3 heightVec(0, 1, 0);
1299 osg::Geometry* geometry;
1300 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1301 simgear::EffectGeode* geode = new simgear::EffectGeode;
1302 geode->setName("test");
1303 geode->addDrawable(geometry);
1304 //osg::Node *custom_obj;
1306 if (segments[k]->hasBlock(now)) {
1307 mat = matlib->find("UnidirectionalTaperRed", segCenter);
1309 mat = matlib->find("UnidirectionalTaperGreen", segCenter);
1312 geode->setEffect(mat->get_effect());
1313 obj_trans->addChild(geode);
1314 // wire as much of the scene graph together as we can
1315 //->addChild( obj_trans );
1316 group->addChild( obj_trans );
1321 globals->get_scenery()->get_scene_graph()->addChild(group);
1325 string FGGroundNetwork::getName() {
1326 return string(parent->getId() + "-ground");
1329 void FGGroundNetwork::update(double dt)
1331 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1332 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1333 (*tsi)->unblock(now);
1336 //sort(activeTraffic.begin(), activeTraffic.end(), compare_trafficrecords);
1337 // Handle traffic that is under ground control first; this way we'll prevent clutter at the gate areas.
1338 // Don't allow an aircraft to pushback when a taxiing aircraft is currently using part of the intended route.
1339 for (TrafficVectorIterator i = parent->getDynamics()->getStartupController()->getActiveTraffic().begin();
1340 i != parent->getDynamics()->getStartupController()->getActiveTraffic().end(); i++) {
1342 i->setPriority(priority++);
1343 // in meters per second;
1344 double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1345 if (i->isActive(0)) {
1347 // Check for all active aircraft whether it's current pos segment is
1348 // an opposite of one of the departing aircraft's intentions
1349 for (TrafficVectorIterator j = activeTraffic.begin(); j != activeTraffic.end(); j++) {
1350 int pos = j->getCurrentPosition();
1352 FGTaxiSegment *seg = segments[pos-1]->opposite();
1354 int posReverse = seg->getIndex();
1355 for (intVecIterator k = i->getIntentions().begin(); k != i->getIntentions().end(); k++) {
1356 if ((*k) == posReverse) {
1358 segments[posReverse-1]->block(i->getId(), now, now);
1364 // if the current aircraft is still allowed to pushback, we can start reserving a route for if by blocking all the entry taxiways.
1365 if (i->pushBackAllowed()) {
1367 int pos = i->getCurrentPosition();
1369 FGTaxiSegment *seg = segments[pos-1];
1370 FGTaxiNode *node = seg->getEnd();
1371 length = seg->getLength();
1372 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1373 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1374 (*tsi)->block(i->getId(), now, now);
1378 for (intVecIterator j = i->getIntentions().begin(); j != i->getIntentions().end(); j++) {
1381 FGTaxiSegment *seg = segments[pos-1];
1382 FGTaxiNode *node = seg->getEnd();
1383 length += seg->getLength();
1384 time_t blockTime = now + (length / vTaxi);
1385 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1386 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1387 (*tsi)->block(i->getId(), blockTime-30, now);
1395 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1397 double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1398 i->setPriority(priority++);
1399 int pos = i->getCurrentPosition();
1401 length = segments[pos-1]->getLength();
1402 if (segments[pos-1]->hasBlock(now)) {
1403 //SG_LOG(SG_GENERAL, SG_ALERT, "Taxiway incursion for AI aircraft" << i->getAircraft()->getCallSign());
1408 for (ivi = i->getIntentions().begin(); ivi != i->getIntentions().end(); ivi++) {
1409 int segIndex = (*ivi);
1411 if (segments[segIndex-1]->hasBlock(now))
1415 //after this, ivi points just behind the last valid unblocked taxi segment.
1416 for (intVecIterator j = i->getIntentions().begin(); j != ivi; j++) {
1419 FGTaxiSegment *seg = segments[pos-1];
1420 FGTaxiNode *node = seg->getEnd();
1421 length += seg->getLength();
1422 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1423 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1424 time_t blockTime = now + (length / vTaxi);
1425 (*tsi)->block(i->getId(), blockTime - 30, now);