+++ /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
- **************************************************************************/
-FGTrafficRecord::FGTrafficRecord() :
- id(0), waitsForId(0),
- currentPos(0),
- leg(0),
- latitude(0),
- longitude(0),
- heading(0),
- speed(0),
- altitude(0),
- radius(0) {
-}
-
-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);
- //cerr << val<< " ";
- if ((val) && (val != pos))
- {
- intentions.push_back(val);
- //cerr << "[set] ";
- }
- }
- //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 << callsign << ": Check Position and intentions: we are on the same taxiway" << other.callsign << "Index = " << currentPos << 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 << callsign << ": Check Position and intentions: .other.current matches" << other.callsign << "Index = " << (*i) << 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::onRoute(FGGroundNetwork *net, FGTrafficRecord &other)
-{
- int node = -1, othernode = -1;
- if (currentPos >0)
- node = net->findSegment(currentPos)->getEnd()->getIndex();
- if (other.currentPos > 0)
- othernode = net->findSegment(other.currentPos)->getEnd()->getIndex();
- if ((node == othernode) && (node != -1))
- return true;
- if (other.intentions.size())
- {
- for (intVecIterator i = other.intentions.begin(); i != other.intentions.end(); i++)
- {
- if (*i > 0)
- {
- othernode = net->findSegment(*i)->getEnd()->getIndex();
- if ((node == othernode) && (node > -1))
- return true;
- }
- }
- }
- //if (other.currentPos > 0)
- // othernode = net->findSegment(other.currentPos)->getEnd()->getIndex();
- //if (intentions.size())
- // {
- // for (intVecIterator i = intentions.begin(); i != intentions.end(); i++)
- // {
- // if (*i > 0)
- // {
- // node = net->findSegment(*i)->getEnd()->getIndex();
- // if ((node == othernode) && (node > -1))
- // return true;
- // }
- // }
- // }
- return false;
-}
-
-
-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 (opp = net->findSegment(other.currentPos)->opposite())
- {
- if ((*i) > 0)
- if (opp->getIndex() == net->findSegment(*i)->getIndex())
- {
- if (net->findSegment(*i)->getStart()->getIndex() == node) {
- {
- //cerr << "Found the node " << node << endl;
- return true;
- }
- }
- }
- }
- 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 " << 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;
- resolveCircularWait = false;
-
- double speed = 0;
- double heading = 0;
- double alt = 0;
-}
-
-bool FGATCInstruction::hasInstruction()
-{
- return (holdPattern || holdPosition || changeSpeed || changeHeading || changeAltitude || resolveCircularWait);
-}
-
-
-
-/***************************************************************************
- * 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,
- string callsign)
-{
- 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);
- rec.setCallSign(callsign);
- 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;
- bool resolveCircularWait;
-
- 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; };
-
- bool getCheckForCircularWait() { return resolveCircularWait; };
-
- 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 setResolveCircularWait (bool val) { resolveCircularWait = 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,
- string callsign) = 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;
- string callsign;
-
-
-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 onRoute(FGGroundNetwork *, FGTrafficRecord &other);
-
- 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; };
-
- void setResolveCircularWait() { instruction.setResolveCircularWait(true); };
- void clearResolveCircularWait() { instruction.setResolveCircularWait(false); };
-
- string getRunway() { return runway; };
- void setCallSign(string clsgn) { callsign = clsgn; };
- string getCallSign() { return callsign; };
-};
-
-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,
- string callsign);
- 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