2 // groundnet.cxx - Implimentation of the FlightGear airport ground handling code
4 // Written by Durk Talsma, started June 2005.
6 // Copyright (C) 2004 Durk Talsma.
8 // This program is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU General Public License as
10 // published by the Free Software Foundation; either version 2 of the
11 // License, or (at your option) any later version.
13 // This program is distributed in the hope that it will be useful, but
14 // WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 // General Public License for more details.
18 // You should have received a copy of the GNU General Public License
19 // along with this program; if not, write to the Free Software
20 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
33 #include <osg/Geometry>
34 #include <osg/MatrixTransform>
37 #include <simgear/debug/logstream.hxx>
38 #include <simgear/route/waypoint.hxx>
39 #include <simgear/scene/material/EffectGeode.hxx>
40 #include <simgear/scene/material/matlib.hxx>
41 #include <simgear/scene/material/mat.hxx>
43 #include <Airports/simple.hxx>
44 #include <Airports/dynamics.hxx>
46 #include <AIModel/AIAircraft.hxx>
47 #include <AIModel/AIFlightPlan.hxx>
49 #include <ATC/atc_mgr.hxx>
51 #include <Scenery/scenery.hxx>
53 #include "groundnetwork.hxx"
55 /***************************************************************************
57 **************************************************************************/
59 void FGTaxiSegment::setStart(FGTaxiNodeVector * nodes)
61 FGTaxiNodeVectorIterator i = nodes->begin();
62 while (i != nodes->end()) {
63 //cerr << "Scanning start node index" << (*i)->getIndex() << endl;
64 if ((*i)->getIndex() == startNode) {
65 start = (*i)->getAddress();
66 (*i)->addSegment(this);
71 SG_LOG(SG_GENERAL, SG_ALERT,
72 "Could not find start node " << startNode << endl);
75 void FGTaxiSegment::setEnd(FGTaxiNodeVector * nodes)
77 FGTaxiNodeVectorIterator i = nodes->begin();
78 while (i != nodes->end()) {
79 //cerr << "Scanning end node index" << (*i)->getIndex() << endl;
80 if ((*i)->getIndex() == endNode) {
81 end = (*i)->getAddress();
86 SG_LOG(SG_GENERAL, SG_ALERT,
87 "Could not find end node " << endNode << endl);
92 // There is probably a computationally cheaper way of
94 void FGTaxiSegment::setDimensions(double elevation)
96 length = SGGeodesy::distanceM(start->getGeod(), end->getGeod());
97 //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
99 double az2; //, distanceM;
100 SGGeodesy::inverse(start->getGeod(), end->getGeod(), heading, az2, length);
101 double coveredDistance = length * 0.5;
102 SGGeodesy::direct(start->getGeod(), heading, coveredDistance, center, az2);
103 //start->setElevation(elevation);
104 //end->setElevation(elevation);
105 //cerr << "Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
109 //void FGTaxiSegment::setCourseDiff(double crse)
111 // headingDiff = fabs(course - crse);
113 // if (headingDiff > 180)
114 // headingDiff = fabs(headingDiff - 360);
118 /***************************************************************************
120 **************************************************************************/
121 bool FGTaxiRoute::next(int *nde)
123 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
124 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
125 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
126 //if (currNode != nodes.end())
127 // cerr << "true" << endl;
129 // cerr << "false" << endl;
130 //if (nodes.size() != (routes.size()) +1)
131 // cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
133 if (currNode == nodes.end())
136 if (currNode != nodes.begin()) // make sure route corresponds to the end node
142 bool FGTaxiRoute::next(int *nde, int *rte)
144 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
145 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
146 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
147 //if (currNode != nodes.end())
148 // cerr << "true" << endl;
150 // cerr << "false" << endl;
151 if (nodes.size() != (routes.size()) + 1) {
152 SG_LOG(SG_GENERAL, SG_ALERT,
153 "ALERT: Misconfigured TaxiRoute : " << nodes.
154 size() << " " << routes.size());
157 if (currNode == nodes.end())
160 //*rte = *(currRoute);
161 if (currNode != nodes.begin()) // Make sure route corresponds to the end node
166 // If currNode points to the first node, this means the aircraft is not on the taxi node
167 // yet. Make sure to return a unique identifyer in this situation though, because otherwise
168 // the speed adjust AI code may be unable to resolve whether two aircraft are on the same
169 // taxi route or not. the negative of the preceding route seems a logical choice, as it is
170 // unique for any starting location.
171 // Note that this is probably just a temporary fix until I get Parking / tower control working.
172 *rte = -1 * *(currRoute);
179 void FGTaxiRoute::rewind(int route)
185 if (!(next(&currPoint, &currRoute))) {
186 SG_LOG(SG_GENERAL, SG_ALERT,
187 "Error in rewinding TaxiRoute: current" << currRoute <<
190 } while (currRoute != route);
196 /***************************************************************************
198 **************************************************************************/
199 bool compare_nodes(FGTaxiNode * a, FGTaxiNode * b)
204 bool compare_segments(FGTaxiSegment * a, FGTaxiSegment * b)
209 FGGroundNetwork::FGGroundNetwork()
217 currTraffic = activeTraffic.begin();
219 networkInitialized = false;
223 FGGroundNetwork::~FGGroundNetwork()
225 //cerr << "Running Groundnetwork Destructor " << endl;
226 bool saveData = false;
228 if (fgGetBool("/sim/ai/groundnet-cache")) {
229 SGPath cacheData(fgGetString("/sim/fg-home"));
230 cacheData.append("ai");
231 string airport = parent->getId();
233 if ((airport) != "") {
235 ::snprintf(buffer, 128, "%c/%c/%c/",
236 airport[0], airport[1], airport[2]);
237 cacheData.append(buffer);
238 if (!cacheData.exists()) {
239 cacheData.create_dir(0777);
241 cacheData.append(airport + "-groundnet-cache.txt");
242 cachefile.open(cacheData.str().c_str());
246 for (FGTaxiNodeVectorIterator node = nodes.begin();
247 node != nodes.end(); node++) {
249 cachefile << (*node)->getIndex () << " "
250 << (*node)->getElevation () << " "
256 pushBackNodes.clear();
257 for (FGTaxiSegmentVectorIterator seg = segments.begin();
258 seg != segments.end(); seg++) {
267 void FGGroundNetwork::saveElevationCache() {
268 //cerr << "Running Groundnetwork Destructor " << endl;
269 bool saveData = false;
271 if (fgGetBool("/sim/ai/groundnet-cache")) {
272 SGPath cacheData(fgGetString("/sim/fg-home"));
273 cacheData.append("ai");
274 string airport = parent->getId();
276 if ((airport) != "") {
278 ::snprintf(buffer, 128, "%c/%c/%c/",
279 airport[0], airport[1], airport[2]);
280 cacheData.append(buffer);
281 if (!cacheData.exists()) {
282 cacheData.create_dir(0777);
284 cacheData.append(airport + "-groundnet-cache.txt");
285 cachefile.open(cacheData.str().c_str());
289 for (FGTaxiNodeVectorIterator node = nodes.begin();
290 node != nodes.end(); node++) {
292 cachefile << (*node)->getIndex () << " "
293 << (*node)->getElevation () << " "
302 void FGGroundNetwork::addSegment(const FGTaxiSegment & seg)
304 segments.push_back(new FGTaxiSegment(seg));
307 void FGGroundNetwork::addNode(const FGTaxiNode & node)
309 nodes.push_back(new FGTaxiNode(node));
312 void FGGroundNetwork::addNodes(FGParkingVec * parkings)
315 FGParkingVecIterator i = parkings->begin();
316 while (i != parkings->end()) {
317 n.setIndex(i->getIndex());
318 n.setLatitude(i->getLatitude());
319 n.setLongitude(i->getLongitude());
320 n.setElevation(parent->getElevation());
321 nodes.push_back(new FGTaxiNode(n));
329 void FGGroundNetwork::init()
331 if (networkInitialized) {
332 FGATCController::init();
333 //cerr << "FGground network already initialized" << endl;
339 sort(nodes.begin(), nodes.end(), compare_nodes);
340 //sort(segments.begin(), segments.end(), compare_segments());
341 FGTaxiSegmentVectorIterator i = segments.begin();
342 while (i != segments.end()) {
343 (*i)->setStart(&nodes);
344 (*i)->setEnd(&nodes);
345 (*i)->setDimensions(parent->getElevation() * SG_FEET_TO_METER);
346 (*i)->setIndex(index);
347 if ((*i)->isPushBack()) {
348 pushBackNodes.push_back((*i)->getEnd());
350 //SG_LOG(SG_GENERAL, SG_BULK, "initializing segment " << (*i)->getIndex() << endl);
351 //SG_LOG(SG_GENERAL, SG_BULK, "Track distance = " << (*i)->getLength() << endl);
352 //SG_LOG(SG_GENERAL, SG_BULK, "Track runs from " << (*i)->getStart()->getIndex() << " to "
353 // << (*i)->getEnd()->getIndex() << endl);
358 i = segments.begin();
359 while (i != segments.end()) {
360 FGTaxiSegmentVectorIterator j = (*i)->getEnd()->getBeginRoute();
361 while (j != (*i)->getEnd()->getEndRoute()) {
362 if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex()) {
363 // int start1 = (*i)->getStart()->getIndex();
364 // int end1 = (*i)->getEnd() ->getIndex();
365 // int start2 = (*j)->getStart()->getIndex();
366 // int end2 = (*j)->getEnd()->getIndex();
367 // int oppIndex = (*j)->getIndex();
368 //cerr << "Opposite of " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") "
369 // << "happens to be " << oppIndex << " (" << start2 << "," << end2 << ") " << endl;
370 (*i)->setOpposite(*j);
377 //FGTaxiNodeVectorIterator j = nodes.begin();
378 //while (j != nodes.end()) {
379 // if ((*j)->getHoldPointType() == 3) {
380 // pushBackNodes.push_back((*j));
384 //cerr << "Done initializing ground network" << endl;
386 if (fgGetBool("/sim/ai/groundnet-cache")) {
387 SGPath cacheData(fgGetString("/sim/fg-home"));
388 cacheData.append("ai");
389 string airport = parent->getId();
391 if ((airport) != "") {
393 ::snprintf(buffer, 128, "%c/%c/%c/",
394 airport[0], airport[1], airport[2]);
395 cacheData.append(buffer);
396 if (!cacheData.exists()) {
397 cacheData.create_dir(0777);
401 cacheData.append(airport + "-groundnet-cache.txt");
402 if (cacheData.exists()) {
403 ifstream data(cacheData.c_str());
404 for (FGTaxiNodeVectorIterator i = nodes.begin();
407 (*i)->setElevation(parent->getElevation() * SG_FEET_TO_METER);
408 data >> index >> elev;
411 if (index != (*i)->getIndex()) {
412 SG_LOG(SG_GENERAL, SG_ALERT, "Index read from ground network cache at airport " << airport << " does not match index in the network itself");
414 (*i)->setElevation(elev);
420 //cerr << "Finished initializing " << parent->getId() << " groundnetwork " << endl;
421 networkInitialized = true;
424 int FGGroundNetwork::findNearestNode(const SGGeod & aGeod)
426 double minDist = HUGE_VAL;
429 for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end();
431 double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
434 index = (*itr)->getIndex();
435 //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
442 int FGGroundNetwork::findNearestNode(double lat, double lon)
444 return findNearestNode(SGGeod::fromDeg(lon, lat));
447 FGTaxiNode *FGGroundNetwork::findNode(unsigned idx)
449 for (FGTaxiNodeVectorIterator
451 itr != nodes.end(); itr++)
453 if (itr->getIndex() == idx)
454 return itr->getAddress();
457 if ((idx >= 0) && (idx < nodes.size()))
458 return nodes[idx]->getAddress();
463 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx)
465 for (FGTaxiSegmentVectorIterator
466 itr = segments.begin();
467 itr != segments.end(); itr++)
469 if (itr->getIndex() == idx)
470 return itr->getAddress();
473 if ((idx > 0) && (idx <= segments.size()))
474 return segments[idx - 1]->getAddress();
476 //cerr << "Alert: trying to find invalid segment " << idx << endl;
482 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end,
485 //implements Dijkstra's algorithm to find shortest distance route from start to end
486 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
488 //double INFINITE = 100000000000.0;
489 // initialize scoring values
490 int nParkings = parent->getDynamics()->getNrOfParkings();
491 FGTaxiNodeVector *currNodesSet;
493 currNodesSet = &nodes;
495 currNodesSet = &pushBackNodes;
498 for (FGTaxiNodeVectorIterator
499 itr = currNodesSet->begin(); itr != currNodesSet->end(); itr++) {
500 (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means
501 (*itr)->setPreviousNode(0); //
502 (*itr)->setPreviousSeg(0); //
505 FGTaxiNode *firstNode = findNode(start);
506 firstNode->setPathScore(0);
508 FGTaxiNode *lastNode = findNode(end);
510 FGTaxiNodeVector unvisited(*currNodesSet); // working copy
512 while (!unvisited.empty()) {
513 FGTaxiNode *best = *(unvisited.begin());
514 for (FGTaxiNodeVectorIterator
515 itr = unvisited.begin(); itr != unvisited.end(); itr++) {
516 if ((*itr)->getPathScore() < best->getPathScore())
520 FGTaxiNodeVectorIterator newend =
521 remove(unvisited.begin(), unvisited.end(), best);
522 unvisited.erase(newend, unvisited.end());
524 if (best == lastNode) { // found route or best not connected
527 for (FGTaxiSegmentVectorIterator
528 seg = best->getBeginRoute();
529 seg != best->getEndRoute(); seg++) {
530 if (fullSearch || (*seg)->isPushBack()) {
531 FGTaxiNode *tgt = (*seg)->getEnd();
533 best->getPathScore() + (*seg)->getLength() +
534 (*seg)->getPenalty(nParkings);
535 if (alt < tgt->getPathScore()) { // Relax (u,v)
536 tgt->setPathScore(alt);
537 tgt->setPreviousNode(best);
538 tgt->setPreviousSeg(*seg); //
541 // // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl;
547 if (lastNode->getPathScore() == HUGE_VAL) {
548 // no valid route found
550 SG_LOG(SG_GENERAL, SG_ALERT,
551 "Failed to find route from waypoint " << start << " to "
552 << end << " at " << parent->getId());
556 //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
558 // assemble route from backtrace information
559 intVec nodes, routes;
560 FGTaxiNode *bt = lastNode;
561 while (bt->getPreviousNode() != 0) {
562 nodes.push_back(bt->getIndex());
563 routes.push_back(bt->getPreviousSegment()->getIndex());
564 bt = bt->getPreviousNode();
566 nodes.push_back(start);
567 reverse(nodes.begin(), nodes.end());
568 reverse(routes.begin(), routes.end());
570 return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0);
574 int FGTaxiSegment::getPenalty(int nGates)
577 if (end->getIndex() < nGates) {
580 if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active.
586 /* ATC Related Functions */
588 void FGGroundNetwork::announcePosition(int id,
589 FGAIFlightPlan * intendedRoute,
590 int currentPosition, double lat,
591 double lon, double heading,
592 double speed, double alt,
593 double radius, int leg,
594 FGAIAircraft * aircraft)
597 TrafficVectorIterator i = activeTraffic.begin();
598 // Search search if the current id alread has an entry
599 // This might be faster using a map instead of a vector, but let's start by taking a safe route
600 if (activeTraffic.size()) {
601 //while ((i->getId() != id) && i != activeTraffic.end()) {
602 while (i != activeTraffic.end()) {
603 if (i->getId() == id) {
609 // Add a new TrafficRecord if no one exsists for this aircraft.
610 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
614 rec.setPositionAndIntentions(currentPosition, intendedRoute);
615 rec.setPositionAndHeading(lat, lon, heading, speed, alt);
616 rec.setRadius(radius); // only need to do this when creating the record.
617 rec.setAircraft(aircraft);
618 activeTraffic.push_back(rec);
620 i->setPositionAndIntentions(currentPosition, intendedRoute);
621 i->setPositionAndHeading(lat, lon, heading, speed, alt);
626 void FGGroundNetwork::signOff(int id)
628 TrafficVectorIterator i = activeTraffic.begin();
629 // Search search if the current id alread has an entry
630 // This might be faster using a map instead of a vector, but let's start by taking a safe route
631 if (activeTraffic.size()) {
632 //while ((i->getId() != id) && i != activeTraffic.end()) {
633 while (i != activeTraffic.end()) {
634 if (i->getId() == id) {
640 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
641 SG_LOG(SG_GENERAL, SG_ALERT,
642 "AI error: Aircraft without traffic record is signing off");
644 i = activeTraffic.erase(i);
648 * The ground network can deal with the following states:
649 * 0 = Normal; no action required
650 * 1 = "Acknowledge "Hold position
651 * 2 = "Acknowledge "Resume taxi".
652 * 3 = "Issue TaxiClearance"
653 * 4 = Acknowledge Taxi Clearance"
654 * 5 = Post acknowlegde taxiclearance: Start taxiing
656 * 7 = Acknowledge report runway
657 * 8 = Switch tower frequency
658 * 9 = Acknowledge switch tower frequency
659 *************************************************************************************************************************/
660 bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId,
663 int state = i->getState();
664 if ((state >= minState) && (state <= maxState) && available) {
665 if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) {
666 //cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl;
667 static SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true);
668 int n = trans_num->getIntValue();
670 trans_num->setIntValue(-1);
672 //cerr << "Selected transmission message " << n << endl;
673 FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc");
674 atc->getATCDialog()->removeEntry(1);
676 //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl;
677 transmit(&(*i), msgId, msgDir, false);
681 transmit(&(*i), msgId, msgDir, true);
683 lastTransmission = now;
690 void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
691 double heading, double speed, double alt,
694 time_t currentTime = time(NULL);
695 if (nextSave < currentTime) {
696 saveElevationCache();
697 nextSave = currentTime + 100 + rand() % 200;
699 // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
700 // Transmit air-to-ground "Ready to taxi request:
701 // Transmit ground to air approval / hold
702 // Transmit confirmation ...
703 // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
706 TrafficVectorIterator i = activeTraffic.begin();
707 // Search search if the current id has an entry
708 // This might be faster using a map instead of a vector, but let's start by taking a safe route
709 TrafficVectorIterator current, closest;
710 if (activeTraffic.size()) {
711 //while ((i->getId() != id) && i != activeTraffic.end()) {
712 while (i != activeTraffic.end()) {
713 if (i->getId() == id) {
719 // update position of the current aircraft
720 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
721 SG_LOG(SG_GENERAL, SG_ALERT,
722 "AI error: updating aircraft without traffic record");
724 i->setPositionAndHeading(lat, lon, heading, speed, alt);
730 // Update every three secs, but add some randomness
731 // to prevent all IA objects doing this in synchrony
732 //if (getDt() < (3.0) + (rand() % 10))
736 current->clearResolveCircularWait();
737 current->setWaitsForId(0);
738 checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
739 bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
740 if (!needsTaxiClearance) {
741 checkHoldPosition(id, lat, lon, heading, speed, alt);
742 if (checkForCircularWaits(id)) {
743 i->setResolveCircularWait();
746 current->setHoldPosition(true);
747 int state = current->getState();
748 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
749 if ((now - lastTransmission) > 15) {
752 if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
753 current->setState(3);
755 if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) {
756 current->setState(4);
758 if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
759 current->setState(5);
761 if ((state == 5) && available) {
762 current->setState(0);
763 current->getAircraft()->setTaxiClearanceRequest(false);
764 current->setHoldPosition(false);
772 Scan for a speed adjustment change. Find the nearest aircraft that is in front
773 and adjust speed when we get too close. Only do this when current position and/or
774 intentions of the current aircraft match current taxiroute position of the proximate
775 aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
776 instruction. See below for the hold position instruction.
778 Note that there currently still is one flaw in the logic that needs to be addressed.
779 There can be situations where one aircraft is in front of the current aircraft, on a separate
780 route, but really close after an intersection coming off the current route. This
781 aircraft is still close enough to block the current aircraft. This situation is currently
782 not addressed yet, but should be.
785 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
786 double lon, double heading,
787 double speed, double alt)
790 TrafficVectorIterator current, closest;
791 TrafficVectorIterator i = activeTraffic.begin();
792 bool otherReasonToSlowDown = false;
793 bool previousInstruction;
794 if (activeTraffic.size()) {
795 //while ((i->getId() != id) && (i != activeTraffic.end()))
796 while (i != activeTraffic.end()) {
797 if (i->getId() == id) {
805 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
806 SG_LOG(SG_GENERAL, SG_ALERT,
807 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment");
812 previousInstruction = current->getSpeedAdjustment();
813 double mindist = HUGE_VAL;
814 if (activeTraffic.size()) {
815 double course, dist, bearing, minbearing, az2;
816 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
817 //TrafficVector iterator closest;
819 for (TrafficVectorIterator i = activeTraffic.begin();
820 i != activeTraffic.end(); i++) {
825 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
828 SGGeodesy::inverse(curr, other, course, az2, dist);
829 bearing = fabs(heading - course);
831 bearing = 360 - bearing;
832 if ((dist < mindist) && (bearing < 60.0)) {
835 minbearing = bearing;
838 //Check traffic at the tower controller
839 if (towerController->hasActiveTraffic()) {
840 for (TrafficVectorIterator i =
841 towerController->getActiveTraffic().begin();
842 i != towerController->getActiveTraffic().end(); i++) {
843 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
844 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
847 SGGeodesy::inverse(curr, other, course, az2, dist);
848 bearing = fabs(heading - course);
850 bearing = 360 - bearing;
851 if ((dist < mindist) && (bearing < 60.0)) {
852 //cerr << "Current aircraft " << current->getAircraft()->getTrafficRef()->getCallSign()
853 // << " is closest to " << i->getAircraft()->getTrafficRef()->getCallSign()
854 // << ", which has status " << i->getAircraft()->isScheduledForTakeoff()
858 minbearing = bearing;
859 otherReasonToSlowDown = true;
863 // Finally, check UserPosition
864 // Note, as of 2011-08-01, this should no longer be necessecary.
866 double userLatitude = fgGetDouble("/position/latitude-deg");
867 double userLongitude = fgGetDouble("/position/longitude-deg");
868 SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
869 SGGeodesy::inverse(curr, user, course, az2, dist);
871 bearing = fabs(heading - course);
873 bearing = 360 - bearing;
874 if ((dist < mindist) && (bearing < 60.0)) {
877 minbearing = bearing;
878 otherReasonToSlowDown = true;
881 current->clearSpeedAdjustment();
883 if (current->checkPositionAndIntentions(*closest)
884 || otherReasonToSlowDown) {
885 double maxAllowableDistance =
886 (1.1 * current->getRadius()) +
887 (1.1 * closest->getRadius());
888 if (mindist < 2 * maxAllowableDistance) {
889 if (current->getId() == closest->getWaitsForId())
892 current->setWaitsForId(closest->getId());
893 if (closest->getId() != current->getId()) {
894 current->setSpeedAdjustment(closest->getSpeed() *
897 closest->getAircraft()->getTakeOffStatus() &&
898 (current->getAircraft()->getTrafficRef()->getDepartureAirport() == closest->getAircraft()->getTrafficRef()->getDepartureAirport()) &&
899 (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway())
901 current->getAircraft()->scheduleForATCTowerDepartureControl(1);
903 current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
905 if (mindist < maxAllowableDistance) {
906 //double newSpeed = (maxAllowableDistance-mindist);
907 //current->setSpeedAdjustment(newSpeed);
908 //if (mindist < 0.5* maxAllowableDistance)
910 current->setSpeedAdjustment(0);
919 Check for "Hold position instruction".
920 The hold position should be issued under the following conditions:
921 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
922 2) For taxiing aircraft that use one taxiway in opposite directions
923 3) For crossing or merging taxiroutes.
926 void FGGroundNetwork::checkHoldPosition(int id, double lat,
927 double lon, double heading,
928 double speed, double alt)
930 TrafficVectorIterator current;
931 TrafficVectorIterator i = activeTraffic.begin();
932 if (activeTraffic.size()) {
933 //while ((i->getId() != id) && i != activeTraffic.end())
934 while (i != activeTraffic.end()) {
935 if (i->getId() == id) {
943 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
944 SG_LOG(SG_GENERAL, SG_ALERT,
945 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition");
948 bool origStatus = current->hasHoldPosition();
949 current->setHoldPosition(false);
950 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
952 for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
953 if (i->getId() != current->getId()) {
954 int node = current->crosses(this, *i);
956 FGTaxiNode *taxiNode = findNode(node);
958 // Determine whether it's save to continue or not.
959 // If we have a crossing route, there are two possibilities:
960 // 1) This is an interestion
961 // 2) This is oncoming two-way traffic, using the same taxiway.
962 //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
964 SGGeod other(SGGeod::
965 fromDegM(i->getLongitude(), i->getLatitude(),
969 if (current->isOpposing(this, *i, node)) {
972 //cerr << "Hold check 2 : " << node << " has opposing segment " << endl;
973 // issue a "Hold Position" as soon as we're close to the offending node
974 // For now, I'm doing this as long as the other aircraft doesn't
975 // have a hold instruction as soon as we're within a reasonable
976 // distance from the offending node.
977 // This may be a bit of a conservative estimate though, as it may
978 // be well possible that both aircraft can both continue to taxi
979 // without crashing into each other.
982 if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 2.0*i->getRadius())
985 //cerr << "Hold check 3 : " << id <<" Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue "
989 //cerr << "Hold check 4: " << id << " Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
994 SGGeodesy::distanceM(curr, taxiNode->getGeod());
995 if (!(i->hasHoldPosition())) {
997 if ((dist < 200) && //2.5*current->getRadius()) &&
998 (needsToWait) && (i->onRoute(this, *current)) &&
999 //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) &&
1000 (!(current->getId() == i->getWaitsForId())))
1001 //(!(i->getSpeedAdjustment()))) // &&
1002 //(!(current->getSpeedAdjustment())))
1005 if (!(isUserAircraft(i->getAircraft()))) { // test code. Don't wait for the user, let the user wait for you.
1006 current->setHoldPosition(true);
1007 current->setWaitsForId(i->getId());
1009 //cerr << "Hold check 5: " << current->getCallSign() <<" Setting Hold Position: distance to node (" << node << ") "
1010 // << dist << " meters. Waiting for " << i->getCallSign();
1012 //cerr <<" [opposing] " << endl;
1014 // cerr << "[non-opposing] " << endl;
1015 //if (i->hasSpeefAdjustment())
1017 // cerr << " (which in turn waits for ) " << i->
1019 //cerr << "Hold check 6: " << id << " No need to hold yet: Distance to node : " << dist << " nm"<< endl;
1025 bool currStatus = current->hasHoldPosition();
1026 current->setHoldPosition(origStatus);
1027 // Either a Hold Position or a resume taxi transmission has been issued
1028 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1029 if ((now - lastTransmission) > 2) {
1032 if (current->getState() == 0) {
1033 if ((origStatus != currStatus) && available) {
1034 //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
1035 if (currStatus == true) { // No has a hold short instruction
1036 transmit(&(*current), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
1037 //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
1038 current->setState(1);
1040 transmit(&(*current), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
1041 //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
1042 current->setState(2);
1044 lastTransmission = now;
1046 // Don't act on the changed instruction until the transmission is confirmed
1047 // So set back to original status
1048 //cerr << "Current state " << current->getState() << endl;
1052 // 6 = Report runway
1053 // 7 = Acknowledge report runway
1054 // 8 = Switch tower frequency
1055 //9 = Acknowledge switch tower frequency
1057 //int state = current->getState();
1058 if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
1059 current->setState(0);
1060 current->setHoldPosition(true);
1062 if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
1063 current->setState(0);
1064 current->setHoldPosition(false);
1066 if (current->getAircraft()->getTakeOffStatus() && (current->getState() == 0)) {
1067 //cerr << "Scheduling " << current->getAircraft()->getCallSign() << " for hold short" << endl;
1068 current->setState(6);
1070 if (checkTransmissionState(6,6, current, now, MSG_REPORT_RUNWAY_HOLD_SHORT, ATC_AIR_TO_GROUND)) {
1072 if (checkTransmissionState(7,7, current, now, MSG_ACKNOWLEDGE_REPORT_RUNWAY_HOLD_SHORT, ATC_GROUND_TO_AIR)) {
1074 if (checkTransmissionState(8,8, current, now, MSG_SWITCH_TOWER_FREQUENCY, ATC_GROUND_TO_AIR)) {
1076 if (checkTransmissionState(9,9, current, now, MSG_ACKNOWLEDGE_SWITCH_TOWER_FREQUENCY, ATC_AIR_TO_GROUND)) {
1081 //current->setState(0);
1085 * Check whether situations occur where the current aircraft is waiting for itself
1086 * due to higher order interactions.
1087 * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
1088 * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
1089 * through this list of waiting aircraft, we can check if we'd eventually end back
1090 * at the current aircraft.
1092 * Note that we should consider the situation where we are actually checking aircraft
1093 * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
1094 * the looping aircraft. If we don't check for that, this function will get stuck into
1098 bool FGGroundNetwork::checkForCircularWaits(int id)
1100 //cerr << "Performing Wait check " << id << endl;
1102 TrafficVectorIterator current, other;
1103 TrafficVectorIterator i = activeTraffic.begin();
1104 int trafficSize = activeTraffic.size();
1106 while (i != activeTraffic.end()) {
1107 if (i->getId() == id) {
1115 if (i == activeTraffic.end() || (trafficSize == 0)) {
1116 SG_LOG(SG_GENERAL, SG_ALERT,
1117 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1121 target = current->getWaitsForId();
1122 //bool printed = false; // Note that this variable is for debugging purposes only.
1126 //cerr << "aircraft waits for user" << endl;
1131 while ((target > 0) && (target != id) && counter++ < trafficSize) {
1133 TrafficVectorIterator i = activeTraffic.begin();
1135 //while ((i->getId() != id) && i != activeTraffic.end())
1136 while (i != activeTraffic.end()) {
1137 if (i->getId() == target) {
1145 if (i == activeTraffic.end() || (trafficSize == 0)) {
1146 //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
1147 // The target id is not found on the current network, which means it's at the tower
1148 //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1152 target = other->getWaitsForId();
1154 // actually this trap isn't as impossible as it first seemed:
1155 // the setWaitsForID(id) is set to current when the aircraft
1156 // is waiting for the user controlled aircraft.
1157 //if (current->getId() == other->getId()) {
1158 // cerr << "Caught the impossible trap" << endl;
1159 // cerr << "Current = " << current->getId() << endl;
1160 // cerr << "Other = " << other ->getId() << endl;
1161 // for (TrafficVectorIterator at = activeTraffic.begin();
1162 // at != activeTraffic.end();
1164 // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1167 if (current->getId() == other->getId())
1170 //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign()
1171 // << " (" << other->getId() << "); " << endl;;
1181 // cerr << "[done] " << endl << endl;;
1183 SG_LOG(SG_GENERAL, SG_WARN,
1184 "Detected circular wait condition: Id = " << id <<
1185 "target = " << target);
1192 // Note that this function is probably obsolete...
1193 bool FGGroundNetwork::hasInstruction(int id)
1195 TrafficVectorIterator i = activeTraffic.begin();
1196 // Search search if the current id has an entry
1197 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1198 if (activeTraffic.size()) {
1199 //while ((i->getId() != id) && i != activeTraffic.end()) {
1200 while (i != activeTraffic.end()) {
1201 if (i->getId() == id) {
1207 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1208 SG_LOG(SG_GENERAL, SG_ALERT,
1209 "AI error: checking ATC instruction for aircraft without traffic record");
1211 return i->hasInstruction();
1216 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1218 TrafficVectorIterator i = activeTraffic.begin();
1219 // Search search if the current id has an entry
1220 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1221 if (activeTraffic.size()) {
1222 //while ((i->getId() != id) && i != activeTraffic.end()) {
1223 while (i != activeTraffic.end()) {
1224 if (i->getId() == id) {
1230 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1231 SG_LOG(SG_GENERAL, SG_ALERT,
1232 "AI error: requesting ATC instruction for aircraft without traffic record");
1234 return i->getInstruction();
1236 return FGATCInstruction();
1239 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
1240 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
1241 double lon, double elev, double hdg, double slope)
1243 SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
1244 obj_pos = geod.makeZUpFrame();
1245 // hdg is not a compass heading, but a counter-clockwise rotation
1246 // around the Z axis
1247 obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
1249 obj_pos.preMult(osg::Matrix::rotate(slope * SGD_DEGREES_TO_RADIANS,
1256 void FGGroundNetwork::render(bool visible)
1259 SGMaterialLib *matlib = globals->get_matlib();
1262 globals->get_scenery()->get_scene_graph()->removeChild(group);
1263 //while (group->getNumChildren()) {
1264 // cerr << "Number of children: " << group->getNumChildren() << endl;
1265 //simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
1266 //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
1267 //geode->releaseGLObjects();
1268 //group->removeChild(geode);
1273 group = new osg::Group;
1274 FGScenery * local_scenery = globals->get_scenery();
1275 double elevation_meters = 0.0;
1276 double elevation_feet = 0.0;
1277 //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
1279 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1280 // Handle start point
1281 int pos = i->getCurrentPosition() - 1;
1284 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1285 SGGeod end (SGGeod::fromDeg(segments[pos]->getEnd()->getLongitude(), segments[pos]->getEnd()->getLatitude()));
1287 double length = SGGeodesy::distanceM(start, end);
1288 //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
1290 double az2, heading; //, distanceM;
1291 SGGeodesy::inverse(start, end, heading, az2, length);
1292 double coveredDistance = length * 0.5;
1294 SGGeodesy::direct(start, heading, coveredDistance, center, az2);
1295 //cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
1296 ///////////////////////////////////////////////////////////////////////////////
1297 // Make a helper function out of this
1298 osg::Matrix obj_pos;
1299 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1300 obj_trans->setDataVariance(osg::Object::STATIC);
1301 // Experimental: Calculate slope here, based on length, and the individual elevations
1302 double elevationStart;
1303 if (isUserAircraft((i)->getAircraft())) {
1304 elevationStart = fgGetDouble("/position/ground-elev-m");
1306 elevationStart = ((i)->getAircraft()->_getAltitude());
1308 double elevationEnd = segments[pos]->getEnd()->getElevation();
1309 //cerr << "Using elevation " << elevationEnd << endl;
1311 if (elevationEnd == 0) {
1312 SGGeod center2 = end;
1313 center2.setElevationM(SG_MAX_ELEVATION_M);
1314 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1315 elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1316 //elevation_meters += 0.5;
1319 elevationEnd = parent->getElevation()+8+dx;
1321 segments[pos]->getEnd()->setElevation(elevationEnd);
1323 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1324 double elevDiff = elevationEnd - elevationStart;
1326 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1328 //cerr << "1. Using mean elevation : " << elevationMean << " and " << slope << endl;
1330 WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), elevationMean+ 0.5, -(heading), slope );
1332 obj_trans->setMatrix( obj_pos );
1333 //osg::Vec3 center(0, 0, 0)
1335 float width = length /2.0;
1336 osg::Vec3 corner(-width, 0, 0.25f);
1337 osg::Vec3 widthVec(2*width + 1, 0, 0);
1338 osg::Vec3 heightVec(0, 1, 0);
1339 osg::Geometry* geometry;
1340 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1341 simgear::EffectGeode* geode = new simgear::EffectGeode;
1342 geode->setName("test");
1343 geode->addDrawable(geometry);
1344 //osg::Node *custom_obj;
1345 SGMaterial *mat = matlib->find("UnidirectionalTaper");
1347 geode->setEffect(mat->get_effect());
1348 obj_trans->addChild(geode);
1349 // wire as much of the scene graph together as we can
1350 //->addChild( obj_trans );
1351 group->addChild( obj_trans );
1352 /////////////////////////////////////////////////////////////////////
1354 //cerr << "BIG FAT WARNING: current position is here : " << pos << endl;
1356 for(intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
1357 osg::Matrix obj_pos;
1360 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1361 obj_trans->setDataVariance(osg::Object::STATIC);
1363 // Experimental: Calculate slope here, based on length, and the individual elevations
1364 double elevationStart = segments[k]->getStart()->getElevation();
1365 double elevationEnd = segments[k]->getEnd ()->getElevation();
1366 if (elevationStart == 0) {
1367 SGGeod center2 = segments[k]->getStart()->getGeod();
1368 center2.setElevationM(SG_MAX_ELEVATION_M);
1369 if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) {
1370 elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5;
1371 //elevation_meters += 0.5;
1374 elevationStart = parent->getElevation()+8+dx;
1376 segments[k]->getStart()->setElevation(elevationStart);
1378 if (elevationEnd == 0) {
1379 SGGeod center2 = segments[k]->getEnd()->getGeod();
1380 center2.setElevationM(SG_MAX_ELEVATION_M);
1381 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1382 elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1383 //elevation_meters += 0.5;
1386 elevationEnd = parent->getElevation()+8+dx;
1388 segments[k]->getEnd()->setElevation(elevationEnd);
1391 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1392 double elevDiff = elevationEnd - elevationStart;
1393 double length = segments[k]->getLength();
1394 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1396 // cerr << "2. Using mean elevation : " << elevationMean << " and " << slope << endl;
1399 WorldCoordinate( obj_pos, segments[k]->getLatitude(), segments[k]->getLongitude(), elevationMean+ 0.5, -(segments[k]->getHeading()), slope );
1401 obj_trans->setMatrix( obj_pos );
1402 //osg::Vec3 center(0, 0, 0)
1404 float width = segments[k]->getLength() /2.0;
1405 osg::Vec3 corner(-width, 0, 0.25f);
1406 osg::Vec3 widthVec(2*width + 1, 0, 0);
1407 osg::Vec3 heightVec(0, 1, 0);
1408 osg::Geometry* geometry;
1409 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1410 simgear::EffectGeode* geode = new simgear::EffectGeode;
1411 geode->setName("test");
1412 geode->addDrawable(geometry);
1413 //osg::Node *custom_obj;
1414 SGMaterial *mat = matlib->find("UnidirectionalTaper");
1416 geode->setEffect(mat->get_effect());
1417 obj_trans->addChild(geode);
1418 // wire as much of the scene graph together as we can
1419 //->addChild( obj_trans );
1420 group->addChild( obj_trans );
1425 globals->get_scenery()->get_scene_graph()->addChild(group);
1429 string FGGroundNetwork::getName() {
1430 return string(parent->getId() + "-ground");