// along with this program; if not, write to the Free Software
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+#ifdef HAVE_CONFIG_H
+# include <config.h>
+#endif
+
+#include <simgear/misc/sg_path.hxx>
#include <simgear/math/sg_random.h>
+#include <simgear/debug/logstream.hxx>
+#include <simgear/misc/sgstream.hxx>
#include <simgear/constants.h>
+#include <Main/globals.hxx>
+
+#include <stdlib.h>
+#include STL_FSTREAM
#include "ground.hxx"
+#include "ATCmgr.hxx"
+#include "ATCutils.hxx"
+#include "ATCdisplay.hxx"
+#include "AILocalTraffic.hxx"
+
+SG_USING_STD(ifstream);
+SG_USING_STD(cout);
+
+node::node() {
+}
+
+node::~node() {
+ for(unsigned int i=0; i < arcs.size(); ++i) {
+ delete arcs[i];
+ }
+}
+
+// Make sure that a_path.cost += distance is safe from the moment it's created.
+a_path::a_path() {
+ cost = 0;
+}
FGGround::FGGround() {
- display = false;
+ ATCmgr = globals->get_ATC_mgr();
+ _type = GROUND;
+ networkLoadOK = false;
+ ground_traffic.erase(ground_traffic.begin(), ground_traffic.end());
+ ground_traffic_itr = ground_traffic.begin();
+
+ // Init the property nodes - TODO - need to make sure we're getting surface winds.
+ wind_from_hdg = fgGetNode("/environment/wind-from-heading-deg", true);
+ wind_speed_knots = fgGetNode("/environment/wind-speed-kt", true);
+
+ // TODO - get the actual airport elevation
+ aptElev = 0.0;
+}
+
+FGGround::FGGround(const string& id) {
+ ATCmgr = globals->get_ATC_mgr();
+ networkLoadOK = false;
+ ground_traffic.erase(ground_traffic.begin(), ground_traffic.end());
+ ground_traffic_itr = ground_traffic.begin();
+ ident = id;
+
+ // Init the property nodes - TODO - need to make sure we're getting surface winds.
+ wind_from_hdg = fgGetNode("/environment/wind-from-heading-deg", true);
+ wind_speed_knots = fgGetNode("/environment/wind-speed-kt", true);
+
+ // TODO - get the actual airport elevation
+ aptElev = 0.0;
}
FGGround::~FGGround() {
}
-void FGGround::Init() {
- display = false;
+void FGGround::ParseRwyExits(node* np, char* es) {
+ char* token;
+ char estr[20];
+ strcpy(estr, es);
+ const char delimiters[] = "-";
+ token = strtok(estr, delimiters);
+ while(token != NULL) {
+ int i = atoi(token);
+ //cout << "token = " << token << endl;
+ //cout << "rwy number = " << i << endl;
+ //runways[(atoi(token))].exits.push_back(np);
+ runways[i].exits.push_back(np);
+ //cout << "token = " << token << '\n';
+ token = strtok(NULL, delimiters);
+ }
+}
- // Build hardwired (very simplified) logical network for KEMT for now
- // Once it works we'll progress to reading KEMT data from file,
- // and finally to reading any airport with specified taxiway data from file.
+// Load the ground logical network of the current instances airport
+// Return true if successfull.
+// TODO - currently the file is assumed to reside in the base/ATC directory.
+// This might change to something more thought out in the future.
+// NOTE - currently it is assumed that all nodes are loaded before any arcs.
+// It won't work ATM if this doesn't hold true.
+bool FGGround::LoadNetwork() {
node* np;
arc* ap;
Gate* gp;
- // For now we'll hardwire the threshold end
- Point3D P010(-118.037483, 34.081358, 296 * SG_FEET_TO_METER);
- double hdg = 25.32;
- ortho.Init(P010, hdg);
-
- // HARDWIRED FOR TESTING - for now we'll only allow exit at each end of runway
-
- ///////////////////////////////////////////////////////
- // NODES
- ///////////////////////////////////////////////////////
-
- // node - runway01 threshold
- Point3D p1(-118.0372167, 34.08178333, 0.0);
- np = new node;
- np->struct_type = NODE;
- np->pos = p1;
- np->orthoPos = ortho.ConvertToLocal(p1);
- np->name = "rwy 01";
- np->nodeID = 0;
- np->type = JUNCTION;
- runways[1].exits.push_back(np);
- runways[19].exits.push_back(np);
- //np->max_turn_radius = ...
- network.push_back(np);
-
- // node - runway19 threshold
- Point3D p2(-118.0321833, 34.09066667, 0.0);
- np = new node;
- np->struct_type = NODE;
- np->pos = p2;
- np->orthoPos = ortho.ConvertToLocal(p2);
- np->name = "rwy 19";
- np->nodeID = 1;
- np->type = JUNCTION;
- runways[1].exits.push_back(np);
- runways[19].exits.push_back(np);
- //np->max_turn_radius = ...
- network.push_back(np);
-
- // node - AlphaSouth
- Point3D p3(-118.0369167, 34.08166667, 0.0);
- np = new node;
- np->struct_type = NODE;
- np->pos = p3;
- np->orthoPos = ortho.ConvertToLocal(p3);
- np->name = "";
- np->nodeID = 2;
- np->type = HOLD;
- //np->max_turn_radius = ...
- network.push_back(np);
-
- // node - AlphaNorth
- Point3D p4(-118.03185, 34.0906, 0.0);
- np = new node;
- np->struct_type = NODE;
- np->pos = p4;
- np->orthoPos = ortho.ConvertToLocal(p4);
- np->name = "";
- np->nodeID = 3;
- np->type = HOLD;
- //np->max_turn_radius = ...
- network.push_back(np);
-
- // node - southern turnoff to parking
- Point3D p5(-118.03515, 34.0848, 0.0);
- np = new node;
- np->struct_type = NODE;
- np->pos = p5;
- np->orthoPos = ortho.ConvertToLocal(p5);
- np->name = "";
- np->nodeID = 4;
- np->type = TJUNCTION;
- //np->max_turn_radius = ...
- network.push_back(np);
-
- // node - northern turnoff to parking
- Point3D p6(-118.0349667, 34.08511667, 0.0);
- np = new node;
- np->struct_type = NODE;
- np->pos = p6;
- np->orthoPos = ortho.ConvertToLocal(p6);
- np->name = "";
- np->nodeID = 5;
- np->type = TJUNCTION;
- //np->max_turn_radius = ...
- network.push_back(np);
-
- // GATES
-
- // node - Turn into gate 1 (Western-most gate, ie. nearest rwy)
- Point3D p7(-118.0348333, 34.08466667, 0.0);
- np = new node;
- np->struct_type = NODE;
- np->pos = p7;
- np->orthoPos = ortho.ConvertToLocal(p7);
- np->name = "";
- np->nodeID = 6;
- np->type = TJUNCTION;
- //np->max_turn_radius = ...
- network.push_back(np);
-
- // node - Gate 1
- Point3D p8(-118.0347333, 34.08483333, 0.0);
- gp = new Gate;
- gp->struct_type = NODE;
- gp->pos = p8;
- gp->orthoPos = ortho.ConvertToLocal(p8);
- gp->name = "";
- gp->nodeID = 7;
- gp->type = GATE;
- gp->heading = 10;
- //np->max_turn_radius = ...
- network.push_back(gp);
- gates[1] = *gp;
-
- // node - Turn out of gate 1
- Point3D p9(-118.03465, 34.08498333, 0.0);
- np = new node;
- np->struct_type = NODE;
- np->pos = p9;
- np->orthoPos = ortho.ConvertToLocal(p9);
- np->name = "";
- np->nodeID = 8;
- np->type = TJUNCTION;
- //np->max_turn_radius = ...
- network.push_back(np);
-
- // node - Turn into gate 2
- Point3D p10(-118.0346, 34.08456667, 0.0);
- np = new node;
- np->struct_type = NODE;
- np->pos = p10;
- np->orthoPos = ortho.ConvertToLocal(p10);
- np->name = "";
- np->nodeID = 9;
- np->type = TJUNCTION;
- //np->max_turn_radius = ...
- network.push_back(np);
- gates[2] = *gp;
-
- // node - Gate 2
- Point3D p11(-118.0345167, 34.08473333, 0.0);
- gp = new Gate;
- gp->struct_type = NODE;
- gp->pos = p11;
- gp->orthoPos = ortho.ConvertToLocal(p11);
- gp->name = "";
- gp->nodeID = 10;
- gp->type = GATE;
- gp->heading = 10;
- //np->max_turn_radius = ...
- network.push_back(gp);
-
- // node - Turn out of gate 2
- Point3D p12(-118.0344167, 34.0849, 0.0);
- np = new node;
- np->struct_type = NODE;
- np->pos = p12;
- np->orthoPos = ortho.ConvertToLocal(p12);
- np->name = "";
- np->nodeID = 11;
- np->type = TJUNCTION;
- //np->max_turn_radius = ...
- network.push_back(np);
-
- /////////////////////////////////////////////////////////
- // ARCS
- /////////////////////////////////////////////////////////
-
- // Each arc connects two nodes
- // Eventually the nodeID of the nodes that the arc connects will be read from file
- // For now we just 'know' them !!
-
- // arc - the runway - connects nodes 0 and 1
- ap = new arc;
- ap->struct_type = ARC;
- ap->name = "";
- ap->type = RUNWAY;
- ap->directed = false;
- ap->n1 = 0;
- ap->n2 = 1;
- network[0]->arcs.push_back(ap);
- network[1]->arcs.push_back(ap);
-
- // arc - the exit from 01 threshold to alpha - connects nodes 0 and 2
- ap = new arc;
- ap->struct_type = ARC;
- ap->name = "";
- ap->type = TAXIWAY;
- ap->directed = false;
- ap->n1 = 0;
- ap->n2 = 2;
- network[0]->arcs.push_back(ap);
- network[2]->arcs.push_back(ap);
-
- // arc - the exit from 19 threshold to alpha - connects nodes 1 and 3
- ap = new arc;
- ap->struct_type = ARC;
- ap->name = "";
- ap->type = TAXIWAY;
- ap->directed = false;
- ap->n1 = 1;
- ap->n2 = 3;
- network[1]->arcs.push_back(ap);
- network[3]->arcs.push_back(ap);
-
- // arc - Alpha south - connects nodes 2 and 4
- ap = new arc;
- ap->struct_type = ARC;
- ap->name = "";
- ap->type = TAXIWAY;
- ap->directed = false;
- ap->n1 = 2;
- ap->n2 = 4;
- network[2]->arcs.push_back(ap);
- network[4]->arcs.push_back(ap);
-
- // arc - Alpha middle - connects nodes 4 and 5
- ap = new arc;
- ap->struct_type = ARC;
- ap->name = "";
- ap->type = TAXIWAY;
- ap->directed = false;
- ap->n1 = 4;
- ap->n2 = 5;
- network[4]->arcs.push_back(ap);
- network[5]->arcs.push_back(ap);
-
- // arc - Alpha North - connects nodes 3 and 5
- ap = new arc;
- ap->struct_type = ARC;
- ap->name = "";
- ap->type = TAXIWAY;
- ap->directed = false;
- ap->n1 = 3;
- ap->n2 = 5;
- network[3]->arcs.push_back(ap);
- network[5]->arcs.push_back(ap);
-
- // arc - connects nodes 4 and 6
- ap = new arc;
- ap->struct_type = ARC;
- ap->name = "";
- ap->type = TAXIWAY;
- ap->directed = true;
- ap->n1 = 4;
- ap->n2 = 6;
- network[4]->arcs.push_back(ap);
- network[6]->arcs.push_back(ap);
-
- // arc - connects nodes 6 and 9
- ap = new arc;
- ap->struct_type = ARC;
- ap->name = "";
- ap->type = TAXIWAY;
- ap->directed = true;
- ap->n1 = 6;
- ap->n2 = 9;
- network[6]->arcs.push_back(ap);
- network[9]->arcs.push_back(ap);
-
- // arc - connects nodes 5 and 8
- ap = new arc;
- ap->struct_type = ARC;
- ap->name = "";
- ap->type = TAXIWAY;
- ap->directed = true;
- ap->n1 = 5;
- ap->n2 = 8;
- network[5]->arcs.push_back(ap);
- network[8]->arcs.push_back(ap);
-
- // arc - connects nodes 8 and 11
- ap = new arc;
- ap->struct_type = ARC;
- ap->name = "";
- ap->type = TAXIWAY;
- ap->directed = true;
- ap->n1 = 8;
- ap->n2 = 11;
- network[8]->arcs.push_back(ap);
- network[11]->arcs.push_back(ap);
-
- // arc - connects nodes 6 and 7
- ap = new arc;
- ap->struct_type = ARC;
- ap->name = "";
- ap->type = TAXIWAY;
- ap->directed = true;
- ap->n1 = 6;
- ap->n2 = 7;
- network[6]->arcs.push_back(ap);
- network[7]->arcs.push_back(ap);
-
- // arc - connects nodes 7 and 8
- ap = new arc;
- ap->struct_type = ARC;
- ap->name = "";
- ap->type = TAXIWAY;
- ap->directed = true;
- ap->n1 = 7;
- ap->n2 = 8;
- network[7]->arcs.push_back(ap);
- network[8]->arcs.push_back(ap);
-
- // arc - connects nodes 9 and 10
- ap = new arc;
- ap->struct_type = ARC;
- ap->name = "";
- ap->type = TAXIWAY;
- ap->directed = true;
- ap->n1 = 9;
- ap->n2 = 10;
- network[9]->arcs.push_back(ap);
- network[10]->arcs.push_back(ap);
-
- // arc - connects nodes 10 and 11
- ap = new arc;
- ap->struct_type = ARC;
- ap->name = "";
- ap->type = TAXIWAY;
- ap->directed = true;
- ap->n1 = 10;
- ap->n2 = 11;
- network[10]->arcs.push_back(ap);
- network[11]->arcs.push_back(ap);
+ int gateCount = 0; // This is used to allocate gateID's from zero upwards
+ // This may well change in the future - probably to reading in the real-world
+ // gate numbers from file.
+
+ ifstream fin;
+ SGPath path = globals->get_fg_root();
+ //string taxiPath = "ATC/" + ident + ".taxi";
+ string taxiPath = "ATC/KEMT.taxi"; // FIXME - HARDWIRED FOR TESTING
+ path.append(taxiPath);
+
+ SG_LOG(SG_ATC, SG_INFO, "Trying to read taxiway data for " << ident << "...");
+ //cout << "Trying to read taxiway data for " << ident << "..." << endl;
+ fin.open(path.c_str(), ios::in);
+ if(!fin) {
+ SG_LOG(SG_ATC, SG_ALERT, "Unable to open taxiway data input file " << path.c_str());
+ //cout << "Unable to open taxiway data input file " << path.c_str() << endl;
+ return(false);
+ }
+
+ char ch;
+ char buf[30];
+ while(!fin.eof()) {
+ fin >> buf;
+ // Node, arc, or [End]?
+ //cout << "Read in ground network element type = " << buf << endl;
+ if(!strcmp(buf, "[End]")) { // TODO - maybe make this more robust to spelling errors by just looking for '['
+ SG_LOG(SG_ATC, SG_INFO, "Done reading " << path.c_str() << endl);
+ break;
+ } else if(!strcmp(buf, "N")) {
+ // Node
+ np = new node;
+ np->struct_type = NODE;
+ fin >> buf;
+ np->nodeID = atoi(buf);
+ fin >> buf;
+ np->pos.setlon(atof(buf));
+ fin >> buf;
+ np->pos.setlat(atof(buf));
+ fin >> buf;
+ np->pos.setelev(atof(buf));
+ fin >> buf; // node type
+ if(!strcmp(buf, "J")) {
+ np->type = JUNCTION;
+ } else if(!strcmp(buf, "T")) {
+ np->type = TJUNCTION;
+ } else if(!strcmp(buf, "H")) {
+ np->type = HOLD;
+ } else {
+ SG_LOG(SG_ATC, SG_ALERT, "**** ERROR ***** Unknown node type in taxi network...\n");
+ delete np;
+ return(false);
+ }
+ fin >> buf; // rwy exit information - gets parsed later - FRAGILE - will break if buf is reused.
+ // Now the name
+ fin >> ch; // strip the leading " off
+ np->name = "";
+ while(1) {
+ fin.unsetf(ios::skipws);
+ fin >> ch;
+ if((ch == '"') || (ch == 0x0A)) {
+ break;
+ } // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the "
+ np->name += ch;
+ }
+ fin.setf(ios::skipws);
+ network.push_back(np);
+ // FIXME - fragile - replies on buf not getting modified from exits read to here
+ // see if we also need to push it onto the runway exit list
+ //cout << "strlen(buf) = " << strlen(buf) << endl;
+ if(strlen(buf) > 2) {
+ //cout << "Calling ParseRwyExits for " << buf << endl;
+ ParseRwyExits(np, buf);
+ }
+ } else if(!strcmp(buf, "A")) {
+ ap = new arc;
+ ap->struct_type = ARC;
+ fin >> buf;
+ ap->n1 = atoi(buf);
+ fin >> buf;
+ ap->n2 = atoi(buf);
+ fin >> buf;
+ if(!strcmp(buf, "R")) {
+ ap->type = RUNWAY;
+ } else if(!strcmp(buf, "T")) {
+ ap->type = TAXIWAY;
+ } else {
+ SG_LOG(SG_ATC, SG_ALERT, "**** ERROR ***** Unknown arc type in taxi network...\n");
+ delete ap;
+ return(false);
+ }
+ // directed?
+ fin >> buf;
+ if(!strcmp(buf, "Y")) {
+ ap->directed = true;
+ } else if(!strcmp(buf, "N")) {
+ ap->directed = false;
+ } else {
+ SG_LOG(SG_ATC, SG_ALERT, "**** ERROR ***** Unknown arc directed value in taxi network - should be Y/N !!!\n");
+ delete ap;
+ return(false);
+ }
+ // Now the name
+ ap->name = "";
+ while(1) {
+ fin.unsetf(ios::skipws);
+ fin >> ch;
+ ap->name += ch;
+ if((ch == '"') || (ch == 0x0A)) {
+ break;
+ } // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the "
+ }
+ fin.setf(ios::skipws);
+ ap->distance = (int)dclGetHorizontalSeparation(network[ap->n1]->pos, network[ap->n2]->pos);
+ //cout << "Distance = " << ap->distance << '\n';
+ network[ap->n1]->arcs.push_back(ap);
+ network[ap->n2]->arcs.push_back(ap);
+ } else if(!strcmp(buf, "G")) {
+ gp = new Gate;
+ gp->struct_type = NODE;
+ gp->type = GATE;
+ fin >> buf;
+ gp->nodeID = atoi(buf);
+ fin >> buf;
+ gp->pos.setlon(atof(buf));
+ fin >> buf;
+ gp->pos.setlat(atof(buf));
+ fin >> buf;
+ gp->pos.setelev(atof(buf));
+ fin >> buf; // gate type - ignore this for now
+ fin >> buf; // gate heading
+ gp->heading = atoi(buf);
+ // Now the name
+ gp->name = "";
+ while(1) {
+ fin.unsetf(ios::skipws);
+ fin >> ch;
+ gp->name += ch;
+ if((ch == '"') || (ch == 0x0A)) {
+ break;
+ } // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the "
+ }
+ fin.setf(ios::skipws);
+ gp->id = gateCount; // Warning - this will likely change in the future.
+ gp->used = false;
+ network.push_back(gp);
+ gates[gateCount] = gp;
+ gateCount++;
+ } else {
+ // Something has gone seriously pear-shaped
+ SG_LOG(SG_ATC, SG_ALERT, "********* ERROR - unknown ground network element type... aborting read of " << path.c_str() << '\n');
+ return(false);
+ }
+
+ fin >> skipeol;
+ }
+ return(true);
}
-void FGGround::Update() {
+void FGGround::Init() {
+ untowered = false;
+
+ // Figure out which is the active runway - TODO - it would be better to have ground call tower
+ // for runway operation details, but at the moment we can't guarantee that tower control at a given airport
+ // will be initialised before ground so we can't do that.
+ DoRwyDetails();
+ //cout << "In FGGround::Init, active rwy is " << activeRwy << '\n';
+ ortho.Init(rwy.threshold_pos, rwy.hdg);
+
+ networkLoadOK = LoadNetwork();
+}
+
+void FGGround::Update(double dt) {
// Each time step, what do we need to do?
// We need to go through the list of outstanding requests and acknowedgements
// and process at least one of them.
// Lets take the example of a plane which has just contacted ground
// following landing - presumably requesting where to go?
// First we need to establish the position of the plane within the logical network.
- // Next we need to decide where its going.
+ // Next we need to decide where its going.
+
+ if(ground_traffic.size()) {
+ if(ground_traffic_itr == ground_traffic.end()) {
+ ground_traffic_itr = ground_traffic.begin();
+ }
+
+ //Process(*ground_traffic_itr);
+ GroundRec* g = *ground_traffic_itr;
+ if(g->taxiRequestOutstanding) {
+ double responseTime = 10.0; // seconds - this should get more sophisticated at some point
+ if(g->clearanceCounter > responseTime) {
+ // DO CLEARANCE
+ // TODO - move the mechanics of making up the transmission out of the main Update(...) routine.
+ string trns = "";
+ trns += g->plane.callsign;
+ trns += " taxi holding point runway "; // TODO - add the holding point name
+ // eg " taxi holding point G2 runway "
+ trns += ConvertRwyNumToSpokenString(activeRwy);
+ if(_display) {
+ fgSetString("/sim/messages/ground", trns.c_str());
+ globals->get_ATC_display()->RegisterSingleMessage(trns, 0);
+ }
+ g->planePtr->RegisterTransmission(1); // cleared to taxi
+ g->clearanceCounter = 0.0;
+ g->taxiRequestOutstanding = false;
+ } else {
+ g->clearanceCounter += (dt * ground_traffic.size());
+ }
+ } else if(((FGAILocalTraffic*)(g->planePtr))->AtHoldShort()) { // That's a hack - eventually we should monitor actual position
+ // HACK ALERT - the automatic cast to AILocalTraffic has to go once we have other sorts working!!!!! FIXME TODO
+ // NOTE - we don't need to do the contact tower bit unless we have separate tower and ground
+ string trns = g->plane.callsign;
+ trns += " contact Tower ";
+ double f = globals->get_ATC_mgr()->GetFrequency(ident, TOWER) / 100.0;
+ char buf[10];
+ sprintf(buf, "%.2f", f);
+ trns += buf;
+ if(_display) {
+ fgSetString("/sim/messages/ground", trns.c_str());
+ globals->get_ATC_display()->RegisterSingleMessage(trns, 0);
+ }
+ g->planePtr->RegisterTransmission(2); // contact tower
+ delete *ground_traffic_itr;
+ ground_traffic.erase(ground_traffic_itr);
+ ground_traffic_itr = ground_traffic.begin();
+ }
+ ++ground_traffic_itr;
+ }
+
+ // Call the base class update for the response time handling.
+ FGATC::Update(dt);
+}
+
+// Figure out which runways are active.
+// For now we'll just be simple and do one active runway - eventually this will get much more complex
+// Copied from FGTower - TODO - it would be better to implement this just once, and have ground call tower
+// for runway operation details, but at the moment we can't guarantee that tower control at a given airport
+// will be initialised before ground so we can't do that.
+void FGGround::DoRwyDetails() {
+ //cout << "GetRwyDetails called" << endl;
+
+ // Based on the airport-id and wind get the active runway
+
+ //wind
+ double hdg = wind_from_hdg->getDoubleValue();
+ double speed = wind_speed_knots->getDoubleValue();
+ hdg = (speed == 0.0 ? 270.0 : hdg);
+ //cout << "Heading = " << hdg << '\n';
+
+ FGRunway runway;
+ bool rwyGood = globals->get_runways()->search(ident, int(hdg), &runway);
+ if(rwyGood) {
+ activeRwy = runway._rwy_no;
+ rwy.rwyID = runway._rwy_no;
+ SG_LOG(SG_ATC, SG_INFO, "In FGGround, active runway for airport " << ident << " is " << activeRwy);
+
+ // Get the threshold position
+ double other_way = runway._heading - 180.0;
+ while(other_way <= 0.0) {
+ other_way += 360.0;
+ }
+ // move to the +l end/center of the runway
+ //cout << "Runway center is at " << runway._lon << ", " << runway._lat << '\n';
+ Point3D origin = Point3D(runway._lon, runway._lat, aptElev);
+ Point3D ref = origin;
+ double tshlon, tshlat, tshr;
+ double tolon, tolat, tor;
+ rwy.length = runway._length * SG_FEET_TO_METER;
+ geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), other_way,
+ rwy.length / 2.0 - 25.0, &tshlat, &tshlon, &tshr );
+ geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), runway._heading,
+ rwy.length / 2.0 - 25.0, &tolat, &tolon, &tor );
+ // Note - 25 meters in from the runway end is a bit of a hack to put the plane ahead of the user.
+ // now copy what we need out of runway into rwy
+ rwy.threshold_pos = Point3D(tshlon, tshlat, aptElev);
+ Point3D takeoff_end = Point3D(tolon, tolat, aptElev);
+ //cout << "Threshold position = " << tshlon << ", " << tshlat << ", " << aptElev << '\n';
+ //cout << "Takeoff position = " << tolon << ", " << tolat << ", " << aptElev << '\n';
+ rwy.hdg = runway._heading;
+ // Set the projection for the local area based on this active runway
+ ortho.Init(rwy.threshold_pos, rwy.hdg);
+ rwy.end1ortho = ortho.ConvertToLocal(rwy.threshold_pos); // should come out as zero
+ rwy.end2ortho = ortho.ConvertToLocal(takeoff_end);
+ } else {
+ SG_LOG(SG_ATC, SG_ALERT, "Help - can't get good runway in FGTower!!");
+ activeRwy = "NN";
+ }
}
-// FIXME - at the moment this assumes there is at least one gate and crashes if none
-// FIXME - In fact, at the moment this routine doesn't work at all and hence is munged to always return Gate 1 !!!!
+// Return a random gate ID of an unused gate.
+// Two error values may be returned and must be checked for by the calling function:
+// -2 signifies that no gates exist at this airport.
+// -1 signifies that all gates are currently full.
int FGGround::GetRandomGateID() {
- //cout << "GetRandomGateID called" << endl;
- return(1);
+ // Check that this airport actually has some gates!!
+ if(!gates.size()) {
+ return(-2);
+ }
gate_vec_type gateVec;
- //gate_vec_iterator gateVecItr;
int num = 0;
int thenum;
int ID;
gatesItr = gates.begin();
while(gatesItr != gates.end()) {
- if(gatesItr->second.used == false) {
+ if((gatesItr->second)->used == false) {
gateVec.push_back(gatesItr->second);
num++;
}
++gatesItr;
}
+
+ // Check that there are some unused gates!
+ if(!gateVec.size()) {
+ return(-1);
+ }
// Randomly select one from the list
+ sg_srandom_time();
thenum = (int)(sg_random() * gateVec.size());
- ID = gateVec[thenum].id;
- //cout << "Returning gate ID " << ID << " from GetRandomGateID" << endl;
+ ID = gateVec[thenum]->id;
+
return(ID);
}
-// Return a pointer to a gate node based on the gate ID
-Gate* FGGround::GetGateNode(int gateID) {
- //TODO - ought to add some sanity checking here - ie does a gate of this ID exist?!
- return(&(gates[gateID]));
+// Return a pointer to an unused gate node
+Gate* FGGround::GetGateNode() {
+ int id = GetRandomGateID();
+ if(id < 0) {
+ return(NULL);
+ } else {
+ return(gates[id]);
+ }
+}
+
+
+node* FGGround::GetHoldShortNode(const string& rwyID) {
+ return(NULL); // TODO - either implement me or remove me!!!
+}
+
+
+// WARNING - This is hardwired to my prototype logical network format
+// and will almost certainly change when Bernie's stuff comes on-line.
+// Returns NULL if it can't find a valid node.
+node* FGGround::GetThresholdNode(const string& rwyID) {
+ // For now go through all the nodes and parse their names
+ // Maybe in the future we'll map threshold nodes by ID
+ //cout << "Size of network is " << network.size() << '\n';
+ for(unsigned int i=0; i<network.size(); ++i) {
+ //cout << "Name = " << network[i]->name << '\n';
+ if(network[i]->name.size()) {
+ string s = network[i]->name;
+ // Warning - the next bit is fragile and dependent on my current naming scheme
+ //cout << "substr = " << s.substr(0,3) << '\n';
+ //cout << "size of s = " << s.size() << '\n';
+ if(s.substr(0,3) == "rwy") {
+ //cout << "subsubstr = " << s.substr(4, s.size() - 4) << '\n';
+ if(s.substr(4, s.size() - 4) == rwyID) {
+ return network[i];
+ }
+ }
+ }
+ }
+ return NULL;
}
+
// Get a path from a point on a runway to a gate
+// TODO !!
// Get a path from a node to another node
// Eventually we will need complex algorithms for this taking other traffic,
-// shortest path and suitable paths into accout. For now we're going to hardwire for KEMT!!!!
-ground_network_path_type FGGround::GetPath(node* A, node* B) {
- ground_network_path_type path;
- //arc_array_iterator arcItr;
- //bool found;
-
- // VERY HARDWIRED - this hardwires a path from the far end of R01 to Gate 1.
- // In fact in real life the area between R01/19 and Taxiway Alpha at KEMT is tarmaced and planes
- // are supposed to exit the rwy asap.
- // OK - for now very hardwire this for testing
- path.push_back(network[1]);
- path.push_back(network[1]->arcs[1]); // ONLY BECAUSE WE KNOW THIS IS THE ONE !!!!!
- path.push_back(network[3]);
- path.push_back(network[3]->arcs[1]);
- path.push_back(network[5]);
- path.push_back(network[5]->arcs[0]);
- path.push_back(network[4]);
- path.push_back(network[4]->arcs[2]);
- path.push_back(network[6]);
- path.push_back(network[6]->arcs[2]);
- path.push_back(network[7]); // THE GATE!! Note that for now we're not even looking at the requested exit and gate passed in !!!!!
-
-#if 0
- // In this hardwired scheme there are two possibilities - taxiing from rwy to gate or gate to rwy.
- if(B->type == GATE) {
- //return an inward path
- path.push_back(A);
- // In this hardwired scheme we know A is a rwy exit and should have one taxiway arc only
- // THIS WILL NOT HOLD TRUE IN THE GENERAL CASE
- arcItr = A->arcs.begin();
- found = false;
- while(arcItr != A->arcs.end()) {
- if(arcItr->type == TAXIWAY) {
- path.push_back(&(*arcItr));
- found = true;
- break;
- }
- }
- if(found == false) {
- //cout << "AI/ATC SUBSYSTEM ERROR - no taxiway from runway exit in airport.cxx" << endl;
+// shortest path and suitable paths into accout.
+// For now we'll just call the shortest path algorithm.
+ground_network_path_type FGGround::GetPath(node* A, node* B) {
+ return(GetShortestPath(A, B));
+};
+
+// Get a path from a node to a runway threshold
+ground_network_path_type FGGround::GetPath(node* A, const string& rwyID) {
+ node* b = GetThresholdNode(rwyID);
+ if(b == NULL) {
+ SG_LOG(SG_ATC, SG_ALERT, "ERROR - unable to find path to runway theshold in ground.cxx for airport " << ident << '\n');
+ ground_network_path_type emptyPath;
+ emptyPath.erase(emptyPath.begin(), emptyPath.end());
+ return(emptyPath);
+ }
+ return GetShortestPath(A, b);
+}
+
+// Get a path from a node to a runway hold short point
+// Bit of a hack this at the moment!
+ground_network_path_type FGGround::GetPathToHoldShort(node* A, const string& rwyID) {
+ ground_network_path_type path = GetPath(A, rwyID);
+ path.pop_back(); // That should be the threshold stripped of
+ path.pop_back(); // and that should be the arc from hold short to threshold
+ // This isn't robust though - TODO - implement properly!
+ return(path);
+}
+
+// A shortest path algorithm from memory (ie. I can't find the bl&*dy book again!)
+// I'm sure there must be enchancements that we can make to this, such as biasing the
+// order in which the nodes are searched out from in favour of those geographically
+// closer to the destination.
+// Note that we are working with the master set of nodes and arcs so we mustn't change
+// or delete them - we only delete the paths that we create during the algorithm.
+ground_network_path_type FGGround::GetShortestPath(node* A, node* B) {
+ a_path* pathPtr;
+ shortest_path_map_type pathMap;
+ node_array_type nodesLeft;
+
+ // Debugging check
+ int pathsCreated = 0;
+
+ // Initialise the algorithm
+ nodesLeft.push_back(A);
+ pathPtr = new a_path;
+ pathsCreated++;
+ pathPtr->path.push_back(A);
+ pathPtr->cost = 0;
+ pathMap[A->nodeID] = pathPtr;
+ bool solution_found = false; // Flag to indicate that at least one candidate path has been found
+ int solution_cost = -1; // Cost of current best cost solution. -1 indicates no solution found yet.
+ a_path solution_path;
+
+ node* nPtr; // nPtr is used to point to the node we are currently working with
+
+ while(nodesLeft.size()) {
+ //cout << "\n*****nodesLeft*****\n";
+ //for(unsigned int i=0; i<nodesLeft.size(); ++i) {
+ //cout << nodesLeft[i]->nodeID << '\n';
+ //}
+ //cout << "*******************\n\n";
+ nPtr = *nodesLeft.begin(); // Thought - definate optimization possibilities here in the choice of which nodes we process first.
+ nodesLeft.erase(nodesLeft.begin());
+ //cout << "Walking out from node " << nPtr->nodeID << '\n';
+ for(unsigned int i=0; i<nPtr->arcs.size(); ++i) {
+ //cout << "ARC TO " << ((nPtr->arcs[i]->n1 == nPtr->nodeID) ? nPtr->arcs[i]->n2 : nPtr->arcs[i]->n1) << '\n';
}
- // Then push back the start of taxiway node
- // Then push back the taxiway arc
- arcItr = A->arcs.begin();
- found = false;
- while(arcItr != A->arcs.end()) {
- if(arcItr->type == TAXIWAY) { // FIXME - OOPS - two taxiways go off this node
- // How are we going to differentiate, apart from one called Alpha.
- // I suppose eventually the traversal algorithms will select.
- path.push_back(&(*arcItr));
- found = true;
- break;
- }
+ if((solution_found) && (solution_cost <= pathMap[nPtr->nodeID]->cost)) {
+ // Do nothing - we've already found a solution and this partial path is already more expensive
+ } else {
+ // This path could still be better than the current solution - check it out
+ for(unsigned int i=0; i<(nPtr->arcs.size()); i++) {
+ // Map the new path against the end node, ie. *not* the one we just started with.
+ unsigned int end_nodeID = ((nPtr->arcs[i]->n1 == nPtr->nodeID) ? nPtr->arcs[i]->n2 : nPtr->arcs[i]->n1);
+ //cout << "end_nodeID = " << end_nodeID << '\n';
+ //cout << "pathMap size is " << pathMap.size() << '\n';
+ if(end_nodeID == nPtr->nodeID) {
+ //cout << "Circular arc!\n";
+ // Then its a circular arc - don't bother!!
+ //nPtr->arcs.erase(nPtr->arcs.begin() + i);
+ } else {
+ // see if the end node is already in the map or not
+ if(pathMap.find(end_nodeID) == pathMap.end()) {
+ //cout << "Not in the map" << endl;;
+ // Not in the map - easy!
+ pathPtr = new a_path;
+ pathsCreated++;
+ *pathPtr = *pathMap[nPtr->nodeID]; // *copy* the path
+ pathPtr->path.push_back(nPtr->arcs[i]);
+ pathPtr->path.push_back(network[end_nodeID]);
+ pathPtr->cost += nPtr->arcs[i]->distance;
+ pathMap[end_nodeID] = pathPtr;
+ nodesLeft.push_back(network[end_nodeID]); // By definition this can't be in the list already, or
+ // it would also have been in the map and hence OR'd with this one.
+ if(end_nodeID == B->nodeID) {
+ //cout << "Solution found!!!" << endl;
+ // Since this node wasn't in the map this is by definition the first solution
+ solution_cost = pathPtr->cost;
+ solution_path = *pathPtr;
+ solution_found = true;
+ }
+ } else {
+ //cout << "Already in the map" << endl;
+ // In the map - not so easy - need to get rid of an arc from the higher cost one.
+ //cout << "Current cost of node " << end_nodeID << " is " << pathMap[end_nodeID]->cost << endl;
+ int newCost = pathMap[nPtr->nodeID]->cost + nPtr->arcs[i]->distance;
+ //cout << "New cost is of node " << nPtr->nodeID << " is " << newCost << endl;
+ if(newCost >= pathMap[end_nodeID]->cost) {
+ // No need to do anything.
+ //cout << "Not doing anything!" << endl;
+ } else {
+ delete pathMap[end_nodeID];
+ pathsCreated--;
+
+ pathPtr = new a_path;
+ pathsCreated++;
+ *pathPtr = *pathMap[nPtr->nodeID]; // *copy* the path
+ pathPtr->path.push_back(nPtr->arcs[i]);
+ pathPtr->path.push_back(network[end_nodeID]);
+ pathPtr->cost += nPtr->arcs[i]->distance;
+ pathMap[end_nodeID] = pathPtr;
+
+ // We need to add this node to the list-to-do again to force a recalculation
+ // onwards from this node with the new lower cost to node cost.
+ nodesLeft.push_back(network[end_nodeID]);
+
+ if(end_nodeID == B->nodeID) {
+ //cout << "Solution found!!!" << endl;
+ // Need to check if there is a previous better solution
+ if((solution_cost < 0) || (pathPtr->cost < solution_cost)) {
+ solution_cost = pathPtr->cost;
+ solution_path = *pathPtr;
+ solution_found = true;
+ }
+ }
+ }
+ }
+ }
+ }
}
- if(found == false) {
- //cout << "AI/ATC SUBSYSTEM ERROR - no taxiway from runway exit in airport.cxx" << endl;
+ }
+
+ // delete all the paths before returning
+ shortest_path_map_iterator spItr = pathMap.begin();
+ while(spItr != pathMap.end()) {
+ if(spItr->second != NULL) {
+ delete spItr->second;
+ --pathsCreated;
}
- // Then push back the junction node
- // Planes always face one way in the parking, so depending on which parking exit we have either take it or push back another taxiway node
- // Repeat if necessary
- // Then push back the gate B
- path.push_back(B);
- } else {
- //return an outward path
- }
-
- // WARNING TODO FIXME - this is VERY FRAGILE - eg taxi to apron!!! but should be enough to
- // see an AI plane physically taxi.
-#endif // 0
+ ++spItr;
+ }
- return(path);
-};
+ //cout << "pathsCreated = " << pathsCreated << '\n';
+ if(pathsCreated > 0) {
+ SG_LOG(SG_ATC, SG_ALERT, "WARNING - Possible memory leak in FGGround::GetShortestPath\n\
+ Please report to flightgear-devel@flightgear.org\n");
+ }
+
+ //cout << (solution_found ? "Result: solution found\n" : "Result: no solution found\n");
+ return(solution_path.path); // TODO - we really ought to have a fallback position incase a solution isn't found.
+}
+
// Randomly or otherwise populate some of the gates with parked planes
// (might eventually be done by the AIMgr if and when lots of AI traffic is generated)
// Return a list of exits from a given runway
-node_array_type FGGround::GetExits(int rwyID) {
- return(runways[rwyID].exits);
+// It is up to the calling function to check for non-zero size of returned array before use
+node_array_type FGGround::GetExits(const string& rwyID) {
+ // FIXME - get a 07L or similar in here and we're stuffed!!!
+ return(runways[atoi(rwyID.c_str())].exits);
+}
+
+void FGGround::RequestDeparture(const PlaneRec& plane, FGAIEntity* requestee) {
+ // For now we'll just automatically clear all planes to the runway hold.
+ // This communication needs to be delayed 20 sec or so from receiving the request.
+ // Even if display=false we still need to start the timer in case display=true when communication starts.
+ // We also need to bear in mind we also might have other outstanding communications, although for now we'll punt that issue!
+ // FIXME - sort the above!
+
+ // HACK - assume that anything requesting departure is new for now - FIXME LATER
+ GroundRec* g = new GroundRec;
+ g->plane = plane;
+ g->planePtr = requestee;
+ g->taxiRequestOutstanding = true;
+ g->clearanceCounter = 0;
+ g->cleared = false;
+ g->incoming = false;
+ // TODO - need to handle the next 3 as well
+ //Point3D current_pos;
+ //node* destination;
+ //node* last_clearance;
+
+ ground_traffic.push_back(g);
}
+
#if 0
void FGGround::NewArrival(plane_rec plane) {
// What are we going to do here?
// we'll hardwire the gate!
// In the long run the logic of which gate or area to send the plane to could be somewhat non-trivial.
}
-#endif //0
\ No newline at end of file
+#endif //0
+
projects / flightgear.git / blobdiff