--- /dev/null
+// 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 <config.h>
+#endif
+
+#include "trafficcontrol.hxx"
+#include <AIModel/AIFlightPlan.hxx>
+
+
+/***************************************************************************
+ * 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 <<endl;
+ return currentTargetNode;
+ }
+ }
+ }
+ }
+ if (other.intentions.size())
+ {
+ for (i = other.intentions.begin(); i != other.intentions.end(); i++)
+ {
+ if ((*i) > 0) {
+ if (otherTargetNode == net->findSegment(*i)->getEnd()->getIndex())
+ {
+ cerr << "Other crosses at " << currentTargetNode <<endl;
+ return otherTargetNode;
+ }
+ }
+ }
+ }
+ if (intentions.size() && other.intentions.size())
+ {
+ for (i = intentions.begin(); i != intentions.end(); i++)
+ {
+ for (j = other.intentions.begin(); j != other.intentions.end(); j++)
+ {
+ //cerr << "finding segment " << *i << " and " << *j << endl;
+ if (((*i) > 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();
+}
+
--- /dev/null
+// 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 <simgear/compiler.h>
+#include <simgear/debug/logstream.hxx>
+
+
+#include STL_STRING
+#include <vector>
+
+SG_USING_STD(string);
+SG_USING_STD(vector);
+
+
+typedef vector<int> intVec;
+typedef vector<int>::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<FGTrafficRecord> TrafficVector;
+typedef vector<FGTrafficRecord>::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<ActiveRunway> ActiveRunwayVec;
+typedef vector<ActiveRunway>::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
projects / flightgear.git / commitdiff