1 // FGTower - a class to provide tower control at towered airports.
3 // Written by David Luff, started March 2002.
5 // Copyright (C) 2002 David C. Luff - david.luff@nottingham.ac.uk
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Public License for more details.
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
21 #include <Main/globals.hxx>
22 #include <Airports/runways.hxx>
23 #include <simgear/math/sg_geodesy.hxx>
24 #include <simgear/debug/logstream.hxx>
27 #include "ATCdisplay.hxx"
29 #include "ATCutils.hxx"
30 #include "commlist.hxx"
31 #include "AILocalTraffic.hxx"
37 TowerPlaneRec::TowerPlaneRec() :
39 clearedToLineUp(false),
40 clearedToTakeOff(false),
41 holdShortReported(false),
42 longFinalReported(false),
43 longFinalAcknowledged(false),
45 finalAcknowledged(false),
50 plane.callsign = "UNKNOWN";
53 TowerPlaneRec::TowerPlaneRec(PlaneRec p) :
55 clearedToLineUp(false),
56 clearedToTakeOff(false),
57 holdShortReported(false),
58 longFinalReported(false),
59 longFinalAcknowledged(false),
61 finalAcknowledged(false),
69 TowerPlaneRec::TowerPlaneRec(Point3D pt) :
71 clearedToLineUp(false),
72 clearedToTakeOff(false),
73 holdShortReported(false),
74 longFinalReported(false),
75 longFinalAcknowledged(false),
77 finalAcknowledged(false),
82 plane.callsign = "UNKNOWN";
86 TowerPlaneRec::TowerPlaneRec(PlaneRec p, Point3D pt) :
88 clearedToLineUp(false),
89 clearedToTakeOff(false),
90 holdShortReported(false),
91 longFinalReported(false),
92 longFinalAcknowledged(false),
94 finalAcknowledged(false),
107 ATCmgr = globals->get_ATC_mgr();
109 // Init the property nodes - TODO - need to make sure we're getting surface winds.
110 wind_from_hdg = fgGetNode("/environment/wind-from-heading-deg", true);
111 wind_speed_knots = fgGetNode("/environment/wind-speed-kt", true);
113 holdListItr = holdList.begin();
114 appListItr = appList.begin();
115 depListItr = depList.begin();
116 rwyListItr = rwyList.begin();
117 circuitListItr = circuitList.begin();
118 trafficListItr = trafficList.begin();
121 FGTower::~FGTower() {
122 if(!separateGround) {
127 void FGTower::Init() {
130 // Pointers to user's position
131 user_lon_node = fgGetNode("/position/longitude-deg", true);
132 user_lat_node = fgGetNode("/position/latitude-deg", true);
133 user_elev_node = fgGetNode("/position/altitude-ft", true);
134 user_hdg_node = fgGetNode("/orientation/heading-deg", true);
136 // Need some way to initialise rwyOccupied flag correctly if the user is on the runway and to know its the user.
137 // I'll punt the startup issue for now though!!!
140 // Setup the ground control at this airport
142 //cout << "Tower ident = " << ident << '\n';
143 if(ATCmgr->GetAirportATCDetails(ident, &a)) {
144 if(a.ground_freq) { // Ground control
145 ground = (FGGround*)ATCmgr->GetATCPointer(ident, GROUND);
146 separateGround = true;
148 // Something has gone wrong :-(
149 SG_LOG(SG_ATC, SG_WARN, "ERROR - ground has frequency but can't get ground pointer :-(");
150 ground = new FGGround(ident);
151 separateGround = false;
154 ground->SetDisplay();
156 ground->SetNoDisplay();
160 // Initialise ground anyway to do the shortest path stuff!
161 // Note that we're now responsible for updating and deleting this - NOT the ATCMgr.
162 ground = new FGGround(ident);
163 separateGround = false;
166 ground->SetDisplay();
168 ground->SetNoDisplay();
172 SG_LOG(SG_ATC, SG_ALERT, "Unable to find airport details for " << ident << " in FGTower::Init()");
173 // Initialise ground anyway to avoid segfault later
174 ground = new FGGround(ident);
175 separateGround = false;
178 ground->SetDisplay();
180 ground->SetNoDisplay();
184 // Get the airport elevation
185 aptElev = dclGetAirportElev(ident.c_str()) * SG_FEET_TO_METER;
189 // FIXME - this currently assumes use of the active rwy by the user.
190 rwyOccupied = OnAnyRunway(Point3D(user_lon_node->getDoubleValue(), user_lat_node->getDoubleValue(), 0.0));
192 // Assume the user is started at the threshold ready to take-off
193 TowerPlaneRec* t = new TowerPlaneRec;
194 t->plane.callsign = "Charlie Foxtrot Sierra"; // C-FGFS !!! - fixme - this is a bit hardwired
195 t->opType = TTT_UNKNOWN; // We don't know if the user wants to do circuits or a departure...
196 t->leg = TAKEOFF_ROLL;
199 t->clearedToTakeOff = true;
200 rwyList.push_back(t);
204 void FGTower::Update(double dt) {
205 static int ii = 0; // Counter for spreading the load
207 //cout << "T" << flush;
208 // Each time step, what do we need to do?
209 // We need to go through the list of outstanding requests and acknowedgements
210 // and process at least one of them.
211 // We need to go through the list of planes under our control and check if
212 // any need to be addressed.
213 // We need to check for planes not under our control coming within our
214 // control area and address if necessary.
216 // TODO - a lot of the below probably doesn't need to be called every frame and should be staggered.
218 // Sort the arriving planes
220 // Calculate the eta of each plane to the threshold.
221 // For ground traffic this is the fastest they can get there.
222 // For air traffic this is the middle approximation.
224 doThresholdETACalc();
227 // Order the list of traffic as per expected threshold use and flag any conflicts
229 bool conflicts = doThresholdUseOrder();
232 // sortConficts() !!!
234 // Do one plane from the hold list
236 if(holdList.size()) {
237 //cout << "*holdListItr = " << *holdListItr << endl;
238 if(holdListItr == holdList.end()) {
239 holdListItr = holdList.begin();
241 //cout << "*holdListItr = " << *holdListItr << endl;
242 //Process(*holdListItr);
243 TowerPlaneRec* t = *holdListItr;
244 //cout << "t = " << t << endl;
245 if(t->holdShortReported) {
246 double responseTime = 10.0; // seconds - this should get more sophisticated at some point
247 if(t->clearanceCounter > responseTime) {
250 // Do nothing for now - consider acknowloging hold short eventually
253 string trns = t->plane.callsign;
254 //if(departed plane < some threshold in time away) {
257 t->clearedToLineUp = true;
258 t->planePtr->RegisterTransmission(3); // cleared to line-up
259 t->leg = TAKEOFF_ROLL;
260 //} else if(arriving plane < some threshold away) {
261 } else if(0) { // FIXME
262 trns += " cleared immediate take-off";
263 // TODO - add traffic is... ?
264 t->clearedToTakeOff = true;
265 t->planePtr->RegisterTransmission(4); // cleared to take-off - TODO differentiate between immediate and normal take-off
266 t->leg = TAKEOFF_ROLL;
268 trns += " cleared for take-off";
269 // TODO - add traffic is... ?
270 t->clearedToTakeOff = true;
271 t->planePtr->RegisterTransmission(4); // cleared to take-off
272 t->leg = TAKEOFF_ROLL;
275 globals->get_ATC_display()->RegisterSingleMessage(trns, 0);
277 t->holdShortReported = false;
278 t->clearanceCounter = 0;
279 rwyList.push_back(t);
281 holdList.erase(holdListItr);
282 holdListItr = holdList.begin();
285 // possibly tell him to hold and what position he is?
288 t->clearanceCounter += (dt * holdList.size() * ii_max);
295 // Do the runway list - we'll do the whole runway list since it's important and there'll never be many planes on the rwy at once!!
298 if(!rwyList.size()) {
301 rwyListItr = rwyList.begin();
302 TowerPlaneRec* t = *rwyListItr;
304 bool on_rwy = OnActiveRunway(Point3D(user_lon_node->getDoubleValue(), user_lat_node->getDoubleValue(), 0.0));
305 // TODO - how do we find the position when it's not the user?
307 if((t->opType == INBOUND) || (t->opType == STRAIGHT_IN)) {
310 // TODO - tell it to taxi / contact ground / don't delete it etc!
311 } else if(t->opType == OUTBOUND) {
312 depList.push_back(t);
314 } else if(t->opType == CIRCUIT) {
315 circuitList.push_back(t);
317 } else if(t->opType == TTT_UNKNOWN) {
318 depList.push_back(t);
319 circuitList.push_back(t);
322 // HELP - we shouldn't ever get here!!!
325 } // else TODO figure out what to do when it's not the user
330 // do the ciruit list
332 // Clear the constraints - we recalculate here.
334 downwind_leg_pos = 0.0;
335 crosswind_leg_pos = 0.0;
336 if(circuitList.size()) {
337 circuitListItr = circuitList.begin(); // TODO - at the moment we're constraining plane 2 based on plane 1 - this won't work for 3 planes in the circuit!!
338 TowerPlaneRec* t = *circuitListItr;
340 t->pos.setlon(user_lon_node->getDoubleValue());
341 t->pos.setlat(user_lat_node->getDoubleValue());
342 t->pos.setelev(user_elev_node->getDoubleValue());
344 // TODO - set/update the position if it's an AI plane
346 Point3D tortho = ortho.ConvertToLocal(t->pos);
348 // Need to figure out which leg he's on
349 //cout << "rwy.hdg = " << rwy.hdg << " user hdg = " << user_hdg_node->getDoubleValue();
350 double ho = GetAngleDiff_deg(user_hdg_node->getDoubleValue(), rwy.hdg);
351 //cout << " ho = " << ho << '\n';
352 // TODO FIXME - get the wind and convert this to track, or otherwise use track somehow!!!
353 // If it's gusty might need to filter the value, although we are leaving 30 degrees each way leeway!
355 // could be either takeoff, climbout or landing - check orthopos.y
356 //cout << "tortho.y = " << tortho.y() << '\n';
357 if((tortho.y() < 0) || (t->leg == TURN4) || (t->leg == FINAL)) {
361 t->leg = CLIMBOUT; // TODO - check elev wrt. apt elev to differentiate takeoff roll and climbout
362 //cout << "Climbout\n";
363 // If it's the user we may be unsure of his/her intentions.
364 // (Hopefully the AI planes won't try confusing the sim!!!)
365 if(t->opType == TTT_UNKNOWN) {
366 if(tortho.y() > 5000) {
367 // 5 km out from threshold - assume it's a departure
368 t->opType = OUTBOUND;
369 // Since we are unknown operation we should be in depList already.
370 circuitList.erase(circuitListItr);
371 circuitListItr = circuitList.begin();
373 } else if(t->opType == CIRCUIT) {
374 if(tortho.y() > 10000) {
375 // 10 km out - assume the user has abandoned the circuit!!
376 t->opType = OUTBOUND;
377 depList.push_back(t);
378 circuitList.erase(circuitListItr);
379 circuitListItr = circuitList.begin();
383 } else if(abs(ho) < 60) {
385 // TODO - either fix or doublecheck this hack by looking at heading and pattern direction
386 if((t->leg == CLIMBOUT) || (t->leg == TURN1)) {
393 } else if(abs(ho) < 120) {
395 // TODO - either fix or doublecheck this hack by looking at heading and pattern direction
396 if((t->leg == TURN1) || (t->leg == CROSSWIND)) {
398 //cout << "Crosswind\n";
403 } else if(abs(ho) < 150) {
405 // TODO - either fix or doublecheck this hack by looking at heading and pattern direction
406 if((t->leg == CROSSWIND) || (t->leg == TURN2)) {
416 //cout << "Downwind\n";
419 t->leg = t->planePtr->GetLeg();
423 // Base leg must be at least as far out as the plane is - actually possibly not necessary for separation, but we'll use that for now.
424 base_leg_pos = tortho.y();
425 //cout << "base_leg_pos = " << base_leg_pos << '\n';
428 // Fall through to base
430 base_leg_pos = tortho.y();
431 //cout << "base_leg_pos = " << base_leg_pos << '\n';
434 // Fall through to downwind
436 // Only have the downwind leg pos as turn-to-base constraint if more negative than we already have.
437 base_leg_pos = (tortho.y() < base_leg_pos ? tortho.y() : base_leg_pos);
438 //cout << "base_leg_pos = " << base_leg_pos;
439 downwind_leg_pos = tortho.x(); // Assume that a following plane can simply be constrained by the immediately in front downwind plane
440 //cout << " downwind_leg_pos = " << downwind_leg_pos << '\n';
443 // Fall through to crosswind
445 crosswind_leg_pos = tortho.y();
446 //cout << "crosswind_leg_pos = " << crosswind_leg_pos << '\n';
449 // Fall through to climbout
451 // Only use current by constraint as largest
452 crosswind_leg_pos = (tortho.y() > crosswind_leg_pos ? tortho.y() : crosswind_leg_pos);
453 //cout << "crosswind_leg_pos = " << crosswind_leg_pos << '\n';
469 if(!separateGround) {
470 // The display stuff might have to get more clever than this when not separate
471 // since the tower and ground might try communicating simultaneously even though
472 // they're mean't to be the same contoller/frequency!!
474 ground->SetDisplay();
476 ground->SetNoDisplay();
482 // How big should ii get - ie how long should the update cycle interval stretch?
487 if(ident == "KEMT") {
488 // For AI debugging convienience - may be removed
490 user_pos.setlon(user_lon_node->getDoubleValue());
491 user_pos.setlat(user_lat_node->getDoubleValue());
492 user_pos.setelev(user_elev_node->getDoubleValue());
493 Point3D user_ortho_pos = ortho.ConvertToLocal(user_pos);
494 fgSetDouble("/AI/user/ortho-x", user_ortho_pos.x());
495 fgSetDouble("/AI/user/ortho-y", user_ortho_pos.y());
496 fgSetDouble("/AI/user/elev", user_elev_node->getDoubleValue());
501 // Returns true if positions of crosswind/downwind/base leg turns should be constrained by previous traffic
502 // plus the constraint position as a rwy orientated orthopos (meters)
503 bool FGTower::GetCrosswindConstraint(double& cpos) {
504 if(crosswind_leg_pos != 0.0) {
505 cpos = crosswind_leg_pos;
512 bool FGTower::GetDownwindConstraint(double& dpos) {
513 if(downwind_leg_pos != 0.0) {
514 dpos = downwind_leg_pos;
521 bool FGTower::GetBaseConstraint(double& bpos) {
522 if(base_leg_pos != 0.0) {
532 // Figure out which runways are active.
533 // For now we'll just be simple and do one active runway - eventually this will get much more complex
534 // This is a private function - public interface to the results of this is through GetActiveRunway
535 void FGTower::DoRwyDetails() {
536 //cout << "GetRwyDetails called" << endl;
538 // Based on the airport-id and wind get the active runway
541 double hdg = wind_from_hdg->getDoubleValue();
542 double speed = wind_speed_knots->getDoubleValue();
543 hdg = (speed == 0.0 ? 270.0 : hdg);
544 //cout << "Heading = " << hdg << '\n';
547 bool rwyGood = globals->get_runways()->search(ident, int(hdg), &runway);
549 activeRwy = runway.rwy_no;
550 rwy.rwyID = runway.rwy_no;
551 SG_LOG(SG_ATC, SG_INFO, "Active runway for airport " << ident << " is " << activeRwy);
553 // Get the threshold position
554 double other_way = runway.heading - 180.0;
555 while(other_way <= 0.0) {
558 // move to the +l end/center of the runway
559 //cout << "Runway center is at " << runway.lon << ", " << runway.lat << '\n';
560 Point3D origin = Point3D(runway.lon, runway.lat, aptElev);
561 Point3D ref = origin;
562 double tshlon, tshlat, tshr;
563 double tolon, tolat, tor;
564 rwy.length = runway.length * SG_FEET_TO_METER;
565 rwy.width = runway.width * SG_FEET_TO_METER;
566 geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), other_way,
567 rwy.length / 2.0 - 25.0, &tshlat, &tshlon, &tshr );
568 geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), runway.heading,
569 rwy.length / 2.0 - 25.0, &tolat, &tolon, &tor );
570 // Note - 25 meters in from the runway end is a bit of a hack to put the plane ahead of the user.
571 // now copy what we need out of runway into rwy
572 rwy.threshold_pos = Point3D(tshlon, tshlat, aptElev);
573 Point3D takeoff_end = Point3D(tolon, tolat, aptElev);
574 //cout << "Threshold position = " << tshlon << ", " << tshlat << ", " << aptElev << '\n';
575 //cout << "Takeoff position = " << tolon << ", " << tolat << ", " << aptElev << '\n';
576 rwy.hdg = runway.heading;
577 // Set the projection for the local area based on this active runway
578 ortho.Init(rwy.threshold_pos, rwy.hdg);
579 rwy.end1ortho = ortho.ConvertToLocal(rwy.threshold_pos); // should come out as zero
580 rwy.end2ortho = ortho.ConvertToLocal(takeoff_end);
582 SG_LOG(SG_ATC, SG_ALERT, "Help - can't get good runway in FGTower!!");
588 // Figure out if a given position lies on the active runway
589 // Might have to change when we consider more than one active rwy.
590 bool FGTower::OnActiveRunway(Point3D pt) {
591 // TODO - check that the centre calculation below isn't confused by displaced thesholds etc.
592 Point3D xyc((rwy.end1ortho.x() + rwy.end2ortho.x())/2.0, (rwy.end1ortho.y() + rwy.end2ortho.y())/2.0, 0.0);
593 Point3D xyp = ortho.ConvertToLocal(pt);
595 //cout << "Length offset = " << fabs(xyp.y() - xyc.y()) << '\n';
596 //cout << "Width offset = " << fabs(xyp.x() - xyc.x()) << '\n';
598 double rlen = rwy.length/2.0 + 5.0;
599 double rwidth = rwy.width/2.0;
600 double ldiff = fabs(xyp.y() - xyc.y());
601 double wdiff = fabs(xyp.x() - xyc.x());
603 return((ldiff < rlen) && (wdiff < rwidth));
607 // Figure out if a given position lies on any runway or not
608 // Only call this at startup - reading the runways database is expensive and needs to be fixed!
609 bool FGTower::OnAnyRunway(Point3D pt) {
611 double dist = current_commlist->FindClosest(lon, lat, elev, ad, TOWER, 10.0);
615 // Based on the airport-id, go through all the runways and check for a point in them
617 // TODO - do we actually need to search for the airport - surely we already know our ident and
618 // can just search runways of our airport???
619 //cout << "Airport ident is " << ad.ident << '\n';
621 bool rwyGood = globals->get_runways()->search(ad.ident, &runway);
623 SG_LOG(SG_ATC, SG_WARN, "Unable to find any runways for airport ID " << ad.ident << " in FGTower");
626 while(runway.id == ad.ident) {
627 on = OnRunway(pt, runway);
628 //cout << "Runway " << runway.rwy_no << ": On = " << (on ? "true\n" : "false\n");
630 globals->get_runways()->next(&runway);
636 // Calculate the eta of each plane to the threshold.
637 // For ground traffic this is the fastest they can get there.
638 // For air traffic this is the middle approximation.
639 void FGTower::doThresholdETACalc() {
640 // For now we'll be very crude and hardwire expected speeds to C172-like values
641 // The speeds below are specified in knots IAS and then converted to m/s
642 double app_ias = 100.0 * 0.514444; // Speed during straight-in approach
643 double circuit_ias = 80.0 * 0.514444; // Speed around circuit
644 double final_ias = 70.0 * 0.514444; // Speed during final approach
646 tower_plane_rec_list_iterator twrItr;
648 // Sign convention - dist_out is -ve for approaching planes and +ve for departing planes
649 // dist_across is +ve in the pattern direction - ie a plane correctly on downwind will have a +ve dist_across
650 for(twrItr = trafficList.begin(); twrItr != trafficList.end(); twrItr++) {
651 TowerPlaneRec* tpr = *twrItr;
652 Point3D op = ortho.ConvertToLocal(tpr->pos);
653 double dist_out_m = op.y();
654 double dist_across_m = fabs(op.x()); // FIXME = the fabs is a hack to cope with the fact that we don't know the circuit direction yet
655 //cout << "Doing ETA calc for " << tpr->plane.callsign << '\n';
656 if(tpr->opType == CIRCUIT) {
657 // It's complicated - depends on if base leg is delayed or not
658 if(tpr->leg == LANDING_ROLL) {
660 } else if((tpr->leg == FINAL) || (tpr->leg == TURN4)) {
661 tpr->eta = fabs(dist_out_m) / final_ias;
662 } else if((tpr->leg == BASE) || (tpr->leg == TURN3)) {
663 tpr->eta = (fabs(dist_out_m) / final_ias) + (dist_across_m / circuit_ias);
665 // Need to calculate where base leg is likely to be
666 // FIXME - for now I'll hardwire it to 1000m which is what AILocalTraffic uses!!!
667 // TODO - as a matter of design - AILocalTraffic should get the nominal no-traffic base turn distance from Tower, since in real life the published pattern might differ from airport to airport
668 double nominal_base_dist_out_m = -1000;
669 double current_base_dist_out_m = nominal_base_dist_out_m;
670 double nominal_dist_across_m = 1000; // Hardwired value from AILocalTraffic
671 double nominal_cross_dist_out_m = 1000; // Bit of a guess - AI plane turns to crosswind at 600ft agl.
672 tpr->eta = fabs(current_base_dist_out_m) / final_ias; // final
673 if((tpr->leg == DOWNWIND) || (tpr->leg == TURN2)) {
674 tpr->eta += dist_across_m / circuit_ias;
675 tpr->eta += fabs(current_base_dist_out_m - dist_out_m) / circuit_ias;
676 } else if((tpr->leg == CROSSWIND) || (tpr->leg == TURN1)) {
677 tpr->eta += nominal_dist_across_m / circuit_ias; // should we use the dist across of the previous plane if there is previous still on downwind?
678 tpr->eta += fabs(current_base_dist_out_m - nominal_cross_dist_out_m) / circuit_ias;
679 tpr->eta += (nominal_dist_across_m - dist_across_m) / circuit_ias;
681 // We've only just started - why not use a generic estimate?
684 } else if((tpr->opType == INBOUND) || (tpr->opType == STRAIGHT_IN)) {
687 // Must be outbound - ignore it!
689 //cout << "ETA = " << tpr->eta << '\n';
694 bool FGTower::doThresholdUseOrder() {
698 void FGTower::doCommunication() {
701 void FGTower::ContactAtHoldShort(PlaneRec plane, FGAIPlane* requestee, tower_traffic_type operation) {
702 // HACK - assume that anything contacting at hold short is new for now - FIXME LATER
703 TowerPlaneRec* t = new TowerPlaneRec;
705 t->planePtr = requestee;
706 t->holdShortReported = true;
707 t->clearanceCounter = 0;
708 t->clearedToLineUp = false;
709 t->clearedToTakeOff = false;
710 t->opType = operation;
712 // HACK ALERT - THIS IS HARDWIRED FOR TESTING - FIXME TODO ETC
715 //cout << "t = " << t << '\n';
717 holdList.push_back(t);
720 void FGTower::RequestLandingClearance(string ID) {
721 //cout << "Request Landing Clearance called...\n";
723 void FGTower::RequestDepartureClearance(string ID) {
724 //cout << "Request Departure Clearance called...\n";
726 //void FGTower::ReportFinal(string ID);
727 //void FGTower::ReportLongFinal(string ID);
728 //void FGTower::ReportOuterMarker(string ID);
729 //void FGTower::ReportMiddleMarker(string ID);
730 //void FGTower::ReportInnerMarker(string ID);
731 //void FGTower::ReportGoingAround(string ID);
732 void FGTower::ReportRunwayVacated(string ID) {
733 //cout << "Report Runway Vacated Called...\n";