diff --git a/src/Airports/trafficcontrol.cxx b/src/Airports/trafficcontrol.cxx new file mode 100644 index 000000000..fe64bb624 --- /dev/null +++ b/src/Airports/trafficcontrol.cxx @@ -0,0 +1,447 @@ +// trafficrecord.cxx - Implementation of AIModels ATC code. +// +// Written by Durk Talsma, started September 2006. +// +// Copyright (C) 2006 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 "trafficcontrol.hxx" +#include + + +/*************************************************************************** + * FGTrafficRecord + **************************************************************************/ +void FGTrafficRecord::setPositionAndIntentions(int pos, FGAIFlightPlan *route) +{ + + currentPos = pos; + if (intentions.size()) { + intVecIterator i = intentions.begin(); + if ((*i) != pos) { + SG_LOG(SG_GENERAL, SG_ALERT, "Error in FGTrafficRecord::setPositionAndIntentions"); + cerr << "Pos : " << pos << " Curr " << *(intentions.begin()) << endl; + for (intVecIterator i = intentions.begin(); i != intentions.end() ; i++) { + cerr << (*i) << " "; + } + cerr << endl; + } + intentions.erase(i); + } else { + //int legNr, routeNr; + //FGAIFlightPlan::waypoint* const wpt= route->getCurrentWaypoint(); + int size = route->getNrOfWayPoints(); + //cerr << "Setting pos" << pos << " "; + //cerr << "setting intentions "; + for (int i = 0; i < size; i++) { + int val = route->getRouteIndex(i); + + if ((val) && (val != pos)) + { + intentions.push_back(val); + //cerr << val<< " "; + } + } + //cerr << endl; + //while (route->next(&legNr, &routeNr)) { + //intentions.push_back(routeNr); + //} + //route->rewind(currentPos); + } + //exit(1); +} + +bool FGTrafficRecord::checkPositionAndIntentions(FGTrafficRecord &other) +{ + bool result = false; + //cerr << "Start check 1" << endl; + if (currentPos == other.currentPos) + { + //cerr << "Check Position and intentions: current matches" << endl; + result = true; + } + // else if (other.intentions.size()) + // { + // cerr << "Start check 2" << endl; + // intVecIterator i = other.intentions.begin(); + // while (!((i == other.intentions.end()) || ((*i) == currentPos))) + // i++; + // if (i != other.intentions.end()) { + // cerr << "Check Position and intentions: current matches other.intentions" << endl; + // result = true; + // } + else if (intentions.size()) { + //cerr << "Start check 3" << endl; + intVecIterator i = intentions.begin(); + //while (!((i == intentions.end()) || ((*i) == other.currentPos))) + while (i != intentions.end()) { + if ((*i) == other.currentPos) { + break; + } + i++; + } + if (i != intentions.end()) { + //cerr << "Check Position and intentions: .other.current matches" << endl; + result = true; + } + } + //cerr << "Done !!" << endl; + return result; +} + +void FGTrafficRecord::setPositionAndHeading(double lat, double lon, double hdg, + double spd, double alt) +{ + latitude = lat; + longitude = lon; + heading = hdg; + speed = spd; + altitude = alt; +} + +int FGTrafficRecord::crosses(FGGroundNetwork *net, FGTrafficRecord &other) +{ + if (checkPositionAndIntentions(other) || (other.checkPositionAndIntentions(*this))) + return -1; + intVecIterator i, j; + int currentTargetNode = 0, otherTargetNode = 0; + if (currentPos > 0) + currentTargetNode = net->findSegment(currentPos )->getEnd()->getIndex(); // OKAY,... + if (other.currentPos > 0) + otherTargetNode = net->findSegment(other.currentPos)->getEnd()->getIndex(); // OKAY,... + if ((currentTargetNode == otherTargetNode) && currentTargetNode > 0) + return currentTargetNode; + if (intentions.size()) + { + for (i = intentions.begin(); i != intentions.end(); i++) + { + if ((*i) > 0) { + if ((currentTargetNode == net->findSegment(*i)->getEnd()->getIndex())) + { + cerr << "Current crosses at " << currentTargetNode < 0) { + if (otherTargetNode == net->findSegment(*i)->getEnd()->getIndex()) + { + cerr << "Other crosses at " << currentTargetNode < 0) && ((*j) > 0)) { + currentTargetNode = net->findSegment(*i)->getEnd()->getIndex(); + otherTargetNode = net->findSegment(*j)->getEnd()->getIndex(); + if (currentTargetNode == otherTargetNode) + { + //cerr << "Routes will cross at " << currentTargetNode << endl; + return currentTargetNode; + } + } + } + } + } + return -1; +} + +bool FGTrafficRecord::isOpposing (FGGroundNetwork *net, FGTrafficRecord &other, int node) +{ + // Check if current segment is the reverse segment for the other aircraft + FGTaxiSegment *opp; + //cerr << "Current segment " << currentPos << endl; + if ((currentPos > 0) && (other.currentPos > 0)) + { + opp = net->findSegment(currentPos)->opposite(); + if (opp) { + if (opp->getIndex() == other.currentPos) + return true; + } + + for (intVecIterator i = intentions.begin(); i != intentions.end(); i++) + { + if (other.intentions.size()) + { + for (intVecIterator j = other.intentions.begin(); j != other.intentions.end(); j++) + { + // cerr << "Current segment 1 " << (*i) << endl; + if ((*i) > 0) { + if (opp = net->findSegment(*i)->opposite()) + { + if (opp->getIndex() == + net->findSegment(*j)->getIndex()) + { + cerr << "Nodes " << net->findSegment(*i)->getIndex() + << " and " << net->findSegment(*j)->getIndex() + << " are opposites " << endl; + if (net->findSegment(*i)->getStart()->getIndex() == node) { + { + cerr << "Found the node" << endl; + return true; + } + } + } + } + } + } + } + } + } + return false; +} + +void FGTrafficRecord::setSpeedAdjustment(double spd) +{ + instruction.setChangeSpeed(true); + instruction.setSpeed(spd); +} + +void FGTrafficRecord::setHeadingAdjustment(double heading) +{ + instruction.setChangeHeading(true); + instruction.setHeading(heading); +} + + + +/*************************************************************************** + * FGATCInstruction + * + **************************************************************************/ +FGATCInstruction::FGATCInstruction() +{ + holdPattern = false; + holdPosition = false; + changeSpeed = false; + changeHeading = false; + changeAltitude = false; + + double speed = 0; + double heading = 0; + double alt = 0; +} + +bool FGATCInstruction::hasInstruction() +{ + return (holdPattern || holdPosition || changeSpeed || changeHeading || changeAltitude); +} + + + +/*************************************************************************** + * class FGTowerController + * + **************************************************************************/ +FGTowerController::FGTowerController() : + FGATCController() +{ +} + +// +void FGTowerController::announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentPosition, + double lat, double lon, double heading, + double speed, double alt, double radius, int leg) +{ + TrafficVectorIterator i = activeTraffic.begin(); + // Search whether 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.setPositionAndHeading(lat, lon, heading, speed, alt); + rec.setRunway(intendedRoute->getRunway()); + rec.setLeg(leg); + activeTraffic.push_back(rec); + } else { + i->setPositionAndHeading(lat, lon, heading, speed, alt); + } +} + +void FGTowerController::update(int id, double lat, double lon, double heading, double speed, double alt, + double dt) +{ + 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); + +// // see if we already have a clearance record for the currently active runway + ActiveRunwayVecIterator rwy = activeRunways.begin(); + // again, a map might be more efficient here + if (activeRunways.size()) { + //while ((rwy->getRunwayName() != current->getRunway()) && (rwy != activeRunways.end())) { + while (rwy != activeRunways.end()) { + if (rwy->getRunwayName() == current->getRunway()) { + break; + } + rwy++; + } + } + if (rwy == activeRunways.end()) { + ActiveRunway aRwy(current->getRunway(), id); + activeRunways.push_back(aRwy); // Since there are no clearance records for this runway yet + current->setHoldPosition(false); // Clear the current aircraft to continue + } + else { + // Okay, we have a clearance record for this runway, so check + // whether the clearence ID matches that of the current aircraft + if (id == rwy->getCleared()) { + current->setHoldPosition(false); + } else { + current->setHoldPosition(true); + } + } +} + + +void FGTowerController::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 this aircraft has left the runway, we can clear the departure record for this runway + ActiveRunwayVecIterator rwy = activeRunways.begin(); + if (activeRunways.size()) { + //while ((rwy->getRunwayName() != i->getRunway()) && (rwy != activeRunways.end())) { + while (rwy != activeRunways.end()) { + if (rwy->getRunwayName() == i->getRunway()) { + break; + } + rwy++; + } + if (rwy != activeRunways.end()) { + rwy = activeRunways.erase(rwy); + } else { + SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Attempting to erase non-existing runway clearance record in FGTowerController::signoff"); + } + } + if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { + SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Aircraft without traffic record is signing off from tower"); + } else { + i = activeTraffic.erase(i); + } +} + +// NOTE: +// IF WE MAKE TRAFFICRECORD A MEMBER OF THE BASE CLASS +// THE FOLLOWING THREE FUNCTIONS: SIGNOFF, HAS INSTRUCTION AND GETINSTRUCTION CAN +// BECOME DEVIRTUALIZED AND BE A MEMBER OF THE BASE ATCCONTROLLER CLASS +// WHICH WOULD SIMPLIFY CODE MAINTENANCE. +// Note that this function is probably obsolete +bool FGTowerController::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; +} + + +FGATCInstruction FGTowerController::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(); +} + diff --git a/src/Airports/trafficcontrol.hxx b/src/Airports/trafficcontrol.hxx new file mode 100644 index 000000000..e0329a569 --- /dev/null +++ b/src/Airports/trafficcontrol.hxx @@ -0,0 +1,222 @@ +// trafficcontrol.hxx - classes to manage AIModels based air traffic control +// Written by Durk Talsma, started September 2006. +// +// 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 _TRAFFIC_CONTROL_HXX_ +#define _TRAFFIC_CONTROL_HXX_ + + +#ifndef __cplusplus +# error This library requires C++ +#endif + + +#include +#include + + +#include STL_STRING +#include + +SG_USING_STD(string); +SG_USING_STD(vector); + + +typedef vector intVec; +typedef vector::iterator intVecIterator; + + +class FGAIFlightPlan; // forward reference +class FGGroundNetwork; // forward reference + +/************************************************************************************** + * class FGATCInstruction + * like class FGATC Controller, this class definition should go into its own file + * and or directory... For now, just testing this stuff out though... + *************************************************************************************/ +class FGATCInstruction +{ +private: + bool holdPattern; + bool holdPosition; + bool changeSpeed; + bool changeHeading; + bool changeAltitude; + + double speed; + double heading; + double alt; +public: + + FGATCInstruction(); + bool hasInstruction (); + bool getHoldPattern () { return holdPattern; }; + bool getHoldPosition () { return holdPosition; }; + bool getChangeSpeed () { return changeSpeed; }; + bool getChangeHeading () { return changeHeading; }; + bool getChangeAltitude() { return changeAltitude; }; + + double getSpeed () { return speed; }; + double getHeading () { return heading; }; + double getAlt () { return alt; }; + + void setHoldPattern (bool val) { holdPattern = val; }; + void setHoldPosition (bool val) { holdPosition = val; }; + void setChangeSpeed (bool val) { changeSpeed = val; }; + void setChangeHeading (bool val) { changeHeading = val; }; + void setChangeAltitude(bool val) { changeAltitude = val; }; + + void setSpeed (double val) { speed = val; }; + void setHeading (double val) { heading = val; }; + void setAlt (double val) { alt = val; }; +}; + + +/************************************************************************************** + * class FGATCController + * NOTE: this class serves as an abstraction layer for all sorts of ATC controller. + *************************************************************************************/ +class FGATCController +{ +private: + double dt_count; +public: + FGATCController() { dt_count = 0;}; + virtual ~FGATCController() {}; + virtual void announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentRoute, + double lat, double lon, + double hdg, double spd, double alt, double radius, int leg) = 0; + virtual void signOff(int id) = 0; + virtual void update(int id, double lat, double lon, + double heading, double speed, double alt, double dt) = 0; + virtual bool hasInstruction(int id) = 0; + virtual FGATCInstruction getInstruction(int id) = 0; + + double getDt() { return dt_count; }; + void setDt(double dt) { dt_count = dt;}; +}; + + +/************************************************************************************** + * class FGTrafficRecord + *************************************************************************************/ +class FGTrafficRecord +{ +private: + int id, waitsForId; + int currentPos; + int leg; + intVec intentions; + FGATCInstruction instruction; + double latitude, longitude, heading, speed, altitude, radius; + string runway; + + +public: + FGTrafficRecord() {}; + + void setId(int val) { id = val; }; + void setRadius(double rad) { radius = rad;}; + void setPositionAndIntentions(int pos, FGAIFlightPlan *route); + void setRunway(string rwy) { runway = rwy;}; + void setLeg(int lg) { leg = lg;}; + int getId() { return id;}; + FGATCInstruction getInstruction() { return instruction;}; + bool hasInstruction() { return instruction.hasInstruction(); }; + void setPositionAndHeading(double lat, double lon, double hdg, double spd, double alt); + bool checkPositionAndIntentions(FGTrafficRecord &other); + int crosses (FGGroundNetwork *, FGTrafficRecord &other); + bool isOpposing (FGGroundNetwork *, FGTrafficRecord &other, int node); + + bool getSpeedAdjustment() { return instruction.getChangeSpeed(); }; + + double getLatitude () { return latitude ; }; + double getLongitude() { return longitude; }; + double getHeading () { return heading ; }; + double getSpeed () { return speed ; }; + double getAltitude () { return altitude ; }; + double getRadius () { return radius ; }; + + int getWaitsForId () { return waitsForId; }; + + void setSpeedAdjustment(double spd); + void setHeadingAdjustment(double heading); + void clearSpeedAdjustment () { instruction.setChangeSpeed (false); }; + void clearHeadingAdjustment() { instruction.setChangeHeading(false); }; + + bool hasHeadingAdjustment() { return instruction.getChangeHeading(); }; + bool hasHoldPosition() { return instruction.getHoldPosition(); }; + void setHoldPosition (bool inst) { instruction.setHoldPosition(inst); }; + + void setWaitsForId(int id) { waitsForId = id; }; + + string getRunway() { return runway; }; + +}; + +typedef vector TrafficVector; +typedef vector::iterator TrafficVectorIterator; + +/*********************************************************************** + * Active runway, a utility class to keep track of which aircraft has + * clearance for a given runway. + **********************************************************************/ +class ActiveRunway +{ +private: + string rwy; + int currentlyCleared; +public: + ActiveRunway(string r, int cc) { rwy = r; currentlyCleared = cc; }; + + string getRunwayName() { return rwy; }; + int getCleared () { return currentlyCleared; }; +}; + +typedef vector ActiveRunwayVec; +typedef vector::iterator ActiveRunwayVecIterator; + +/****************************************************************************** + * class FGTowerControl + *****************************************************************************/ +class FGTowerController : public FGATCController +{ +private: + TrafficVector activeTraffic; + ActiveRunwayVec activeRunways; + +public: + FGTowerController(); + virtual ~FGTowerController() {}; + virtual void announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentRoute, + double lat, double lon, + double hdg, double spd, double alt, double radius, int leg); + virtual void signOff(int id); + virtual void update(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 hasActiveTraffic() { return activeTraffic.size() != 0; }; + TrafficVector &getActiveTraffic() { return activeTraffic; }; +}; + + + +#endif // _TRAFFIC_CONTROL_HXX