1 // GroundController.hxx - forked from groundnetwork.cxx
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>
45 #include <simgear/timing/sg_time.hxx>
47 #include <Airports/airport.hxx>
48 #include <Airports/dynamics.hxx>
49 #include <Airports/runways.hxx>
50 #include <Airports/groundnetwork.hxx>
52 #include <Main/globals.hxx>
53 #include <Main/fg_props.hxx>
54 #include <AIModel/AIAircraft.hxx>
55 #include <AIModel/performancedata.hxx>
56 #include <AIModel/AIFlightPlan.hxx>
57 #include <Navaids/NavDataCache.hxx>
59 #include <ATC/atc_mgr.hxx>
61 #include <Scenery/scenery.hxx>
63 #include "GroundController.hxx"
68 /***************************************************************************
69 * FGGroundController()
70 **************************************************************************/
72 bool compare_trafficrecords(FGTrafficRecord a, FGTrafficRecord b)
74 return (a.getIntentions().size() < b.getIntentions().size());
77 FGGroundController::FGGroundController() :
82 currTraffic = activeTraffic.begin();
85 networkInitialized = false;
89 FGGroundController::~FGGroundController()
93 void FGGroundController::init(FGAirportDynamics* aDynamics)
95 FGATCController::init();
98 parent = dynamics->parent();
100 networkInitialized = true;
103 void FGGroundController::announcePosition(int id,
104 FGAIFlightPlan * intendedRoute,
105 int currentPosition, double lat,
106 double lon, double heading,
107 double speed, double alt,
108 double radius, int leg,
109 FGAIAircraft * aircraft)
111 if (!aircraft || !aircraft->getPerformance()) {
112 SG_LOG(SG_ATC, SG_ALERT, "announcePosition: missing aircraft performance");
116 TrafficVectorIterator i = activeTraffic.begin();
117 // Search search if the current id alread has an entry
118 // This might be faster using a map instead of a vector, but let's start by taking a safe route
119 if (activeTraffic.size()) {
120 //while ((i->getId() != id) && i != activeTraffic.end()) {
121 while (i != activeTraffic.end()) {
122 if (i->getId() == id) {
128 // Add a new TrafficRecord if no one exsists for this aircraft.
129 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
133 rec.setPositionAndIntentions(currentPosition, intendedRoute);
134 rec.setPositionAndHeading(lat, lon, heading, speed, alt);
135 rec.setRadius(radius); // only need to do this when creating the record.
136 rec.setAircraft(aircraft);
138 activeTraffic.push_front(rec);
140 activeTraffic.push_back(rec);
144 i->setPositionAndIntentions(currentPosition, intendedRoute);
145 i->setPositionAndHeading(lat, lon, heading, speed, alt);
150 void FGGroundController::signOff(int id)
152 TrafficVectorIterator i = activeTraffic.begin();
153 // Search search if the current id alread has an entry
154 // This might be faster using a map instead of a vector, but let's start by taking a safe route
155 if (activeTraffic.size()) {
156 //while ((i->getId() != id) && i != activeTraffic.end()) {
157 while (i != activeTraffic.end()) {
158 if (i->getId() == id) {
164 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
165 SG_LOG(SG_GENERAL, SG_ALERT,
166 "AI error: Aircraft without traffic record is signing off at " << SG_ORIGIN);
168 i = activeTraffic.erase(i);
172 * The ground network can deal with the following states:
173 * 0 = Normal; no action required
174 * 1 = "Acknowledge "Hold position
175 * 2 = "Acknowledge "Resume taxi".
176 * 3 = "Issue TaxiClearance"
177 * 4 = Acknowledge Taxi Clearance"
178 * 5 = Post acknowlegde taxiclearance: Start taxiing
180 * 7 = Acknowledge report runway
181 * 8 = Switch tower frequency
182 * 9 = Acknowledge switch tower frequency
183 *************************************************************************************************************************/
184 bool FGGroundController::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId,
187 int state = i->getState();
188 if ((state >= minState) && (state <= maxState) && available) {
189 if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) {
190 //cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl;
191 SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true);
192 int n = trans_num->getIntValue();
194 trans_num->setIntValue(-1);
196 //cerr << "Selected transmission message " << n << endl;
197 //FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc");
198 FGATCDialogNew::instance()->removeEntry(1);
200 //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl;
201 transmit(&(*i), dynamics, msgId, msgDir, false);
205 transmit(&(*i), dynamics, msgId, msgDir, true);
207 lastTransmission = now;
214 void FGGroundController::updateAircraftInformation(int id, double lat, double lon,
215 double heading, double speed, double alt,
218 // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
219 // Transmit air-to-ground "Ready to taxi request:
220 // Transmit ground to air approval / hold
221 // Transmit confirmation ...
222 // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
225 TrafficVectorIterator i = activeTraffic.begin();
226 // Search search if the current id has an entry
227 // This might be faster using a map instead of a vector, but let's start by taking a safe route
228 TrafficVectorIterator current, closest;
229 if (activeTraffic.size()) {
230 //while ((i->getId() != id) && i != activeTraffic.end()) {
231 while (i != activeTraffic.end()) {
232 if (i->getId() == id) {
238 // update position of the current aircraft
239 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
240 SG_LOG(SG_GENERAL, SG_ALERT,
241 "AI error: updating aircraft without traffic record at " << SG_ORIGIN);
243 i->setPositionAndHeading(lat, lon, heading, speed, alt);
249 // Update every three secs, but add some randomness
250 // to prevent all IA objects doing this in synchrony
251 //if (getDt() < (3.0) + (rand() % 10))
255 current->clearResolveCircularWait();
256 current->setWaitsForId(0);
257 checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
258 bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
259 if (!needsTaxiClearance) {
260 checkHoldPosition(id, lat, lon, heading, speed, alt);
261 //if (checkForCircularWaits(id)) {
262 // i->setResolveCircularWait();
265 current->setHoldPosition(true);
266 int state = current->getState();
267 time_t now = globals->get_time_params()->get_cur_time();
269 if ((now - lastTransmission) > 15) {
272 if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
273 current->setState(3);
275 if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) {
276 current->setState(4);
278 if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
279 current->setState(5);
281 if ((state == 5) && available) {
282 current->setState(0);
283 current->getAircraft()->setTaxiClearanceRequest(false);
284 current->setHoldPosition(false);
292 Scan for a speed adjustment change. Find the nearest aircraft that is in front
293 and adjust speed when we get too close. Only do this when current position and/or
294 intentions of the current aircraft match current taxiroute position of the proximate
295 aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
296 instruction. See below for the hold position instruction.
298 Note that there currently still is one flaw in the logic that needs to be addressed.
299 There can be situations where one aircraft is in front of the current aircraft, on a separate
300 route, but really close after an intersection coming off the current route. This
301 aircraft is still close enough to block the current aircraft. This situation is currently
302 not addressed yet, but should be.
305 void FGGroundController::checkSpeedAdjustment(int id, double lat,
306 double lon, double heading,
307 double speed, double alt)
310 TrafficVectorIterator current, closest, closestOnNetwork;
311 TrafficVectorIterator i = activeTraffic.begin();
312 bool otherReasonToSlowDown = false;
313 // bool previousInstruction;
314 if (activeTraffic.size()) {
315 //while ((i->getId() != id) && (i != activeTraffic.end()))
316 while (i != activeTraffic.end()) {
317 if (i->getId() == id) {
325 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
326 SG_LOG(SG_GENERAL, SG_ALERT,
327 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment at " << SG_ORIGIN);
332 // previousInstruction = current->getSpeedAdjustment();
333 double mindist = HUGE_VAL;
334 if (activeTraffic.size()) {
335 double course, dist, bearing, az2; // minbearing,
336 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
337 //TrafficVector iterator closest;
339 closestOnNetwork = current;
340 for (TrafficVectorIterator i = activeTraffic.begin();
341 i != activeTraffic.end(); i++) {
346 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
349 SGGeodesy::inverse(curr, other, course, az2, dist);
350 bearing = fabs(heading - course);
352 bearing = 360 - bearing;
353 if ((dist < mindist) && (bearing < 60.0)) {
356 closestOnNetwork = i;
357 // minbearing = bearing;
361 //Check traffic at the tower controller
362 if (towerController->hasActiveTraffic()) {
363 for (TrafficVectorIterator i =
364 towerController->getActiveTraffic().begin();
365 i != towerController->getActiveTraffic().end(); i++) {
366 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
367 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
370 SGGeodesy::inverse(curr, other, course, az2, dist);
371 bearing = fabs(heading - course);
373 bearing = 360 - bearing;
374 if ((dist < mindist) && (bearing < 60.0)) {
375 //cerr << "Current aircraft " << current->getAircraft()->getTrafficRef()->getCallSign()
376 // << " is closest to " << i->getAircraft()->getTrafficRef()->getCallSign()
377 // << ", which has status " << i->getAircraft()->isScheduledForTakeoff()
381 // minbearing = bearing;
382 otherReasonToSlowDown = true;
386 // Finally, check UserPosition
387 // Note, as of 2011-08-01, this should no longer be necessecary.
389 double userLatitude = fgGetDouble("/position/latitude-deg");
390 double userLongitude = fgGetDouble("/position/longitude-deg");
391 SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
392 SGGeodesy::inverse(curr, user, course, az2, dist);
394 bearing = fabs(heading - course);
396 bearing = 360 - bearing;
397 if ((dist < mindist) && (bearing < 60.0)) {
400 minbearing = bearing;
401 otherReasonToSlowDown = true;
404 current->clearSpeedAdjustment();
405 bool needBraking = false;
406 if (current->checkPositionAndIntentions(*closest)
407 || otherReasonToSlowDown) {
408 double maxAllowableDistance =
409 (1.1 * current->getRadius()) +
410 (1.1 * closest->getRadius());
411 if (mindist < 2 * maxAllowableDistance) {
412 if (current->getId() == closest->getWaitsForId())
415 current->setWaitsForId(closest->getId());
416 if (closest->getId() != current->getId()) {
417 current->setSpeedAdjustment(closest->getSpeed() *
422 // closest->getAircraft()->getTakeOffStatus() &&
423 // (current->getAircraft()->getTrafficRef()->getDepartureAirport() == closest->getAircraft()->getTrafficRef()->getDepartureAirport()) &&
424 // (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway())
426 // current->getAircraft()->scheduleForATCTowerDepartureControl(1);
428 current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
430 if (mindist < maxAllowableDistance) {
431 //double newSpeed = (maxAllowableDistance-mindist);
432 //current->setSpeedAdjustment(newSpeed);
433 //if (mindist < 0.5* maxAllowableDistance)
435 current->setSpeedAdjustment(0);
440 if ((closest->getId() == closestOnNetwork->getId()) && (current->getPriority() < closest->getPriority()) && needBraking) {
441 swap(current, closest);
447 Check for "Hold position instruction".
448 The hold position should be issued under the following conditions:
449 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
450 2) For taxiing aircraft that use one taxiway in opposite directions
451 3) For crossing or merging taxiroutes.
454 void FGGroundController::checkHoldPosition(int id, double lat,
455 double lon, double heading,
456 double speed, double alt)
458 FGGroundNetwork* network = dynamics->getGroundNetwork();
459 TrafficVectorIterator current;
460 TrafficVectorIterator i = activeTraffic.begin();
461 if (activeTraffic.size()) {
462 //while ((i->getId() != id) && i != activeTraffic.end())
463 while (i != activeTraffic.end()) {
464 if (i->getId() == id) {
473 time_t now = globals->get_time_params()->get_cur_time();
474 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
475 SG_LOG(SG_GENERAL, SG_ALERT,
476 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition at " << SG_ORIGIN);
480 if (current->getAircraft()->getTakeOffStatus() == 1) {
481 current->setHoldPosition(true);
484 if (current->getAircraft()->getTakeOffStatus() == 2) {
485 //cerr << current->getAircraft()->getCallSign() << ". Taxi in position and hold" << endl;
486 current->setHoldPosition(false);
487 current->clearSpeedAdjustment();
490 bool origStatus = current->hasHoldPosition();
491 current->setHoldPosition(false);
492 //SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
493 int currentRoute = i->getCurrentPosition();
495 if (i->getIntentions().size()) {
496 nextRoute = (*(i->getIntentions().begin()));
500 if (currentRoute > 0) {
501 FGTaxiSegment *tx = network->findSegment(currentRoute);
504 nx = network->findSegment(nextRoute);
508 //if (tx->hasBlock(now) || nx->hasBlock(now) ) {
509 // current->setHoldPosition(true);
511 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
512 SGGeod end (nx->getStart()->geod());
514 double distance = SGGeodesy::distanceM(start, end);
515 if (nx->hasBlock(now) && (distance < i->getRadius() * 4)) {
516 current->setHoldPosition(true);
518 intVecIterator ivi = i->getIntentions().begin();
519 while (ivi != i->getIntentions().end()) {
521 FGTaxiSegment* seg = network->findSegment(*ivi);
522 distance += seg->getLength();
523 if ((seg->hasBlock(now)) && (distance < i->getRadius() * 4)) {
524 current->setHoldPosition(true);
532 bool currStatus = current->hasHoldPosition();
533 current->setHoldPosition(origStatus);
534 // Either a Hold Position or a resume taxi transmission has been issued
535 if ((now - lastTransmission) > 2) {
538 if (current->getState() == 0) {
539 if ((origStatus != currStatus) && available) {
540 //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
541 if (currStatus == true) { // No has a hold short instruction
542 transmit(&(*current), dynamics, MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
543 //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
544 current->setState(1);
546 transmit(&(*current), dynamics, MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
547 //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
548 current->setState(2);
550 lastTransmission = now;
552 // Don't act on the changed instruction until the transmission is confirmed
553 // So set back to original status
554 //cerr << "Current state " << current->getState() << endl;
559 // 7 = Acknowledge report runway
560 // 8 = Switch tower frequency
561 //9 = Acknowledge switch tower frequency
563 //int state = current->getState();
564 if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
565 current->setState(0);
566 current->setHoldPosition(true);
568 if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
569 current->setState(0);
570 current->setHoldPosition(false);
572 if (current->getAircraft()->getTakeOffStatus() && (current->getState() == 0)) {
573 //cerr << "Scheduling " << current->getAircraft()->getCallSign() << " for hold short" << endl;
574 current->setState(6);
576 if (checkTransmissionState(6,6, current, now, MSG_REPORT_RUNWAY_HOLD_SHORT, ATC_AIR_TO_GROUND)) {
578 if (checkTransmissionState(7,7, current, now, MSG_ACKNOWLEDGE_REPORT_RUNWAY_HOLD_SHORT, ATC_GROUND_TO_AIR)) {
580 if (checkTransmissionState(8,8, current, now, MSG_SWITCH_TOWER_FREQUENCY, ATC_GROUND_TO_AIR)) {
582 if (checkTransmissionState(9,9, current, now, MSG_ACKNOWLEDGE_SWITCH_TOWER_FREQUENCY, ATC_AIR_TO_GROUND)) {
587 //current->setState(0);
591 * Check whether situations occur where the current aircraft is waiting for itself
592 * due to higher order interactions.
593 * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
594 * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
595 * through this list of waiting aircraft, we can check if we'd eventually end back
596 * at the current aircraft.
598 * Note that we should consider the situation where we are actually checking aircraft
599 * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
600 * the looping aircraft. If we don't check for that, this function will get stuck into
604 bool FGGroundController::checkForCircularWaits(int id)
606 //cerr << "Performing Wait check " << id << endl;
608 TrafficVectorIterator current, other;
609 TrafficVectorIterator i = activeTraffic.begin();
610 int trafficSize = activeTraffic.size();
612 while (i != activeTraffic.end()) {
613 if (i->getId() == id) {
621 if (i == activeTraffic.end() || (trafficSize == 0)) {
622 SG_LOG(SG_GENERAL, SG_ALERT,
623 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits at " << SG_ORIGIN);
627 target = current->getWaitsForId();
628 //bool printed = false; // Note that this variable is for debugging purposes only.
632 //cerr << "aircraft waits for user" << endl;
637 while ((target > 0) && (target != id) && counter++ < trafficSize) {
639 TrafficVectorIterator i = activeTraffic.begin();
641 //while ((i->getId() != id) && i != activeTraffic.end())
642 while (i != activeTraffic.end()) {
643 if (i->getId() == target) {
651 if (i == activeTraffic.end() || (trafficSize == 0)) {
652 //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
653 // The target id is not found on the current network, which means it's at the tower
654 //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
658 target = other->getWaitsForId();
660 // actually this trap isn't as impossible as it first seemed:
661 // the setWaitsForID(id) is set to current when the aircraft
662 // is waiting for the user controlled aircraft.
663 //if (current->getId() == other->getId()) {
664 // cerr << "Caught the impossible trap" << endl;
665 // cerr << "Current = " << current->getId() << endl;
666 // cerr << "Other = " << other ->getId() << endl;
667 // for (TrafficVectorIterator at = activeTraffic.begin();
668 // at != activeTraffic.end();
670 // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
673 if (current->getId() == other->getId())
676 //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign()
677 // << " (" << other->getId() << "); " << endl;;
687 // cerr << "[done] " << endl << endl;;
689 SG_LOG(SG_GENERAL, SG_WARN,
690 "Detected circular wait condition: Id = " << id <<
691 "target = " << target);
698 // Note that this function is probably obsolete...
699 bool FGGroundController::hasInstruction(int id)
701 TrafficVectorIterator i = activeTraffic.begin();
702 // Search search if the current id has an entry
703 // This might be faster using a map instead of a vector, but let's start by taking a safe route
704 if (activeTraffic.size()) {
705 //while ((i->getId() != id) && i != activeTraffic.end()) {
706 while (i != activeTraffic.end()) {
707 if (i->getId() == id) {
713 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
714 SG_LOG(SG_GENERAL, SG_ALERT,
715 "AI error: checking ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
717 return i->hasInstruction();
722 FGATCInstruction FGGroundController::getInstruction(int id)
724 TrafficVectorIterator i = activeTraffic.begin();
725 // Search search if the current id has an entry
726 // This might be faster using a map instead of a vector, but let's start by taking a safe route
727 if (activeTraffic.size()) {
728 //while ((i->getId() != id) && i != activeTraffic.end()) {
729 while (i != activeTraffic.end()) {
730 if (i->getId() == id) {
736 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
737 SG_LOG(SG_GENERAL, SG_ALERT,
738 "AI error: requesting ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
740 return i->getInstruction();
742 return FGATCInstruction();
745 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
746 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
747 double lon, double elev, double hdg, double slope)
749 SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
750 obj_pos = makeZUpFrame(geod);
751 // hdg is not a compass heading, but a counter-clockwise rotation
753 obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
755 obj_pos.preMult(osg::Matrix::rotate(slope * SGD_DEGREES_TO_RADIANS,
762 void FGGroundController::render(bool visible)
764 SGMaterialLib *matlib = globals->get_matlib();
765 FGGroundNetwork* network = dynamics->getGroundNetwork();
769 globals->get_scenery()->get_scene_graph()->removeChild(group);
770 //while (group->getNumChildren()) {
771 // cerr << "Number of children: " << group->getNumChildren() << endl;
772 //simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
773 //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
774 //geode->releaseGLObjects();
775 //group->removeChild(geode);
780 group = new osg::Group;
781 FGScenery * local_scenery = globals->get_scenery();
782 // double elevation_meters = 0.0;
783 // double elevation_feet = 0.0;
784 time_t now = globals->get_time_params()->get_cur_time();
786 //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
789 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
790 // Handle start point i.e. the segment that is connected to the aircraft itself on the starting end
791 // and to the the first "real" taxi segment on the other end.
792 const int pos = i->getCurrentPosition();
794 FGTaxiSegment* segment = network->findSegment(pos);
795 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
796 SGGeod end (segment->getEnd()->geod());
798 double length = SGGeodesy::distanceM(start, end);
799 //heading = SGGeodesy::headingDeg(start->geod(), end->geod());
801 double az2, heading; //, distanceM;
802 SGGeodesy::inverse(start, end, heading, az2, length);
803 double coveredDistance = length * 0.5;
805 SGGeodesy::direct(start, heading, coveredDistance, center, az2);
806 //std::cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << std::endl;
807 ///////////////////////////////////////////////////////////////////////////////
808 // Make a helper function out of this
810 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
811 obj_trans->setDataVariance(osg::Object::STATIC);
812 // Experimental: Calculate slope here, based on length, and the individual elevations
813 double elevationStart;
814 if (isUserAircraft((i)->getAircraft())) {
815 elevationStart = fgGetDouble("/position/ground-elev-m");
817 elevationStart = ((i)->getAircraft()->_getAltitude());
819 double elevationEnd = segment->getEnd()->getElevationM();
820 //cerr << "Using elevation " << elevationEnd << endl;
822 if ((elevationEnd == 0) || (elevationEnd = parent->getElevation())) {
823 SGGeod center2 = end;
824 center2.setElevationM(SG_MAX_ELEVATION_M);
825 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
826 // elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
827 //elevation_meters += 0.5;
830 elevationEnd = parent->getElevation();
832 segment->getEnd()->setElevation(elevationEnd);
834 double elevationMean = (elevationStart + elevationEnd) / 2.0;
835 double elevDiff = elevationEnd - elevationStart;
837 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
839 //cerr << "1. Using mean elevation : " << elevationMean << " and " << slope << endl;
841 WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), elevationMean+ 0.5, -(heading), slope );
843 obj_trans->setMatrix( obj_pos );
844 //osg::Vec3 center(0, 0, 0)
846 float width = length /2.0;
847 osg::Vec3 corner(-width, 0, 0.25f);
848 osg::Vec3 widthVec(2*width + 1, 0, 0);
849 osg::Vec3 heightVec(0, 1, 0);
850 osg::Geometry* geometry;
851 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
852 simgear::EffectGeode* geode = new simgear::EffectGeode;
853 geode->setName("test");
854 geode->addDrawable(geometry);
855 //osg::Node *custom_obj;
857 if (segment->hasBlock(now)) {
858 mat = matlib->find("UnidirectionalTaperRed", center);
860 mat = matlib->find("UnidirectionalTaperGreen", center);
863 geode->setEffect(mat->get_effect());
864 obj_trans->addChild(geode);
865 // wire as much of the scene graph together as we can
866 //->addChild( obj_trans );
867 group->addChild( obj_trans );
868 /////////////////////////////////////////////////////////////////////
870 //std::cerr << "BIG FAT WARNING: current position is here : " << pos << std::endl;
872 // Next: Draw the other taxi segments.
873 for (intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
877 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
878 obj_trans->setDataVariance(osg::Object::STATIC);
879 FGTaxiSegment* segmentK = network->findSegment(k);
880 // Experimental: Calculate slope here, based on length, and the individual elevations
881 double elevationStart = segmentK->getStart()->getElevationM();
882 double elevationEnd = segmentK->getEnd ()->getElevationM();
883 if ((elevationStart == 0) || (elevationStart == parent->getElevation())) {
884 SGGeod center2 = segmentK->getStart()->geod();
885 center2.setElevationM(SG_MAX_ELEVATION_M);
886 if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) {
887 // elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5;
888 //elevation_meters += 0.5;
891 elevationStart = parent->getElevation();
893 segmentK->getStart()->setElevation(elevationStart);
895 if ((elevationEnd == 0) || (elevationEnd == parent->getElevation())) {
896 SGGeod center2 = segmentK->getEnd()->geod();
897 center2.setElevationM(SG_MAX_ELEVATION_M);
898 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
899 // elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
900 //elevation_meters += 0.5;
903 elevationEnd = parent->getElevation();
905 segmentK->getEnd()->setElevation(elevationEnd);
908 double elevationMean = (elevationStart + elevationEnd) / 2.0;
909 double elevDiff = elevationEnd - elevationStart;
910 double length = segmentK->getLength();
911 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
913 // cerr << "2. Using mean elevation : " << elevationMean << " and " << slope << endl;
915 SGGeod segCenter = segmentK->getCenter();
916 WorldCoordinate( obj_pos, segCenter.getLatitudeDeg(), segCenter.getLongitudeDeg(),
917 elevationMean+ 0.5, -(segmentK->getHeading()), slope );
919 obj_trans->setMatrix( obj_pos );
920 //osg::Vec3 center(0, 0, 0)
922 float width = segmentK->getLength() /2.0;
923 osg::Vec3 corner(-width, 0, 0.25f);
924 osg::Vec3 widthVec(2*width + 1, 0, 0);
925 osg::Vec3 heightVec(0, 1, 0);
926 osg::Geometry* geometry;
927 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
928 simgear::EffectGeode* geode = new simgear::EffectGeode;
929 geode->setName("test");
930 geode->addDrawable(geometry);
931 //osg::Node *custom_obj;
933 if (segmentK->hasBlock(now)) {
934 mat = matlib->find("UnidirectionalTaperRed", segCenter);
936 mat = matlib->find("UnidirectionalTaperGreen", segCenter);
939 geode->setEffect(mat->get_effect());
940 obj_trans->addChild(geode);
941 // wire as much of the scene graph together as we can
942 //->addChild( obj_trans );
943 group->addChild( obj_trans );
948 globals->get_scenery()->get_scene_graph()->addChild(group);
952 string FGGroundController::getName() {
953 return string(parent->getId() + "-ground");
956 void FGGroundController::update(double dt)
958 time_t now = globals->get_time_params()->get_cur_time();
959 FGGroundNetwork* network = dynamics->getGroundNetwork();
960 network->unblockAllSegments(now);
963 TrafficVector& startupTraffic(dynamics->getStartupController()->getActiveTraffic());
964 TrafficVectorIterator i;
966 //sort(activeTraffic.begin(), activeTraffic.end(), compare_trafficrecords);
967 // Handle traffic that is under ground control first; this way we'll prevent clutter at the gate areas.
968 // Don't allow an aircraft to pushback when a taxiing aircraft is currently using part of the intended route.
969 for (i = startupTraffic.begin(); i != startupTraffic.end(); ++i) {
970 updateStartupTraffic(i, priority, now);
973 for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
974 updateActiveTraffic(i, priority, now);
978 void FGGroundController::updateStartupTraffic(TrafficVectorIterator i,
982 if (!i->getAircraft()) {
983 SG_LOG(SG_ATC, SG_ALERT, "updateStartupTraffic: missing aircraft");
987 if (!i->getAircraft()->getPerformance()) {
988 SG_LOG(SG_ATC, SG_ALERT, "updateStartupTraffic: missing aircraft performance");
993 i->setPriority(priority++);
994 // in meters per second;
995 double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
996 if (!i->isActive(0)) {
1000 FGGroundNetwork* network = dynamics->getGroundNetwork();
1003 SG_LOG(SG_ATC, SG_ALERT, "updateStartupTraffic: missing ground network");
1007 // Check for all active aircraft whether it's current pos segment is
1008 // an opposite of one of the departing aircraft's intentions
1009 for (TrafficVectorIterator j = activeTraffic.begin(); j != activeTraffic.end(); j++) {
1010 int pos = j->getCurrentPosition();
1012 FGTaxiSegment *seg = network->findOppositeSegment(pos-1);
1014 int posReverse = seg->getIndex();
1015 for (intVecIterator k = i->getIntentions().begin(); k != i->getIntentions().end(); k++) {
1016 if ((*k) == posReverse) {
1018 network->findSegment(posReverse)->block(i->getId(), now, now);
1024 // if the current aircraft is still allowed to pushback, we can start reserving a route for if by blocking all the entry taxiways.
1025 if (!i->pushBackAllowed()) {
1030 int pos = i->getCurrentPosition();
1032 FGTaxiSegment *seg = network->findSegment(pos);
1033 length = seg->getLength();
1034 network->blockSegmentsEndingAt(seg, i->getId(), now, now);
1036 for (intVecIterator j = i->getIntentions().begin(); j != i->getIntentions().end(); j++) {
1039 FGTaxiSegment *seg = network->findSegment(pos);
1040 length += seg->getLength();
1041 time_t blockTime = now + (length / vTaxi);
1042 network->blockSegmentsEndingAt(seg, i->getId(), blockTime - 30, now);
1047 void FGGroundController::updateActiveTraffic(TrafficVectorIterator i,
1051 if (!i->getAircraft()) {
1052 SG_LOG(SG_ATC, SG_ALERT, "updateActiveTraffic: missing aircraft");
1056 if (!i->getAircraft()->getPerformance()) {
1057 SG_LOG(SG_ATC, SG_ALERT, "updateActiveTraffic: missing aircraft performance");
1062 double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1063 FGGroundNetwork* network = dynamics->getGroundNetwork();
1066 SG_LOG(SG_ATC, SG_ALERT, "updateActiveTraffic: missing ground network");
1070 i->setPriority(priority++);
1071 int pos = i->getCurrentPosition();
1073 FGTaxiSegment* segment = network->findSegment(pos);
1074 length = segment->getLength();
1075 if (segment->hasBlock(now)) {
1076 //SG_LOG(SG_GENERAL, SG_ALERT, "Taxiway incursion for AI aircraft" << i->getAircraft()->getCallSign());
1081 for (ivi = i->getIntentions().begin(); ivi != i->getIntentions().end(); ivi++) {
1082 int segIndex = (*ivi);
1084 FGTaxiSegment* seg = network->findSegment(segIndex);
1085 if (seg->hasBlock(now)) {
1090 //after this, ivi points just behind the last valid unblocked taxi segment.
1091 for (intVecIterator j = i->getIntentions().begin(); j != ivi; j++) {
1094 FGTaxiSegment *seg = network->findSegment(pos);
1095 length += seg->getLength();
1096 time_t blockTime = now + (length / vTaxi);
1097 network->blockSegmentsEndingAt(seg, i->getId(), blockTime - 30, now);