diff --git a/src/AIModel/AIAircraft.cxx b/src/AIModel/AIAircraft.cxx index 2bf71b3b1..0a8c1d546 100644 --- a/src/AIModel/AIAircraft.cxx +++ b/src/AIModel/AIAircraft.cxx @@ -80,6 +80,7 @@ FGAIAircraft::FGAIAircraft(FGAISchedule *ref) : FGAIBase(otAircraft) { headingError = 0; holdPos = false; + needsTaxiClearance = false; _performance = 0; //TODO initialize to JET_TRANSPORT from PerformanceDB dt = 0; @@ -663,9 +664,8 @@ bool FGAIAircraft::handleAirportEndPoints(FGAIFlightPlan::waypoint* prev, time_t //cerr << trafficRef->getCallSign() << " has passed waypoint " << prev->name << " at speed " << speed << endl; if (prev->name == "PushBackPoint") { dep->getDynamics()->releaseParking(fp->getGate()); - time_t holdUntil = now + 120; - fp->setTime(holdUntil); - //cerr << _getCallsign() << "Holding at pushback point" << endl; + AccelTo(0.0); + setTaxiClearanceRequest(true); } // This is the last taxi waypoint, and marks the the end of the flight plan diff --git a/src/AIModel/AIAircraft.hxx b/src/AIModel/AIAircraft.hxx index 6a53e5a2b..7e65d0df5 100644 --- a/src/AIModel/AIAircraft.hxx +++ b/src/AIModel/AIAircraft.hxx @@ -70,6 +70,8 @@ public: void announcePositionToController(); //TODO have to be public? void processATC(FGATCInstruction instruction); + void setTaxiClearanceRequest(bool arg) { needsTaxiClearance = arg; }; + bool getTaxiClearanceRequest() { return needsTaxiClearance; }; FGAISchedule * getTrafficRef() { return trafficRef; }; virtual const char* getTypeString(void) const { return "aircraft"; } @@ -150,6 +152,7 @@ private: const char * _getTransponderCode() const; bool reachedWaypoint; + bool needsTaxiClearance; time_t timeElapsed; PerformanceData* _performance; // the performance data for this aircraft diff --git a/src/ATC/trafficcontrol.cxx b/src/ATC/trafficcontrol.cxx index f9c2fee4e..98b2ab46e 100644 --- a/src/ATC/trafficcontrol.cxx +++ b/src/ATC/trafficcontrol.cxx @@ -340,11 +340,27 @@ FGATCInstruction::FGATCInstruction() alt = 0; } + bool FGATCInstruction::hasInstruction() { return (holdPattern || holdPosition || changeSpeed || changeHeading || changeAltitude || resolveCircularWait); } +/*************************************************************************** + * FGATCController + * + **************************************************************************/ + + + + +FGATCController::FGATCController() +{ + dt_count = 0; + available = true; + lastTransmission = 0; +} + string FGATCController::getGateName(FGAIAircraft *ref) { return ref->atGate(); @@ -370,6 +386,7 @@ void FGATCController::transmit(FGTrafficRecord *rec, AtcMsgId msgId, AtcMsgDir m //double commFreqD; sender = rec->getAircraft()->getTrafficRef()->getCallSign(); + //cerr << "transmitting for: " << sender << "Leg = " << rec->getLeg() << endl; switch (rec->getLeg()) { case 2: case 3: @@ -449,6 +466,12 @@ void FGATCController::transmit(FGTrafficRecord *rec, AtcMsgId msgId, AtcMsgDir m taxiFreqStr = formatATCFrequency3_2(taxiFreq); text = receiver + ". Switching to " + taxiFreqStr + ". " + sender; break; + case MSG_INITIATE_CONTACT: + text = receiver + ". With you. " + sender; + break; + case MSG_ACKNOWLEDGE_INITIATE_CONTACT: + text = receiver + ". Roger. " + sender; + break; case MSG_REQUEST_PUSHBACK_CLEARANCE: text = receiver + ". Request push-back. " + sender; break; @@ -457,9 +480,30 @@ void FGATCController::transmit(FGTrafficRecord *rec, AtcMsgId msgId, AtcMsgDir m break; case MSG_HOLD_PUSHBACK_CLEARANCE: text = receiver + ". Standby. " + sender; - break; - default: - text = sender + ". Transmitting unknown Message"; + break; + case MSG_REQUEST_TAXI_CLEARANCE: + text = receiver + ". Ready to Taxi. " + sender; + break; + case MSG_ISSUE_TAXI_CLEARANCE: + text = receiver + ". Cleared to taxi. " + sender; + break; + case MSG_ACKNOWLEDGE_TAXI_CLEARANCE: + text = receiver + ". Cleared to taxi. " + sender; + break; + case MSG_HOLD_POSITION: + text = receiver + ". Hold Position. " + sender; + break; + case MSG_ACKNOWLEDGE_HOLD_POSITION: + text = receiver + ". Holding Position. " + sender; + break; + case MSG_RESUME_TAXI: + text = receiver + ". Resume Taxiing. " + sender; + break; + case MSG_ACKNOWLEDGE_RESUME_TAXI: + text = receiver + ". Continuing Taxi. " + sender; + break; + default: + text = text + sender + ". Transmitting unknown Message"; break; } double onBoardRadioFreq0 = fgGetDouble("/instrumentation/comm[0]/frequencies/selected-mhz"); @@ -691,8 +735,6 @@ FGATCInstruction FGTowerController::getInstruction(int id) FGStartupController::FGStartupController() : FGATCController() { - available = false; - lastTransmission = 0; } void FGStartupController::announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentPosition, @@ -874,7 +916,7 @@ void FGStartupController::update(int id, double lat, double lon, double heading, available = false; } } - // Note: The next two stages are only necessesary when Startup control is + // Note: The next four stages are only necessesary when Startup control is // on a different frequency, compared to ground control if ((state == 4) && available){ if (now > startTime+130) { @@ -885,20 +927,36 @@ void FGStartupController::update(int id, double lat, double lon, double heading, available = false; } } - - - // TODO: Switch to APRON control and request pushback Clearance. - // Get Push back clearance if ((state == 5) && available){ - if (now > startTime+160) { - transmit(&(*i), MSG_REQUEST_PUSHBACK_CLEARANCE, ATC_AIR_TO_GROUND); + if (now > startTime+140) { + transmit(&(*i), MSG_INITIATE_CONTACT, ATC_AIR_TO_GROUND); i->updateState(); lastTransmission = now; available = false; } } if ((state == 6) && available){ + if (now > startTime+150) { + transmit(&(*i), MSG_ACKNOWLEDGE_INITIATE_CONTACT, ATC_GROUND_TO_AIR); + i->updateState(); + lastTransmission = now; + available = false; + } + } + + + // TODO: Switch to APRON control and request pushback Clearance. + // Get Push back clearance + if ((state == 7) && available){ if (now > startTime+180) { + transmit(&(*i), MSG_REQUEST_PUSHBACK_CLEARANCE, ATC_AIR_TO_GROUND); + i->updateState(); + lastTransmission = now; + available = false; + } + } + if ((state == 8) && available){ + if (now > startTime+200) { if (i->pushBackAllowed()) { i->allowRepeatedTransmissions(); transmit(&(*i), MSG_PERMIT_PUSHBACK_CLEARANCE, ATC_GROUND_TO_AIR); @@ -911,7 +969,7 @@ void FGStartupController::update(int id, double lat, double lon, double heading, available = false; } } - if ((state == 6) && available){ + if ((state == 9) && available){ i->setHoldPosition(false); } } diff --git a/src/ATC/trafficcontrol.hxx b/src/ATC/trafficcontrol.hxx index f97e7cf22..d069a5b84 100644 --- a/src/ATC/trafficcontrol.hxx +++ b/src/ATC/trafficcontrol.hxx @@ -129,6 +129,7 @@ public: void setLeg(int lg) { leg = lg;}; int getId() { return id;}; int getState() { return state;}; + void setState(int s) { state = s;} FGATCInstruction getInstruction() { return instruction;}; bool hasInstruction() { return instruction.hasInstruction(); }; void setPositionAndHeading(double lat, double lon, double hdg, double spd, double alt); @@ -210,7 +211,10 @@ typedef vector::iterator ActiveRunwayVecIterator; *************************************************************************************/ class FGATCController { -private: +protected: + bool available; + time_t lastTransmission; + double dt_count; @@ -220,18 +224,27 @@ private: public: typedef enum { MSG_ANNOUNCE_ENGINE_START, - MSG_REQUEST_ENGINE_START, + MSG_REQUEST_ENGINE_START, MSG_PERMIT_ENGINE_START, MSG_DENY_ENGINE_START, MSG_ACKNOWLEDGE_ENGINE_START, MSG_REQUEST_PUSHBACK_CLEARANCE, - MSG_PERMIT_PUSHBACK_CLEARANCE, + MSG_PERMIT_PUSHBACK_CLEARANCE, MSG_HOLD_PUSHBACK_CLEARANCE, - MSG_ACKNOWLEDGE_SWITCH_GROUND_FREQUENCY } AtcMsgId; + MSG_ACKNOWLEDGE_SWITCH_GROUND_FREQUENCY, + MSG_INITIATE_CONTACT, + MSG_ACKNOWLEDGE_INITIATE_CONTACT, + MSG_REQUEST_TAXI_CLEARANCE, + MSG_ISSUE_TAXI_CLEARANCE, + MSG_ACKNOWLEDGE_TAXI_CLEARANCE, + MSG_HOLD_POSITION, + MSG_ACKNOWLEDGE_HOLD_POSITION, + MSG_RESUME_TAXI, + MSG_ACKNOWLEDGE_RESUME_TAXI } AtcMsgId; typedef enum { ATC_AIR_TO_GROUND, ATC_GROUND_TO_AIR } AtcMsgDir; - FGATCController() { dt_count = 0;}; + FGATCController(); virtual ~FGATCController() {}; virtual void announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentRoute, double lat, double lon, @@ -284,8 +297,6 @@ class FGStartupController : public FGATCController { private: TrafficVector activeTraffic; - bool available; - time_t lastTransmission; //ActiveRunwayVec activeRunways; public: diff --git a/src/Airports/apt_loader.cxx b/src/Airports/apt_loader.cxx index 8984214d9..e64ee5923 100644 --- a/src/Airports/apt_loader.cxx +++ b/src/Airports/apt_loader.cxx @@ -237,7 +237,6 @@ private: SGGeod pos(SGGeod::fromDegFt(lon, lat, last_apt_elev)); FGAirport* apt = new FGAirport(last_apt_id, pos, tower, last_apt_name, false, fptypeFromRobinType(atoi(last_apt_type.c_str()))); - apt->setRunwaysAndTaxiways(runways, taxiways, pavements); } diff --git a/src/Airports/dynamics.cxx b/src/Airports/dynamics.cxx index 3e658a147..a323d809c 100644 --- a/src/Airports/dynamics.cxx +++ b/src/Airports/dynamics.cxx @@ -50,31 +50,34 @@ using std::random_shuffle; #include "simple.hxx" #include "dynamics.hxx" -FGAirportDynamics::FGAirportDynamics(FGAirport* ap) : - _ap(ap), rwyPrefs(ap), SIDs(ap) { - lastUpdate = 0; +FGAirportDynamics::FGAirportDynamics(FGAirport * ap): +_ap(ap), rwyPrefs(ap), SIDs(ap) +{ + lastUpdate = 0; - // For testing only. This needs to be refined when we move ATIS functionality over. - atisInformation = "Sierra"; + // For testing only. This needs to be refined when we move ATIS functionality over. + atisInformation = "Sierra"; } // Note that the ground network should also be copied -FGAirportDynamics::FGAirportDynamics(const FGAirportDynamics& other) : - rwyPrefs(other.rwyPrefs), - SIDs(other.SIDs) +FGAirportDynamics:: +FGAirportDynamics(const FGAirportDynamics & other):rwyPrefs(other. + rwyPrefs), +SIDs(other.SIDs) { - for (FGParkingVecConstIterator ip= other.parkings.begin(); ip != other.parkings.end(); ip++) - parkings.push_back(*(ip)); - // rwyPrefs = other.rwyPrefs; - lastUpdate = other.lastUpdate; - - stringVecConstIterator il; - for (il = other.landing.begin(); il != other.landing.end(); il++) - landing.push_back(*il); - for (il = other.takeoff.begin(); il != other.takeoff.end(); il++) - takeoff.push_back(*il); - lastUpdate = other.lastUpdate; - atisInformation = other.atisInformation; + for (FGParkingVecConstIterator ip = other.parkings.begin(); + ip != other.parkings.end(); ip++) + parkings.push_back(*(ip)); + // rwyPrefs = other.rwyPrefs; + lastUpdate = other.lastUpdate; + + stringVecConstIterator il; + for (il = other.landing.begin(); il != other.landing.end(); il++) + landing.push_back(*il); + for (il = other.takeoff.begin(); il != other.takeoff.end(); il++) + takeoff.push_back(*il); + lastUpdate = other.lastUpdate; + atisInformation = other.atisInformation; } // Destructor @@ -84,449 +87,404 @@ FGAirportDynamics::~FGAirportDynamics() // Initialization required after XMLRead -void FGAirportDynamics::init() +void FGAirportDynamics::init() { - // This may seem a bit weird to first randomly shuffle the parkings - // and then sort them again. However, parkings are sorted here by ascending - // radius. Since many parkings have similar radii, with each radius class they will - // still be allocated relatively systematically. Randomizing prior to sorting will - // prevent any initial orderings to be destroyed, leading (hopefully) to a more - // naturalistic gate assignment. - random_shuffle(parkings.begin(), parkings.end()); - sort(parkings.begin(), parkings.end()); - // add the gate positions to the ground network. - groundNetwork.addNodes(&parkings); - groundNetwork.init(); - groundNetwork.setTowerController(&towerController); - groundNetwork.setParent(_ap); + // This may seem a bit weird to first randomly shuffle the parkings + // and then sort them again. However, parkings are sorted here by ascending + // radius. Since many parkings have similar radii, with each radius class they will + // still be allocated relatively systematically. Randomizing prior to sorting will + // prevent any initial orderings to be destroyed, leading (hopefully) to a more + // naturalistic gate assignment. + random_shuffle(parkings.begin(), parkings.end()); + sort(parkings.begin(), parkings.end()); + // add the gate positions to the ground network. + groundNetwork.addNodes(&parkings); + groundNetwork.init(); + groundNetwork.setTowerController(&towerController); + groundNetwork.setParent(_ap); } -bool FGAirportDynamics::getAvailableParking(double *lat, double *lon, double *heading, int *gateId, double rad, const string &flType, const string &acType, const string &airline) +bool FGAirportDynamics::getAvailableParking(double *lat, double *lon, + double *heading, int *gateId, + double rad, + const string & flType, + const string & acType, + const string & airline) { - bool found = false; - bool available = false; - //string gateType; + bool found = false; + bool available = false; - FGParkingVecIterator i; -// if (flType == "cargo") -// { -// gateType = "RAMP_CARGO"; -// } -// else if (flType == "ga") -// { -// gateType = "RAMP_GA"; -// } -// else gateType = "GATE"; - - if (parkings.begin() == parkings.end()) - { - //cerr << "Could not find parking spot at " << _ap->getId() << endl; - *lat = _ap->getLatitude(); - *lon = _ap->getLongitude(); - *heading = 0; - found = true; - } - else - { - // First try finding a parking with a designated airline code - for (i = parkings.begin(); !(i == parkings.end() || found); i++) - { - //cerr << "Gate Id: " << i->getIndex() - // << " Type : " << i->getType() - // << " Codes : " << i->getCodes() - // << " Radius: " << i->getRadius() - // << " Name : " << i->getName() - // << " Available: " << i->isAvailable() << endl; - available = true; - // Taken by another aircraft - if (!(i->isAvailable())) - { - available = false; - continue; - } - // No airline codes, so skip - if (i->getCodes().empty()) - { - available = false; - continue; - } - else // Airline code doesn't match - { - //cerr << "Code = " << airline << ": Codes " << i->getCodes(); - if (i->getCodes().find(airline, 0) == string::npos) - { - available = false; - //cerr << "Unavailable" << endl; - continue; - } - else - { - //cerr << "Available" << endl; - } - } - // Type doesn't match - if (i->getType() != flType) - { - available = false; - continue; - } - // too small - if (i->getRadius() < rad) - { - available = false; - continue; - } - - if (available) - { - *lat = i->getLatitude (); - *lon = i->getLongitude(); - *heading = i->getHeading (); - *gateId = i->getIndex (); - i->setAvailable(false); - found = true; - } - } - // then try again for those without codes. - for (i = parkings.begin(); !(i == parkings.end() || found); i++) - { - available = true; - if (!(i->isAvailable())) - { - available = false; - continue; - } - if (!(i->getCodes().empty())) - { - if ((i->getCodes().find(airline,0) == string::npos)) - { - available = false; - continue; - } - } - if (i->getType() != flType) - { - available = false; - continue; - } - - if (i->getRadius() < rad) - { - available = false; - continue; - } - - if (available) - { - *lat = i->getLatitude (); - *lon = i->getLongitude(); - *heading = i->getHeading (); - *gateId = i->getIndex (); - i->setAvailable(false); - found = true; - } - } - // And finally once more if that didn't work. Now ignore the airline codes, as a last resort - for (i = parkings.begin(); !(i == parkings.end() || found); i++) - { - available = true; - if (!(i->isAvailable())) - { - available = false; - continue; - } - if (i->getType() != flType) - { - available = false; - continue; - } - - if (i->getRadius() < rad) - { - available = false; - continue; - } - - if (available) - { - *lat = i->getLatitude (); - *lon = i->getLongitude(); - *heading = i->getHeading (); - *gateId = i->getIndex (); - i->setAvailable(false); - found = true; - } - } - } - if (!found) - { - //cerr << "Traffic overflow at" << _ap->getId() - // << ". flType = " << flType - // << ". airline = " << airline - // << " Radius = " <getLatitude(); - *lon = _ap->getLongitude(); - *heading = 0; - *gateId = -1; - //exit(1); - } - return found; -} -void FGAirportDynamics::getParking (int id, double *lat, double* lon, double *heading) -{ - if (id < 0) - { - *lat = _ap->getLatitude(); - *lon = _ap->getLongitude(); - *heading = 0; - } - else - { - FGParkingVecIterator i = parkings.begin(); - for (i = parkings.begin(); i != parkings.end(); i++) - { - if (id == i->getIndex()) - { - *lat = i->getLatitude(); - *lon = i->getLongitude(); - *heading = i->getHeading(); - } - } - } -} + FGParkingVecIterator i; + if (parkings.begin() == parkings.end()) { + //cerr << "Could not find parking spot at " << _ap->getId() << endl; + *lat = _ap->getLatitude(); + *lon = _ap->getLongitude(); + *heading = 0; + found = true; + } else { + // First try finding a parking with a designated airline code + for (i = parkings.begin(); !(i == parkings.end() || found); i++) { + available = true; + // Taken by another aircraft + if (!(i->isAvailable())) { + available = false; + continue; + } + // No airline codes, so skip + if (i->getCodes().empty()) { + available = false; + continue; + } else { // Airline code doesn't match + //cerr << "Code = " << airline << ": Codes " << i->getCodes(); + if (i->getCodes().find(airline, 0) == string::npos) { + available = false; + //cerr << "Unavailable" << endl; + continue; + } else { + //cerr << "Available" << endl; + } + } + // Type doesn't match + if (i->getType() != flType) { + available = false; + continue; + } + // too small + if (i->getRadius() < rad) { + available = false; + continue; + } -FGParking *FGAirportDynamics::getParking(int id) -{ - FGParkingVecIterator i = parkings.begin(); - for (i = parkings.begin(); i != parkings.end(); i++) - { - if (id == i->getIndex()) { - return &(*i); - } + if (available) { + *lat = i->getLatitude(); + *lon = i->getLongitude(); + *heading = i->getHeading(); + *gateId = i->getIndex(); + i->setAvailable(false); + found = true; + } } + // then try again for those without codes. + for (i = parkings.begin(); !(i == parkings.end() || found); i++) { + available = true; + if (!(i->isAvailable())) { + available = false; + continue; + } + if (!(i->getCodes().empty())) { + if ((i->getCodes().find(airline, 0) == string::npos)) { + available = false; + continue; + } + } + if (i->getType() != flType) { + available = false; + continue; + } + + if (i->getRadius() < rad) { + available = false; + continue; + } + + if (available) { + *lat = i->getLatitude(); + *lon = i->getLongitude(); + *heading = i->getHeading(); + *gateId = i->getIndex(); + i->setAvailable(false); + found = true; + } + } + // And finally once more if that didn't work. Now ignore the airline codes, as a last resort + for (i = parkings.begin(); !(i == parkings.end() || found); i++) { + available = true; + if (!(i->isAvailable())) { + available = false; + continue; + } + if (i->getType() != flType) { + available = false; + continue; + } + + if (i->getRadius() < rad) { + available = false; + continue; + } + + if (available) { + *lat = i->getLatitude(); + *lon = i->getLongitude(); + *heading = i->getHeading(); + *gateId = i->getIndex(); + i->setAvailable(false); + found = true; + } + } + } + if (!found) { + //cerr << "Traffic overflow at" << _ap->getId() + // << ". flType = " << flType + // << ". airline = " << airline + // << " Radius = " <getLatitude(); + *lon = _ap->getLongitude(); + *heading = 0; + *gateId = -1; + //exit(1); + } + return found; +} + +void FGAirportDynamics::getParking(int id, double *lat, double *lon, + double *heading) +{ + if (id < 0) { + *lat = _ap->getLatitude(); + *lon = _ap->getLongitude(); + *heading = 0; + } else { + FGParkingVecIterator i = parkings.begin(); + for (i = parkings.begin(); i != parkings.end(); i++) { + if (id == i->getIndex()) { + *lat = i->getLatitude(); + *lon = i->getLongitude(); + *heading = i->getHeading(); + } + } + } +} + +FGParking *FGAirportDynamics::getParking(int id) +{ + FGParkingVecIterator i = parkings.begin(); + for (i = parkings.begin(); i != parkings.end(); i++) { + if (id == i->getIndex()) { + return &(*i); + } + } return 0; } -string FGAirportDynamics::getParkingName(int id) -{ + +string FGAirportDynamics::getParkingName(int id) +{ FGParkingVecIterator i = parkings.begin(); - for (i = parkings.begin(); i != parkings.end(); i++) - { - if (id == i->getIndex()) { - return i->getName(); - } + for (i = parkings.begin(); i != parkings.end(); i++) { + if (id == i->getIndex()) { + return i->getName(); } + } return string("overflow"); } + void FGAirportDynamics::releaseParking(int id) { - if (id >= 0) - { - - FGParkingVecIterator i = parkings.begin(); - for (i = parkings.begin(); i != parkings.end(); i++) - { - if (id == i->getIndex()) - { - i -> setAvailable(true); - } - } + if (id >= 0) { + + FGParkingVecIterator i = parkings.begin(); + for (i = parkings.begin(); i != parkings.end(); i++) { + if (id == i->getIndex()) { + i->setAvailable(true); + } + } } } - -void FGAirportDynamics::setRwyUse(const FGRunwayPreference& ref) + +void FGAirportDynamics::setRwyUse(const FGRunwayPreference & ref) { - rwyPrefs = ref; - //cerr << "Exiting due to not implemented yet" << endl; - //exit(1); + rwyPrefs = ref; + //cerr << "Exiting due to not implemented yet" << endl; + //exit(1); } -bool FGAirportDynamics::innerGetActiveRunway(const string &trafficType, int action, string &runway, double heading) +bool FGAirportDynamics::innerGetActiveRunway(const string & trafficType, + int action, string & runway, + double heading) { -double windSpeed; - double windHeading; - double maxTail; - double maxCross; - string name; - string type; + double windSpeed; + double windHeading; + double maxTail; + double maxCross; + string name; + string type; - if (!rwyPrefs.available()) { - return false; - } - - RunwayGroup *currRunwayGroup = 0; - int nrActiveRunways = 0; - time_t dayStart = fgGetLong("/sim/time/utc/day-seconds"); - if ((abs((long)(dayStart - lastUpdate)) > 600) || trafficType != prevTrafficType) - { - landing.clear(); - takeoff.clear(); - lastUpdate = dayStart; - prevTrafficType = trafficType; + if (!rwyPrefs.available()) { + return false; + } - FGEnvironment - stationweather = ((FGEnvironmentMgr *) globals->get_subsystem("environment")) - ->getEnvironment(getLatitude(), - getLongitude(), - getElevation()); - - windSpeed = stationweather.get_wind_speed_kt(); - windHeading = stationweather.get_wind_from_heading_deg(); - string scheduleName; - //cerr << "finding active Runway for" << _ap->getId() << endl; - //cerr << "Nr of seconds since day start << " << dayStart << endl; + RunwayGroup *currRunwayGroup = 0; + int nrActiveRunways = 0; + time_t dayStart = fgGetLong("/sim/time/utc/day-seconds"); + if ((abs((long) (dayStart - lastUpdate)) > 600) + || trafficType != prevTrafficType) { + landing.clear(); + takeoff.clear(); + lastUpdate = dayStart; + prevTrafficType = trafficType; - ScheduleTime *currSched; - //cerr << "A"<< endl; - currSched = rwyPrefs.getSchedule(trafficType.c_str()); - if (!(currSched)) - return false; - //cerr << "B"<< endl; - scheduleName = currSched->getName(dayStart); - maxTail = currSched->getTailWind (); - maxCross = currSched->getCrossWind (); - //cerr << "SChedule anme = " << scheduleName << endl; - if (scheduleName.empty()) - return false; - //cerr << "C"<< endl; - currRunwayGroup = rwyPrefs.getGroup(scheduleName); - //cerr << "D"<< endl; - if (!(currRunwayGroup)) - return false; - nrActiveRunways = currRunwayGroup->getNrActiveRunways(); + FGEnvironment + stationweather = + ((FGEnvironmentMgr *) globals->get_subsystem("environment")) + ->getEnvironment(getLatitude(), getLongitude(), + getElevation()); + + windSpeed = stationweather.get_wind_speed_kt(); + windHeading = stationweather.get_wind_from_heading_deg(); + string scheduleName; + //cerr << "finding active Runway for" << _ap->getId() << endl; + //cerr << "Nr of seconds since day start << " << dayStart << endl; + + ScheduleTime *currSched; + //cerr << "A"<< endl; + currSched = rwyPrefs.getSchedule(trafficType.c_str()); + if (!(currSched)) + return false; + //cerr << "B"<< endl; + scheduleName = currSched->getName(dayStart); + maxTail = currSched->getTailWind(); + maxCross = currSched->getCrossWind(); + //cerr << "SChedule anme = " << scheduleName << endl; + if (scheduleName.empty()) + return false; + //cerr << "C"<< endl; + currRunwayGroup = rwyPrefs.getGroup(scheduleName); + //cerr << "D"<< endl; + if (!(currRunwayGroup)) + return false; + nrActiveRunways = currRunwayGroup->getNrActiveRunways(); // Keep a history of the currently active runways, to ensure // that an already established selection of runways will not // be overridden once a more preferred selection becomes // available as that can lead to random runway swapping. - if (trafficType == "com") { - currentlyActive = &comActive; + if (trafficType == "com") { + currentlyActive = &comActive; } else if (trafficType == "gen") { - currentlyActive = &genActive; + currentlyActive = &genActive; } else if (trafficType == "mil") { - currentlyActive = &milActive; + currentlyActive = &milActive; } else if (trafficType == "ul") { - currentlyActive = &ulActive; + currentlyActive = &ulActive; } - // - currRunwayGroup->setActive(_ap, - windSpeed, - windHeading, - maxTail, - maxCross, - currentlyActive); - // Note that I SHOULD keep multiple lists in memory, one for - // general aviation, one for commercial and one for military - // traffic. - currentlyActive->clear(); - nrActiveRunways = currRunwayGroup->getNrActiveRunways(); - //cerr << "Choosing runway for " << trafficType << endl; - for (int i = 0; i < nrActiveRunways; i++) - { - type = "unknown"; // initialize to something other than landing or takeoff - currRunwayGroup->getActive(i, name, type); - if (type == "landing") - { - landing.push_back(name); - currentlyActive->push_back(name); - //cerr << "Landing " << name << endl; - } - if (type == "takeoff") - { - takeoff.push_back(name); - currentlyActive->push_back(name); - //cerr << "takeoff " << name << endl; - } - } - //cerr << endl; - } - - if (action == 1) // takeoff - { - int nr = takeoff.size(); - if (nr) - { - // Note that the randomization below, is just a placeholder to choose between - // multiple active runways for this action. This should be - // under ATC control. - runway = chooseRwyByHeading (takeoff, heading); - } - else - { // Fallback - runway = chooseRunwayFallback(); - } - } - - if (action == 2) // landing - { - int nr = landing.size(); - if (nr) - { - runway = chooseRwyByHeading (landing, heading); - } - else - { //fallback - runway = chooseRunwayFallback(); - } - } + currRunwayGroup->setActive(_ap, + windSpeed, + windHeading, + maxTail, maxCross, currentlyActive); - return true; + // Note that I SHOULD keep multiple lists in memory, one for + // general aviation, one for commercial and one for military + // traffic. + currentlyActive->clear(); + nrActiveRunways = currRunwayGroup->getNrActiveRunways(); + //cerr << "Choosing runway for " << trafficType << endl; + for (int i = 0; i < nrActiveRunways; i++) { + type = "unknown"; // initialize to something other than landing or takeoff + currRunwayGroup->getActive(i, name, type); + if (type == "landing") { + landing.push_back(name); + currentlyActive->push_back(name); + //cerr << "Landing " << name << endl; + } + if (type == "takeoff") { + takeoff.push_back(name); + currentlyActive->push_back(name); + //cerr << "takeoff " << name << endl; + } + } + //cerr << endl; + } + + if (action == 1) // takeoff + { + int nr = takeoff.size(); + if (nr) { + // Note that the randomization below, is just a placeholder to choose between + // multiple active runways for this action. This should be + // under ATC control. + runway = chooseRwyByHeading(takeoff, heading); + } else { // Fallback + runway = chooseRunwayFallback(); + } + } + + if (action == 2) // landing + { + int nr = landing.size(); + if (nr) { + runway = chooseRwyByHeading(landing, heading); + } else { //fallback + runway = chooseRunwayFallback(); + } + } + + return true; } -string FGAirportDynamics::chooseRwyByHeading(stringVec rwys, double heading) { - double bestError = 360.0; - double rwyHeading, headingError; - string runway; - for (stringVecIterator i = rwys.begin(); i != rwys.end(); i++) { - FGRunway *rwy = _ap->getRunwayByIdent((*i)); - rwyHeading = rwy->headingDeg(); - headingError = fabs(heading - rwyHeading); +string FGAirportDynamics::chooseRwyByHeading(stringVec rwys, + double heading) +{ + double bestError = 360.0; + double rwyHeading, headingError; + string runway; + for (stringVecIterator i = rwys.begin(); i != rwys.end(); i++) { + FGRunway *rwy = _ap->getRunwayByIdent((*i)); + rwyHeading = rwy->headingDeg(); + headingError = fabs(heading - rwyHeading); if (headingError > 180) headingError = fabs(headingError - 360); if (headingError < bestError) { runway = (*i); bestError = headingError; } - } - //cerr << "Using active runway " << runway << " for heading " << heading << endl; - return runway; + } + //cerr << "Using active runway " << runway << " for heading " << heading << endl; + return runway; } -void FGAirportDynamics::getActiveRunway(const string &trafficType, int action, string &runway, double heading) +void FGAirportDynamics::getActiveRunway(const string & trafficType, + int action, string & runway, + double heading) { - bool ok = innerGetActiveRunway(trafficType, action, runway, heading); - if (!ok) { - runway = chooseRunwayFallback(); - } + bool ok = innerGetActiveRunway(trafficType, action, runway, heading); + if (!ok) { + runway = chooseRunwayFallback(); + } } string FGAirportDynamics::chooseRunwayFallback() -{ - FGRunway* rwy = _ap->getActiveRunwayForUsage(); - return rwy->ident(); +{ + FGRunway *rwy = _ap->getActiveRunwayForUsage(); + return rwy->ident(); } -void FGAirportDynamics::addParking(FGParking& park) { - parkings.push_back(park); +void FGAirportDynamics::addParking(FGParking & park) +{ + parkings.push_back(park); } -double FGAirportDynamics::getLatitude() const { - return _ap->getLatitude(); +double FGAirportDynamics::getLatitude() const +{ + return _ap->getLatitude(); } -double FGAirportDynamics::getLongitude() const { - return _ap->getLongitude(); +double FGAirportDynamics::getLongitude() const +{ + return _ap->getLongitude(); } -double FGAirportDynamics::getElevation() const { - return _ap->getElevation(); +double FGAirportDynamics::getElevation() const +{ + return _ap->getElevation(); } -const string& FGAirportDynamics::getId() const { - return _ap->getId(); +const string & FGAirportDynamics::getId() const +{ + return _ap->getId(); } // Experimental: Return a different ground frequency depending on the leg of the @@ -536,28 +494,34 @@ const string& FGAirportDynamics::getId() const { // so that at least I can start working on assigning different frequencies to different // operations. -int FGAirportDynamics::getGroundFrequency(unsigned leg) { - //return freqGround.size() ? freqGround[0] : 0; }; - int groundFreq = 0; - if (leg < 2) { - SG_LOG(SG_ATC, SG_ALERT, "Leg value is smaller than two at " << SG_ORIGIN); - } - if (freqGround.size() == 0) { - return 0; - } - if ((freqGround.size() > leg-1) && (leg > 1)) { - groundFreq = freqGround[leg-1]; - } - if ((freqGround.size() < leg-1) && (leg > 1)) { - groundFreq = (freqGround.size() < (leg-1)) ? freqGround[freqGround.size()-1] : freqGround[leg-2]; - } - if ((freqGround.size() >= leg-1) && (leg > 1)) { - groundFreq = freqGround[leg-2]; - } +int FGAirportDynamics::getGroundFrequency(unsigned leg) +{ + //return freqGround.size() ? freqGround[0] : 0; }; + int groundFreq = 0; + if (leg < 2) { + SG_LOG(SG_ATC, SG_ALERT, + "Leg value is smaller than two at " << SG_ORIGIN); + } + if (freqGround.size() == 0) { + return 0; + } + if ((freqGround.size() > leg - 1) && (leg > 1)) { + groundFreq = freqGround[leg - 1]; + } + if ((freqGround.size() < leg - 1) && (leg > 1)) { + groundFreq = + (freqGround.size() < + (leg - 1)) ? freqGround[freqGround.size() - + 1] : freqGround[leg - 2]; + } + if ((freqGround.size() >= leg - 1) && (leg > 1)) { + groundFreq = freqGround[leg - 2]; + } return groundFreq; } -FGAIFlightPlan *FGAirportDynamics::getSID(string activeRunway, double heading) +FGAIFlightPlan *FGAirportDynamics::getSID(string activeRunway, + double heading) { - return SIDs.getBest(activeRunway, heading); + return SIDs.getBest(activeRunway, heading); } diff --git a/src/Airports/groundnetwork.cxx b/src/Airports/groundnetwork.cxx index 321a3af68..011acb866 100644 --- a/src/Airports/groundnetwork.cxx +++ b/src/Airports/groundnetwork.cxx @@ -32,6 +32,7 @@ #include +#include #include #include "groundnetwork.hxx" @@ -40,37 +41,35 @@ * FGTaxiSegment **************************************************************************/ -void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes) +void FGTaxiSegment::setStart(FGTaxiNodeVector * nodes) { - FGTaxiNodeVectorIterator i = nodes->begin(); - while (i != nodes->end()) - { - //cerr << "Scanning start node index" << (*i)->getIndex() << endl; - if ((*i)->getIndex() == startNode) - { - start = (*i)->getAddress(); - (*i)->addSegment(this); - return; - } - i++; + FGTaxiNodeVectorIterator i = nodes->begin(); + while (i != nodes->end()) { + //cerr << "Scanning start node index" << (*i)->getIndex() << endl; + if ((*i)->getIndex() == startNode) { + start = (*i)->getAddress(); + (*i)->addSegment(this); + return; + } + i++; } - SG_LOG(SG_GENERAL, SG_ALERT, "Could not find start node " << startNode << endl); + SG_LOG(SG_GENERAL, SG_ALERT, + "Could not find start node " << startNode << endl); } -void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes) +void FGTaxiSegment::setEnd(FGTaxiNodeVector * nodes) { - FGTaxiNodeVectorIterator i = nodes->begin(); - while (i != nodes->end()) - { - //cerr << "Scanning end node index" << (*i)->getIndex() << endl; - if ((*i)->getIndex() == endNode) - { - end = (*i)->getAddress(); - return; - } - i++; + FGTaxiNodeVectorIterator i = nodes->begin(); + while (i != nodes->end()) { + //cerr << "Scanning end node index" << (*i)->getIndex() << endl; + if ((*i)->getIndex() == endNode) { + end = (*i)->getAddress(); + return; + } + i++; } - SG_LOG(SG_GENERAL, SG_ALERT, "Could not find end node " << endNode << endl); + SG_LOG(SG_GENERAL, SG_ALERT, + "Could not find end node " << endNode << endl); } @@ -79,90 +78,91 @@ void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes) // doing this. void FGTaxiSegment::setTrackDistance() { - length = SGGeodesy::distanceM(start->getGeod(), end->getGeod()); + length = SGGeodesy::distanceM(start->getGeod(), end->getGeod()); } void FGTaxiSegment::setCourseDiff(double crse) { - headingDiff = fabs(course-crse); - - if (headingDiff > 180) - headingDiff = fabs(headingDiff - 360); + headingDiff = fabs(course - crse); + + if (headingDiff > 180) + headingDiff = fabs(headingDiff - 360); } /*************************************************************************** * FGTaxiRoute **************************************************************************/ -bool FGTaxiRoute::next(int *nde) -{ - //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++) - // cerr << "FGTaxiRoute contains : " << *(i) << endl; - //cerr << "Offset from end: " << nodes.end() - currNode << endl; - //if (currNode != nodes.end()) - // cerr << "true" << endl; - //else - // cerr << "false" << endl; - //if (nodes.size() != (routes.size()) +1) - // cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl; - - if (currNode == nodes.end()) - return false; - *nde = *(currNode); - if (currNode != nodes.begin()) // make sure route corresponds to the end node - currRoute++; - currNode++; - return true; +bool FGTaxiRoute::next(int *nde) +{ + //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++) + // cerr << "FGTaxiRoute contains : " << *(i) << endl; + //cerr << "Offset from end: " << nodes.end() - currNode << endl; + //if (currNode != nodes.end()) + // cerr << "true" << endl; + //else + // cerr << "false" << endl; + //if (nodes.size() != (routes.size()) +1) + // cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl; + + if (currNode == nodes.end()) + return false; + *nde = *(currNode); + if (currNode != nodes.begin()) // make sure route corresponds to the end node + currRoute++; + currNode++; + return true; }; -bool FGTaxiRoute::next(int *nde, int *rte) -{ - //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++) - // cerr << "FGTaxiRoute contains : " << *(i) << endl; - //cerr << "Offset from end: " << nodes.end() - currNode << endl; - //if (currNode != nodes.end()) - // cerr << "true" << endl; - //else - // cerr << "false" << endl; - if (nodes.size() != (routes.size()) +1) { - SG_LOG(SG_GENERAL, SG_ALERT, "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size()); - exit(1); - } - if (currNode == nodes.end()) - return false; - *nde = *(currNode); - //*rte = *(currRoute); - if (currNode != nodes.begin()) // Make sure route corresponds to the end node - { - *rte = *(currRoute); - currRoute++; +bool FGTaxiRoute::next(int *nde, int *rte) +{ + //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++) + // cerr << "FGTaxiRoute contains : " << *(i) << endl; + //cerr << "Offset from end: " << nodes.end() - currNode << endl; + //if (currNode != nodes.end()) + // cerr << "true" << endl; + //else + // cerr << "false" << endl; + if (nodes.size() != (routes.size()) + 1) { + SG_LOG(SG_GENERAL, SG_ALERT, + "ALERT: Misconfigured TaxiRoute : " << nodes. + size() << " " << routes.size()); + exit(1); } - else + if (currNode == nodes.end()) + return false; + *nde = *(currNode); + //*rte = *(currRoute); + if (currNode != nodes.begin()) // Make sure route corresponds to the end node { - // If currNode points to the first node, this means the aircraft is not on the taxi node - // yet. Make sure to return a unique identifyer in this situation though, because otherwise - // the speed adjust AI code may be unable to resolve whether two aircraft are on the same - // taxi route or not. the negative of the preceding route seems a logical choice, as it is - // unique for any starting location. - // Note that this is probably just a temporary fix until I get Parking / tower control working. - *rte = -1 * *(currRoute); + *rte = *(currRoute); + currRoute++; + } else { + // If currNode points to the first node, this means the aircraft is not on the taxi node + // yet. Make sure to return a unique identifyer in this situation though, because otherwise + // the speed adjust AI code may be unable to resolve whether two aircraft are on the same + // taxi route or not. the negative of the preceding route seems a logical choice, as it is + // unique for any starting location. + // Note that this is probably just a temporary fix until I get Parking / tower control working. + *rte = -1 * *(currRoute); } - currNode++; - return true; + currNode++; + return true; }; void FGTaxiRoute::rewind(int route) { - int currPoint; - int currRoute; - first(); - do { - if (!(next(&currPoint, &currRoute))) { - SG_LOG(SG_GENERAL,SG_ALERT, "Error in rewinding TaxiRoute: current" << currRoute - << " goal " << route); - } - } while (currRoute != route); + int currPoint; + int currRoute; + first(); + do { + if (!(next(&currPoint, &currRoute))) { + SG_LOG(SG_GENERAL, SG_ALERT, + "Error in rewinding TaxiRoute: current" << currRoute << + " goal " << route); + } + } while (currRoute != route); } @@ -171,67 +171,64 @@ void FGTaxiRoute::rewind(int route) /*************************************************************************** * FGGroundNetwork() **************************************************************************/ -bool compare_nodes(FGTaxiNode *a, FGTaxiNode *b) { -return (*a) < (*b); +bool compare_nodes(FGTaxiNode * a, FGTaxiNode * b) +{ + return (*a) < (*b); } -bool compare_segments(FGTaxiSegment *a, FGTaxiSegment *b) { -return (*a) < (*b); +bool compare_segments(FGTaxiSegment * a, FGTaxiSegment * b) +{ + return (*a) < (*b); } FGGroundNetwork::FGGroundNetwork() { - hasNetwork = false; - foundRoute = false; - totalDistance = 0; - maxDistance = 0; - //maxDepth = 1000; - count = 0; - currTraffic = activeTraffic.begin(); + hasNetwork = false; + foundRoute = false; + totalDistance = 0; + maxDistance = 0; + //maxDepth = 1000; + count = 0; + currTraffic = activeTraffic.begin(); } FGGroundNetwork::~FGGroundNetwork() { - for (FGTaxiNodeVectorIterator node = nodes.begin(); - node != nodes.end(); - node++) - { - delete (*node); + for (FGTaxiNodeVectorIterator node = nodes.begin(); + node != nodes.end(); node++) { + delete(*node); } - nodes.clear(); - pushBackNodes.clear(); - for (FGTaxiSegmentVectorIterator seg = segments.begin(); - seg != segments.end(); - seg++) - { - delete (*seg); + nodes.clear(); + pushBackNodes.clear(); + for (FGTaxiSegmentVectorIterator seg = segments.begin(); + seg != segments.end(); seg++) { + delete(*seg); } - segments.clear(); + segments.clear(); } -void FGGroundNetwork::addSegment(const FGTaxiSegment &seg) +void FGGroundNetwork::addSegment(const FGTaxiSegment & seg) { - segments.push_back(new FGTaxiSegment(seg)); + segments.push_back(new FGTaxiSegment(seg)); } -void FGGroundNetwork::addNode(const FGTaxiNode &node) +void FGGroundNetwork::addNode(const FGTaxiNode & node) { - nodes.push_back(new FGTaxiNode(node)); + nodes.push_back(new FGTaxiNode(node)); } -void FGGroundNetwork::addNodes(FGParkingVec *parkings) +void FGGroundNetwork::addNodes(FGParkingVec * parkings) { - FGTaxiNode n; - FGParkingVecIterator i = parkings->begin(); - while (i != parkings->end()) - { - n.setIndex(i->getIndex()); - n.setLatitude(i->getLatitude()); - n.setLongitude(i->getLongitude()); - nodes.push_back(new FGTaxiNode(n)); + FGTaxiNode n; + FGParkingVecIterator i = parkings->begin(); + while (i != parkings->end()) { + n.setIndex(i->getIndex()); + n.setLatitude(i->getLatitude()); + n.setLongitude(i->getLongitude()); + nodes.push_back(new FGTaxiNode(n)); - i++; + i++; } } @@ -239,120 +236,117 @@ void FGGroundNetwork::addNodes(FGParkingVec *parkings) void FGGroundNetwork::init() { - hasNetwork = true; - int index = 1; - sort(nodes.begin(), nodes.end(), compare_nodes); - //sort(segments.begin(), segments.end(), compare_segments()); - FGTaxiSegmentVectorIterator i = segments.begin(); - while(i != segments.end()) { - (*i)->setStart(&nodes); - (*i)->setEnd (&nodes); - (*i)->setTrackDistance(); - (*i)->setIndex(index); - if ((*i)->isPushBack()) { - pushBackNodes.push_back((*i)->getEnd()); + hasNetwork = true; + int index = 1; + sort(nodes.begin(), nodes.end(), compare_nodes); + //sort(segments.begin(), segments.end(), compare_segments()); + FGTaxiSegmentVectorIterator i = segments.begin(); + while (i != segments.end()) { + (*i)->setStart(&nodes); + (*i)->setEnd(&nodes); + (*i)->setTrackDistance(); + (*i)->setIndex(index); + if ((*i)->isPushBack()) { + pushBackNodes.push_back((*i)->getEnd()); + } + //SG_LOG(SG_GENERAL, SG_BULK, "initializing segment " << (*i)->getIndex() << endl); + //SG_LOG(SG_GENERAL, SG_BULK, "Track distance = " << (*i)->getLength() << endl); + //SG_LOG(SG_GENERAL, SG_BULK, "Track runs from " << (*i)->getStart()->getIndex() << " to " + // << (*i)->getEnd()->getIndex() << endl); + i++; + index++; } - //SG_LOG(SG_GENERAL, SG_BULK, "initializing segment " << (*i)->getIndex() << endl); - //SG_LOG(SG_GENERAL, SG_BULK, "Track distance = " << (*i)->getLength() << endl); - //SG_LOG(SG_GENERAL, SG_BULK, "Track runs from " << (*i)->getStart()->getIndex() << " to " - // << (*i)->getEnd()->getIndex() << endl); - i++; - index++; - } - - i = segments.begin(); - while(i != segments.end()) { - FGTaxiSegmentVectorIterator j = (*i)->getEnd()->getBeginRoute(); - while (j != (*i)->getEnd()->getEndRoute()) - { - if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex()) - { -// int start1 = (*i)->getStart()->getIndex(); -// int end1 = (*i)->getEnd() ->getIndex(); -// int start2 = (*j)->getStart()->getIndex(); -// int end2 = (*j)->getEnd()->getIndex(); -// int oppIndex = (*j)->getIndex(); - //cerr << "Opposite of " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") " - // << "happens to be " << oppIndex << " (" << start2 << "," << end2 << ") " << endl; - (*i)->setOpposite(*j); - break; - } - j++; - } - i++; - } - //FGTaxiNodeVectorIterator j = nodes.begin(); - //while (j != nodes.end()) { - // if ((*j)->getHoldPointType() == 3) { - // pushBackNodes.push_back((*j)); - // } - // j++; - //} - //cerr << "Done initializing ground network" << endl; - //exit(1); + + i = segments.begin(); + while (i != segments.end()) { + FGTaxiSegmentVectorIterator j = (*i)->getEnd()->getBeginRoute(); + while (j != (*i)->getEnd()->getEndRoute()) { + if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex()) { +// int start1 = (*i)->getStart()->getIndex(); +// int end1 = (*i)->getEnd() ->getIndex(); +// int start2 = (*j)->getStart()->getIndex(); +// int end2 = (*j)->getEnd()->getIndex(); +// int oppIndex = (*j)->getIndex(); + //cerr << "Opposite of " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") " + // << "happens to be " << oppIndex << " (" << start2 << "," << end2 << ") " << endl; + (*i)->setOpposite(*j); + break; + } + j++; + } + i++; + } + //FGTaxiNodeVectorIterator j = nodes.begin(); + //while (j != nodes.end()) { + // if ((*j)->getHoldPointType() == 3) { + // pushBackNodes.push_back((*j)); + // } + // j++; + //} + //cerr << "Done initializing ground network" << endl; + //exit(1); } -int FGGroundNetwork::findNearestNode(const SGGeod& aGeod) +int FGGroundNetwork::findNearestNode(const SGGeod & aGeod) { - double minDist = HUGE_VAL; - int index = -1; - - for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end(); itr++) - { - double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod()); - if (d < minDist) - { - minDist = d; - index = (*itr)->getIndex(); - //cerr << "Minimum distance of " << minDist << " for index " << index << endl; + double minDist = HUGE_VAL; + int index = -1; + + for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end(); + itr++) { + double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod()); + if (d < minDist) { + minDist = d; + index = (*itr)->getIndex(); + //cerr << "Minimum distance of " << minDist << " for index " << index << endl; + } } - } - - return index; + + return index; } int FGGroundNetwork::findNearestNode(double lat, double lon) { - return findNearestNode(SGGeod::fromDeg(lon, lat)); + return findNearestNode(SGGeod::fromDeg(lon, lat)); } FGTaxiNode *FGGroundNetwork::findNode(unsigned idx) -{ /* - for (FGTaxiNodeVectorIterator - itr = nodes.begin(); - itr != nodes.end(); itr++) - { - if (itr->getIndex() == idx) - return itr->getAddress(); - }*/ - - if ((idx >= 0) && (idx < nodes.size())) - return nodes[idx]->getAddress(); - else - return 0; +{ /* + for (FGTaxiNodeVectorIterator + itr = nodes.begin(); + itr != nodes.end(); itr++) + { + if (itr->getIndex() == idx) + return itr->getAddress(); + } */ + + if ((idx >= 0) && (idx < nodes.size())) + return nodes[idx]->getAddress(); + else + return 0; } FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx) -{/* - for (FGTaxiSegmentVectorIterator - itr = segments.begin(); - itr != segments.end(); itr++) - { - if (itr->getIndex() == idx) - return itr->getAddress(); - } - */ - if ((idx > 0) && (idx <= segments.size())) - return segments[idx-1]->getAddress(); - else - { - //cerr << "Alert: trying to find invalid segment " << idx << endl; - return 0; +{ /* + for (FGTaxiSegmentVectorIterator + itr = segments.begin(); + itr != segments.end(); itr++) + { + if (itr->getIndex() == idx) + return itr->getAddress(); + } + */ + if ((idx > 0) && (idx <= segments.size())) + return segments[idx - 1]->getAddress(); + else { + //cerr << "Alert: trying to find invalid segment " << idx << endl; + return 0; } } -FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end, bool fullSearch) +FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end, + bool fullSearch) { //implements Dijkstra's algorithm to find shortest distance route from start to end //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm @@ -362,38 +356,37 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end, bool fullSear int nParkings = parent->getDynamics()->getNrOfParkings(); FGTaxiNodeVector *currNodesSet; if (fullSearch) { - currNodesSet = &nodes; + currNodesSet = &nodes; } else { - currNodesSet = &pushBackNodes; + currNodesSet = &pushBackNodes; } for (FGTaxiNodeVectorIterator - itr = currNodesSet->begin(); - itr != currNodesSet->end(); itr++) { - (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means - (*itr)->setPreviousNode(0); // - (*itr)->setPreviousSeg (0); // - } + itr = currNodesSet->begin(); itr != currNodesSet->end(); itr++) { + (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means + (*itr)->setPreviousNode(0); // + (*itr)->setPreviousSeg(0); // + } FGTaxiNode *firstNode = findNode(start); firstNode->setPathScore(0); - FGTaxiNode *lastNode = findNode(end); + FGTaxiNode *lastNode = findNode(end); - FGTaxiNodeVector unvisited(*currNodesSet); // working copy + FGTaxiNodeVector unvisited(*currNodesSet); // working copy while (!unvisited.empty()) { - FGTaxiNode* best = *(unvisited.begin()); + FGTaxiNode *best = *(unvisited.begin()); for (FGTaxiNodeVectorIterator - itr = unvisited.begin(); - itr != unvisited.end(); itr++) { - if ((*itr)->getPathScore() < best->getPathScore()) - best = (*itr); + itr = unvisited.begin(); itr != unvisited.end(); itr++) { + if ((*itr)->getPathScore() < best->getPathScore()) + best = (*itr); } - FGTaxiNodeVectorIterator newend = remove(unvisited.begin(), unvisited.end(), best); + FGTaxiNodeVectorIterator newend = + remove(unvisited.begin(), unvisited.end(), best); unvisited.erase(newend, unvisited.end()); - + if (best == lastNode) { // found route or best not connected break; } else { @@ -401,15 +394,17 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end, bool fullSear seg = best->getBeginRoute(); seg != best->getEndRoute(); seg++) { if (fullSearch || (*seg)->isPushBack()) { - FGTaxiNode* tgt = (*seg)->getEnd(); - double alt = best->getPathScore() + (*seg)->getLength() + (*seg)->getPenalty(nParkings); - if (alt < tgt->getPathScore()) { // Relax (u,v) + FGTaxiNode *tgt = (*seg)->getEnd(); + double alt = + best->getPathScore() + (*seg)->getLength() + + (*seg)->getPenalty(nParkings); + if (alt < tgt->getPathScore()) { // Relax (u,v) tgt->setPathScore(alt); tgt->setPreviousNode(best); - tgt->setPreviousSeg(*seg); // - } + tgt->setPreviousSeg(*seg); // + } } else { - // // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl; + // // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl; } } } @@ -417,17 +412,18 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end, bool fullSear if (lastNode->getPathScore() == HUGE_VAL) { // no valid route found - if (fullSearch) { - SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find route from waypoint " << start << " to " << end << " at " << - parent->getId()); + if (fullSearch) { + SG_LOG(SG_GENERAL, SG_ALERT, + "Failed to find route from waypoint " << start << " to " + << end << " at " << parent->getId()); } - FGTaxiRoute empty; - return empty; + FGTaxiRoute empty; + return empty; //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's } else { // assemble route from backtrace information intVec nodes, routes; - FGTaxiNode* bt = lastNode; + FGTaxiNode *bt = lastNode; while (bt->getPreviousNode() != 0) { nodes.push_back(bt->getIndex()); routes.push_back(bt->getPreviousSegment()->getIndex()); @@ -441,116 +437,151 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end, bool fullSear } } -int FGTaxiSegment::getPenalty(int nGates) { - int penalty = 0; - if (end->getIndex() < nGates) { - penalty += 10000; - } - if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active. - penalty += 1000; - } - return penalty; +int FGTaxiSegment::getPenalty(int nGates) +{ + int penalty = 0; + if (end->getIndex() < nGates) { + penalty += 10000; + } + if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active. + penalty += 1000; + } + return penalty; } /* 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::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.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); - activeTraffic.push_back(rec); - } else { - i->setPositionAndIntentions(currentPosition, intendedRoute); - i->setPositionAndHeading(lat, lon, heading, speed, alt); - } + 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); + 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"); - } else { - i = activeTraffic.erase(i); - } +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"); + } else { + i = activeTraffic.erase(i); + } } -void FGGroundNetwork::update(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. +void FGGroundNetwork::update(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"); - } 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); - checkHoldPosition (id, lat, lon, heading, speed, alt); - if (checkForCircularWaits(id)) { - i->setResolveCircularWait(); - } + 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"); + } 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); + checkHoldPosition(id, lat, lon, heading, speed, alt); + if (checkForCircularWaits(id)) { + i->setResolveCircularWait(); + } + bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest(); + int state = current->getState(); + time_t now = time(NULL) + fgGetLong("/sim/time/warp"); + if ((now - lastTransmission) > 15) { + available = true; + } + if (needsTaxiClearance && available) { + transmit(&(*current), MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND); + current->getAircraft()->setTaxiClearanceRequest(false); + current->setState(3); + lastTransmission = now; + available = false; + } + if ((state == 3) && available) { + transmit(&(*current), MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR); + current->setState(4); + lastTransmission = now; + available = false; + } + if ((state == 4) && available) { + transmit(&(*current), MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND); + current->setState(0); + lastTransmission = now; + available = false; + } } /** @@ -567,129 +598,123 @@ void FGGroundNetwork::update(int id, double lat, double lon, double heading, dou not addressed yet, but should be. */ -void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, - double lon, double heading, - double speed, double alt) +void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, + double lon, double heading, + double speed, double alt) { - - TrafficVectorIterator current, closest; - 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"); - } - current = i; - //closest = current; - - previousInstruction = current->getSpeedAdjustment(); - double mindist = HUGE_VAL; - if (activeTraffic.size()) - { - double course, dist, bearing, minbearing, az2; - SGGeod curr(SGGeod::fromDegM(lon, lat, alt)); - //TrafficVector iterator closest; - closest = 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; - 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)) - { - mindist = dist; - closest = i; - minbearing = bearing; - otherReasonToSlowDown = true; - } - } - } - // Finally, check UserPosition - 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; - } + TrafficVectorIterator current, closest; + 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"); + } + current = i; + //closest = current; - current->clearSpeedAdjustment(); - - 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)); - 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); - // } - } - } - } + previousInstruction = current->getSpeedAdjustment(); + double mindist = HUGE_VAL; + if (activeTraffic.size()) { + double course, dist, bearing, minbearing, az2; + SGGeod curr(SGGeod::fromDegM(lon, lat, alt)); + //TrafficVector iterator closest; + closest = 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; + 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)) { + mindist = dist; + closest = i; + minbearing = bearing; + otherReasonToSlowDown = true; + } + } + } + // Finally, check UserPosition + 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(); + + 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)); + 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); + // } + } + } + } } } @@ -701,114 +726,146 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, 3) For crossing or merging taxiroutes. */ -void FGGroundNetwork::checkHoldPosition(int id, double lat, - double lon, double heading, - double speed, double alt) +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++; - } + 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; } - 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::checkHoldPosition"); - } - current = i; - current->setHoldPosition(false); - SGGeod curr(SGGeod::fromDegM(lon, lat, alt)); - - for (i = activeTraffic.begin(); - i != activeTraffic.end(); i++) - { - if (i->getId() != current->getId()) - { - int node = current->crosses(this, *i); - if (node != -1) - { - FGTaxiNode* taxiNode = findNode(node); - - // Determine whether it's save to continue or not. - // If we have a crossing route, there are two possibilities: - // 1) This is an interestion - // 2) This is oncoming two-way traffic, using the same taxiway. - //cerr << "Hold check 1 : " << id << " has common node " << node << endl; - - SGGeod other(SGGeod::fromDegM(i->getLongitude(), i->getLatitude(), i->getAltitude())); - bool needsToWait; - bool opposing; - if (current->isOpposing(this, *i, node)) - { - needsToWait = true; - opposing = true; - //cerr << "Hold check 2 : " << node << " has opposing segment " << endl; - // issue a "Hold Position" as soon as we're close to the offending node - // For now, I'm doing this as long as the other aircraft doesn't - // have a hold instruction as soon as we're within a reasonable - // distance from the offending node. - // This may be a bit of a conservative estimate though, as it may - // be well possible that both aircraft can both continue to taxi - // without crashing into each other. - } - else - { - opposing = false; - if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 2.0*i->getRadius()) - { - needsToWait = false; - //cerr << "Hold check 3 : " << id <<" Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue " - // << endl; - } - else - { - needsToWait = true; - //cerr << "Hold check 4: " << id << " Would need to wait for other aircraft : distance = " << dist << " meters" << endl; - } - } - - double dist = SGGeodesy::distanceM(curr, taxiNode->getGeod()); - if (!(i->hasHoldPosition())) - { - - if ((dist < 200) && //2.5*current->getRadius()) && - (needsToWait) && - (i->onRoute(this, *current)) && - //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) && - (!(current->getId() == i->getWaitsForId()))) - //(!(i->getSpeedAdjustment()))) // && - //(!(current->getSpeedAdjustment()))) - - { - current->setHoldPosition(true); - current->setWaitsForId(i->getId()); - //cerr << "Hold check 5: " << current->getCallSign() <<" Setting Hold Position: distance to node (" << node << ") " - // << dist << " meters. Waiting for " << i->getCallSign(); - //if (opposing) - //cerr <<" [opposing] " << endl; - //else - // cerr << "[non-opposing] " << endl; - //if (i->hasSpeefAdjustment()) - // { - // cerr << " (which in turn waits for ) " << i-> - } - else - { - //cerr << "Hold check 6: " << id << " No need to hold yet: Distance to node : " << dist << " nm"<< endl; - } - } - } - } + if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { + SG_LOG(SG_GENERAL, SG_ALERT, + "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition"); + } + current = i; + bool origStatus = current->hasHoldPosition(); + current->setHoldPosition(false); + SGGeod curr(SGGeod::fromDegM(lon, lat, alt)); + + for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) { + if (i->getId() != current->getId()) { + int node = current->crosses(this, *i); + if (node != -1) { + FGTaxiNode *taxiNode = findNode(node); + + // Determine whether it's save to continue or not. + // If we have a crossing route, there are two possibilities: + // 1) This is an interestion + // 2) This is oncoming two-way traffic, using the same taxiway. + //cerr << "Hold check 1 : " << id << " has common node " << node << endl; + + SGGeod other(SGGeod:: + fromDegM(i->getLongitude(), i->getLatitude(), + i->getAltitude())); + bool needsToWait; + bool opposing; + if (current->isOpposing(this, *i, node)) { + needsToWait = true; + opposing = true; + //cerr << "Hold check 2 : " << node << " has opposing segment " << endl; + // issue a "Hold Position" as soon as we're close to the offending node + // For now, I'm doing this as long as the other aircraft doesn't + // have a hold instruction as soon as we're within a reasonable + // distance from the offending node. + // This may be a bit of a conservative estimate though, as it may + // be well possible that both aircraft can both continue to taxi + // without crashing into each other. + } else { + opposing = false; + if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 2.0*i->getRadius()) + { + needsToWait = false; + //cerr << "Hold check 3 : " << id <<" Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue " + // << endl; + } else { + needsToWait = true; + //cerr << "Hold check 4: " << id << " Would need to wait for other aircraft : distance = " << dist << " meters" << endl; + } + } + + double dist = + SGGeodesy::distanceM(curr, taxiNode->getGeod()); + if (!(i->hasHoldPosition())) { + + if ((dist < 200) && //2.5*current->getRadius()) && + (needsToWait) && (i->onRoute(this, *current)) && + //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) && + (!(current->getId() == i->getWaitsForId()))) + //(!(i->getSpeedAdjustment()))) // && + //(!(current->getSpeedAdjustment()))) + + { + current->setHoldPosition(true); + current->setWaitsForId(i->getId()); + //cerr << "Hold check 5: " << current->getCallSign() <<" Setting Hold Position: distance to node (" << node << ") " + // << dist << " meters. Waiting for " << i->getCallSign(); + //if (opposing) + //cerr <<" [opposing] " << endl; + //else + // cerr << "[non-opposing] " << endl; + //if (i->hasSpeefAdjustment()) + // { + // cerr << " (which in turn waits for ) " << i-> + } else { + //cerr << "Hold check 6: " << id << " No need to hold yet: Distance to node : " << dist << " nm"<< endl; + } + } + } + } + } + bool currStatus = current->hasHoldPosition(); + + // Either a Hold Position or a resume taxi transmission has been issued + time_t now = time(NULL) + fgGetLong("/sim/time/warp"); + if ((now - lastTransmission) > 2) { + available = true; + } + if ((origStatus != currStatus) && available) { + //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl; + if (currStatus == true) { // No has a hold short instruction + transmit(&(*current), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR); + //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl; + current->setState(1); + } else { + transmit(&(*current), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR); + //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 + current->setHoldPosition(origStatus); + //cerr << "Current state " << current->getState() << endl; + } else { + } + int state = current->getState(); + if ((state == 1) && (available)) { + //cerr << "ACKNOWLEDGE HOLD" << endl; + transmit(&(*current), MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND); + current->setState(0); + current->setHoldPosition(true); + lastTransmission = now; + available = false; + + } + if ((state == 2) && (available)) { + //cerr << "ACKNOWLEDGE RESUME" << endl; + transmit(&(*current), MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND); + current->setState(0); + current->setHoldPosition(false); + lastTransmission = now; + available = false; } } @@ -827,142 +884,142 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat, */ 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) { +{ + //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"); - } - - 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++; + if (i->getId() == id) { + break; + } + i++; } - } - else { + } 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(); + if (i == activeTraffic.end() || (trafficSize == 0)) { + SG_LOG(SG_GENERAL, SG_ALERT, + "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits"); + } - // 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; - } + 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; - } + //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"); - } else { - return i->hasInstruction(); - } - return false; + // 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"); + } 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"); - } else { - return i->getInstruction(); - } - return FGATCInstruction(); + 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"); + } else { + return i->getInstruction(); + } + return FGATCInstruction(); } - - diff --git a/src/Airports/groundnetwork.hxx b/src/Airports/groundnetwork.hxx index 733cede9e..fe83d4df2 100644 --- a/src/Airports/groundnetwork.hxx +++ b/src/Airports/groundnetwork.hxx @@ -252,10 +252,10 @@ public: void init(); bool exists() { return hasNetwork; }; void setTowerController(FGTowerController *twrCtrlr) { towerController = twrCtrlr; }; - + int findNearestNode(double lat, double lon); int findNearestNode(const SGGeod& aGeod); - + FGTaxiNode *findNode(unsigned idx); FGTaxiSegment *findSegment(unsigned idx); FGTaxiRoute findShortestRoute(int start, int end, bool fullSearch=true); diff --git a/src/Traffic/Schedule.cxx b/src/Traffic/Schedule.cxx index 517bb3c21..3b990e377 100644 --- a/src/Traffic/Schedule.cxx +++ b/src/Traffic/Schedule.cxx @@ -201,15 +201,17 @@ bool FGAISchedule::update(time_t now, const SGVec3d& userCart) if (firstRun) { if (fgGetBool("/sim/traffic-manager/instantaneous-action") == true) { - deptime = now + rand() % 300; // Wait up to 5 minutes until traffic starts moving to prevent too many aircraft + deptime = now; // + rand() % 300; // Wait up to 5 minutes until traffic starts moving to prevent too many aircraft // from cluttering the gate areas. } firstRun = false; } FGScheduledFlight* flight = flights.front(); - if (!deptime) + if (!deptime) { deptime = flight->getDepartureTime(); + //cerr << "Settiing departure time " << deptime << endl; + } if (AIManagerRef) { // Check if this aircraft has been released.