From 666910a7937712d02e62a38a83ced564f6227f52 Mon Sep 17 00:00:00 2001 From: durk Date: Fri, 6 Oct 2006 17:36:31 +0000 Subject: [PATCH] Major update of traffic manager and AI related airport facilities. - Moved AIModels/Traffic Manager related AI functions to a new file - Rewrote the traffic manager so that the containers use pointers to objects instead of the objects themselves, which will allow for a more flexible memory management. - Rewrote parts of the airport groundnetwork code, also because the stl containers now contain object pointers instead of the objects themselves. - Fixed an uninitialized iterator in the AI distance tracking code - Fixed flawed logic in some of the traffic controller's while loops - Added a tower controller, which paces take-off behavior of AITraffic in a more realistic way. - Various other minor fixes and fine tuning. --- src/AIModel/AIAircraft.cxx | 98 ++-- src/AIModel/AIBase.cxx | 1 + src/AIModel/AIFlightPlan.hxx | 1 + src/AIModel/AIFlightPlanCreate.cxx | 64 ++- src/AIModel/AIManager.cxx | 24 +- src/AIModel/AIManager.hxx | 5 +- src/Airports/Makefile.am | 3 +- src/Airports/dynamics.cxx | 2 +- src/Airports/dynamics.hxx | 9 +- src/Airports/groundnetwork.cxx | 809 +++++++++++++---------------- src/Airports/groundnetwork.hxx | 153 +----- src/Airports/simple.cxx | 2 +- src/Traffic/SchedFlight.cxx | 6 + src/Traffic/SchedFlight.hxx | 7 +- src/Traffic/Schedule.cxx | 73 +-- src/Traffic/Schedule.hxx | 18 +- src/Traffic/TrafficMgr.cxx | 35 +- src/Traffic/TrafficMgr.hxx | 2 +- 18 files changed, 631 insertions(+), 681 deletions(-) diff --git a/src/AIModel/AIAircraft.cxx b/src/AIModel/AIAircraft.cxx index c46cd0320..c6450677a 100644 --- a/src/AIModel/AIAircraft.cxx +++ b/src/AIModel/AIAircraft.cxx @@ -43,6 +43,7 @@ SG_USING_STD(string); #include "AIAircraft.hxx" +//#include static string tempReg; // // accel, decel, climb_rate, descent_rate, takeoff_speed, climb_speed, @@ -93,6 +94,8 @@ FGAIAircraft::FGAIAircraft(FGAISchedule *ref) : FGAIAircraft::~FGAIAircraft() { //delete fp; + if (controller) + controller->signOff(getID()); } @@ -180,15 +183,29 @@ void FGAIAircraft::Run(double dt) { if (now < fp->getStartTime()) { // Do execute Ground elev for inactive aircraft, so they // Are repositioned to the correct ground altitude when the user flies within visibility range. + // In addition, check whether we are out of user range, so this aircraft + // can be deleted. if (no_roll) { Transform(); // make sure aip is initialized. - getGroundElev(dt); // make sure it's exectuted first time around, so force a large dt value - //getGroundElev(dt); // Need to do this twice. - //cerr << trafficRef->getRegistration() << " Setting altitude to " << tgt_altitude; - doGroundAltitude(); - //cerr << " Actual altitude " << altitude << endl; - // Transform(); - pos.setElevationFt(altitude_ft); + if (trafficRef) { + double userLatitude = fgGetDouble("/position/latitude-deg"); + double userLongitude = fgGetDouble("/position/longitude-deg"); + double course, distance; + SGWayPoint current(pos.getLongitudeDeg(), pos.getLatitudeDeg(), 0); + SGWayPoint user (userLongitude, userLatitude, 0); + user.CourseAndDistance(current, &course, &distance); + if ((distance * SG_METER_TO_NM) > TRAFFICTOAIDIST) { + setDie(true); + return; + } + getGroundElev(dt); // make sure it's exectuted first time around, so force a large dt value + //getGroundElev(dt); // Need to do this twice. + //cerr << trafficRef->getRegistration() << " Setting altitude to " << tgt_altitude; + doGroundAltitude(); + //cerr << " Actual altitude " << altitude << endl; + // Transform(); + pos.setElevationFt(altitude_ft); + } } return; } @@ -229,10 +246,7 @@ void FGAIAircraft::Run(double dt) { hdg, speed, altitude_ft, dt); - //if (controller->hasInstruction(getID())) - // { - processATC(controller->getInstruction(getID())); - // } + processATC(controller->getInstruction(getID())); } double turn_radius_ft; @@ -972,29 +986,34 @@ void FGAIAircraft::announcePositionToController() int leg = fp->getLeg(); - // For starters, I'll only do this for departure and arrival taxi. The mechanism - // could be extended to include any controller however. - //int node, currentTaxiSegment; - //if (taxiRoute->next(&node, ¤tTaxiSegment)) { - if (fp->getCurrentWaypoint()->routeIndex != 0) { + //if (fp->getCurrentWaypoint()->routeIndex != 0) { //char buffer[10]; //snprintf (buffer, 10, "%d", node); + // Note that leg was been incremented after creating the current leg, so we should use + // leg numbers here that are one higher than the number that is used to create the leg + // switch (leg) { - case 3: - //cerr << trafficRef->getRegistration() - // << " taxiing to runway at segment " - // << fp->getCurrentWaypoint()->routeIndex - // << endl; - //cerr << "Match check between taxisegment and taxiroute : " << node << " " - // << fp->getCurrentWaypoint()->name << endl; + case 3: // Taxiing to runway if (trafficRef->getDepartureAirport()->getDynamics()->getGroundNetwork()->exists()) controller = trafficRef->getDepartureAirport()->getDynamics()->getGroundNetwork(); break; - case 9: - //cerr << trafficRef->getRegistration() - // << " taxiing to parking at segment " - // << fp->getCurrentWaypoint()->routeIndex - // << endl; + case 4: //Take off tower controller + if (trafficRef->getDepartureAirport()->getDynamics()) + { + controller = trafficRef->getDepartureAirport()->getDynamics()->getTowerController(); + //if (trafficRef->getDepartureAirport()->getId() == "EHAM") { + //cerr << trafficRef->getCallSign() << " at runway " << fp->getRunway() << "Ready for departure " + // << trafficRef->getFlightType() << " to " << trafficRef->getArrivalAirport()->getId() << endl; + // if (controller == 0) { + //cerr << "Error in assigning controller at " << trafficRef->getDepartureAirport()->getId() << endl; + //} + + } + else { + cerr << "Error: Could not find Dynamics at airport : " << trafficRef->getDepartureAirport()->getId() << endl; + } + break; + case 9: // Taxiing for parking if (trafficRef->getArrivalAirport()->getDynamics()->getGroundNetwork()->exists()) controller = trafficRef->getArrivalAirport()->getDynamics()->getGroundNetwork(); break; @@ -1002,20 +1021,27 @@ void FGAIAircraft::announcePositionToController() controller = 0; break; } - } else { - //fp->deleteTaxiRoute(); - controller = 0; - } + if ((controller != prevController) && (prevController != 0)) { prevController->signOff(getID()); - //cerr << trafficRef->getRegistration() - // << " signing off " << endl; + string callsign = trafficRef->getCallSign(); + if ( trafficRef->getHeavy()) + callsign += "Heavy"; + switch (leg) { + case 3: + cerr << callsign << " ready to taxi to runway " << fp->getRunway() << endl; + break; + case 4: + cerr << callsign << " at runway " << fp->getRunway() << "Ready for take-off. " + << trafficRef->getFlightRules() << " to " << trafficRef->getArrivalAirport()->getId() + << "(" << trafficRef->getArrivalAirport()->getName() << ")."<< endl; + } } prevController = controller; if (controller) { controller->announcePosition(getID(), fp, fp->getCurrentWaypoint()->routeIndex, - _getLatitude(), _getLongitude(), hdg, speed, altitude_ft, - trafficRef->getRadius()); + _getLatitude(), _getLongitude(), hdg, speed, altitude_ft, + trafficRef->getRadius(), leg); } } } diff --git a/src/AIModel/AIBase.cxx b/src/AIModel/AIBase.cxx index 1fe6a7e0e..2c274cd37 100644 --- a/src/AIModel/AIBase.cxx +++ b/src/AIModel/AIBase.cxx @@ -176,6 +176,7 @@ ssgBranch * FGAIBase::load3DModel(const string& fg_root, personality_branch->addKid( model ); return personality_branch; + //return model; } bool FGAIBase::isa( object_type otype ) { diff --git a/src/AIModel/AIFlightPlan.hxx b/src/AIModel/AIFlightPlan.hxx index 28f285b17..775cacbb9 100644 --- a/src/AIModel/AIFlightPlan.hxx +++ b/src/AIModel/AIFlightPlan.hxx @@ -98,6 +98,7 @@ public: int getRouteIndex(int i); // returns the AI related index of this current routes. FGTaxiRoute *getTaxiRoute() { return taxiRoute; }; void deleteTaxiRoute(); + string getRunway() { return activeRunway; }; private: diff --git a/src/AIModel/AIFlightPlanCreate.cxx b/src/AIModel/AIFlightPlanCreate.cxx index adeb775f9..ccb677bb8 100644 --- a/src/AIModel/AIFlightPlanCreate.cxx +++ b/src/AIModel/AIFlightPlanCreate.cxx @@ -598,6 +598,7 @@ void FGAIFlightPlan::createTakeOff(bool firstFlight, FGAirport *apt, double spee exit(1); } } + // Acceleration point, 105 meters into the runway, heading = rwy._heading; double azimuth = heading + 180.0; while ( azimuth >= 360.0 ) { azimuth -= 360.0; } @@ -622,13 +623,34 @@ void FGAIFlightPlan::createTakeOff(bool firstFlight, FGAirport *apt, double spee lon = lon2; az = az2; - //Next: the Start of Climb + //Start Climbing to 3000 ft. Let's do this + // at the center of the runway for now: + // geo_direct_wgs_84 ( 0, lat, lon, heading, 2560 * SG_FEET_TO_METER, &lat2, &lon2, &az2 ); wpt = new waypoint; wpt->name = "SOC"; + wpt->latitude = rwy._lat; + wpt->longitude = rwy._lon; + wpt->altitude = apt->getElevation()+1000; + wpt->speed = speed; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = false; + wpt->routeIndex = 0; + waypoints.push_back(wpt); + + + geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, heading, + rwy._length * SG_FEET_TO_METER, + &lat2, &lon2, &az2 ); + + wpt = new waypoint; + wpt->name = "3000 ft"; wpt->latitude = lat2; wpt->longitude = lon2; wpt->altitude = apt->getElevation()+3000; @@ -640,6 +662,46 @@ void FGAIFlightPlan::createTakeOff(bool firstFlight, FGAirport *apt, double spee wpt->on_ground = false; wpt->routeIndex = 0; waypoints.push_back(wpt); + +// Finally, add two more waypoints, so that aircraft will remain under + // Tower control until they have reached the 3000 ft climb point + + + geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, heading, + 5000, + &lat2, &lon2, &az2 ); + + + wpt = new waypoint; + wpt->name = "5000 ft"; + wpt->latitude = lat2; + wpt->longitude = lon2; + wpt->altitude = apt->getElevation()+5000; + wpt->speed = speed; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = false; + wpt->routeIndex = 0; + waypoints.push_back(wpt); + + // geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, heading, +// 100000, +// &lat2, &lon2, &az2 ); +// wpt = new waypoint; +// wpt->name = "5100 ft"; +// wpt->latitude = lat2; +// wpt->longitude = lon2; +// wpt->altitude = apt->getElevation()+5100; +// wpt->speed = speed; +// wpt->crossat = -10000; +// wpt->gear_down = true; +// wpt->flaps_down= true; +// wpt->finished = false; +// wpt->on_ground = false; +// wpt->routeIndex = 0; +// waypoints.push_back(wpt); } /******************************************************************* diff --git a/src/AIModel/AIManager.cxx b/src/AIModel/AIManager.cxx index 207913bb1..130ed2cec 100644 --- a/src/AIModel/AIManager.cxx +++ b/src/AIModel/AIManager.cxx @@ -259,10 +259,30 @@ FGAIManager::loadScenarioFile(const std::string& filename) // This code keeps track of models that have already been loaded // Eventually we'd prbably need to find a way to keep track of models // that are unloaded again -ssgBranch * FGAIManager::getModel(const string& path) const +ssgBranch * FGAIManager::getModel(const string& path) { - ModelVecIterator i = loadedModels.begin(); + ModelVecIterator i = loadedModels.begin(); + cerr << "Reference count summary " << endl; + int count = 0; while (i != loadedModels.end()) + { + count += i->getNumRefs() -1; + cerr << "Model " << i->getPath() << " has reference count of " << i->getNumRefs() << " "; + if (i->getNumRefs() == 1) + { + i = loadedModels.erase(i); + cerr << "[ Deleted ]" << endl; + } + else + { + i++; + cerr << endl; + } + } + cerr << "Reference summary end : " << count << "models allocated" << endl; + i = loadedModels.begin(); + + while (i != loadedModels.end()) { if (i->getPath() == path) return i->getModelId(); diff --git a/src/AIModel/AIManager.hxx b/src/AIModel/AIManager.hxx index 5eaaf464f..42b279bf2 100644 --- a/src/AIModel/AIManager.hxx +++ b/src/AIModel/AIManager.hxx @@ -49,10 +49,11 @@ public: FGModelID(const string& pth, ssgBranch * mdl) { path =pth; model=mdl;}; ssgBranch * const getModelId() const { return model;}; const string & getPath() const { return path;}; + int getNumRefs() const { return model.getNumRefs(); }; }; typedef vector ModelVec; -typedef vector::const_iterator ModelVecIterator; +typedef vector::iterator ModelVecIterator; class FGAIThermal; @@ -99,7 +100,7 @@ public: void processScenario( const string &filename ); - ssgBranch * getModel(const string& path) const; + ssgBranch * getModel(const string& path); void setModel(const string& path, ssgBranch *model); static SGPropertyNode_ptr loadScenarioFile(const std::string& filename); diff --git a/src/Airports/Makefile.am b/src/Airports/Makefile.am index bb9e960f7..1c6748146 100644 --- a/src/Airports/Makefile.am +++ b/src/Airports/Makefile.am @@ -9,7 +9,8 @@ libAirports_a_SOURCES = \ runwayprefs.cxx runwayprefs.hxx \ parking.cxx parking.hxx \ groundnetwork.cxx groundnetwork.hxx \ - dynamics.cxx dynamics.hxx + dynamics.cxx dynamics.hxx \ + trafficcontrol.hxx trafficcontrol.cxx calc_loc_SOURCES = calc_loc.cxx calc_loc_LDADD = -lsgmath -lsgdebug -lsgmisc -lz $(base_LIBS) diff --git a/src/Airports/dynamics.cxx b/src/Airports/dynamics.cxx index 9a3dcacc2..a95c63399 100644 --- a/src/Airports/dynamics.cxx +++ b/src/Airports/dynamics.cxx @@ -91,7 +91,6 @@ FGAirportDynamics::FGAirportDynamics(const FGAirportDynamics& other) // Destructor FGAirportDynamics::~FGAirportDynamics() { - } @@ -109,6 +108,7 @@ void FGAirportDynamics::init() // add the gate positions to the ground network. groundNetwork.addNodes(&parkings); groundNetwork.init(); + groundNetwork .setTowerController(&towerController); } bool FGAirportDynamics::getAvailableParking(double *lat, double *lon, double *heading, int *gateId, double rad, const string &flType, const string &acType, const string &airline) diff --git a/src/Airports/dynamics.hxx b/src/Airports/dynamics.hxx index 7543ffca9..cbd9953bc 100644 --- a/src/Airports/dynamics.hxx +++ b/src/Airports/dynamics.hxx @@ -32,6 +32,7 @@ #include "parking.hxx" #include "groundnetwork.hxx" #include "runwayprefs.hxx" +#include "trafficcontrol.hxx" class FGAirportDynamics : public XMLVisitor { @@ -42,9 +43,10 @@ private: double _elevation; // ft string _id; - FGParkingVec parkings; + FGParkingVec parkings; FGRunwayPreference rwyPrefs; - FGGroundNetwork groundNetwork; + FGGroundNetwork groundNetwork; + FGTowerController towerController; time_t lastUpdate; string prevTrafficType; @@ -86,7 +88,8 @@ public: //const string &getName() const { return _name;}; // Returns degrees - FGGroundNetwork* getGroundNetwork() { return &groundNetwork; }; + FGGroundNetwork *getGroundNetwork() { return &groundNetwork; }; + FGTowerController *getTowerController() { return &towerController; }; void setRwyUse(const FGRunwayPreference& ref); diff --git a/src/Airports/groundnetwork.cxx b/src/Airports/groundnetwork.cxx index 7936a3e19..6d856f23a 100644 --- a/src/Airports/groundnetwork.cxx +++ b/src/Airports/groundnetwork.cxx @@ -49,6 +49,8 @@ SG_USING_STD(sort); + + /************************************************************************** * FGTaxiNode *************************************************************************/ @@ -56,6 +58,11 @@ FGTaxiNode::FGTaxiNode() { } + +bool compare_nodes(FGTaxiNode *a, FGTaxiNode *b) { +return (*a) < (*b); +} + /*************************************************************************** * FGTaxiSegment **************************************************************************/ @@ -69,10 +76,10 @@ void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes) FGTaxiNodeVectorIterator i = nodes->begin(); while (i != nodes->end()) { - if (i->getIndex() == startNode) + if ((*i)->getIndex() == startNode) { - start = i->getAddress(); - i->addSegment(this); + start = (*i)->getAddress(); + (*i)->addSegment(this); return; } i++; @@ -84,9 +91,9 @@ void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes) FGTaxiNodeVectorIterator i = nodes->begin(); while (i != nodes->end()) { - if (i->getIndex() == endNode) + if ((*i)->getIndex() == endNode) { - end = i->getAddress(); + end = (*i)->getAddress(); return; } i++; @@ -106,6 +113,11 @@ void FGTaxiSegment::setTrackDistance() 0); first.CourseAndDistance(second, &course, &length); } + +bool compare_segments(FGTaxiSegment *a, FGTaxiSegment *b) { +return (*a) < (*b); +} + /*************************************************************************** * FGTaxiRoute **************************************************************************/ @@ -179,184 +191,7 @@ void FGTaxiRoute::rewind(int route) } while (currRoute != route); } -/*************************************************************************** - * FGTrafficRecord - **************************************************************************/ -void FGTrafficRecord::setPositionAndIntentions(int pos, FGAIFlightPlan *route) -{ - - currentPos = pos; - if (intentions.size()) { - if (*intentions.begin() != 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(intentions.begin()); - } 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))) - 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 (currentTargetNode == net->findSegment(*i)->getEnd()->getIndex()) - { - cerr << "Current crosses at " << currentTargetNode <findSegment(*i)->getEnd()->getIndex()) - { - cerr << "Other crosses at " << currentTargetNode <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++) - { - for (intVecIterator j = intentions.begin(); j != intentions.end(); j++) - { - // cerr << "Current segment 1 " << (*i) << endl; - 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; -} /*************************************************************************** @@ -373,14 +208,32 @@ FGGroundNetwork::FGGroundNetwork() } +FGGroundNetwork::~FGGroundNetwork() +{ + for (FGTaxiNodeVectorIterator node = nodes.begin(); + node != nodes.end(); + node++) + { + delete (*node); + } + nodes.clear(); + for (FGTaxiSegmentVectorIterator seg = segments.begin(); + seg != segments.end(); + seg++) + { + delete (*seg); + } + segments.clear(); +} + void FGGroundNetwork::addSegment(const FGTaxiSegment &seg) { - segments.push_back(seg); + segments.push_back(new FGTaxiSegment(seg)); } void FGGroundNetwork::addNode(const FGTaxiNode &node) { - nodes.push_back(node); + nodes.push_back(new FGTaxiNode(node)); } void FGGroundNetwork::addNodes(FGParkingVec *parkings) @@ -392,7 +245,7 @@ void FGGroundNetwork::addNodes(FGParkingVec *parkings) n.setIndex(i->getIndex()); n.setLatitude(i->getLatitude()); n.setLongitude(i->getLongitude()); - nodes.push_back(n); + nodes.push_back(new FGTaxiNode(n)); i++; } @@ -404,15 +257,15 @@ void FGGroundNetwork::init() { hasNetwork = true; int index = 1; - sort(nodes.begin(), nodes.end()); - sort(segments.begin(), segments.end()); + sort(nodes.begin(), nodes.end(), compare_nodes); + //sort(segments.begin(), segments.end(), compare_segments()); FGTaxiSegmentVectorIterator i = segments.begin(); while(i != segments.end()) { //cerr << "initializing node " << i->getIndex() << endl; - i->setStart(&nodes); - i->setEnd (&nodes); - i->setTrackDistance(); - i->setIndex(index); + (*i)->setStart(&nodes); + (*i)->setEnd (&nodes); + (*i)->setTrackDistance(); + (*i)->setIndex(index); //cerr << "Track distance = " << i->getLength() << endl; //cerr << "Track ends at" << i->getEnd()->getIndex() << endl; i++; @@ -420,17 +273,17 @@ void FGGroundNetwork::init() } i = segments.begin(); while(i != segments.end()) { - FGTaxiSegmentPointerVectorIterator j = i->getEnd()->getBeginRoute(); - while (j != i->getEnd()->getEndRoute()) + FGTaxiSegmentVectorIterator j = (*i)->getEnd()->getBeginRoute(); + while (j != (*i)->getEnd()->getEndRoute()) { - if ((*j)->getEnd()->getIndex() == i->getStart()->getIndex()) + if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex()) { - int start1 = i->getStart()->getIndex(); - int end1 = i->getEnd() ->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 << "Oppossite of " << i->getIndex() << " (" << start1 << "," << end1 << ") " + cerr << "Opposite of " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") " << "happens to be " << oppIndex << " (" << start2 << "," << end2 << ") " << endl; break; } @@ -455,14 +308,14 @@ int FGGroundNetwork::findNearestNode(double lat, double lon) itr != nodes.end(); itr++) { double course; - SGWayPoint second (itr->getLongitude(), - itr->getLatitude(), + SGWayPoint second ((*itr)->getLongitude(), + (*itr)->getLatitude(), 0); first.CourseAndDistance(second, &course, &dist); if (dist < minDist) { minDist = dist; - index = itr->getIndex(); + index = (*itr)->getIndex(); //cerr << "Minimum distance of " << minDist << " for index " << index << endl; } } @@ -480,7 +333,7 @@ FGTaxiNode *FGGroundNetwork::findNode(int idx) }*/ if ((idx >= 0) && (idx < nodes.size())) - return nodes[idx].getAddress(); + return nodes[idx]->getAddress(); else return 0; } @@ -496,7 +349,7 @@ FGTaxiSegment *FGGroundNetwork::findSegment(int idx) } */ if ((idx > 0) && (idx <= segments.size())) - return segments[idx-1].getAddress(); + return segments[idx-1]->getAddress(); else { cerr << "Alert: trying to find invalid segment " << idx << endl; @@ -515,13 +368,14 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end) routes.clear(); nodesStack.clear(); routesStack.clear(); - + //cerr << "Begin of Trace " << endl; trace(firstNode, end, 0, 0); + //cerr << "End of Trace" << endl; FGTaxiRoute empty; if (!foundRoute) { - SG_LOG( SG_GENERAL, SG_INFO, "Failed to find route from waypoint " << start << " to " << end ); + SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find route from waypoint " << start << " to " << end ); exit(1); } sort(routes.begin(), routes.end()); @@ -547,8 +401,8 @@ void FGGroundNetwork::trace(FGTaxiNode *currNode, int end, int depth, double dis } nodesStack.push_back(currNode->getIndex()); totalDistance += distance; - //cerr << "Starting trace " << depth << " total distance: " << totalDistance<< endl; - //<< currNode->getIndex() << endl; + //cerr << "Starting trace " << depth << " total distance: " << totalDistance<< " " + // << currNode->getIndex() << endl; // If the current route matches the required end point we found a valid route // So we can add this to the routing table @@ -618,7 +472,7 @@ void FGGroundNetwork::trace(FGTaxiNode *currNode, int end, int depth, double dis if (currNode->getBeginRoute() != currNode->getEndRoute()) { //cerr << "3" << endl; - for (FGTaxiSegmentPointerVectorIterator + for (FGTaxiSegmentVectorIterator i = currNode->getBeginRoute(); i != currNode->getEndRoute(); i++) @@ -675,75 +529,87 @@ void FGGroundNetwork::printRoutingError(string mess) void FGGroundNetwork::announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentPosition, double lat, double lon, double heading, - double speed, double alt, double radius) + double speed, double alt, double radius, int leg) { - 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()) { - 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. - 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.setPositionAndIntentions(currentPosition, intendedRoute); + rec.setPositionAndHeading(lat, lon, heading, speed, alt); + rec.setRadius(radius); // only need to do this when creating the record. + 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()) { - 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); - } + 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) { - 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()) { - 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; - } + 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); + 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); - checkSpeedAdjustment(id, lat, lon, heading, speed, alt); - checkHoldPosition (id, lat, lon, heading, speed, alt); + // 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); + checkSpeedAdjustment(id, lat, lon, heading, speed, alt); + checkHoldPosition (id, lat, lon, heading, speed, alt); } void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, @@ -758,46 +624,105 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, // instruction. See below for the hold position instruction. TrafficVectorIterator current, closest; TrafficVectorIterator i = activeTraffic.begin(); + bool otherReasonToSlowDown = false; if (activeTraffic.size()) { - while ((i->getId() != id) && (i != activeTraffic.end())) - { - i++; + //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; double mindist = HUGE; if (activeTraffic.size()) { double course, dist, bearing, minbearing; - + SGWayPoint curr (lon, + lat, + alt); //TrafficVector iterator closest; + closest = current; for (TrafficVectorIterator i = activeTraffic.begin(); - i != activeTraffic.end(); i++) + i != activeTraffic.end(); i++) + { + if (i != current) { + //SGWayPoint curr (lon, + // lat, + // alt); + SGWayPoint other (i->getLongitude (), + i->getLatitude (), + i->getAltitude ()); + other.CourseAndDistance(curr, &course, &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()) { - if (i != current) { - SGWayPoint curr (lon, - lat, - alt); - SGWayPoint other (i->getLongitude (), - i->getLatitude (), - i->getAltitude ()); - other.CourseAndDistance(curr, &course, &dist); - bearing = fabs(heading-course); - if (bearing > 180) - bearing = 360-bearing; - if ((dist < mindist) && (bearing < 60.0)) - { - mindist = dist; - closest = i; - minbearing = bearing; + for (TrafficVectorIterator i = towerController->getActiveTraffic().begin(); + i != towerController->getActiveTraffic().end(); i++) + { + if (i != current) { + //SGWayPoint curr (lon, + // lat, + // alt); + SGWayPoint other (i->getLongitude (), + i->getLatitude (), + i->getAltitude ()); + other.CourseAndDistance(curr, &course, &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"); + SGWayPoint user (userLongitude, + userLatitude, + alt); // Alt is not really important here. + user.CourseAndDistance(curr, &course, &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; } + + // if (closest == current) { + // //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: closest and current match"); + // //return; + // } //cerr << "Distance : " << dist << " bearing : " << bearing << " heading : " << heading // << " course : " << course << endl; current->clearSpeedAdjustment(); @@ -813,44 +738,49 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, } else { - if (current->getId() == closest->getWaitsForId()) - return; - else - current->setWaitsForId(closest->getId()); - // Getting close: Slow down to a bit less than the other aircraft - double maxAllowableDistance = (1.1*current->getRadius()) + (1.1*closest->getRadius()); - if (mindist > maxAllowableDistance) - { - if (current->checkPositionAndIntentions(*closest)) - { - // Adjust speed, but don't let it drop to below 1 knots - //if (fabs(speed) > 1) - if (!(current->hasHeadingAdjustment())) - { - current->setSpeedAdjustment(closest->getSpeed()* (mindist/100)); - //cerr << "Adjusting speed to " << closest->getSpeed() * (mindist / 100) << " " - // << "Bearing = " << minbearing << " Distance = " << mindist - // << " Latitude = " <getSpeed() * (mindist / 100) << " " + // << "Bearing = " << minbearing << " Distance = " << mindist + // << " Latitude = " <setHoldPosition(false); + SGWayPoint curr (lon, + lat, + alt); double course, dist, bearing, minbearing; for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) { if (i != current) - { - int node = current->crosses(this, *i); - if (node != -1) - { - // 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; - SGWayPoint nodePos(findNode(node)->getLongitude (), - findNode(node)->getLatitude (), - alt); - SGWayPoint curr (lon, - lat, - alt); + { + int node = current->crosses(this, *i); + if (node != -1) + { + // 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; + SGWayPoint nodePos(findNode(node)->getLongitude (), + findNode(node)->getLatitude (), + alt); - SGWayPoint other (i->getLongitude (), - i->getLatitude (), - i->getAltitude ()); - //other.CourseAndDistance(curr, &course, &dist); - bool needsToWait; - if (current->isOpposing(this, *i, node)) - { - needsToWait = true; - //cerr << "Hold check 2 : " << id << " 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 - { - other.CourseAndDistance(nodePos, &course, &dist); - if (dist > 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 << " nm" << endl; - } - } - curr.CourseAndDistance(nodePos, &course, &dist); - if (!(i->hasHoldPosition())) - { + + SGWayPoint other (i->getLongitude (), + i->getLatitude (), + i->getAltitude ()); + //other.CourseAndDistance(curr, &course, &dist); + bool needsToWait; + if (current->isOpposing(this, *i, node)) + { + needsToWait = true; + //cerr << "Hold check 2 : " << id << " 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 + { + other.CourseAndDistance(nodePos, &course, &dist); + if (dist > 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 << " nm" << endl; + } + } + curr.CourseAndDistance(nodePos, &course, &dist); + if (!(i->hasHoldPosition())) + { - if ((dist < 2.5*current->getRadius()) && - (needsToWait) && - (!(current->getId() == i->getWaitsForId())) && - (!(current->getSpeedAdjustment()))) + if ((dist < 2.5*current->getRadius()) && + (needsToWait) && + (!(current->getId() == i->getWaitsForId())) && + (!(current->getSpeedAdjustment()))) - { - current->setHoldPosition(true); - //cerr << "Hold check 5: " << id <<" Setting Hold Position: distance to node : " << dist << " nm"<< endl; - } - else - { - //cerr << "Hold check 6: " << id << " No need to hold yet: Distance to node : " << dist << " nm"<< endl; - } - } - } - } + { + current->setHoldPosition(true); + //cerr << "Hold check 5: " << id <<" Setting Hold Position: distance to node : " << dist << " nm"<< endl; + } + else + { + //cerr << "Hold check 6: " << id << " No need to hold yet: Distance to node : " << dist << " nm"<< endl; + } + } + } + } } } - +// 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()) { - 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(); - } + 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; } @@ -987,47 +928,21 @@ 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()) { - 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(); - } + // 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(); } - - - -/*************************************************************************** - * FGATCInstruction - * - * This class is really out of place here, and should be combined with - * FGATC controller and go into it's own file / directory - * I'm leaving it for now though, because I'm testing this stuff quite - * heavily. - **************************************************************************/ -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); -} - - diff --git a/src/Airports/groundnetwork.hxx b/src/Airports/groundnetwork.hxx index fb6089dfa..b3dd99b9c 100644 --- a/src/Airports/groundnetwork.hxx +++ b/src/Airports/groundnetwork.hxx @@ -35,17 +35,18 @@ SG_USING_STD(vector); #include "parking.hxx" -//#include +#include "trafficcontrol.hxx" class FGTaxiSegment; // forward reference class FGAIFlightPlan; // forward reference -typedef vector FGTaxiSegmentVector; -typedef vector FGTaxiSegmentPointerVector; -typedef vector::iterator FGTaxiSegmentVectorIterator; -typedef vector::iterator FGTaxiSegmentPointerVectorIterator; +typedef vector FGTaxiSegmentVector; +typedef vector::iterator FGTaxiSegmentVectorIterator; + +//typedef vector FGTaxiSegmentPointerVector; +//typedef vector::iterator FGTaxiSegmentPointerVectorIterator; /************************************************************************************** * class FGTaxiNode @@ -56,7 +57,7 @@ private: double lat; double lon; int index; - FGTaxiSegmentPointerVector next; // a vector to all the segments leaving from this node + FGTaxiSegmentVector next; // a vector of pointers to all the segments leaving from this node public: FGTaxiNode(); @@ -74,13 +75,13 @@ public: int getIndex() { return index; }; FGTaxiNode *getAddress() { return this;}; - FGTaxiSegmentPointerVectorIterator getBeginRoute() { return next.begin(); }; - FGTaxiSegmentPointerVectorIterator getEndRoute() { return next.end(); }; + FGTaxiSegmentVectorIterator getBeginRoute() { return next.begin(); }; + FGTaxiSegmentVectorIterator getEndRoute() { return next.end(); }; bool operator<(const FGTaxiNode &other) const { return index < other.index; }; }; -typedef vector FGTaxiNodeVector; -typedef vector::iterator FGTaxiNodeVectorIterator; +typedef vector FGTaxiNodeVector; +typedef vector::iterator FGTaxiNodeVectorIterator; /*************************************************************************************** * class FGTaxiSegment @@ -160,133 +161,6 @@ public: typedef vector TaxiRouteVector; typedef vector::iterator TaxiRouteVectorIterator; -/************************************************************************************** - * 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 FGGroundNetwork; - -/************************************************************************************** - * class FGTrafficRecord - *************************************************************************************/ -class FGTrafficRecord -{ -private: - int id, waitsForId; - int currentPos; - intVec intentions; - FGATCInstruction instruction; - double latitude, longitude, heading, speed, altitude, radius; - - -public: - FGTrafficRecord() {}; - - void setId(int val) { id = val; }; - void setRadius(double rad) { radius = rad;}; - void setPositionAndIntentions(int pos, FGAIFlightPlan *route); - 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) { instruction.setChangeSpeed(true); - instruction.setSpeed(spd); }; - void setHeadingAdjustment(double heading) { instruction.setChangeHeading(true); - instruction.setHeading(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; }; - -}; - -typedef vector TrafficVector; -typedef vector::iterator TrafficVectorIterator; - - - - -/************************************************************************************** - * class FGATCController - * NOTE: this class serves as an abstraction layer for all sorts of ATC controller, - * Ground and air, so eventually it should move to its own file / directory. - *************************************************************************************/ -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) = 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;}; -}; @@ -308,6 +182,7 @@ private: bool foundRoute; double totalDistance, maxDistance; + FGTowerController *towerController; void printRoutingError(string); @@ -318,6 +193,7 @@ private: public: FGGroundNetwork(); + ~FGGroundNetwork(); void addNode (const FGTaxiNode& node); void addNodes (FGParkingVec *parkings); @@ -325,6 +201,7 @@ public: void init(); bool exists() { return hasNetwork; }; + void setTowerController(FGTowerController *twrCtrlr) { towerController = twrCtrlr; }; int findNearestNode(double lat, double lon); FGTaxiNode *findNode(int idx); FGTaxiSegment *findSegment(int idx); @@ -332,7 +209,7 @@ public: void trace(FGTaxiNode *, int, int, double dist); virtual void announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentRoute, - double lat, double lon, double hdg, double spd, double alt, double radius); + 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); diff --git a/src/Airports/simple.cxx b/src/Airports/simple.cxx index cf969de1f..fa159ca96 100644 --- a/src/Airports/simple.cxx +++ b/src/Airports/simple.cxx @@ -90,7 +90,7 @@ FGAirportDynamics * FGAirport::getDynamics() return dynamics; } else { FGRunwayPreference rwyPrefs; - //cerr << "Trying to load dynamics for " << _id << endl; + cerr << "Trying to load dynamics for " << _id << endl; dynamics = new FGAirportDynamics(_latitude, _longitude, _elevation, _id); SGPath parkpath( globals->get_fg_root() ); diff --git a/src/Traffic/SchedFlight.cxx b/src/Traffic/SchedFlight.cxx index 0247b7064..a66915a1e 100644 --- a/src/Traffic/SchedFlight.cxx +++ b/src/Traffic/SchedFlight.cxx @@ -280,3 +280,9 @@ bool FGScheduledFlight::initializeAirports() initialized = true; return true; } + + +bool compareScheduledFlights(FGScheduledFlight *a, FGScheduledFlight *b) +{ + return (*a) < (*b); +}; diff --git a/src/Traffic/SchedFlight.hxx b/src/Traffic/SchedFlight.hxx index 7e74afcc5..4f85ed993 100644 --- a/src/Traffic/SchedFlight.hxx +++ b/src/Traffic/SchedFlight.hxx @@ -95,13 +95,16 @@ public: { return (departureTime < other.departureTime); }; + string& getFlightRules() { return fltRules; }; time_t processTimeString(const string& time); const string& getCallSign() {return callsign; }; }; -typedef vector FGScheduledFlightVec; -typedef vector::iterator FGScheduledFlightVecIterator; +typedef vector FGScheduledFlightVec; +typedef vector::iterator FGScheduledFlightVecIterator; + +bool compareScheduledFlights(FGScheduledFlight *a, FGScheduledFlight *b); #endif diff --git a/src/Traffic/Schedule.cxx b/src/Traffic/Schedule.cxx index d1e181e6e..c136dbfca 100644 --- a/src/Traffic/Schedule.cxx +++ b/src/Traffic/Schedule.cxx @@ -106,7 +106,7 @@ FGAISchedule::FGAISchedule(string mdl, for (FGScheduledFlightVecIterator i = flt.begin(); i != flt.end(); i++) - flights.push_back(FGScheduledFlight((*i))); + flights.push_back(new FGScheduledFlight((*(*i)))); AIManagerRef = 0; score = scre; firstRun = true; @@ -136,7 +136,11 @@ FGAISchedule::FGAISchedule(const FGAISchedule &other) FGAISchedule::~FGAISchedule() { - + for (FGScheduledFlightVecIterator flt = flights.begin(); flt != flights.end(); flt++) + { + delete (*flt); + } + flights.clear(); } bool FGAISchedule::init() @@ -154,7 +158,7 @@ bool FGAISchedule::init() i++) { //i->adjustTime(now); - if (!(i->initializeAirports())) + if (!((*i)->initializeAirports())) return false; } //sort(flights.begin(), flights.end()); @@ -216,19 +220,20 @@ bool FGAISchedule::update(time_t now) i != flights.end(); i++) { - i->adjustTime(now); + (*i)->adjustTime(now); } if (fgGetBool("/sim/traffic-manager/instantaneous-action") == true) - deptime = now; + 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; } // Sort all the scheduled flights according to scheduled departure time. // Because this is done at every update, we only need to check the status // of the first listed flight. - sort(flights.begin(), flights.end()); + sort(flights.begin(), flights.end(), compareScheduledFlights); if (!deptime) - deptime = flights.begin()->getDepartureTime(); + deptime = (*flights.begin())->getDepartureTime(); FGScheduledFlightVecIterator i = flights.begin(); if (AIManagerRef) { @@ -246,9 +251,9 @@ bool FGAISchedule::update(time_t now) //cerr << "Estimated minimum distance to user: " << distanceToUser << endl; // This flight entry is entirely in the past, do we need to // push it forward in time to the next scheduled departure. - if ((i->getDepartureTime() < now) && (i->getArrivalTime() < now)) + if (((*i)->getDepartureTime() < now) && ((*i)->getArrivalTime() < now)) { - i->update(); + (*i)->update(); return true; } @@ -260,10 +265,10 @@ bool FGAISchedule::update(time_t now) // Part of this flight is in the future. - if (i->getArrivalTime() > now) + if ((*i)->getArrivalTime() > now) { - dep = i->getDepartureAirport(); - arr = i->getArrivalAirport (); + dep = (*i)->getDepartureAirport(); + arr = (*i)->getArrivalAirport (); if (!(dep && arr)) return false; @@ -295,13 +300,13 @@ bool FGAISchedule::update(time_t now) // position of the aircraft by calculating the ratio between // total time enroute and elapsed time enroute. - totalTimeEnroute = i->getArrivalTime() - i->getDepartureTime(); - if (now > i->getDepartureTime()) + totalTimeEnroute = (*i)->getArrivalTime() - (*i)->getDepartureTime(); + if (now > (*i)->getDepartureTime()) { //err << "Lat = " << lat << ", lon = " << lon << endl; //cerr << "Time diff: " << now-i->getDepartureTime() << endl; - elapsedTimeEnroute = now - i->getDepartureTime(); - remainingTimeEnroute = i->getArrivalTime() - now; + elapsedTimeEnroute = now - (*i)->getDepartureTime(); + remainingTimeEnroute = (*i)->getArrivalTime() - now; } else { @@ -327,7 +332,7 @@ bool FGAISchedule::update(time_t now) } temp = sgCartToPolar3d(Point3D(newPos[0], newPos[1],newPos[2])); - if (now > i->getDepartureTime()) + if (now > (*i)->getDepartureTime()) { //cerr << "Lat = " << lat << ", lon = " << lon << endl; //cerr << "Time diff: " << now-i->getDepartureTime() << endl; @@ -343,13 +348,13 @@ bool FGAISchedule::update(time_t now) SGWayPoint current (lon, lat, - i->getCruiseAlt()); + (*i)->getCruiseAlt()); SGWayPoint user ( userLongitude, userLatitude, - i->getCruiseAlt()); + (*i)->getCruiseAlt()); SGWayPoint dest ( arr->getLongitude(), arr->getLatitude(), - i->getCruiseAlt()); + (*i)->getCruiseAlt()); // We really only need distance to user // and course to destination user.CourseAndDistance(current, &courseToUser, &distanceToUser); @@ -392,12 +397,12 @@ bool FGAISchedule::update(time_t now) //aircraft->setFlightPlan(flightPlanName); aircraft->setLatitude(lat); aircraft->setLongitude(lon); - aircraft->setAltitude(i->getCruiseAlt()*100); // convert from FL to feet + aircraft->setAltitude((*i)->getCruiseAlt()*100); // convert from FL to feet aircraft->setSpeed(speed); aircraft->setBank(0); aircraft->SetFlightPlan(new FGAIFlightPlan(flightPlanName, courseToDest, deptime, dep, arr,true, radius, - i->getCruiseAlt()*100, + (*i)->getCruiseAlt()*100, lat, lon, speed, flightType, acType, airline)); aimgr->attach(aircraft); @@ -425,9 +430,9 @@ bool FGAISchedule::update(time_t now) // Currently this status is mostly ignored, but in future // versions, code should go here that -if within user range- // positions these aircraft at parking locations at the airport. - if ((i->getDepartureTime() > now) && (i->getArrivalTime() > now)) + if (((*i)->getDepartureTime() > now) && ((*i)->getArrivalTime() > now)) { - dep = i->getDepartureAirport(); + dep = (*i)->getDepartureAirport(); return true; } } @@ -445,8 +450,8 @@ bool FGAISchedule::update(time_t now) void FGAISchedule::next() { - flights.begin()->update(); - sort(flights.begin(), flights.end()); + (*flights.begin())->update(); + sort(flights.begin(), flights.end(), compareScheduledFlights); } double FGAISchedule::getSpeed() @@ -457,24 +462,29 @@ double FGAISchedule::getSpeed() FGAirport *dep, *arr; FGScheduledFlightVecIterator i = flights.begin(); - dep = i->getDepartureAirport(); - arr = i->getArrivalAirport (); + dep = (*i)->getDepartureAirport(); + arr = (*i)->getArrivalAirport (); if (!(dep && arr)) return 0; SGWayPoint dest ( dep->getLongitude(), dep->getLatitude(), - i->getCruiseAlt()); + (*i)->getCruiseAlt()); SGWayPoint curr ( arr->getLongitude(), arr->getLatitude(), - i->getCruiseAlt()); - remainingTimeEnroute = i->getArrivalTime() - i->getDepartureTime(); + (*i)->getCruiseAlt()); + remainingTimeEnroute = (*i)->getArrivalTime() - (*i)->getDepartureTime(); dest.CourseAndDistance(curr, &courseToDest, &distanceToDest); speed = (distanceToDest*SG_METER_TO_NM) / ((double) remainingTimeEnroute/3600.0); return speed; } +bool compareSchedules(FGAISchedule*a, FGAISchedule*b) +{ + return (*a) < (*b); +} + // void FGAISchedule::setClosestDistanceToUser() // { @@ -520,3 +530,4 @@ double FGAISchedule::getSpeed() // } // //return distToUser; // } + diff --git a/src/Traffic/Schedule.hxx b/src/Traffic/Schedule.hxx index e29329ab9..f18d7492e 100644 --- a/src/Traffic/Schedule.hxx +++ b/src/Traffic/Schedule.hxx @@ -71,29 +71,31 @@ class FGAISchedule //void setClosestDistanceToUser(); void next(); // forces the schedule to move on to the next flight. - time_t getDepartureTime () { return flights.begin()->getDepartureTime (); }; - FGAirport * getDepartureAirport () { return flights.begin()->getDepartureAirport(); }; - FGAirport * getArrivalAirport () { return flights.begin()->getArrivalAirport (); }; - int getCruiseAlt () { return flights.begin()->getCruiseAlt (); }; + time_t getDepartureTime () { return (*flights.begin())->getDepartureTime (); }; + FGAirport * getDepartureAirport () { return (*flights.begin())->getDepartureAirport(); }; + FGAirport * getArrivalAirport () { return (*flights.begin())->getArrivalAirport (); }; + int getCruiseAlt () { return (*flights.begin())->getCruiseAlt (); }; double getRadius () { return radius; }; double getGroundOffset () { return groundOffset;}; const string& getFlightType () { return flightType;}; const string& getAirline () { return airline; }; const string& getAircraft () { return acType; }; - const string& getCallSign () { return flights.begin()->getCallSign (); }; + const string& getCallSign () { return (*flights.begin())->getCallSign (); }; const string& getRegistration () { return registration;}; + const string& getFlightRules () { return (*flights.begin())->getFlightRules (); }; bool getHeavy () { return heavy; }; // used to sort in decending order of score: I've probably found a better way to // decending order sorting, but still need to test that. bool operator< (const FGAISchedule &other) const { return (score > other.score); }; //void * getAiRef () { return AIManagerRef; }; //FGAISchedule* getAddress () { return this;}; - // More member functions follow later }; -typedef vector ScheduleVector; -typedef vector::iterator ScheduleVectorIterator; +typedef vector ScheduleVector; +typedef vector::iterator ScheduleVectorIterator; + +bool compareSchedules(FGAISchedule*a, FGAISchedule*b); #endif diff --git a/src/Traffic/TrafficMgr.cxx b/src/Traffic/TrafficMgr.cxx index 99a97c9d9..43cce5f74 100644 --- a/src/Traffic/TrafficMgr.cxx +++ b/src/Traffic/TrafficMgr.cxx @@ -79,6 +79,20 @@ FGTrafficManager::FGTrafficManager() score = 0; } +FGTrafficManager:: ~FGTrafficManager() +{ + for (ScheduleVectorIterator sched = scheduledAircraft.begin(); sched != scheduledAircraft.end(); sched++) + { + delete (*sched); + } + scheduledAircraft.clear(); + // for (FGScheduledFlightVecIterator flt = flights.begin(); flt != flights.end(); flt++) +// { +// delete (*flt); +// } +// flights.clear(); +} + void FGTrafficManager::init() { @@ -97,8 +111,9 @@ void FGTrafficManager::init() // currAircraft++; // } // } - //cerr << "Sorting by distance " << endl; - sort(scheduledAircraft.begin(), scheduledAircraft.end()); + // Sort by points: Aircraft with frequent visits to the + // startup airport will be processed first + sort(scheduledAircraft.begin(), scheduledAircraft.end(), compareSchedules); currAircraft = scheduledAircraft.begin(); currAircraftClosest = scheduledAircraft.begin(); //cerr << "Done initializing schedules" << endl; @@ -114,7 +129,7 @@ void FGTrafficManager::update(double something) //cerr << "resetting schedule " << endl; currAircraft = scheduledAircraft.begin(); } - if (!(currAircraft->update(now))) + if (!((*currAircraft)->update(now))) { // after proper initialization, we shouldnt get here. // But let's make sure @@ -232,7 +247,7 @@ void FGTrafficManager::endElement (const char * name) { string apt = fgGetString("/sim/presets/airport-id"); //cerr << "Airport information: " << apt << " " << departurePort << " " << arrivalPort << endl; if (departurePort == apt) score++; - flights.push_back(FGScheduledFlight(callsign, + flights.push_back(new FGScheduledFlight(callsign, fltrules, departurePort, arrivalPort, @@ -244,7 +259,7 @@ void FGTrafficManager::endElement (const char * name) { else if (element == string("aircraft")) { //cerr << "Pushing back aircraft " << registration << endl; - scheduledAircraft.push_back(FGAISchedule(mdl, + scheduledAircraft.push_back(new FGAISchedule(mdl, livery, registration, heavy, @@ -256,8 +271,14 @@ void FGTrafficManager::endElement (const char * name) { offset, score, flights)); - while(flights.begin() != flights.end()) - flights.pop_back(); + // while(flights.begin() != flights.end()) { +// flights.pop_back(); +// } + for (FGScheduledFlightVecIterator flt = flights.begin(); flt != flights.end(); flt++) + { + delete (*flt); + } + flights.clear(); SG_LOG( SG_GENERAL, SG_BULK, "Reading aircraft : " << registration << " with prioritization score " diff --git a/src/Traffic/TrafficMgr.hxx b/src/Traffic/TrafficMgr.hxx index 6c90effd8..20b1d0fdd 100644 --- a/src/Traffic/TrafficMgr.hxx +++ b/src/Traffic/TrafficMgr.hxx @@ -59,7 +59,7 @@ private: public: FGTrafficManager(); - + ~FGTrafficManager(); void init(); void update(double time); void release(int ref); -- 2.39.5