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, int *rte)
150 if (nodes.size() != (routes.size()) + 1) {
151 SG_LOG(SG_GENERAL, SG_ALERT, "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size());
152 throw sg_range_exception("Misconfigured taxi route");
154 if (currNode == nodes.end())
157 if (currNode != nodes.begin()) {
161 // Handle special case for the first node.
162 *rte = -1 * *(currRoute);
168 /***************************************************************************
170 **************************************************************************/
172 bool compare_trafficrecords(FGTrafficRecord a, FGTrafficRecord b)
174 return (a.getIntentions().size() < b.getIntentions().size());
177 FGGroundNetwork::FGGroundNetwork() :
185 currTraffic = activeTraffic.begin();
188 networkInitialized = false;
192 FGGroundNetwork::~FGGroundNetwork()
194 // JMT 2012-09-8 - disabling the groundnet-caching as part of enabling the
195 // navcache. The problem isn't the NavCache - it's that for the past few years,
196 // we have not being running destructors on FGPositioned classes, and hence,
197 // not running this code.
198 // When I fix FGPositioned lifetimes (unloading-at-runtime support), this
199 // will need to be re-visited so it can run safely during shutdown.
201 saveElevationCache();
203 BOOST_FOREACH(FGTaxiSegment* seg, segments) {
208 void FGGroundNetwork::saveElevationCache()
211 bool saveData = false;
213 if (fgGetBool("/sim/ai/groundnet-cache")) {
214 SGPath cacheData(globals->get_fg_home());
215 cacheData.append("ai");
216 string airport = parent->getId();
218 if ((airport) != "") {
220 ::snprintf(buffer, 128, "%c/%c/%c/",
221 airport[0], airport[1], airport[2]);
222 cacheData.append(buffer);
223 if (!cacheData.exists()) {
224 cacheData.create_dir(0755);
226 cacheData.append(airport + "-groundnet-cache.txt");
227 cachefile.open(cacheData.str().c_str());
231 cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl;
232 for (IndexTaxiNodeMap::iterator node = nodes.begin();
233 node != nodes.end(); node++) {
235 cachefile << node->second->getIndex () << " "
236 << node->second->getElevationM (parent->getElevation()*SG_FEET_TO_METER) << " "
246 void FGGroundNetwork::init(FGAirport* pr)
248 if (networkInitialized) {
249 FGATCController::init();
250 //cerr << "FGground network already initialized" << endl;
262 // establish pairing of segments
263 BOOST_FOREACH(FGTaxiSegment* segment, segments) {
264 segment->setIndex(index++);
266 if (segment->oppositeDirection) {
267 continue; // already established
270 FGTaxiSegment* opp = findSegment(segment->endNode, segment->startNode);
272 assert(opp->oppositeDirection == NULL);
273 segment->oppositeDirection = opp;
274 opp->oppositeDirection = segment;
278 if (fgGetBool("/sim/ai/groundnet-cache")) {
282 networkInitialized = true;
285 void FGGroundNetwork::loadSegments()
287 flightgear::NavDataCache* cache = flightgear::NavDataCache::instance();
288 // iterate over all ground-net nodes in this airport
289 BOOST_FOREACH(PositionedID node, cache->groundNetNodes(parent->guid(), false)) {
290 // find all segments leaving the node
291 BOOST_FOREACH(PositionedID end, cache->groundNetEdgesFrom(node, false)) {
292 segments.push_back(new FGTaxiSegment(node, end));
297 void FGGroundNetwork::parseCache()
299 SGPath cacheData(globals->get_fg_home());
300 cacheData.append("ai");
301 string airport = parent->getId();
303 if (airport.empty()) {
308 ::snprintf(buffer, 128, "%c/%c/%c/",
309 airport[0], airport[1], airport[2]);
310 cacheData.append(buffer);
311 if (!cacheData.exists()) {
312 cacheData.create_dir(0755);
316 cacheData.append(airport + "-groundnet-cache.txt");
317 if (cacheData.exists()) {
318 ifstream data(cacheData.c_str());
321 if (revisionStr != "[GroundNetcachedata:ref:2011:09:04]") {
322 SG_LOG(SG_GENERAL, SG_ALERT,"GroundNetwork Warning: discarding outdated cachefile " <<
323 cacheData.c_str() << " for Airport " << airport);
325 for (IndexTaxiNodeMap::iterator i = nodes.begin();
328 i->second->setElevation(parent->elevation() * SG_FEET_TO_METER);
329 data >> index >> elev;
332 if (index != i->second->getIndex()) {
333 SG_LOG(SG_GENERAL, SG_ALERT, "Index read from ground network cache at airport " << airport << " does not match index in the network itself");
335 i->second->setElevation(elev);
343 int FGGroundNetwork::findNearestNode(const SGGeod & aGeod) const
345 const bool onRunway = false;
346 return NavDataCache::instance()->findGroundNetNode(parent->guid(), aGeod, onRunway);
349 int FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod, FGRunway* aRunway) const
351 const bool onRunway = true;
352 return NavDataCache::instance()->findGroundNetNode(parent->guid(), aGeod, onRunway, aRunway);
355 FGTaxiNodeRef FGGroundNetwork::findNode(PositionedID idx) const
357 return FGPositioned::loadById<FGTaxiNode>(idx);
360 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx) const
362 if ((idx > 0) && (idx <= segments.size()))
363 return segments[idx - 1];
365 //cerr << "Alert: trying to find invalid segment " << idx << endl;
370 FGTaxiSegment* FGGroundNetwork::findSegment(PositionedID from, PositionedID to) const
376 // completely boring linear search of segments. Can be improved if/when
377 // this ever becomes a hot-spot
378 BOOST_FOREACH(FGTaxiSegment* seg, segments) {
379 if (seg->startNode != from) {
383 if ((to == 0) || (seg->endNode == to)) {
388 return NULL; // not found
391 static int edgePenalty(FGTaxiNode* tn)
393 return (tn->type() == FGPositioned::PARKING ? 10000 : 0) +
394 (tn->getIsOnRunway() ? 1000 : 0);
397 class ShortestPathData
405 FGTaxiNodeRef previousNode;
408 FGTaxiRoute FGGroundNetwork::findShortestRoute(PositionedID start, PositionedID end,
411 //implements Dijkstra's algorithm to find shortest distance route from start to end
412 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
413 FGTaxiNodeVector unvisited;
414 flightgear::NavDataCache* cache = flightgear::NavDataCache::instance();
415 std::map<FGTaxiNode*, ShortestPathData> searchData;
417 BOOST_FOREACH(PositionedID n, cache->groundNetNodes(parent->guid(), !fullSearch)) {
418 unvisited.push_back(findNode(n));
421 FGTaxiNode *firstNode = findNode(start);
424 SG_LOG(SG_GENERAL, SG_ALERT,
425 "Error in ground network. Failed to find first waypoint: " << start
426 << " at " << ((parent) ? parent->getId() : "<unknown>"));
427 return FGTaxiRoute();
429 searchData[firstNode].score = 0.0;
431 FGTaxiNode *lastNode = findNode(end);
434 SG_LOG(SG_GENERAL, SG_ALERT,
435 "Error in ground network. Failed to find last waypoint: " << end
436 << " at " << ((parent) ? parent->getId() : "<unknown>"));
437 return FGTaxiRoute();
440 while (!unvisited.empty()) {
441 FGTaxiNode *best = unvisited.front();
442 BOOST_FOREACH(FGTaxiNode* i, unvisited) {
443 if (searchData[i].score < searchData[best].score) {
448 // remove 'best' from the unvisited set
449 FGTaxiNodeVectorIterator newend =
450 remove(unvisited.begin(), unvisited.end(), best);
451 unvisited.erase(newend, unvisited.end());
453 if (best == lastNode) { // found route or best not connected
457 BOOST_FOREACH(PositionedID targetId, cache->groundNetEdgesFrom(best->guid(), !fullSearch)) {
458 FGTaxiNodeRef tgt = FGPositioned::loadById<FGTaxiNode>(targetId);
459 double edgeLength = dist(best->cart(), tgt->cart());
460 double alt = searchData[best].score + edgeLength + edgePenalty(tgt);
461 if (alt < searchData[tgt].score) { // Relax (u,v)
462 searchData[tgt].score = alt;
463 searchData[tgt].previousNode = best;
465 } // of outgoing arcs/segments from current best node iteration
466 } // of unvisited nodes remaining
468 if (searchData[lastNode].score == HUGE_VAL) {
469 // no valid route found
471 SG_LOG(SG_GENERAL, SG_ALERT,
472 "Failed to find route from waypoint " << start << " to "
473 << end << " at " << parent->getId());
476 return FGTaxiRoute();
479 // assemble route from backtrace information
480 PositionedIDVec nodes;
482 FGTaxiNode *bt = lastNode;
484 while (searchData[bt].previousNode != 0) {
485 nodes.push_back(bt->guid());
486 FGTaxiSegment *segment = findSegment(searchData[bt].previousNode->guid(), bt->guid());
487 int idx = segment->getIndex();
488 routes.push_back(idx);
489 bt = searchData[bt].previousNode;
492 nodes.push_back(start);
493 reverse(nodes.begin(), nodes.end());
494 reverse(routes.begin(), routes.end());
495 return FGTaxiRoute(nodes, routes, searchData[lastNode].score, 0);
498 /* ATC Related Functions */
500 void FGGroundNetwork::announcePosition(int id,
501 FGAIFlightPlan * intendedRoute,
502 int currentPosition, double lat,
503 double lon, double heading,
504 double speed, double alt,
505 double radius, int leg,
506 FGAIAircraft * aircraft)
512 TrafficVectorIterator i = activeTraffic.begin();
513 // Search search if the current id alread has an entry
514 // This might be faster using a map instead of a vector, but let's start by taking a safe route
515 if (activeTraffic.size()) {
516 //while ((i->getId() != id) && i != activeTraffic.end()) {
517 while (i != activeTraffic.end()) {
518 if (i->getId() == id) {
524 // Add a new TrafficRecord if no one exsists for this aircraft.
525 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
529 rec.setPositionAndIntentions(currentPosition, intendedRoute);
530 rec.setPositionAndHeading(lat, lon, heading, speed, alt);
531 rec.setRadius(radius); // only need to do this when creating the record.
532 rec.setAircraft(aircraft);
534 activeTraffic.push_front(rec);
536 activeTraffic.push_back(rec);
540 i->setPositionAndIntentions(currentPosition, intendedRoute);
541 i->setPositionAndHeading(lat, lon, heading, speed, alt);
546 void FGGroundNetwork::signOff(int id)
548 TrafficVectorIterator i = activeTraffic.begin();
549 // Search search if the current id alread has an entry
550 // This might be faster using a map instead of a vector, but let's start by taking a safe route
551 if (activeTraffic.size()) {
552 //while ((i->getId() != id) && i != activeTraffic.end()) {
553 while (i != activeTraffic.end()) {
554 if (i->getId() == id) {
560 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
561 SG_LOG(SG_GENERAL, SG_ALERT,
562 "AI error: Aircraft without traffic record is signing off at " << SG_ORIGIN);
564 i = activeTraffic.erase(i);
568 * The ground network can deal with the following states:
569 * 0 = Normal; no action required
570 * 1 = "Acknowledge "Hold position
571 * 2 = "Acknowledge "Resume taxi".
572 * 3 = "Issue TaxiClearance"
573 * 4 = Acknowledge Taxi Clearance"
574 * 5 = Post acknowlegde taxiclearance: Start taxiing
576 * 7 = Acknowledge report runway
577 * 8 = Switch tower frequency
578 * 9 = Acknowledge switch tower frequency
579 *************************************************************************************************************************/
580 bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId,
583 int state = i->getState();
584 if ((state >= minState) && (state <= maxState) && available) {
585 if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) {
586 //cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl;
587 SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true);
588 int n = trans_num->getIntValue();
590 trans_num->setIntValue(-1);
592 //cerr << "Selected transmission message " << n << endl;
593 //FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc");
594 FGATCDialogNew::instance()->removeEntry(1);
596 //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl;
597 transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, false);
601 transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, true);
603 lastTransmission = now;
610 void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
611 double heading, double speed, double alt,
614 time_t currentTime = time(NULL);
615 if (nextSave < currentTime) {
616 saveElevationCache();
617 nextSave = currentTime + 100 + rand() % 200;
619 // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
620 // Transmit air-to-ground "Ready to taxi request:
621 // Transmit ground to air approval / hold
622 // Transmit confirmation ...
623 // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
626 TrafficVectorIterator i = activeTraffic.begin();
627 // Search search if the current id has an entry
628 // This might be faster using a map instead of a vector, but let's start by taking a safe route
629 TrafficVectorIterator current, closest;
630 if (activeTraffic.size()) {
631 //while ((i->getId() != id) && i != activeTraffic.end()) {
632 while (i != activeTraffic.end()) {
633 if (i->getId() == id) {
639 // update position of the current aircraft
640 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
641 SG_LOG(SG_GENERAL, SG_ALERT,
642 "AI error: updating aircraft without traffic record at " << SG_ORIGIN);
644 i->setPositionAndHeading(lat, lon, heading, speed, alt);
650 // Update every three secs, but add some randomness
651 // to prevent all IA objects doing this in synchrony
652 //if (getDt() < (3.0) + (rand() % 10))
656 current->clearResolveCircularWait();
657 current->setWaitsForId(0);
658 checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
659 bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
660 if (!needsTaxiClearance) {
661 checkHoldPosition(id, lat, lon, heading, speed, alt);
662 //if (checkForCircularWaits(id)) {
663 // i->setResolveCircularWait();
666 current->setHoldPosition(true);
667 int state = current->getState();
668 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
669 if ((now - lastTransmission) > 15) {
672 if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
673 current->setState(3);
675 if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) {
676 current->setState(4);
678 if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
679 current->setState(5);
681 if ((state == 5) && available) {
682 current->setState(0);
683 current->getAircraft()->setTaxiClearanceRequest(false);
684 current->setHoldPosition(false);
692 Scan for a speed adjustment change. Find the nearest aircraft that is in front
693 and adjust speed when we get too close. Only do this when current position and/or
694 intentions of the current aircraft match current taxiroute position of the proximate
695 aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
696 instruction. See below for the hold position instruction.
698 Note that there currently still is one flaw in the logic that needs to be addressed.
699 There can be situations where one aircraft is in front of the current aircraft, on a separate
700 route, but really close after an intersection coming off the current route. This
701 aircraft is still close enough to block the current aircraft. This situation is currently
702 not addressed yet, but should be.
705 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
706 double lon, double heading,
707 double speed, double alt)
710 TrafficVectorIterator current, closest, closestOnNetwork;
711 TrafficVectorIterator i = activeTraffic.begin();
712 bool otherReasonToSlowDown = false;
713 // bool previousInstruction;
714 if (activeTraffic.size()) {
715 //while ((i->getId() != id) && (i != activeTraffic.end()))
716 while (i != activeTraffic.end()) {
717 if (i->getId() == id) {
725 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
726 SG_LOG(SG_GENERAL, SG_ALERT,
727 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment at " << SG_ORIGIN);
732 // previousInstruction = current->getSpeedAdjustment();
733 double mindist = HUGE_VAL;
734 if (activeTraffic.size()) {
735 double course, dist, bearing, az2; // minbearing,
736 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
737 //TrafficVector iterator closest;
739 closestOnNetwork = current;
740 for (TrafficVectorIterator i = activeTraffic.begin();
741 i != activeTraffic.end(); i++) {
746 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
749 SGGeodesy::inverse(curr, other, course, az2, dist);
750 bearing = fabs(heading - course);
752 bearing = 360 - bearing;
753 if ((dist < mindist) && (bearing < 60.0)) {
756 closestOnNetwork = i;
757 // minbearing = bearing;
761 //Check traffic at the tower controller
762 if (towerController->hasActiveTraffic()) {
763 for (TrafficVectorIterator i =
764 towerController->getActiveTraffic().begin();
765 i != towerController->getActiveTraffic().end(); i++) {
766 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
767 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
770 SGGeodesy::inverse(curr, other, course, az2, dist);
771 bearing = fabs(heading - course);
773 bearing = 360 - bearing;
774 if ((dist < mindist) && (bearing < 60.0)) {
775 //cerr << "Current aircraft " << current->getAircraft()->getTrafficRef()->getCallSign()
776 // << " is closest to " << i->getAircraft()->getTrafficRef()->getCallSign()
777 // << ", which has status " << i->getAircraft()->isScheduledForTakeoff()
781 // minbearing = bearing;
782 otherReasonToSlowDown = true;
786 // Finally, check UserPosition
787 // Note, as of 2011-08-01, this should no longer be necessecary.
789 double userLatitude = fgGetDouble("/position/latitude-deg");
790 double userLongitude = fgGetDouble("/position/longitude-deg");
791 SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
792 SGGeodesy::inverse(curr, user, course, az2, dist);
794 bearing = fabs(heading - course);
796 bearing = 360 - bearing;
797 if ((dist < mindist) && (bearing < 60.0)) {
800 minbearing = bearing;
801 otherReasonToSlowDown = true;
804 current->clearSpeedAdjustment();
805 bool needBraking = false;
806 if (current->checkPositionAndIntentions(*closest)
807 || otherReasonToSlowDown) {
808 double maxAllowableDistance =
809 (1.1 * current->getRadius()) +
810 (1.1 * closest->getRadius());
811 if (mindist < 2 * maxAllowableDistance) {
812 if (current->getId() == closest->getWaitsForId())
815 current->setWaitsForId(closest->getId());
816 if (closest->getId() != current->getId()) {
817 current->setSpeedAdjustment(closest->getSpeed() *
822 // closest->getAircraft()->getTakeOffStatus() &&
823 // (current->getAircraft()->getTrafficRef()->getDepartureAirport() == closest->getAircraft()->getTrafficRef()->getDepartureAirport()) &&
824 // (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway())
826 // current->getAircraft()->scheduleForATCTowerDepartureControl(1);
828 current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
830 if (mindist < maxAllowableDistance) {
831 //double newSpeed = (maxAllowableDistance-mindist);
832 //current->setSpeedAdjustment(newSpeed);
833 //if (mindist < 0.5* maxAllowableDistance)
835 current->setSpeedAdjustment(0);
840 if ((closest->getId() == closestOnNetwork->getId()) && (current->getPriority() < closest->getPriority()) && needBraking) {
841 swap(current, closest);
847 Check for "Hold position instruction".
848 The hold position should be issued under the following conditions:
849 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
850 2) For taxiing aircraft that use one taxiway in opposite directions
851 3) For crossing or merging taxiroutes.
854 void FGGroundNetwork::checkHoldPosition(int id, double lat,
855 double lon, double heading,
856 double speed, double alt)
858 TrafficVectorIterator current;
859 TrafficVectorIterator i = activeTraffic.begin();
860 if (activeTraffic.size()) {
861 //while ((i->getId() != id) && i != activeTraffic.end())
862 while (i != activeTraffic.end()) {
863 if (i->getId() == id) {
871 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
872 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
873 SG_LOG(SG_GENERAL, SG_ALERT,
874 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition at " << SG_ORIGIN);
878 if (current->getAircraft()->getTakeOffStatus() == 1) {
879 current->setHoldPosition(true);
882 if (current->getAircraft()->getTakeOffStatus() == 2) {
883 //cerr << current->getAircraft()->getCallSign() << ". Taxi in position and hold" << endl;
884 current->setHoldPosition(false);
885 current->clearSpeedAdjustment();
888 bool origStatus = current->hasHoldPosition();
889 current->setHoldPosition(false);
890 //SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
891 int currentRoute = i->getCurrentPosition();
893 if (i->getIntentions().size()) {
894 nextRoute = (*(i->getIntentions().begin()));
898 if (currentRoute > 0) {
899 FGTaxiSegment *tx = findSegment(currentRoute);
902 nx = findSegment(nextRoute);
906 //if (tx->hasBlock(now) || nx->hasBlock(now) ) {
907 // current->setHoldPosition(true);
909 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
910 SGGeod end (nx->getStart()->geod());
912 double distance = SGGeodesy::distanceM(start, end);
913 if (nx->hasBlock(now) && (distance < i->getRadius() * 4)) {
914 current->setHoldPosition(true);
916 intVecIterator ivi = i->getIntentions().begin();
917 while (ivi != i->getIntentions().end()) {
919 distance += segments[(*ivi)-1]->getLength();
920 if ((segments[(*ivi)-1]->hasBlock(now)) && (distance < i->getRadius() * 4)) {
921 current->setHoldPosition(true);
929 bool currStatus = current->hasHoldPosition();
930 current->setHoldPosition(origStatus);
931 // Either a Hold Position or a resume taxi transmission has been issued
932 if ((now - lastTransmission) > 2) {
935 if (current->getState() == 0) {
936 if ((origStatus != currStatus) && available) {
937 //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
938 if (currStatus == true) { // No has a hold short instruction
939 transmit(&(*current), &(*parent->getDynamics()), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
940 //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
941 current->setState(1);
943 transmit(&(*current), &(*parent->getDynamics()), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
944 //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
945 current->setState(2);
947 lastTransmission = now;
949 // Don't act on the changed instruction until the transmission is confirmed
950 // So set back to original status
951 //cerr << "Current state " << current->getState() << endl;
956 // 7 = Acknowledge report runway
957 // 8 = Switch tower frequency
958 //9 = Acknowledge switch tower frequency
960 //int state = current->getState();
961 if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
962 current->setState(0);
963 current->setHoldPosition(true);
965 if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
966 current->setState(0);
967 current->setHoldPosition(false);
969 if (current->getAircraft()->getTakeOffStatus() && (current->getState() == 0)) {
970 //cerr << "Scheduling " << current->getAircraft()->getCallSign() << " for hold short" << endl;
971 current->setState(6);
973 if (checkTransmissionState(6,6, current, now, MSG_REPORT_RUNWAY_HOLD_SHORT, ATC_AIR_TO_GROUND)) {
975 if (checkTransmissionState(7,7, current, now, MSG_ACKNOWLEDGE_REPORT_RUNWAY_HOLD_SHORT, ATC_GROUND_TO_AIR)) {
977 if (checkTransmissionState(8,8, current, now, MSG_SWITCH_TOWER_FREQUENCY, ATC_GROUND_TO_AIR)) {
979 if (checkTransmissionState(9,9, current, now, MSG_ACKNOWLEDGE_SWITCH_TOWER_FREQUENCY, ATC_AIR_TO_GROUND)) {
984 //current->setState(0);
988 * Check whether situations occur where the current aircraft is waiting for itself
989 * due to higher order interactions.
990 * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
991 * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
992 * through this list of waiting aircraft, we can check if we'd eventually end back
993 * at the current aircraft.
995 * Note that we should consider the situation where we are actually checking aircraft
996 * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
997 * the looping aircraft. If we don't check for that, this function will get stuck into
1001 bool FGGroundNetwork::checkForCircularWaits(int id)
1003 //cerr << "Performing Wait check " << id << endl;
1005 TrafficVectorIterator current, other;
1006 TrafficVectorIterator i = activeTraffic.begin();
1007 int trafficSize = activeTraffic.size();
1009 while (i != activeTraffic.end()) {
1010 if (i->getId() == id) {
1018 if (i == activeTraffic.end() || (trafficSize == 0)) {
1019 SG_LOG(SG_GENERAL, SG_ALERT,
1020 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits at " << SG_ORIGIN);
1024 target = current->getWaitsForId();
1025 //bool printed = false; // Note that this variable is for debugging purposes only.
1029 //cerr << "aircraft waits for user" << endl;
1034 while ((target > 0) && (target != id) && counter++ < trafficSize) {
1036 TrafficVectorIterator i = activeTraffic.begin();
1038 //while ((i->getId() != id) && i != activeTraffic.end())
1039 while (i != activeTraffic.end()) {
1040 if (i->getId() == target) {
1048 if (i == activeTraffic.end() || (trafficSize == 0)) {
1049 //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
1050 // The target id is not found on the current network, which means it's at the tower
1051 //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1055 target = other->getWaitsForId();
1057 // actually this trap isn't as impossible as it first seemed:
1058 // the setWaitsForID(id) is set to current when the aircraft
1059 // is waiting for the user controlled aircraft.
1060 //if (current->getId() == other->getId()) {
1061 // cerr << "Caught the impossible trap" << endl;
1062 // cerr << "Current = " << current->getId() << endl;
1063 // cerr << "Other = " << other ->getId() << endl;
1064 // for (TrafficVectorIterator at = activeTraffic.begin();
1065 // at != activeTraffic.end();
1067 // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1070 if (current->getId() == other->getId())
1073 //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign()
1074 // << " (" << other->getId() << "); " << endl;;
1084 // cerr << "[done] " << endl << endl;;
1086 SG_LOG(SG_GENERAL, SG_WARN,
1087 "Detected circular wait condition: Id = " << id <<
1088 "target = " << target);
1095 // Note that this function is probably obsolete...
1096 bool FGGroundNetwork::hasInstruction(int id)
1098 TrafficVectorIterator i = activeTraffic.begin();
1099 // Search search if the current id has an entry
1100 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1101 if (activeTraffic.size()) {
1102 //while ((i->getId() != id) && i != activeTraffic.end()) {
1103 while (i != activeTraffic.end()) {
1104 if (i->getId() == id) {
1110 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1111 SG_LOG(SG_GENERAL, SG_ALERT,
1112 "AI error: checking ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1114 return i->hasInstruction();
1119 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1121 TrafficVectorIterator i = activeTraffic.begin();
1122 // Search search if the current id has an entry
1123 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1124 if (activeTraffic.size()) {
1125 //while ((i->getId() != id) && i != activeTraffic.end()) {
1126 while (i != activeTraffic.end()) {
1127 if (i->getId() == id) {
1133 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1134 SG_LOG(SG_GENERAL, SG_ALERT,
1135 "AI error: requesting ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1137 return i->getInstruction();
1139 return FGATCInstruction();
1142 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
1143 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
1144 double lon, double elev, double hdg, double slope)
1146 SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
1147 obj_pos = makeZUpFrame(geod);
1148 // hdg is not a compass heading, but a counter-clockwise rotation
1149 // around the Z axis
1150 obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
1152 obj_pos.preMult(osg::Matrix::rotate(slope * SGD_DEGREES_TO_RADIANS,
1159 void FGGroundNetwork::render(bool visible)
1161 SGMaterialLib *matlib = globals->get_matlib();
1164 globals->get_scenery()->get_scene_graph()->removeChild(group);
1165 //while (group->getNumChildren()) {
1166 // cerr << "Number of children: " << group->getNumChildren() << endl;
1167 //simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
1168 //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
1169 //geode->releaseGLObjects();
1170 //group->removeChild(geode);
1175 group = new osg::Group;
1176 FGScenery * local_scenery = globals->get_scenery();
1177 // double elevation_meters = 0.0;
1178 // double elevation_feet = 0.0;
1179 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1180 //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
1182 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1183 // Handle start point i.e. the segment that is connected to the aircraft itself on the starting end
1184 // and to the the first "real" taxi segment on the other end.
1185 int pos = i->getCurrentPosition() - 1;
1188 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1189 SGGeod end (segments[pos]->getEnd()->geod());
1191 double length = SGGeodesy::distanceM(start, end);
1192 //heading = SGGeodesy::headingDeg(start->geod(), end->geod());
1194 double az2, heading; //, distanceM;
1195 SGGeodesy::inverse(start, end, heading, az2, length);
1196 double coveredDistance = length * 0.5;
1198 SGGeodesy::direct(start, heading, coveredDistance, center, az2);
1199 //std::cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << std::endl;
1200 ///////////////////////////////////////////////////////////////////////////////
1201 // Make a helper function out of this
1202 osg::Matrix obj_pos;
1203 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1204 obj_trans->setDataVariance(osg::Object::STATIC);
1205 // Experimental: Calculate slope here, based on length, and the individual elevations
1206 double elevationStart;
1207 if (isUserAircraft((i)->getAircraft())) {
1208 elevationStart = fgGetDouble("/position/ground-elev-m");
1210 elevationStart = ((i)->getAircraft()->_getAltitude());
1212 double elevationEnd = segments[pos]->getEnd()->getElevationM();
1213 //cerr << "Using elevation " << elevationEnd << endl;
1215 if ((elevationEnd == 0) || (elevationEnd = parent->getElevation())) {
1216 SGGeod center2 = end;
1217 center2.setElevationM(SG_MAX_ELEVATION_M);
1218 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1219 // elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1220 //elevation_meters += 0.5;
1223 elevationEnd = parent->getElevation();
1225 segments[pos]->getEnd()->setElevation(elevationEnd);
1227 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1228 double elevDiff = elevationEnd - elevationStart;
1230 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1232 //cerr << "1. Using mean elevation : " << elevationMean << " and " << slope << endl;
1234 WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), elevationMean+ 0.5, -(heading), slope );
1236 obj_trans->setMatrix( obj_pos );
1237 //osg::Vec3 center(0, 0, 0)
1239 float width = length /2.0;
1240 osg::Vec3 corner(-width, 0, 0.25f);
1241 osg::Vec3 widthVec(2*width + 1, 0, 0);
1242 osg::Vec3 heightVec(0, 1, 0);
1243 osg::Geometry* geometry;
1244 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1245 simgear::EffectGeode* geode = new simgear::EffectGeode;
1246 geode->setName("test");
1247 geode->addDrawable(geometry);
1248 //osg::Node *custom_obj;
1250 if (segments[pos]->hasBlock(now)) {
1251 mat = matlib->find("UnidirectionalTaperRed", center);
1253 mat = matlib->find("UnidirectionalTaperGreen", center);
1256 geode->setEffect(mat->get_effect());
1257 obj_trans->addChild(geode);
1258 // wire as much of the scene graph together as we can
1259 //->addChild( obj_trans );
1260 group->addChild( obj_trans );
1261 /////////////////////////////////////////////////////////////////////
1263 //std::cerr << "BIG FAT WARNING: current position is here : " << pos << std::endl;
1265 // Next: Draw the other taxi segments.
1266 for (intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
1267 osg::Matrix obj_pos;
1270 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1271 obj_trans->setDataVariance(osg::Object::STATIC);
1273 // Experimental: Calculate slope here, based on length, and the individual elevations
1274 double elevationStart = segments[k]->getStart()->getElevationM();
1275 double elevationEnd = segments[k]->getEnd ()->getElevationM();
1276 if ((elevationStart == 0) || (elevationStart == parent->getElevation())) {
1277 SGGeod center2 = segments[k]->getStart()->geod();
1278 center2.setElevationM(SG_MAX_ELEVATION_M);
1279 if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) {
1280 // elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5;
1281 //elevation_meters += 0.5;
1284 elevationStart = parent->getElevation();
1286 segments[k]->getStart()->setElevation(elevationStart);
1288 if ((elevationEnd == 0) || (elevationEnd == parent->getElevation())) {
1289 SGGeod center2 = segments[k]->getEnd()->geod();
1290 center2.setElevationM(SG_MAX_ELEVATION_M);
1291 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1292 // elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1293 //elevation_meters += 0.5;
1296 elevationEnd = parent->getElevation();
1298 segments[k]->getEnd()->setElevation(elevationEnd);
1301 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1302 double elevDiff = elevationEnd - elevationStart;
1303 double length = segments[k]->getLength();
1304 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1306 // cerr << "2. Using mean elevation : " << elevationMean << " and " << slope << endl;
1308 SGGeod segCenter = segments[k]->getCenter();
1309 WorldCoordinate( obj_pos, segCenter.getLatitudeDeg(), segCenter.getLongitudeDeg(), elevationMean+ 0.5, -(segments[k]->getHeading()), slope );
1311 obj_trans->setMatrix( obj_pos );
1312 //osg::Vec3 center(0, 0, 0)
1314 float width = segments[k]->getLength() /2.0;
1315 osg::Vec3 corner(-width, 0, 0.25f);
1316 osg::Vec3 widthVec(2*width + 1, 0, 0);
1317 osg::Vec3 heightVec(0, 1, 0);
1318 osg::Geometry* geometry;
1319 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1320 simgear::EffectGeode* geode = new simgear::EffectGeode;
1321 geode->setName("test");
1322 geode->addDrawable(geometry);
1323 //osg::Node *custom_obj;
1325 if (segments[k]->hasBlock(now)) {
1326 mat = matlib->find("UnidirectionalTaperRed", segCenter);
1328 mat = matlib->find("UnidirectionalTaperGreen", segCenter);
1331 geode->setEffect(mat->get_effect());
1332 obj_trans->addChild(geode);
1333 // wire as much of the scene graph together as we can
1334 //->addChild( obj_trans );
1335 group->addChild( obj_trans );
1340 globals->get_scenery()->get_scene_graph()->addChild(group);
1344 string FGGroundNetwork::getName() {
1345 return string(parent->getId() + "-ground");
1348 void FGGroundNetwork::update(double dt)
1350 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1351 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1352 (*tsi)->unblock(now);
1355 //sort(activeTraffic.begin(), activeTraffic.end(), compare_trafficrecords);
1356 // Handle traffic that is under ground control first; this way we'll prevent clutter at the gate areas.
1357 // Don't allow an aircraft to pushback when a taxiing aircraft is currently using part of the intended route.
1358 for (TrafficVectorIterator i = parent->getDynamics()->getStartupController()->getActiveTraffic().begin();
1359 i != parent->getDynamics()->getStartupController()->getActiveTraffic().end(); i++) {
1361 i->setPriority(priority++);
1362 // in meters per second;
1363 double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1364 if (i->isActive(0)) {
1366 // Check for all active aircraft whether it's current pos segment is
1367 // an opposite of one of the departing aircraft's intentions
1368 for (TrafficVectorIterator j = activeTraffic.begin(); j != activeTraffic.end(); j++) {
1369 int pos = j->getCurrentPosition();
1371 FGTaxiSegment *seg = segments[pos-1]->opposite();
1373 int posReverse = seg->getIndex();
1374 for (intVecIterator k = i->getIntentions().begin(); k != i->getIntentions().end(); k++) {
1375 if ((*k) == posReverse) {
1377 segments[posReverse-1]->block(i->getId(), now, now);
1383 // if the current aircraft is still allowed to pushback, we can start reserving a route for if by blocking all the entry taxiways.
1384 if (i->pushBackAllowed()) {
1386 int pos = i->getCurrentPosition();
1388 FGTaxiSegment *seg = segments[pos-1];
1389 FGTaxiNode *node = seg->getEnd();
1390 length = seg->getLength();
1391 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1392 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1393 (*tsi)->block(i->getId(), now, now);
1397 for (intVecIterator j = i->getIntentions().begin(); j != i->getIntentions().end(); j++) {
1400 FGTaxiSegment *seg = segments[pos-1];
1401 FGTaxiNode *node = seg->getEnd();
1402 length += seg->getLength();
1403 time_t blockTime = now + (length / vTaxi);
1404 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1405 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1406 (*tsi)->block(i->getId(), blockTime-30, now);
1414 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1416 double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1417 i->setPriority(priority++);
1418 int pos = i->getCurrentPosition();
1420 length = segments[pos-1]->getLength();
1421 if (segments[pos-1]->hasBlock(now)) {
1422 //SG_LOG(SG_GENERAL, SG_ALERT, "Taxiway incursion for AI aircraft" << i->getAircraft()->getCallSign());
1427 for (ivi = i->getIntentions().begin(); ivi != i->getIntentions().end(); ivi++) {
1428 int segIndex = (*ivi);
1430 if (segments[segIndex-1]->hasBlock(now))
1434 //after this, ivi points just behind the last valid unblocked taxi segment.
1435 for (intVecIterator j = i->getIntentions().begin(); j != ivi; j++) {
1438 FGTaxiSegment *seg = segments[pos-1];
1439 FGTaxiNode *node = seg->getEnd();
1440 length += seg->getLength();
1441 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1442 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1443 time_t blockTime = now + (length / vTaxi);
1444 (*tsi)->block(i->getId(), blockTime - 30, now);