From 6446d674319e7f96a69ab465cbd238177988b638 Mon Sep 17 00:00:00 2001 From: James Turner Date: Wed, 9 Dec 2015 14:01:24 +0100 Subject: [PATCH] Split GroundNetwork class down the middle - ATC functions move to GroundController, which layers above remaining GroundNetwork functionality - dynamics owns both the groundNetwork and the ground controller. --- src/AIModel/AIAircraft.cxx | 10 +- src/AIModel/AIFlightPlanCreate.cxx | 2 + src/AIModel/AIFlightPlanCreatePushBack.cxx | 3 +- src/ATC/CMakeLists.txt | 2 + src/ATC/GroundController.cxx | 1059 ++++++++++++++++++++ src/ATC/GroundController.hxx | 99 ++ src/Airports/dynamics.cxx | 15 +- src/Airports/dynamics.hxx | 17 +- src/Airports/groundnetwork.cxx | 998 +----------------- src/Airports/groundnetwork.hxx | 38 +- 10 files changed, 1225 insertions(+), 1018 deletions(-) create mode 100644 src/ATC/GroundController.cxx create mode 100644 src/ATC/GroundController.hxx diff --git a/src/AIModel/AIAircraft.cxx b/src/AIModel/AIAircraft.cxx index 5e803ef5e..b7967a5af 100644 --- a/src/AIModel/AIAircraft.cxx +++ b/src/AIModel/AIAircraft.cxx @@ -581,8 +581,8 @@ void FGAIAircraft::announcePositionToController() { controller = trafficRef->getDepartureAirport()->getDynamics()->getStartupController(); break; case 2: // Taxiing to runway - if (trafficRef->getDepartureAirport()->getDynamics()->getGroundNetwork()->exists()) - controller = trafficRef->getDepartureAirport()->getDynamics()->getGroundNetwork(); + if (trafficRef->getDepartureAirport()->getDynamics()->getGroundController()->exists()) + controller = trafficRef->getDepartureAirport()->getDynamics()->getGroundController(); break; case 3: //Take off tower controller if (trafficRef->getDepartureAirport()->getDynamics()) { @@ -593,13 +593,13 @@ void FGAIAircraft::announcePositionToController() { } break; case 6: - if (trafficRef->getDepartureAirport()->getDynamics()) { + if (trafficRef->getArrivalAirport()->getDynamics()) { controller = trafficRef->getArrivalAirport()->getDynamics()->getApproachController(); } break; case 8: // Taxiing for parking - if (trafficRef->getArrivalAirport()->getDynamics()->getGroundNetwork()->exists()) - controller = trafficRef->getArrivalAirport()->getDynamics()->getGroundNetwork(); + if (trafficRef->getArrivalAirport()->getDynamics()->getGroundController()->exists()) + controller = trafficRef->getArrivalAirport()->getDynamics()->getGroundController(); break; default: controller = 0; diff --git a/src/AIModel/AIFlightPlanCreate.cxx b/src/AIModel/AIFlightPlanCreate.cxx index 4b055abd6..9688c391b 100644 --- a/src/AIModel/AIFlightPlanCreate.cxx +++ b/src/AIModel/AIFlightPlanCreate.cxx @@ -33,6 +33,8 @@ #include #include #include +#include + #include "AIAircraft.hxx" #include "performancedata.hxx" diff --git a/src/AIModel/AIFlightPlanCreatePushBack.cxx b/src/AIModel/AIFlightPlanCreatePushBack.cxx index b7bddd76e..923596d04 100644 --- a/src/AIModel/AIFlightPlanCreatePushBack.cxx +++ b/src/AIModel/AIFlightPlanCreatePushBack.cxx @@ -30,6 +30,7 @@ #include #include #include +#include #include #include @@ -56,7 +57,7 @@ bool FGAIFlightPlan::createPushBack(FGAIAircraft *ac, // must be reset. activeRunway.clear(); - if (!(dep->getDynamics()->getGroundNetwork()->exists())) { + if (!(dep->getDynamics()->getGroundController()->exists())) { //cerr << "Push Back fallback" << endl; createPushBackFallBack(ac, firstFlight, dep, radius, fltType, aircraftType, airline); diff --git a/src/ATC/CMakeLists.txt b/src/ATC/CMakeLists.txt index b3795c951..934338115 100644 --- a/src/ATC/CMakeLists.txt +++ b/src/ATC/CMakeLists.txt @@ -8,6 +8,7 @@ set(SOURCES ATISEncoder.cxx MetarPropertiesATISInformationProvider.cxx CurrentWeatherATISInformationProvider.cxx + GroundController.cxx ) set(HEADERS @@ -18,6 +19,7 @@ set(HEADERS ATISEncoder.hxx MetarPropertiesATISInformationProvider.hxx CurrentWeatherATISInformationProvider.hxx + GroundController.hxx ) flightgear_component(ATC "${SOURCES}" "${HEADERS}") diff --git a/src/ATC/GroundController.cxx b/src/ATC/GroundController.cxx new file mode 100644 index 000000000..17b8f87f9 --- /dev/null +++ b/src/ATC/GroundController.cxx @@ -0,0 +1,1059 @@ +// GroundController.hxx - forked from groundnetwork.cxx + +// Written by Durk Talsma, started June 2005. +// +// Copyright (C) 2004 Durk Talsma. +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +// +// $Id$ + +#ifdef HAVE_CONFIG_H +# include +#endif + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include + +#include "GroundController.hxx" + +using std::string; + + +/*************************************************************************** + * FGGroundController() + **************************************************************************/ + +bool compare_trafficrecords(FGTrafficRecord a, FGTrafficRecord b) +{ + return (a.getIntentions().size() < b.getIntentions().size()); +} + +FGGroundController::FGGroundController() : + parent(NULL) +{ + hasNetwork = false; + count = 0; + currTraffic = activeTraffic.begin(); + group = 0; + version = 0; + networkInitialized = false; + +} + +FGGroundController::~FGGroundController() +{ +} + +void FGGroundController::init(FGAirportDynamics* aDynamics) +{ + FGATCController::init(); + + dynamics = aDynamics; + parent = dynamics->parent(); + hasNetwork = true; + networkInitialized = true; +} + +void FGGroundController::announcePosition(int id, + FGAIFlightPlan * intendedRoute, + int currentPosition, double lat, + double lon, double heading, + double speed, double alt, + double radius, int leg, + FGAIAircraft * aircraft) +{ + TrafficVectorIterator i = activeTraffic.begin(); + // Search search if the current id alread has an entry + // This might be faster using a map instead of a vector, but let's start by taking a safe route + if (activeTraffic.size()) { + //while ((i->getId() != id) && i != activeTraffic.end()) { + while (i != activeTraffic.end()) { + if (i->getId() == id) { + break; + } + i++; + } + } + // Add a new TrafficRecord if no one exsists for this aircraft. + if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { + FGTrafficRecord rec; + rec.setId(id); + rec.setLeg(leg); + rec.setPositionAndIntentions(currentPosition, intendedRoute); + rec.setPositionAndHeading(lat, lon, heading, speed, alt); + rec.setRadius(radius); // only need to do this when creating the record. + rec.setAircraft(aircraft); + if (leg == 2) { + activeTraffic.push_front(rec); + } else { + activeTraffic.push_back(rec); + } + + } else { + i->setPositionAndIntentions(currentPosition, intendedRoute); + i->setPositionAndHeading(lat, lon, heading, speed, alt); + } +} + + +void FGGroundController::signOff(int id) +{ + TrafficVectorIterator i = activeTraffic.begin(); + // Search search if the current id alread has an entry + // This might be faster using a map instead of a vector, but let's start by taking a safe route + if (activeTraffic.size()) { + //while ((i->getId() != id) && i != activeTraffic.end()) { + while (i != activeTraffic.end()) { + if (i->getId() == id) { + break; + } + i++; + } + } + if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { + SG_LOG(SG_GENERAL, SG_ALERT, + "AI error: Aircraft without traffic record is signing off at " << SG_ORIGIN); + } else { + i = activeTraffic.erase(i); + } +} +/** + * The ground network can deal with the following states: + * 0 = Normal; no action required + * 1 = "Acknowledge "Hold position + * 2 = "Acknowledge "Resume taxi". + * 3 = "Issue TaxiClearance" + * 4 = Acknowledge Taxi Clearance" + * 5 = Post acknowlegde taxiclearance: Start taxiing + * 6 = Report runway + * 7 = Acknowledge report runway + * 8 = Switch tower frequency + * 9 = Acknowledge switch tower frequency + *************************************************************************************************************************/ +bool FGGroundController::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId, + AtcMsgDir msgDir) +{ + int state = i->getState(); + if ((state >= minState) && (state <= maxState) && available) { + if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) { + //cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl; + SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true); + int n = trans_num->getIntValue(); + if (n == 0) { + trans_num->setIntValue(-1); + // PopupCallback(n); + //cerr << "Selected transmission message " << n << endl; + //FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc"); + FGATCDialogNew::instance()->removeEntry(1); + } else { + //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl; + transmit(&(*i), dynamics, msgId, msgDir, false); + return false; + } + } + transmit(&(*i), dynamics, msgId, msgDir, true); + i->updateState(); + lastTransmission = now; + available = false; + return true; + } + return false; +} + +void FGGroundController::updateAircraftInformation(int id, double lat, double lon, + double heading, double speed, double alt, + double dt) +{ + // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to + // Transmit air-to-ground "Ready to taxi request: + // Transmit ground to air approval / hold + // Transmit confirmation ... + // Probably use a status mechanism similar to the Engine start procedure in the startup controller. + + + TrafficVectorIterator i = activeTraffic.begin(); + // Search search if the current id has an entry + // This might be faster using a map instead of a vector, but let's start by taking a safe route + TrafficVectorIterator current, closest; + if (activeTraffic.size()) { + //while ((i->getId() != id) && i != activeTraffic.end()) { + while (i != activeTraffic.end()) { + if (i->getId() == id) { + break; + } + i++; + } + } + // update position of the current aircraft + if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { + SG_LOG(SG_GENERAL, SG_ALERT, + "AI error: updating aircraft without traffic record at " << SG_ORIGIN); + } else { + i->setPositionAndHeading(lat, lon, heading, speed, alt); + current = i; + } + + setDt(getDt() + dt); + + // Update every three secs, but add some randomness + // to prevent all IA objects doing this in synchrony + //if (getDt() < (3.0) + (rand() % 10)) + // return; + //else + // setDt(0); + current->clearResolveCircularWait(); + current->setWaitsForId(0); + checkSpeedAdjustment(id, lat, lon, heading, speed, alt); + bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest(); + if (!needsTaxiClearance) { + checkHoldPosition(id, lat, lon, heading, speed, alt); + //if (checkForCircularWaits(id)) { + // i->setResolveCircularWait(); + //} + } else { + current->setHoldPosition(true); + int state = current->getState(); + time_t now = time(NULL) + fgGetLong("/sim/time/warp"); + if ((now - lastTransmission) > 15) { + available = true; + } + if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) { + current->setState(3); + } + if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) { + current->setState(4); + } + if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) { + current->setState(5); + } + if ((state == 5) && available) { + current->setState(0); + current->getAircraft()->setTaxiClearanceRequest(false); + current->setHoldPosition(false); + available = false; + } + + } +} + +/** + Scan for a speed adjustment change. Find the nearest aircraft that is in front + and adjust speed when we get too close. Only do this when current position and/or + intentions of the current aircraft match current taxiroute position of the proximate + aircraft. For traffic that is on other routes we need to issue a "HOLD Position" + instruction. See below for the hold position instruction. + + Note that there currently still is one flaw in the logic that needs to be addressed. + There can be situations where one aircraft is in front of the current aircraft, on a separate + route, but really close after an intersection coming off the current route. This + aircraft is still close enough to block the current aircraft. This situation is currently + not addressed yet, but should be. +*/ + +void FGGroundController::checkSpeedAdjustment(int id, double lat, + double lon, double heading, + double speed, double alt) +{ + + TrafficVectorIterator current, closest, closestOnNetwork; + TrafficVectorIterator i = activeTraffic.begin(); + bool otherReasonToSlowDown = false; +// bool previousInstruction; + if (activeTraffic.size()) { + //while ((i->getId() != id) && (i != activeTraffic.end())) + while (i != activeTraffic.end()) { + if (i->getId() == id) { + break; + } + i++; + } + } else { + return; + } + if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { + SG_LOG(SG_GENERAL, SG_ALERT, + "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment at " << SG_ORIGIN); + } + current = i; + //closest = current; + +// previousInstruction = current->getSpeedAdjustment(); + double mindist = HUGE_VAL; + if (activeTraffic.size()) { + double course, dist, bearing, az2; // minbearing, + SGGeod curr(SGGeod::fromDegM(lon, lat, alt)); + //TrafficVector iterator closest; + closest = current; + closestOnNetwork = current; + for (TrafficVectorIterator i = activeTraffic.begin(); + i != activeTraffic.end(); i++) { + if (i == current) { + continue; + } + + SGGeod other(SGGeod::fromDegM(i->getLongitude(), + i->getLatitude(), + i->getAltitude())); + SGGeodesy::inverse(curr, other, course, az2, dist); + bearing = fabs(heading - course); + if (bearing > 180) + bearing = 360 - bearing; + if ((dist < mindist) && (bearing < 60.0)) { + mindist = dist; + closest = i; + closestOnNetwork = i; +// minbearing = bearing; + + } + } + //Check traffic at the tower controller + if (towerController->hasActiveTraffic()) { + for (TrafficVectorIterator i = + towerController->getActiveTraffic().begin(); + i != towerController->getActiveTraffic().end(); i++) { + //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl; + SGGeod other(SGGeod::fromDegM(i->getLongitude(), + i->getLatitude(), + i->getAltitude())); + SGGeodesy::inverse(curr, other, course, az2, dist); + bearing = fabs(heading - course); + if (bearing > 180) + bearing = 360 - bearing; + if ((dist < mindist) && (bearing < 60.0)) { + //cerr << "Current aircraft " << current->getAircraft()->getTrafficRef()->getCallSign() + // << " is closest to " << i->getAircraft()->getTrafficRef()->getCallSign() + // << ", which has status " << i->getAircraft()->isScheduledForTakeoff() + // << endl; + mindist = dist; + closest = i; +// minbearing = bearing; + otherReasonToSlowDown = true; + } + } + } + // Finally, check UserPosition + // Note, as of 2011-08-01, this should no longer be necessecary. + /* + double userLatitude = fgGetDouble("/position/latitude-deg"); + double userLongitude = fgGetDouble("/position/longitude-deg"); + SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude)); + SGGeodesy::inverse(curr, user, course, az2, dist); + + bearing = fabs(heading - course); + if (bearing > 180) + bearing = 360 - bearing; + if ((dist < mindist) && (bearing < 60.0)) { + mindist = dist; + //closest = i; + minbearing = bearing; + otherReasonToSlowDown = true; + } + */ + current->clearSpeedAdjustment(); + bool needBraking = false; + if (current->checkPositionAndIntentions(*closest) + || otherReasonToSlowDown) { + double maxAllowableDistance = + (1.1 * current->getRadius()) + + (1.1 * closest->getRadius()); + if (mindist < 2 * maxAllowableDistance) { + if (current->getId() == closest->getWaitsForId()) + return; + else + current->setWaitsForId(closest->getId()); + if (closest->getId() != current->getId()) { + current->setSpeedAdjustment(closest->getSpeed() * + (mindist / 100)); + needBraking = true; + +// if ( +// closest->getAircraft()->getTakeOffStatus() && +// (current->getAircraft()->getTrafficRef()->getDepartureAirport() == closest->getAircraft()->getTrafficRef()->getDepartureAirport()) && +// (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway()) +// ) +// current->getAircraft()->scheduleForATCTowerDepartureControl(1); + } else { + current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest + } + if (mindist < maxAllowableDistance) { + //double newSpeed = (maxAllowableDistance-mindist); + //current->setSpeedAdjustment(newSpeed); + //if (mindist < 0.5* maxAllowableDistance) + // { + current->setSpeedAdjustment(0); + // } + } + } + } + if ((closest->getId() == closestOnNetwork->getId()) && (current->getPriority() < closest->getPriority()) && needBraking) { + swap(current, closest); + } + } +} + +/** + Check for "Hold position instruction". + The hold position should be issued under the following conditions: + 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it + 2) For taxiing aircraft that use one taxiway in opposite directions + 3) For crossing or merging taxiroutes. +*/ + +void FGGroundController::checkHoldPosition(int id, double lat, + double lon, double heading, + double speed, double alt) +{ + FGGroundNetwork* network = dynamics->getGroundNetwork(); + TrafficVectorIterator current; + TrafficVectorIterator i = activeTraffic.begin(); + if (activeTraffic.size()) { + //while ((i->getId() != id) && i != activeTraffic.end()) + while (i != activeTraffic.end()) { + if (i->getId() == id) { + break; + } + i++; + } + } else { + return; + } + time_t now = time(NULL) + fgGetLong("/sim/time/warp"); + if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { + SG_LOG(SG_GENERAL, SG_ALERT, + "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition at " << SG_ORIGIN); + } + current = i; + // + if (current->getAircraft()->getTakeOffStatus() == 1) { + current->setHoldPosition(true); + return; + } + if (current->getAircraft()->getTakeOffStatus() == 2) { + //cerr << current->getAircraft()->getCallSign() << ". Taxi in position and hold" << endl; + current->setHoldPosition(false); + current->clearSpeedAdjustment(); + return; + } + bool origStatus = current->hasHoldPosition(); + current->setHoldPosition(false); + //SGGeod curr(SGGeod::fromDegM(lon, lat, alt)); + int currentRoute = i->getCurrentPosition(); + int nextRoute; + if (i->getIntentions().size()) { + nextRoute = (*(i->getIntentions().begin())); + } else { + nextRoute = 0; + } + if (currentRoute > 0) { + FGTaxiSegment *tx = network->findSegment(currentRoute); + FGTaxiSegment *nx; + if (nextRoute) { + nx = network->findSegment(nextRoute); + } else { + nx = tx; + } + //if (tx->hasBlock(now) || nx->hasBlock(now) ) { + // current->setHoldPosition(true); + //} + SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude()))); + SGGeod end (nx->getStart()->geod()); + + double distance = SGGeodesy::distanceM(start, end); + if (nx->hasBlock(now) && (distance < i->getRadius() * 4)) { + current->setHoldPosition(true); + } else { + intVecIterator ivi = i->getIntentions().begin(); + while (ivi != i->getIntentions().end()) { + if ((*ivi) > 0) { + FGTaxiSegment* seg = network->findSegment((*ivi)-1); + distance += seg->getLength(); + if ((seg->hasBlock(now)) && (distance < i->getRadius() * 4)) { + current->setHoldPosition(true); + break; + } + } + ivi++; + } + } + } + bool currStatus = current->hasHoldPosition(); + current->setHoldPosition(origStatus); + // Either a Hold Position or a resume taxi transmission has been issued + if ((now - lastTransmission) > 2) { + available = true; + } + if (current->getState() == 0) { + if ((origStatus != currStatus) && available) { + //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl; + if (currStatus == true) { // No has a hold short instruction + transmit(&(*current), dynamics, MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true); + //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl; + current->setState(1); + } else { + transmit(&(*current), dynamics, MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true); + //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl; + current->setState(2); + } + lastTransmission = now; + available = false; + // Don't act on the changed instruction until the transmission is confirmed + // So set back to original status + //cerr << "Current state " << current->getState() << endl; + } + + } + // 6 = Report runway + // 7 = Acknowledge report runway + // 8 = Switch tower frequency + //9 = Acknowledge switch tower frequency + + //int state = current->getState(); + if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) { + current->setState(0); + current->setHoldPosition(true); + } + if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) { + current->setState(0); + current->setHoldPosition(false); + } + if (current->getAircraft()->getTakeOffStatus() && (current->getState() == 0)) { + //cerr << "Scheduling " << current->getAircraft()->getCallSign() << " for hold short" << endl; + current->setState(6); + } + if (checkTransmissionState(6,6, current, now, MSG_REPORT_RUNWAY_HOLD_SHORT, ATC_AIR_TO_GROUND)) { + } + if (checkTransmissionState(7,7, current, now, MSG_ACKNOWLEDGE_REPORT_RUNWAY_HOLD_SHORT, ATC_GROUND_TO_AIR)) { + } + if (checkTransmissionState(8,8, current, now, MSG_SWITCH_TOWER_FREQUENCY, ATC_GROUND_TO_AIR)) { + } + if (checkTransmissionState(9,9, current, now, MSG_ACKNOWLEDGE_SWITCH_TOWER_FREQUENCY, ATC_AIR_TO_GROUND)) { + } + + + + //current->setState(0); +} + +/** + * Check whether situations occur where the current aircraft is waiting for itself + * due to higher order interactions. + * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits + * for a. Ideally each aircraft only waits for one other aircraft, so by tracing + * through this list of waiting aircraft, we can check if we'd eventually end back + * at the current aircraft. + * + * Note that we should consider the situation where we are actually checking aircraft + * d, which is waiting for aircraft a. d is not part of the loop, but is held back by + * the looping aircraft. If we don't check for that, this function will get stuck into + * endless loop. + */ + +bool FGGroundController::checkForCircularWaits(int id) +{ + //cerr << "Performing Wait check " << id << endl; + int target = 0; + TrafficVectorIterator current, other; + TrafficVectorIterator i = activeTraffic.begin(); + int trafficSize = activeTraffic.size(); + if (trafficSize) { + while (i != activeTraffic.end()) { + if (i->getId() == id) { + break; + } + i++; + } + } else { + return false; + } + if (i == activeTraffic.end() || (trafficSize == 0)) { + SG_LOG(SG_GENERAL, SG_ALERT, + "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits at " << SG_ORIGIN); + } + + current = i; + target = current->getWaitsForId(); + //bool printed = false; // Note that this variable is for debugging purposes only. + int counter = 0; + + if (id == target) { + //cerr << "aircraft waits for user" << endl; + return false; + } + + + while ((target > 0) && (target != id) && counter++ < trafficSize) { + //printed = true; + TrafficVectorIterator i = activeTraffic.begin(); + if (trafficSize) { + //while ((i->getId() != id) && i != activeTraffic.end()) + while (i != activeTraffic.end()) { + if (i->getId() == target) { + break; + } + i++; + } + } else { + return false; + } + if (i == activeTraffic.end() || (trafficSize == 0)) { + //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;; + // The target id is not found on the current network, which means it's at the tower + //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits"); + return false; + } + other = i; + target = other->getWaitsForId(); + + // actually this trap isn't as impossible as it first seemed: + // the setWaitsForID(id) is set to current when the aircraft + // is waiting for the user controlled aircraft. + //if (current->getId() == other->getId()) { + // cerr << "Caught the impossible trap" << endl; + // cerr << "Current = " << current->getId() << endl; + // cerr << "Other = " << other ->getId() << endl; + // for (TrafficVectorIterator at = activeTraffic.begin(); + // at != activeTraffic.end(); + // at++) { + // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl; + // } + // exit(1); + if (current->getId() == other->getId()) + return false; + //} + //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign() + // << " (" << other->getId() << "); " << endl;; + //current = other; + } + + + + + + + //if (printed) + // cerr << "[done] " << endl << endl;; + if (id == target) { + SG_LOG(SG_GENERAL, SG_WARN, + "Detected circular wait condition: Id = " << id << + "target = " << target); + return true; + } else { + return false; + } +} + +// Note that this function is probably obsolete... +bool FGGroundController::hasInstruction(int id) +{ + TrafficVectorIterator i = activeTraffic.begin(); + // Search search if the current id has an entry + // This might be faster using a map instead of a vector, but let's start by taking a safe route + if (activeTraffic.size()) { + //while ((i->getId() != id) && i != activeTraffic.end()) { + while (i != activeTraffic.end()) { + if (i->getId() == id) { + break; + } + i++; + } + } + if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { + SG_LOG(SG_GENERAL, SG_ALERT, + "AI error: checking ATC instruction for aircraft without traffic record at " << SG_ORIGIN); + } else { + return i->hasInstruction(); + } + return false; +} + +FGATCInstruction FGGroundController::getInstruction(int id) +{ + TrafficVectorIterator i = activeTraffic.begin(); + // Search search if the current id has an entry + // This might be faster using a map instead of a vector, but let's start by taking a safe route + if (activeTraffic.size()) { + //while ((i->getId() != id) && i != activeTraffic.end()) { + while (i != activeTraffic.end()) { + if (i->getId() == id) { + break; + } + i++; + } + } + if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { + SG_LOG(SG_GENERAL, SG_ALERT, + "AI error: requesting ATC instruction for aircraft without traffic record at " << SG_ORIGIN); + } else { + return i->getInstruction(); + } + return FGATCInstruction(); +} + +// Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that. +static void WorldCoordinate(osg::Matrix& obj_pos, double lat, + double lon, double elev, double hdg, double slope) +{ + SGGeod geod = SGGeod::fromDegM(lon, lat, elev); + obj_pos = makeZUpFrame(geod); + // hdg is not a compass heading, but a counter-clockwise rotation + // around the Z axis + obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS, + 0.0, 0.0, 1.0)); + obj_pos.preMult(osg::Matrix::rotate(slope * SGD_DEGREES_TO_RADIANS, + 0.0, 1.0, 0.0)); +} + + + + +void FGGroundController::render(bool visible) +{ + SGMaterialLib *matlib = globals->get_matlib(); + FGGroundNetwork* network = dynamics->getGroundNetwork(); + + if (group) { + //int nr = ; + globals->get_scenery()->get_scene_graph()->removeChild(group); + //while (group->getNumChildren()) { + // cerr << "Number of children: " << group->getNumChildren() << endl; + //simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0); + //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0); + //geode->releaseGLObjects(); + //group->removeChild(geode); + //delete geode; + group = 0; + } + if (visible) { + group = new osg::Group; + FGScenery * local_scenery = globals->get_scenery(); + // double elevation_meters = 0.0; +// double elevation_feet = 0.0; + time_t now = time(NULL) + fgGetLong("/sim/time/warp"); + //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) { + //double dx = 0; + + for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) { + // Handle start point i.e. the segment that is connected to the aircraft itself on the starting end + // and to the the first "real" taxi segment on the other end. + const int pos = i->getCurrentPosition() - 1; + if (pos >= 0) { + FGTaxiSegment* segment = network->findSegment(pos); + SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude()))); + SGGeod end (segment->getEnd()->geod()); + + double length = SGGeodesy::distanceM(start, end); + //heading = SGGeodesy::headingDeg(start->geod(), end->geod()); + + double az2, heading; //, distanceM; + SGGeodesy::inverse(start, end, heading, az2, length); + double coveredDistance = length * 0.5; + SGGeod center; + SGGeodesy::direct(start, heading, coveredDistance, center, az2); + //std::cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << std::endl; + /////////////////////////////////////////////////////////////////////////////// + // Make a helper function out of this + osg::Matrix obj_pos; + osg::MatrixTransform *obj_trans = new osg::MatrixTransform; + obj_trans->setDataVariance(osg::Object::STATIC); + // Experimental: Calculate slope here, based on length, and the individual elevations + double elevationStart; + if (isUserAircraft((i)->getAircraft())) { + elevationStart = fgGetDouble("/position/ground-elev-m"); + } else { + elevationStart = ((i)->getAircraft()->_getAltitude()); + } + double elevationEnd = segment->getEnd()->getElevationM(); + //cerr << "Using elevation " << elevationEnd << endl; + + if ((elevationEnd == 0) || (elevationEnd = parent->getElevation())) { + SGGeod center2 = end; + center2.setElevationM(SG_MAX_ELEVATION_M); + if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) { +// elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5; + //elevation_meters += 0.5; + } + else { + elevationEnd = parent->getElevation(); + } + segment->getEnd()->setElevation(elevationEnd); + } + double elevationMean = (elevationStart + elevationEnd) / 2.0; + double elevDiff = elevationEnd - elevationStart; + + double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES; + + //cerr << "1. Using mean elevation : " << elevationMean << " and " << slope << endl; + + WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), elevationMean+ 0.5, -(heading), slope ); + + obj_trans->setMatrix( obj_pos ); + //osg::Vec3 center(0, 0, 0) + + float width = length /2.0; + osg::Vec3 corner(-width, 0, 0.25f); + osg::Vec3 widthVec(2*width + 1, 0, 0); + osg::Vec3 heightVec(0, 1, 0); + osg::Geometry* geometry; + geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec); + simgear::EffectGeode* geode = new simgear::EffectGeode; + geode->setName("test"); + geode->addDrawable(geometry); + //osg::Node *custom_obj; + SGMaterial *mat; + if (segment->hasBlock(now)) { + mat = matlib->find("UnidirectionalTaperRed", center); + } else { + mat = matlib->find("UnidirectionalTaperGreen", center); + } + if (mat) + geode->setEffect(mat->get_effect()); + obj_trans->addChild(geode); + // wire as much of the scene graph together as we can + //->addChild( obj_trans ); + group->addChild( obj_trans ); + ///////////////////////////////////////////////////////////////////// + } else { + //std::cerr << "BIG FAT WARNING: current position is here : " << pos << std::endl; + } + // Next: Draw the other taxi segments. + for (intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) { + osg::Matrix obj_pos; + const int k = (*j)-1; + if (k >= 0) { + osg::MatrixTransform *obj_trans = new osg::MatrixTransform; + obj_trans->setDataVariance(osg::Object::STATIC); + FGTaxiSegment* segmentK = network->findSegment(k); + // Experimental: Calculate slope here, based on length, and the individual elevations + double elevationStart = segmentK->getStart()->getElevationM(); + double elevationEnd = segmentK->getEnd ()->getElevationM(); + if ((elevationStart == 0) || (elevationStart == parent->getElevation())) { + SGGeod center2 = segmentK->getStart()->geod(); + center2.setElevationM(SG_MAX_ELEVATION_M); + if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) { +// elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5; + //elevation_meters += 0.5; + } + else { + elevationStart = parent->getElevation(); + } + segmentK->getStart()->setElevation(elevationStart); + } + if ((elevationEnd == 0) || (elevationEnd == parent->getElevation())) { + SGGeod center2 = segmentK->getEnd()->geod(); + center2.setElevationM(SG_MAX_ELEVATION_M); + if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) { +// elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5; + //elevation_meters += 0.5; + } + else { + elevationEnd = parent->getElevation(); + } + segmentK->getEnd()->setElevation(elevationEnd); + } + + double elevationMean = (elevationStart + elevationEnd) / 2.0; + double elevDiff = elevationEnd - elevationStart; + double length = segmentK->getLength(); + double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES; + + // cerr << "2. Using mean elevation : " << elevationMean << " and " << slope << endl; + + SGGeod segCenter = segmentK->getCenter(); + WorldCoordinate( obj_pos, segCenter.getLatitudeDeg(), segCenter.getLongitudeDeg(), + elevationMean+ 0.5, -(segmentK->getHeading()), slope ); + + obj_trans->setMatrix( obj_pos ); + //osg::Vec3 center(0, 0, 0) + + float width = segmentK->getLength() /2.0; + osg::Vec3 corner(-width, 0, 0.25f); + osg::Vec3 widthVec(2*width + 1, 0, 0); + osg::Vec3 heightVec(0, 1, 0); + osg::Geometry* geometry; + geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec); + simgear::EffectGeode* geode = new simgear::EffectGeode; + geode->setName("test"); + geode->addDrawable(geometry); + //osg::Node *custom_obj; + SGMaterial *mat; + if (segmentK->hasBlock(now)) { + mat = matlib->find("UnidirectionalTaperRed", segCenter); + } else { + mat = matlib->find("UnidirectionalTaperGreen", segCenter); + } + if (mat) + geode->setEffect(mat->get_effect()); + obj_trans->addChild(geode); + // wire as much of the scene graph together as we can + //->addChild( obj_trans ); + group->addChild( obj_trans ); + } + } + //dx += 0.1; + } + globals->get_scenery()->get_scene_graph()->addChild(group); + } +} + +string FGGroundController::getName() { + return string(parent->getId() + "-ground"); +} + +void FGGroundController::update(double dt) +{ + time_t now = time(NULL) + fgGetLong("/sim/time/warp"); + FGGroundNetwork* network = dynamics->getGroundNetwork(); + network->unblockAllSegments(now); + int priority = 1; + + TrafficVector& startupTraffic(dynamics->getStartupController()->getActiveTraffic()); + TrafficVectorIterator i; + + //sort(activeTraffic.begin(), activeTraffic.end(), compare_trafficrecords); + // Handle traffic that is under ground control first; this way we'll prevent clutter at the gate areas. + // Don't allow an aircraft to pushback when a taxiing aircraft is currently using part of the intended route. + for (i = startupTraffic.begin(); i != startupTraffic.end(); ++i) { + updateStartupTraffic(i, priority, now); + } + + for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) { + updateActiveTraffic(i, priority, now); + } +} + +void FGGroundController::updateStartupTraffic(TrafficVectorIterator i, + int& priority, + time_t now) +{ + i->allowPushBack(); + i->setPriority(priority++); + // in meters per second; + double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600; + if (!i->isActive(0)) { + return; + } + + FGGroundNetwork* network = dynamics->getGroundNetwork(); + + // Check for all active aircraft whether it's current pos segment is + // an opposite of one of the departing aircraft's intentions + for (TrafficVectorIterator j = activeTraffic.begin(); j != activeTraffic.end(); j++) { + int pos = j->getCurrentPosition(); + if (pos > 0) { + FGTaxiSegment *seg = network->findOppositeSegment(pos-1); + if (seg) { + int posReverse = seg->getIndex(); + for (intVecIterator k = i->getIntentions().begin(); k != i->getIntentions().end(); k++) { + if ((*k) == posReverse) { + i->denyPushBack(); + network->findSegment(posReverse-1)->block(i->getId(), now, now); + } + } + } + } + } + // if the current aircraft is still allowed to pushback, we can start reserving a route for if by blocking all the entry taxiways. + if (!i->pushBackAllowed()) { + return; + } + + double length = 0; + int pos = i->getCurrentPosition(); + if (pos > 0) { + FGTaxiSegment *seg = network->findSegment(pos-1); + length = seg->getLength(); + network->blockSegmentsEndingAt(seg, i->getId(), now, now); + } + for (intVecIterator j = i->getIntentions().begin(); j != i->getIntentions().end(); j++) { + int pos = (*j); + if (pos > 0) { + FGTaxiSegment *seg = network->findSegment(pos-1); + length += seg->getLength(); + time_t blockTime = now + (length / vTaxi); + network->blockSegmentsEndingAt(seg, i->getId(), blockTime - 30, now); + } + } +} + +void FGGroundController::updateActiveTraffic(TrafficVectorIterator i, + int& priority, + time_t now) +{ + double length = 0; + double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600; + FGGroundNetwork* network = dynamics->getGroundNetwork(); + + i->setPriority(priority++); + int pos = i->getCurrentPosition(); + if (pos > 0) { + FGTaxiSegment* segment = network->findSegment(pos-1); + length = segment->getLength(); + if (segment->hasBlock(now)) { + //SG_LOG(SG_GENERAL, SG_ALERT, "Taxiway incursion for AI aircraft" << i->getAircraft()->getCallSign()); + } + + } + intVecIterator ivi; + for (ivi = i->getIntentions().begin(); ivi != i->getIntentions().end(); ivi++) { + int segIndex = (*ivi); + if (segIndex > 0) { + FGTaxiSegment* seg = network->findSegment(segIndex-1); + if (seg->hasBlock(now)) { + break; + } + } + } + //after this, ivi points just behind the last valid unblocked taxi segment. + for (intVecIterator j = i->getIntentions().begin(); j != ivi; j++) { + int pos = (*j); + if (pos > 0) { + FGTaxiSegment *seg = network->findSegment(pos-1); + length += seg->getLength(); + time_t blockTime = now + (length / vTaxi); + network->blockSegmentsEndingAt(seg, i->getId(), blockTime - 30, now); + } + } +} diff --git a/src/ATC/GroundController.hxx b/src/ATC/GroundController.hxx new file mode 100644 index 000000000..42fe0583e --- /dev/null +++ b/src/ATC/GroundController.hxx @@ -0,0 +1,99 @@ +// GroundController.hxx - forked from groundnetwork.cxx +// +// Written by Durk Talsma, started June 2005. +// +// Copyright (C) 2004 Durk Talsma. +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +// +// $Id$ + +#ifndef ATC_GROUND_CONTROLLER_HXX +#define ATC_GROUND_CONTROLLER_HXX + +#include + +#include + +#include + +class FGAirportDynamics; + +/************************************************************************************** + * class FGGroundNetWork + *************************************************************************************/ +class FGGroundController : public FGATCController +{ +private: + + bool hasNetwork; + bool networkInitialized; + int count; + int version; + + + TrafficVector activeTraffic; + TrafficVectorIterator currTraffic; + + FGTowerController *towerController; + FGAirport *parent; + FGAirportDynamics* dynamics; + + + void checkSpeedAdjustment(int id, double lat, double lon, + double heading, double speed, double alt); + void checkHoldPosition(int id, double lat, double lon, + double heading, double speed, double alt); + + + void updateStartupTraffic(TrafficVectorIterator i, int& priority, time_t now); + void updateActiveTraffic(TrafficVectorIterator i, int& priority, time_t now); +public: + FGGroundController(); + ~FGGroundController(); + + void setVersion (int v) { version = v;}; + int getVersion() { return version; }; + + void init(FGAirportDynamics* pr); + bool exists() { + return hasNetwork; + }; + void setTowerController(FGTowerController *twrCtrlr) { + towerController = twrCtrlr; + }; + + + + virtual void announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentRoute, + double lat, double lon, double hdg, double spd, double alt, + double radius, int leg, FGAIAircraft *aircraft); + virtual void signOff(int id); + virtual void updateAircraftInformation(int id, double lat, double lon, double heading, double speed, double alt, double dt); + virtual bool hasInstruction(int id); + virtual FGATCInstruction getInstruction(int id); + + bool checkTransmissionState(int minState, int MaxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId, + AtcMsgDir msgDir); + bool checkForCircularWaits(int id); + virtual void render(bool); + virtual std::string getName(); + virtual void update(double dt); + + void addVersion(int v) {version = v; }; +}; + + +#endif diff --git a/src/Airports/dynamics.cxx b/src/Airports/dynamics.cxx index ffc4a0959..fae2add25 100644 --- a/src/Airports/dynamics.cxx +++ b/src/Airports/dynamics.cxx @@ -40,6 +40,7 @@ #include
#include
#include +#include #include #include "airport.hxx" @@ -159,6 +160,7 @@ FGAirportDynamics::FGAirportDynamics(FGAirport * ap): { lastUpdate = 0; + groundNetwork.reset(new FGGroundNetwork); } // Destructor @@ -171,16 +173,17 @@ FGAirportDynamics::~FGAirportDynamics() // Initialization required after XMLRead void FGAirportDynamics::init() { - groundNetwork.init(this); - groundNetwork.setTowerController(&towerController); - + + groundNetwork->init(this); + groundController.setTowerController(&towerController); + groundController.init(this); } FGParking* FGAirportDynamics::innerGetAvailableParking(double radius, const string & flType, const string & airline, bool skipEmptyAirlineCode) { - const FGParkingList& parkings(groundNetwork.allParkings()); + const FGParkingList& parkings(groundNetwork->allParkings()); FGParkingList::const_iterator it; for (it = parkings.begin(); it != parkings.end(); ++it) { FGParkingRef parking = *it; @@ -230,7 +233,7 @@ ParkingAssignment FGAirportDynamics::getAvailableParking(double radius, const st ParkingAssignment FGAirportDynamics::getParkingByName(const std::string& name) const { - const FGParkingList& parkings(groundNetwork.allParkings()); + const FGParkingList& parkings(groundNetwork->allParkings()); FGParkingList::const_iterator it; for (it = parkings.begin(); it != parkings.end(); ++it) { if ((*it)->name() == name) { @@ -292,7 +295,7 @@ public: FGParkingList FGAirportDynamics::getParkings(bool onlyAvailable, const std::string &type) const { - FGParkingList result(groundNetwork.allParkings()); + FGParkingList result(groundNetwork->allParkings()); GetParkingsPredicate pred(onlyAvailable, type, this); FGParkingList::iterator it = std::remove_if(result.begin(), result.end(), pred); diff --git a/src/Airports/dynamics.hxx b/src/Airports/dynamics.hxx index 85c98b9d8..cef41fa48 100644 --- a/src/Airports/dynamics.hxx +++ b/src/Airports/dynamics.hxx @@ -27,9 +27,10 @@ #include #include +#include + #include "airports_fwd.hxx" #include "parking.hxx" -#include "groundnetwork.hxx" #include "runwayprefs.hxx" class ParkingAssignment @@ -66,12 +67,13 @@ private: ParkingSet occupiedParkings; + std::auto_ptr groundNetwork; FGRunwayPreference rwyPrefs; FGStartupController startupController; - FGGroundNetwork groundNetwork; FGTowerController towerController; FGApproachController approachController; + FGGroundController groundController; time_t lastUpdate; std::string prevTrafficType; @@ -98,7 +100,7 @@ private: bool skipEmptyAirlineCode); public: FGAirportDynamics(FGAirport* ap); - ~FGAirportDynamics(); + virtual ~FGAirportDynamics(); void addAwosFreq (int val) { freqAwos.push_back(val); @@ -158,8 +160,8 @@ public: FGStartupController *getStartupController() { return &startupController; }; - FGGroundNetwork *getGroundNetwork() { - return &groundNetwork; + FGGroundController *getGroundController() { + return &groundController; }; FGTowerController *getTowerController() { return &towerController; @@ -168,6 +170,11 @@ public: return &approachController; }; + FGGroundNetwork* getGroundNetwork() const + { + return groundNetwork.get(); + } + int getGroundFrequency(unsigned leg); int getTowerFrequency (unsigned nr); diff --git a/src/Airports/groundnetwork.cxx b/src/Airports/groundnetwork.cxx index 1f337a957..5a0705d9f 100644 --- a/src/Airports/groundnetwork.cxx +++ b/src/Airports/groundnetwork.cxx @@ -24,21 +24,15 @@ # include #endif +#include "groundnetwork.hxx" + #include #include #include #include #include -#include -#include -#include -#include - #include -#include -#include -#include #include #include #include @@ -47,17 +41,8 @@ #include #include -#include -#include -#include -#include - -#include - #include -#include "groundnetwork.hxx" - using std::string; using flightgear::NavDataCache; @@ -169,25 +154,13 @@ bool FGTaxiRoute::next(FGTaxiNodeRef& node, int *rte) * FGGroundNetwork() **************************************************************************/ -bool compare_trafficrecords(FGTrafficRecord a, FGTrafficRecord b) -{ - return (a.getIntentions().size() < b.getIntentions().size()); -} - FGGroundNetwork::FGGroundNetwork() : dynamics(NULL), parent(NULL) { hasNetwork = false; - totalDistance = 0; - maxDistance = 0; - //maxDepth = 1000; - count = 0; - currTraffic = activeTraffic.begin(); - group = 0; version = 0; networkInitialized = false; - } FGGroundNetwork::~FGGroundNetwork() @@ -204,7 +177,6 @@ void FGGroundNetwork::init(FGAirportDynamics* dyn) { if (networkInitialized) { SG_LOG(SG_GENERAL, SG_WARN, "duplicate ground-network init"); - FGATCController::init(); return; } @@ -212,9 +184,7 @@ void FGGroundNetwork::init(FGAirportDynamics* dyn) parent = dyn->parent(); assert(parent); hasNetwork = true; - nextSave = 0; - int index = 1; - + int index = 1; // establish pairing of segments BOOST_FOREACH(FGTaxiSegment* segment, segments) { @@ -275,6 +245,14 @@ FGTaxiNodeRef FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod, FGR return result; } +FGTaxiSegment *FGGroundNetwork::findOppositeSegment(unsigned int index) const +{ + FGTaxiSegment* seg = findSegment(index); + if (!seg) + return NULL; + return seg->opposite(); +} + const FGParkingList &FGGroundNetwork::allParkings() const { return m_parkings; @@ -398,195 +376,29 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(FGTaxiNode* start, FGTaxiNode* en return FGTaxiRoute(nodes, routes, searchData[end].score, 0); } -/* ATC Related Functions */ - -void FGGroundNetwork::announcePosition(int id, - FGAIFlightPlan * intendedRoute, - int currentPosition, double lat, - double lon, double heading, - double speed, double alt, - double radius, int leg, - FGAIAircraft * aircraft) +void FGGroundNetwork::unblockAllSegments(time_t now) { - - assert(dynamics); - init(dynamics); - - TrafficVectorIterator i = activeTraffic.begin(); - // Search search if the current id alread has an entry - // This might be faster using a map instead of a vector, but let's start by taking a safe route - if (activeTraffic.size()) { - //while ((i->getId() != id) && i != activeTraffic.end()) { - while (i != activeTraffic.end()) { - if (i->getId() == id) { - break; - } - i++; - } - } - // Add a new TrafficRecord if no one exsists for this aircraft. - if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { - FGTrafficRecord rec; - rec.setId(id); - rec.setLeg(leg); - rec.setPositionAndIntentions(currentPosition, intendedRoute); - rec.setPositionAndHeading(lat, lon, heading, speed, alt); - rec.setRadius(radius); // only need to do this when creating the record. - rec.setAircraft(aircraft); - if (leg == 2) { - activeTraffic.push_front(rec); - } else { - activeTraffic.push_back(rec); - } - - } else { - i->setPositionAndIntentions(currentPosition, intendedRoute); - i->setPositionAndHeading(lat, lon, heading, speed, alt); - } -} - - -void FGGroundNetwork::signOff(int id) -{ - TrafficVectorIterator i = activeTraffic.begin(); - // Search search if the current id alread has an entry - // This might be faster using a map instead of a vector, but let's start by taking a safe route - if (activeTraffic.size()) { - //while ((i->getId() != id) && i != activeTraffic.end()) { - while (i != activeTraffic.end()) { - if (i->getId() == id) { - break; - } - i++; - } - } - if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { - SG_LOG(SG_GENERAL, SG_ALERT, - "AI error: Aircraft without traffic record is signing off at " << SG_ORIGIN); - } else { - i = activeTraffic.erase(i); - } -} -/** - * The ground network can deal with the following states: - * 0 = Normal; no action required - * 1 = "Acknowledge "Hold position - * 2 = "Acknowledge "Resume taxi". - * 3 = "Issue TaxiClearance" - * 4 = Acknowledge Taxi Clearance" - * 5 = Post acknowlegde taxiclearance: Start taxiing - * 6 = Report runway - * 7 = Acknowledge report runway - * 8 = Switch tower frequency - * 9 = Acknowledge switch tower frequency - *************************************************************************************************************************/ -bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId, - AtcMsgDir msgDir) -{ - int state = i->getState(); - if ((state >= minState) && (state <= maxState) && available) { - if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) { - //cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl; - SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true); - int n = trans_num->getIntValue(); - if (n == 0) { - trans_num->setIntValue(-1); - // PopupCallback(n); - //cerr << "Selected transmission message " << n << endl; - //FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc"); - FGATCDialogNew::instance()->removeEntry(1); - } else { - //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl; - transmit(&(*i), dynamics, msgId, msgDir, false); - return false; - } - } - transmit(&(*i), dynamics, msgId, msgDir, true); - i->updateState(); - lastTransmission = now; - available = false; - return true; + FGTaxiSegmentVector::iterator tsi; + for ( tsi = segments.begin(); tsi != segments.end(); tsi++) { + (*tsi)->unblock(now); } - return false; } -void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon, - double heading, double speed, double alt, - double dt) +void FGGroundNetwork::blockSegmentsEndingAt(FGTaxiSegment *seg, int blockId, time_t blockTime, time_t now) { - // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to - // Transmit air-to-ground "Ready to taxi request: - // Transmit ground to air approval / hold - // Transmit confirmation ... - // Probably use a status mechanism similar to the Engine start procedure in the startup controller. + if (!seg) + throw sg_exception("Passed invalid segment"); - - TrafficVectorIterator i = activeTraffic.begin(); - // Search search if the current id has an entry - // This might be faster using a map instead of a vector, but let's start by taking a safe route - TrafficVectorIterator current, closest; - if (activeTraffic.size()) { - //while ((i->getId() != id) && i != activeTraffic.end()) { - while (i != activeTraffic.end()) { - if (i->getId() == id) { - break; - } - i++; - } - } - // update position of the current aircraft - if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { - SG_LOG(SG_GENERAL, SG_ALERT, - "AI error: updating aircraft without traffic record at " << SG_ORIGIN); - } else { - i->setPositionAndHeading(lat, lon, heading, speed, alt); - current = i; - } - - setDt(getDt() + dt); - - // Update every three secs, but add some randomness - // to prevent all IA objects doing this in synchrony - //if (getDt() < (3.0) + (rand() % 10)) - // return; - //else - // setDt(0); - current->clearResolveCircularWait(); - current->setWaitsForId(0); - checkSpeedAdjustment(id, lat, lon, heading, speed, alt); - bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest(); - if (!needsTaxiClearance) { - checkHoldPosition(id, lat, lon, heading, speed, alt); - //if (checkForCircularWaits(id)) { - // i->setResolveCircularWait(); - //} - } else { - current->setHoldPosition(true); - int state = current->getState(); - time_t now = time(NULL) + fgGetLong("/sim/time/warp"); - if ((now - lastTransmission) > 15) { - available = true; - } - if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) { - current->setState(3); - } - if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) { - current->setState(4); - } - if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) { - current->setState(5); - } - if ((state == 5) && available) { - current->setState(0); - current->getAircraft()->setTaxiClearanceRequest(false); - current->setHoldPosition(false); - available = false; + FGTaxiNode *node = seg->getEnd(); + FGTaxiSegmentVector::iterator tsi; + for ( tsi = segments.begin(); tsi != segments.end(); tsi++) { + FGTaxiSegment* otherSegment = *tsi; + if ((otherSegment->getEnd() == node) && (otherSegment != seg)) { + otherSegment->block(blockId, blockTime, now); } - } } - FGTaxiNodeRef FGGroundNetwork::findNodeByIndex(int index) const { FGTaxiNodeVector::const_iterator it; @@ -599,303 +411,6 @@ FGTaxiNodeRef FGGroundNetwork::findNodeByIndex(int index) const return FGTaxiNodeRef(); } - -/** - Scan for a speed adjustment change. Find the nearest aircraft that is in front - and adjust speed when we get too close. Only do this when current position and/or - intentions of the current aircraft match current taxiroute position of the proximate - aircraft. For traffic that is on other routes we need to issue a "HOLD Position" - instruction. See below for the hold position instruction. - - Note that there currently still is one flaw in the logic that needs to be addressed. - There can be situations where one aircraft is in front of the current aircraft, on a separate - route, but really close after an intersection coming off the current route. This - aircraft is still close enough to block the current aircraft. This situation is currently - not addressed yet, but should be. -*/ - -void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, - double lon, double heading, - double speed, double alt) -{ - - TrafficVectorIterator current, closest, closestOnNetwork; - TrafficVectorIterator i = activeTraffic.begin(); - bool otherReasonToSlowDown = false; -// bool previousInstruction; - if (activeTraffic.size()) { - //while ((i->getId() != id) && (i != activeTraffic.end())) - while (i != activeTraffic.end()) { - if (i->getId() == id) { - break; - } - i++; - } - } else { - return; - } - if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { - SG_LOG(SG_GENERAL, SG_ALERT, - "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment at " << SG_ORIGIN); - } - current = i; - //closest = current; - -// previousInstruction = current->getSpeedAdjustment(); - double mindist = HUGE_VAL; - if (activeTraffic.size()) { - double course, dist, bearing, az2; // minbearing, - SGGeod curr(SGGeod::fromDegM(lon, lat, alt)); - //TrafficVector iterator closest; - closest = current; - closestOnNetwork = current; - for (TrafficVectorIterator i = activeTraffic.begin(); - i != activeTraffic.end(); i++) { - if (i == current) { - continue; - } - - SGGeod other(SGGeod::fromDegM(i->getLongitude(), - i->getLatitude(), - i->getAltitude())); - SGGeodesy::inverse(curr, other, course, az2, dist); - bearing = fabs(heading - course); - if (bearing > 180) - bearing = 360 - bearing; - if ((dist < mindist) && (bearing < 60.0)) { - mindist = dist; - closest = i; - closestOnNetwork = i; -// minbearing = bearing; - - } - } - //Check traffic at the tower controller - if (towerController->hasActiveTraffic()) { - for (TrafficVectorIterator i = - towerController->getActiveTraffic().begin(); - i != towerController->getActiveTraffic().end(); i++) { - //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl; - SGGeod other(SGGeod::fromDegM(i->getLongitude(), - i->getLatitude(), - i->getAltitude())); - SGGeodesy::inverse(curr, other, course, az2, dist); - bearing = fabs(heading - course); - if (bearing > 180) - bearing = 360 - bearing; - if ((dist < mindist) && (bearing < 60.0)) { - //cerr << "Current aircraft " << current->getAircraft()->getTrafficRef()->getCallSign() - // << " is closest to " << i->getAircraft()->getTrafficRef()->getCallSign() - // << ", which has status " << i->getAircraft()->isScheduledForTakeoff() - // << endl; - mindist = dist; - closest = i; -// minbearing = bearing; - otherReasonToSlowDown = true; - } - } - } - // Finally, check UserPosition - // Note, as of 2011-08-01, this should no longer be necessecary. - /* - double userLatitude = fgGetDouble("/position/latitude-deg"); - double userLongitude = fgGetDouble("/position/longitude-deg"); - SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude)); - SGGeodesy::inverse(curr, user, course, az2, dist); - - bearing = fabs(heading - course); - if (bearing > 180) - bearing = 360 - bearing; - if ((dist < mindist) && (bearing < 60.0)) { - mindist = dist; - //closest = i; - minbearing = bearing; - otherReasonToSlowDown = true; - } - */ - current->clearSpeedAdjustment(); - bool needBraking = false; - if (current->checkPositionAndIntentions(*closest) - || otherReasonToSlowDown) { - double maxAllowableDistance = - (1.1 * current->getRadius()) + - (1.1 * closest->getRadius()); - if (mindist < 2 * maxAllowableDistance) { - if (current->getId() == closest->getWaitsForId()) - return; - else - current->setWaitsForId(closest->getId()); - if (closest->getId() != current->getId()) { - current->setSpeedAdjustment(closest->getSpeed() * - (mindist / 100)); - needBraking = true; - -// if ( -// closest->getAircraft()->getTakeOffStatus() && -// (current->getAircraft()->getTrafficRef()->getDepartureAirport() == closest->getAircraft()->getTrafficRef()->getDepartureAirport()) && -// (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway()) -// ) -// current->getAircraft()->scheduleForATCTowerDepartureControl(1); - } else { - current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest - } - if (mindist < maxAllowableDistance) { - //double newSpeed = (maxAllowableDistance-mindist); - //current->setSpeedAdjustment(newSpeed); - //if (mindist < 0.5* maxAllowableDistance) - // { - current->setSpeedAdjustment(0); - // } - } - } - } - if ((closest->getId() == closestOnNetwork->getId()) && (current->getPriority() < closest->getPriority()) && needBraking) { - swap(current, closest); - } - } -} - -/** - Check for "Hold position instruction". - The hold position should be issued under the following conditions: - 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it - 2) For taxiing aircraft that use one taxiway in opposite directions - 3) For crossing or merging taxiroutes. -*/ - -void FGGroundNetwork::checkHoldPosition(int id, double lat, - double lon, double heading, - double speed, double alt) -{ - TrafficVectorIterator current; - TrafficVectorIterator i = activeTraffic.begin(); - if (activeTraffic.size()) { - //while ((i->getId() != id) && i != activeTraffic.end()) - while (i != activeTraffic.end()) { - if (i->getId() == id) { - break; - } - i++; - } - } else { - return; - } - time_t now = time(NULL) + fgGetLong("/sim/time/warp"); - if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { - SG_LOG(SG_GENERAL, SG_ALERT, - "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition at " << SG_ORIGIN); - } - current = i; - // - if (current->getAircraft()->getTakeOffStatus() == 1) { - current->setHoldPosition(true); - return; - } - if (current->getAircraft()->getTakeOffStatus() == 2) { - //cerr << current->getAircraft()->getCallSign() << ". Taxi in position and hold" << endl; - current->setHoldPosition(false); - current->clearSpeedAdjustment(); - return; - } - bool origStatus = current->hasHoldPosition(); - current->setHoldPosition(false); - //SGGeod curr(SGGeod::fromDegM(lon, lat, alt)); - int currentRoute = i->getCurrentPosition(); - int nextRoute; - if (i->getIntentions().size()) { - nextRoute = (*(i->getIntentions().begin())); - } else { - nextRoute = 0; - } - if (currentRoute > 0) { - FGTaxiSegment *tx = findSegment(currentRoute); - FGTaxiSegment *nx; - if (nextRoute) { - nx = findSegment(nextRoute); - } else { - nx = tx; - } - //if (tx->hasBlock(now) || nx->hasBlock(now) ) { - // current->setHoldPosition(true); - //} - SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude()))); - SGGeod end (nx->getStart()->geod()); - - double distance = SGGeodesy::distanceM(start, end); - if (nx->hasBlock(now) && (distance < i->getRadius() * 4)) { - current->setHoldPosition(true); - } else { - intVecIterator ivi = i->getIntentions().begin(); - while (ivi != i->getIntentions().end()) { - if ((*ivi) > 0) { - distance += segments[(*ivi)-1]->getLength(); - if ((segments[(*ivi)-1]->hasBlock(now)) && (distance < i->getRadius() * 4)) { - current->setHoldPosition(true); - break; - } - } - ivi++; - } - } - } - bool currStatus = current->hasHoldPosition(); - current->setHoldPosition(origStatus); - // Either a Hold Position or a resume taxi transmission has been issued - if ((now - lastTransmission) > 2) { - available = true; - } - if (current->getState() == 0) { - if ((origStatus != currStatus) && available) { - //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl; - if (currStatus == true) { // No has a hold short instruction - transmit(&(*current), dynamics, MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true); - //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl; - current->setState(1); - } else { - transmit(&(*current), dynamics, MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true); - //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl; - current->setState(2); - } - lastTransmission = now; - available = false; - // Don't act on the changed instruction until the transmission is confirmed - // So set back to original status - //cerr << "Current state " << current->getState() << endl; - } - - } - // 6 = Report runway - // 7 = Acknowledge report runway - // 8 = Switch tower frequency - //9 = Acknowledge switch tower frequency - - //int state = current->getState(); - if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) { - current->setState(0); - current->setHoldPosition(true); - } - if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) { - current->setState(0); - current->setHoldPosition(false); - } - if (current->getAircraft()->getTakeOffStatus() && (current->getState() == 0)) { - //cerr << "Scheduling " << current->getAircraft()->getCallSign() << " for hold short" << endl; - current->setState(6); - } - if (checkTransmissionState(6,6, current, now, MSG_REPORT_RUNWAY_HOLD_SHORT, ATC_AIR_TO_GROUND)) { - } - if (checkTransmissionState(7,7, current, now, MSG_ACKNOWLEDGE_REPORT_RUNWAY_HOLD_SHORT, ATC_GROUND_TO_AIR)) { - } - if (checkTransmissionState(8,8, current, now, MSG_SWITCH_TOWER_FREQUENCY, ATC_GROUND_TO_AIR)) { - } - if (checkTransmissionState(9,9, current, now, MSG_ACKNOWLEDGE_SWITCH_TOWER_FREQUENCY, ATC_AIR_TO_GROUND)) { - } - - - - //current->setState(0); -} - void FGGroundNetwork::addSegment(const FGTaxiNodeRef &from, const FGTaxiNodeRef &to) { FGTaxiSegment* seg = new FGTaxiSegment(from, to); @@ -936,468 +451,3 @@ FGTaxiNodeVector FGGroundNetwork::segmentsFrom(const FGTaxiNodeRef &from) const return result; } -/** - * Check whether situations occur where the current aircraft is waiting for itself - * due to higher order interactions. - * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits - * for a. Ideally each aircraft only waits for one other aircraft, so by tracing - * through this list of waiting aircraft, we can check if we'd eventually end back - * at the current aircraft. - * - * Note that we should consider the situation where we are actually checking aircraft - * d, which is waiting for aircraft a. d is not part of the loop, but is held back by - * the looping aircraft. If we don't check for that, this function will get stuck into - * endless loop. - */ - -bool FGGroundNetwork::checkForCircularWaits(int id) -{ - //cerr << "Performing Wait check " << id << endl; - int target = 0; - TrafficVectorIterator current, other; - TrafficVectorIterator i = activeTraffic.begin(); - int trafficSize = activeTraffic.size(); - if (trafficSize) { - while (i != activeTraffic.end()) { - if (i->getId() == id) { - break; - } - i++; - } - } else { - return false; - } - if (i == activeTraffic.end() || (trafficSize == 0)) { - SG_LOG(SG_GENERAL, SG_ALERT, - "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits at " << SG_ORIGIN); - } - - current = i; - target = current->getWaitsForId(); - //bool printed = false; // Note that this variable is for debugging purposes only. - int counter = 0; - - if (id == target) { - //cerr << "aircraft waits for user" << endl; - return false; - } - - - while ((target > 0) && (target != id) && counter++ < trafficSize) { - //printed = true; - TrafficVectorIterator i = activeTraffic.begin(); - if (trafficSize) { - //while ((i->getId() != id) && i != activeTraffic.end()) - while (i != activeTraffic.end()) { - if (i->getId() == target) { - break; - } - i++; - } - } else { - return false; - } - if (i == activeTraffic.end() || (trafficSize == 0)) { - //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;; - // The target id is not found on the current network, which means it's at the tower - //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits"); - return false; - } - other = i; - target = other->getWaitsForId(); - - // actually this trap isn't as impossible as it first seemed: - // the setWaitsForID(id) is set to current when the aircraft - // is waiting for the user controlled aircraft. - //if (current->getId() == other->getId()) { - // cerr << "Caught the impossible trap" << endl; - // cerr << "Current = " << current->getId() << endl; - // cerr << "Other = " << other ->getId() << endl; - // for (TrafficVectorIterator at = activeTraffic.begin(); - // at != activeTraffic.end(); - // at++) { - // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl; - // } - // exit(1); - if (current->getId() == other->getId()) - return false; - //} - //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign() - // << " (" << other->getId() << "); " << endl;; - //current = other; - } - - - - - - - //if (printed) - // cerr << "[done] " << endl << endl;; - if (id == target) { - SG_LOG(SG_GENERAL, SG_WARN, - "Detected circular wait condition: Id = " << id << - "target = " << target); - return true; - } else { - return false; - } -} - -// Note that this function is probably obsolete... -bool FGGroundNetwork::hasInstruction(int id) -{ - TrafficVectorIterator i = activeTraffic.begin(); - // Search search if the current id has an entry - // This might be faster using a map instead of a vector, but let's start by taking a safe route - if (activeTraffic.size()) { - //while ((i->getId() != id) && i != activeTraffic.end()) { - while (i != activeTraffic.end()) { - if (i->getId() == id) { - break; - } - i++; - } - } - if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { - SG_LOG(SG_GENERAL, SG_ALERT, - "AI error: checking ATC instruction for aircraft without traffic record at " << SG_ORIGIN); - } else { - return i->hasInstruction(); - } - return false; -} - -FGATCInstruction FGGroundNetwork::getInstruction(int id) -{ - TrafficVectorIterator i = activeTraffic.begin(); - // Search search if the current id has an entry - // This might be faster using a map instead of a vector, but let's start by taking a safe route - if (activeTraffic.size()) { - //while ((i->getId() != id) && i != activeTraffic.end()) { - while (i != activeTraffic.end()) { - if (i->getId() == id) { - break; - } - i++; - } - } - if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { - SG_LOG(SG_GENERAL, SG_ALERT, - "AI error: requesting ATC instruction for aircraft without traffic record at " << SG_ORIGIN); - } else { - return i->getInstruction(); - } - return FGATCInstruction(); -} - -// Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that. -static void WorldCoordinate(osg::Matrix& obj_pos, double lat, - double lon, double elev, double hdg, double slope) -{ - SGGeod geod = SGGeod::fromDegM(lon, lat, elev); - obj_pos = makeZUpFrame(geod); - // hdg is not a compass heading, but a counter-clockwise rotation - // around the Z axis - obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS, - 0.0, 0.0, 1.0)); - obj_pos.preMult(osg::Matrix::rotate(slope * SGD_DEGREES_TO_RADIANS, - 0.0, 1.0, 0.0)); -} - - - - -void FGGroundNetwork::render(bool visible) -{ - SGMaterialLib *matlib = globals->get_matlib(); - if (group) { - //int nr = ; - globals->get_scenery()->get_scene_graph()->removeChild(group); - //while (group->getNumChildren()) { - // cerr << "Number of children: " << group->getNumChildren() << endl; - //simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0); - //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0); - //geode->releaseGLObjects(); - //group->removeChild(geode); - //delete geode; - group = 0; - } - if (visible) { - group = new osg::Group; - FGScenery * local_scenery = globals->get_scenery(); - // double elevation_meters = 0.0; -// double elevation_feet = 0.0; - time_t now = time(NULL) + fgGetLong("/sim/time/warp"); - //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) { - //double dx = 0; - for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) { - // Handle start point i.e. the segment that is connected to the aircraft itself on the starting end - // and to the the first "real" taxi segment on the other end. - int pos = i->getCurrentPosition() - 1; - if (pos >= 0) { - - SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude()))); - SGGeod end (segments[pos]->getEnd()->geod()); - - double length = SGGeodesy::distanceM(start, end); - //heading = SGGeodesy::headingDeg(start->geod(), end->geod()); - - double az2, heading; //, distanceM; - SGGeodesy::inverse(start, end, heading, az2, length); - double coveredDistance = length * 0.5; - SGGeod center; - SGGeodesy::direct(start, heading, coveredDistance, center, az2); - //std::cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << std::endl; - /////////////////////////////////////////////////////////////////////////////// - // Make a helper function out of this - osg::Matrix obj_pos; - osg::MatrixTransform *obj_trans = new osg::MatrixTransform; - obj_trans->setDataVariance(osg::Object::STATIC); - // Experimental: Calculate slope here, based on length, and the individual elevations - double elevationStart; - if (isUserAircraft((i)->getAircraft())) { - elevationStart = fgGetDouble("/position/ground-elev-m"); - } else { - elevationStart = ((i)->getAircraft()->_getAltitude()); - } - double elevationEnd = segments[pos]->getEnd()->getElevationM(); - //cerr << "Using elevation " << elevationEnd << endl; - - if ((elevationEnd == 0) || (elevationEnd = parent->getElevation())) { - SGGeod center2 = end; - center2.setElevationM(SG_MAX_ELEVATION_M); - if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) { -// elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5; - //elevation_meters += 0.5; - } - else { - elevationEnd = parent->getElevation(); - } - segments[pos]->getEnd()->setElevation(elevationEnd); - } - double elevationMean = (elevationStart + elevationEnd) / 2.0; - double elevDiff = elevationEnd - elevationStart; - - double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES; - - //cerr << "1. Using mean elevation : " << elevationMean << " and " << slope << endl; - - WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), elevationMean+ 0.5, -(heading), slope ); - - obj_trans->setMatrix( obj_pos ); - //osg::Vec3 center(0, 0, 0) - - float width = length /2.0; - osg::Vec3 corner(-width, 0, 0.25f); - osg::Vec3 widthVec(2*width + 1, 0, 0); - osg::Vec3 heightVec(0, 1, 0); - osg::Geometry* geometry; - geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec); - simgear::EffectGeode* geode = new simgear::EffectGeode; - geode->setName("test"); - geode->addDrawable(geometry); - //osg::Node *custom_obj; - SGMaterial *mat; - if (segments[pos]->hasBlock(now)) { - mat = matlib->find("UnidirectionalTaperRed", center); - } else { - mat = matlib->find("UnidirectionalTaperGreen", center); - } - if (mat) - geode->setEffect(mat->get_effect()); - obj_trans->addChild(geode); - // wire as much of the scene graph together as we can - //->addChild( obj_trans ); - group->addChild( obj_trans ); - ///////////////////////////////////////////////////////////////////// - } else { - //std::cerr << "BIG FAT WARNING: current position is here : " << pos << std::endl; - } - // Next: Draw the other taxi segments. - for (intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) { - osg::Matrix obj_pos; - int k = (*j)-1; - if (k >= 0) { - osg::MatrixTransform *obj_trans = new osg::MatrixTransform; - obj_trans->setDataVariance(osg::Object::STATIC); - - // Experimental: Calculate slope here, based on length, and the individual elevations - double elevationStart = segments[k]->getStart()->getElevationM(); - double elevationEnd = segments[k]->getEnd ()->getElevationM(); - if ((elevationStart == 0) || (elevationStart == parent->getElevation())) { - SGGeod center2 = segments[k]->getStart()->geod(); - center2.setElevationM(SG_MAX_ELEVATION_M); - if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) { -// elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5; - //elevation_meters += 0.5; - } - else { - elevationStart = parent->getElevation(); - } - segments[k]->getStart()->setElevation(elevationStart); - } - if ((elevationEnd == 0) || (elevationEnd == parent->getElevation())) { - SGGeod center2 = segments[k]->getEnd()->geod(); - center2.setElevationM(SG_MAX_ELEVATION_M); - if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) { -// elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5; - //elevation_meters += 0.5; - } - else { - elevationEnd = parent->getElevation(); - } - segments[k]->getEnd()->setElevation(elevationEnd); - } - - double elevationMean = (elevationStart + elevationEnd) / 2.0; - double elevDiff = elevationEnd - elevationStart; - double length = segments[k]->getLength(); - double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES; - - // cerr << "2. Using mean elevation : " << elevationMean << " and " << slope << endl; - - SGGeod segCenter = segments[k]->getCenter(); - WorldCoordinate( obj_pos, segCenter.getLatitudeDeg(), segCenter.getLongitudeDeg(), elevationMean+ 0.5, -(segments[k]->getHeading()), slope ); - - obj_trans->setMatrix( obj_pos ); - //osg::Vec3 center(0, 0, 0) - - float width = segments[k]->getLength() /2.0; - osg::Vec3 corner(-width, 0, 0.25f); - osg::Vec3 widthVec(2*width + 1, 0, 0); - osg::Vec3 heightVec(0, 1, 0); - osg::Geometry* geometry; - geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec); - simgear::EffectGeode* geode = new simgear::EffectGeode; - geode->setName("test"); - geode->addDrawable(geometry); - //osg::Node *custom_obj; - SGMaterial *mat; - if (segments[k]->hasBlock(now)) { - mat = matlib->find("UnidirectionalTaperRed", segCenter); - } else { - mat = matlib->find("UnidirectionalTaperGreen", segCenter); - } - if (mat) - geode->setEffect(mat->get_effect()); - obj_trans->addChild(geode); - // wire as much of the scene graph together as we can - //->addChild( obj_trans ); - group->addChild( obj_trans ); - } - } - //dx += 0.1; - } - globals->get_scenery()->get_scene_graph()->addChild(group); - } -} - -string FGGroundNetwork::getName() { - return string(parent->getId() + "-ground"); -} - -void FGGroundNetwork::update(double dt) -{ - time_t now = time(NULL) + fgGetLong("/sim/time/warp"); - for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) { - (*tsi)->unblock(now); - } - int priority = 1; - //sort(activeTraffic.begin(), activeTraffic.end(), compare_trafficrecords); - // Handle traffic that is under ground control first; this way we'll prevent clutter at the gate areas. - // Don't allow an aircraft to pushback when a taxiing aircraft is currently using part of the intended route. - for (TrafficVectorIterator i = dynamics->getStartupController()->getActiveTraffic().begin(); - i != dynamics->getStartupController()->getActiveTraffic().end(); i++) { - i->allowPushBack(); - i->setPriority(priority++); - // in meters per second; - double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600; - if (i->isActive(0)) { - - // Check for all active aircraft whether it's current pos segment is - // an opposite of one of the departing aircraft's intentions - for (TrafficVectorIterator j = activeTraffic.begin(); j != activeTraffic.end(); j++) { - int pos = j->getCurrentPosition(); - if (pos > 0) { - FGTaxiSegment *seg = segments[pos-1]->opposite(); - if (seg) { - int posReverse = seg->getIndex(); - for (intVecIterator k = i->getIntentions().begin(); k != i->getIntentions().end(); k++) { - if ((*k) == posReverse) { - i->denyPushBack(); - segments[posReverse-1]->block(i->getId(), now, now); - } - } - } - } - } - // if the current aircraft is still allowed to pushback, we can start reserving a route for if by blocking all the entry taxiways. - if (i->pushBackAllowed()) { - double length = 0; - int pos = i->getCurrentPosition(); - if (pos > 0) { - FGTaxiSegment *seg = segments[pos-1]; - FGTaxiNode *node = seg->getEnd(); - length = seg->getLength(); - for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) { - if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) { - (*tsi)->block(i->getId(), now, now); - } - } - } - for (intVecIterator j = i->getIntentions().begin(); j != i->getIntentions().end(); j++) { - int pos = (*j); - if (pos > 0) { - FGTaxiSegment *seg = segments[pos-1]; - FGTaxiNode *node = seg->getEnd(); - length += seg->getLength(); - time_t blockTime = now + (length / vTaxi); - for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) { - if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) { - (*tsi)->block(i->getId(), blockTime-30, now); - } - } - } - } - } - } - } - for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) { - double length = 0; - double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600; - i->setPriority(priority++); - int pos = i->getCurrentPosition(); - if (pos > 0) { - length = segments[pos-1]->getLength(); - if (segments[pos-1]->hasBlock(now)) { - //SG_LOG(SG_GENERAL, SG_ALERT, "Taxiway incursion for AI aircraft" << i->getAircraft()->getCallSign()); - } - - } - intVecIterator ivi; - for (ivi = i->getIntentions().begin(); ivi != i->getIntentions().end(); ivi++) { - int segIndex = (*ivi); - if (segIndex > 0) { - if (segments[segIndex-1]->hasBlock(now)) - break; - } - } - //after this, ivi points just behind the last valid unblocked taxi segment. - for (intVecIterator j = i->getIntentions().begin(); j != ivi; j++) { - int pos = (*j); - if (pos > 0) { - FGTaxiSegment *seg = segments[pos-1]; - FGTaxiNode *node = seg->getEnd(); - length += seg->getLength(); - for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) { - if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) { - time_t blockTime = now + (length / vTaxi); - (*tsi)->block(i->getId(), blockTime - 30, now); - } - } - } - } - } -} - diff --git a/src/Airports/groundnetwork.hxx b/src/Airports/groundnetwork.hxx index 3dd4c1cf0..7f0b5cfdd 100644 --- a/src/Airports/groundnetwork.hxx +++ b/src/Airports/groundnetwork.hxx @@ -30,10 +30,12 @@ #include "gnnode.hxx" #include "parking.hxx" -#include class FGAirportDynamicsXMLLoader; +typedef std::vector intVec; +typedef std::vector::iterator intVecIterator; + class Block { private: @@ -172,7 +174,7 @@ public: /************************************************************************************** * class FGGroundNetWork *************************************************************************************/ -class FGGroundNetwork : public FGATCController +class FGGroundNetwork { private: friend class FGAirportDynamicsXMLLoader; @@ -180,18 +182,11 @@ private: FGAirportDynamics* dynamics; // weak back-pointer to our owner bool hasNetwork; bool networkInitialized; - time_t nextSave; - //int maxDepth; - int count; + int version; FGTaxiSegmentVector segments; - TrafficVector activeTraffic; - TrafficVectorIterator currTraffic; - - double totalDistance, maxDistance; - FGTowerController *towerController; FGAirport *parent; FGParkingList m_parkings; @@ -223,15 +218,14 @@ public: bool exists() { return hasNetwork; }; - void setTowerController(FGTowerController *twrCtrlr) { - towerController = twrCtrlr; - }; FGTaxiNodeRef findNearestNode(const SGGeod& aGeod) const; FGTaxiNodeRef findNearestNodeOnRunway(const SGGeod& aGeod, FGRunway* aRunway = NULL) const; FGTaxiSegment *findSegment(unsigned int idx) const; + FGTaxiSegment* findOppositeSegment(unsigned int index) const; + const FGParkingList& allParkings() const; /** @@ -244,22 +238,12 @@ public: FGTaxiRoute findShortestRoute(FGTaxiNode* start, FGTaxiNode* end, bool fullSearch=true); - virtual void announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentRoute, - double lat, double lon, double hdg, double spd, double alt, - double radius, int leg, FGAIAircraft *aircraft); - virtual void signOff(int id); - virtual void updateAircraftInformation(int id, double lat, double lon, double heading, double speed, double alt, double dt); - virtual bool hasInstruction(int id); - virtual FGATCInstruction getInstruction(int id); - - bool checkTransmissionState(int minState, int MaxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId, - AtcMsgDir msgDir); - bool checkForCircularWaits(int id); - virtual void render(bool); - virtual std::string getName(); - virtual void update(double dt); + + void blockSegmentsEndingAt(FGTaxiSegment* seg, int blockId, + time_t blockTime, time_t now); void addVersion(int v) {version = v; }; + void unblockAllSegments(time_t now); }; -- 2.39.5