]> git.mxchange.org Git - flightgear.git/blob - src/ATC/trafficcontrol.cxx
Merge branch 'next' of http://git.gitorious.org/fg/flightgear into next
[flightgear.git] / src / ATC / trafficcontrol.cxx
1 // trafficrecord.cxx - Implementation of AIModels ATC code.
2 //
3 // Written by Durk Talsma, started September 2006.
4 //
5 // Copyright (C) 2006 Durk Talsma.
6 //
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.
11 //
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.
16 //
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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
20 //
21 // $Id$
22
23 #ifdef HAVE_CONFIG_H
24 #  include <config.h>
25 #endif
26
27 #include <algorithm>
28
29 #include "trafficcontrol.hxx"
30 #include <AIModel/AIAircraft.hxx>
31 #include <AIModel/AIFlightPlan.hxx>
32 #include <AIModel/performancedata.hxx>
33 #include <AIModel/performancedb.hxx>
34 #include <Traffic/TrafficMgr.hxx>
35 #include <Airports/groundnetwork.hxx>
36 #include <Airports/dynamics.hxx>
37 #include <Airports/simple.hxx>
38
39 using std::sort;
40
41 /***************************************************************************
42  * ActiveRunway
43  **************************************************************************/
44 time_t ActiveRunway::requestTimeSlot(time_t eta)
45 {
46     time_t newEta;
47     time_t separation = 90;
48     bool found = false;
49     if (estimatedArrivalTimes.size() == 0) {
50         estimatedArrivalTimes.push_back(eta);
51         return eta;
52     } else {
53         TimeVectorIterator i = estimatedArrivalTimes.begin();
54         //cerr << "Checking eta slots " << eta << ": " << endl;
55         for (i = estimatedArrivalTimes.begin();
56              i != estimatedArrivalTimes.end(); i++) {
57             //cerr << "Stored time : " << (*i) << endl;
58         }
59         i = estimatedArrivalTimes.begin();
60         if ((eta + separation) < (*i)) {
61             newEta = eta;
62             found = true;
63             //cerr << "Storing at beginning" << endl;
64         }
65         while ((i != estimatedArrivalTimes.end()) && (!found)) {
66             TimeVectorIterator j = i + 1;
67             if (j == estimatedArrivalTimes.end()) {
68                 if (((*i) + separation) < eta) {
69                     //cerr << "Storing at end" << endl;
70                     newEta = eta;
71                 } else {
72                     newEta = (*i) + separation;
73                     //cerr << "Storing at end + separation" << endl;
74                 }
75             } else {
76                 if ((((*j) - (*i)) > (separation * 2))) {       // found a potential slot
77                     // now check whether this slow is usable:
78                     // 1) eta should fall between the two points
79                     //    i.e. eta > i AND eta < j
80                     //
81                     //cerr << "Found potential slot after " << (*i) << endl;
82                     if (eta > (*i) && (eta < (*j))) {
83                         found = true;
84                         if (eta < ((*i) + separation)) {
85                             newEta = (*i) + separation;
86                             //cerr << "Using  original" << (*i) << " + separation " << endl;
87                         } else {
88                             newEta = eta;
89                             //cerr << "Using original after " << (*i) << endl;
90                         }
91                     } else if (eta < (*i)) {
92                         found = true;
93                         newEta = (*i) + separation;
94                         //cerr << "Using delayed slot after " << (*i) << endl;
95                     }
96                     /*
97                        if (((*j) - separation) < eta) {
98                        found = true;
99                        if (((*i) + separation) < eta) {
100                        newEta = eta;
101                        cerr << "Using original after " << (*i) << endl;
102                        } else {
103                        newEta = (*i) + separation;
104                        cerr << "Using  " << (*i) << " + separation " << endl;
105                        }
106                        } */
107                 }
108             }
109             i++;
110         }
111     }
112     //cerr << ". done. New ETA : " << newEta << endl;
113
114     estimatedArrivalTimes.push_back(newEta);
115     sort(estimatedArrivalTimes.begin(), estimatedArrivalTimes.end());
116     // do some housekeeping : remove any timestamps that are past
117     time_t now = time(NULL) + fgGetLong("/sim/time/warp");
118     TimeVectorIterator i = estimatedArrivalTimes.begin();
119     while (i != estimatedArrivalTimes.end()) {
120         if ((*i) < now) {
121             //cerr << "Deleting timestamp " << (*i) << " (now = " << now << "). " << endl;
122             estimatedArrivalTimes.erase(i);
123             i = estimatedArrivalTimes.begin();
124         } else {
125             i++;
126         }
127     }
128     return newEta;
129 }
130
131 /***************************************************************************
132  * FGTrafficRecord
133  **************************************************************************/
134 FGTrafficRecord::FGTrafficRecord():
135 id(0), waitsForId(0),
136 currentPos(0),
137 leg(0),
138 frequencyId(0),
139 state(0),
140 allowTransmission(true),
141 latitude(0), longitude(0), heading(0), speed(0), altitude(0), radius(0)
142 {
143 }
144
145 void FGTrafficRecord::setPositionAndIntentions(int pos,
146                                                FGAIFlightPlan * route)
147 {
148
149     currentPos = pos;
150     if (intentions.size()) {
151         intVecIterator i = intentions.begin();
152         if ((*i) != pos) {
153             SG_LOG(SG_GENERAL, SG_ALERT,
154                    "Error in FGTrafficRecord::setPositionAndIntentions");
155             //cerr << "Pos : " << pos << " Curr " << *(intentions.begin())  << endl;
156             for (intVecIterator i = intentions.begin();
157                  i != intentions.end(); i++) {
158                 //cerr << (*i) << " ";
159             }
160             //cerr << endl;
161         }
162         intentions.erase(i);
163     } else {
164         //FGAIFlightPlan::waypoint* const wpt= route->getCurrentWaypoint();
165         int size = route->getNrOfWayPoints();
166         //cerr << "Setting pos" << pos << " ";
167         //cerr << "setting intentions ";
168         for (int i = 0; i < size; i++) {
169             int val = route->getRouteIndex(i);
170             //cerr << val<< " ";
171             if ((val) && (val != pos)) {
172                 intentions.push_back(val);
173                 //cerr << "[set] ";
174             }
175         }
176         //cerr << endl;
177         //while (route->next(&legNr, &routeNr)) {
178         //intentions.push_back(routeNr);
179         //}
180         //route->rewind(currentPos);
181     }
182     //exit(1);
183 }
184
185 bool FGTrafficRecord::checkPositionAndIntentions(FGTrafficRecord & other)
186 {
187     bool result = false;
188     //cerr << "Start check 1" << endl;
189     if (currentPos == other.currentPos) {
190         //cerr << callsign << ": Check Position and intentions: we are on the same taxiway" << other.callsign << "Index = " << currentPos << endl;
191         result = true;
192     }
193     //  else if (other.intentions.size()) 
194     //     {
195     //       cerr << "Start check 2" << endl;
196     //       intVecIterator i = other.intentions.begin(); 
197     //       while (!((i == other.intentions.end()) || ((*i) == currentPos)))
198     //     i++;
199     //       if (i != other.intentions.end()) {
200     //     cerr << "Check Position and intentions: current matches other.intentions" << endl;
201     //     result = true;
202     //       }
203     else if (intentions.size()) {
204         //cerr << "Start check 3" << endl;
205         intVecIterator i = intentions.begin();
206         //while (!((i == intentions.end()) || ((*i) == other.currentPos)))
207         while (i != intentions.end()) {
208             if ((*i) == other.currentPos) {
209                 break;
210             }
211             i++;
212         }
213         if (i != intentions.end()) {
214             //cerr << callsign << ": Check Position and intentions: .other.current matches" << other.callsign << "Index = " << (*i) << endl;
215             result = true;
216         }
217     }
218     //cerr << "Done !!" << endl;
219     return result;
220 }
221
222 void FGTrafficRecord::setPositionAndHeading(double lat, double lon,
223                                             double hdg, double spd,
224                                             double alt)
225 {
226     latitude = lat;
227     longitude = lon;
228     heading = hdg;
229     speed = spd;
230     altitude = alt;
231 }
232
233 int FGTrafficRecord::crosses(FGGroundNetwork * net,
234                              FGTrafficRecord & other)
235 {
236     if (checkPositionAndIntentions(other)
237         || (other.checkPositionAndIntentions(*this)))
238         return -1;
239     intVecIterator i, j;
240     int currentTargetNode = 0, otherTargetNode = 0;
241     if (currentPos > 0)
242         currentTargetNode = net->findSegment(currentPos)->getEnd()->getIndex(); // OKAY,... 
243     if (other.currentPos > 0)
244         otherTargetNode = net->findSegment(other.currentPos)->getEnd()->getIndex();     // OKAY,...
245     if ((currentTargetNode == otherTargetNode) && currentTargetNode > 0)
246         return currentTargetNode;
247     if (intentions.size()) {
248         for (i = intentions.begin(); i != intentions.end(); i++) {
249             if ((*i) > 0) {
250                 if ((currentTargetNode ==
251                      net->findSegment(*i)->getEnd()->getIndex())) {
252                     //cerr << "Current crosses at " << currentTargetNode <<endl;
253                     return currentTargetNode;
254                 }
255             }
256         }
257     }
258     if (other.intentions.size()) {
259         for (i = other.intentions.begin(); i != other.intentions.end();
260              i++) {
261             if ((*i) > 0) {
262                 if (otherTargetNode ==
263                     net->findSegment(*i)->getEnd()->getIndex()) {
264                     //cerr << "Other crosses at " << currentTargetNode <<endl;
265                     return otherTargetNode;
266                 }
267             }
268         }
269     }
270     if (intentions.size() && other.intentions.size()) {
271         for (i = intentions.begin(); i != intentions.end(); i++) {
272             for (j = other.intentions.begin(); j != other.intentions.end();
273                  j++) {
274                 //cerr << "finding segment " << *i << " and " << *j << endl;
275                 if (((*i) > 0) && ((*j) > 0)) {
276                     currentTargetNode =
277                         net->findSegment(*i)->getEnd()->getIndex();
278                     otherTargetNode =
279                         net->findSegment(*j)->getEnd()->getIndex();
280                     if (currentTargetNode == otherTargetNode) {
281                         //cerr << "Routes will cross at " << currentTargetNode << endl;
282                         return currentTargetNode;
283                     }
284                 }
285             }
286         }
287     }
288     return -1;
289 }
290
291 bool FGTrafficRecord::onRoute(FGGroundNetwork * net,
292                               FGTrafficRecord & other)
293 {
294     int node = -1, othernode = -1;
295     if (currentPos > 0)
296         node = net->findSegment(currentPos)->getEnd()->getIndex();
297     if (other.currentPos > 0)
298         othernode =
299             net->findSegment(other.currentPos)->getEnd()->getIndex();
300     if ((node == othernode) && (node != -1))
301         return true;
302     if (other.intentions.size()) {
303         for (intVecIterator i = other.intentions.begin();
304              i != other.intentions.end(); i++) {
305             if (*i > 0) {
306                 othernode = net->findSegment(*i)->getEnd()->getIndex();
307                 if ((node == othernode) && (node > -1))
308                     return true;
309             }
310         }
311     }
312     //if (other.currentPos > 0)
313     //  othernode = net->findSegment(other.currentPos)->getEnd()->getIndex();
314     //if (intentions.size())
315     //  {
316     //    for (intVecIterator i = intentions.begin(); i != intentions.end(); i++)
317     //    {
318     //      if (*i > 0) 
319     //        {
320     //          node = net->findSegment(*i)->getEnd()->getIndex();
321     //          if ((node == othernode) && (node > -1))
322     //            return true;
323     //        }
324     //    }
325     //  }
326     return false;
327 }
328
329
330 bool FGTrafficRecord::isOpposing(FGGroundNetwork * net,
331                                  FGTrafficRecord & other, int node)
332 {
333     // Check if current segment is the reverse segment for the other aircraft
334     FGTaxiSegment *opp;
335     //cerr << "Current segment " << currentPos << endl;
336     if ((currentPos > 0) && (other.currentPos > 0)) {
337         opp = net->findSegment(currentPos)->opposite();
338         if (opp) {
339             if (opp->getIndex() == other.currentPos)
340                 return true;
341         }
342
343         for (intVecIterator i = intentions.begin(); i != intentions.end();
344              i++) {
345             if ((opp = net->findSegment(other.currentPos)->opposite())) {
346                 if ((*i) > 0)
347                     if (opp->getIndex() ==
348                         net->findSegment(*i)->getIndex()) {
349                         if (net->findSegment(*i)->getStart()->getIndex() ==
350                             node) {
351                             {
352                                 //cerr << "Found the node " << node << endl;
353                                 return true;
354                             }
355                         }
356                     }
357             }
358             if (other.intentions.size()) {
359                 for (intVecIterator j = other.intentions.begin();
360                      j != other.intentions.end(); j++) {
361                     // cerr << "Current segment 1 " << (*i) << endl;
362                     if ((*i) > 0) {
363                         if ((opp = net->findSegment(*i)->opposite())) {
364                             if (opp->getIndex() ==
365                                 net->findSegment(*j)->getIndex()) {
366                                 //cerr << "Nodes " << net->findSegment(*i)->getIndex()
367                                 //   << " and  " << net->findSegment(*j)->getIndex()
368                                 //   << " are opposites " << endl;
369                                 if (net->findSegment(*i)->getStart()->
370                                     getIndex() == node) {
371                                     {
372                                         //cerr << "Found the node " << node << endl;
373                                         return true;
374                                     }
375                                 }
376                             }
377                         }
378                     }
379                 }
380             }
381         }
382     }
383     return false;
384 }
385
386 void FGTrafficRecord::setSpeedAdjustment(double spd)
387 {
388     instruction.setChangeSpeed(true);
389     instruction.setSpeed(spd);
390 }
391
392 void FGTrafficRecord::setHeadingAdjustment(double heading)
393 {
394     instruction.setChangeHeading(true);
395     instruction.setHeading(heading);
396 }
397
398 bool FGTrafficRecord::pushBackAllowed()
399 {
400     double course, az2, dist;
401     SGGeod curr(SGGeod::fromDegM(getLongitude(),
402                                  getLatitude(), getAltitude()));
403
404     double userLatitude = fgGetDouble("/position/latitude-deg");
405     double userLongitude = fgGetDouble("/position/longitude-deg");
406     SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
407     SGGeodesy::inverse(curr, user, course, az2, dist);
408     //cerr << "Distance to user : " << dist << endl;
409     return (dist > 250);
410
411 }
412
413
414
415 /***************************************************************************
416  * FGATCInstruction
417  *
418  **************************************************************************/
419 FGATCInstruction::FGATCInstruction()
420 {
421     holdPattern = false;
422     holdPosition = false;
423     changeSpeed = false;
424     changeHeading = false;
425     changeAltitude = false;
426     resolveCircularWait = false;
427
428     speed = 0;
429     heading = 0;
430     alt = 0;
431 }
432
433
434 bool FGATCInstruction::hasInstruction()
435 {
436     return (holdPattern || holdPosition || changeSpeed || changeHeading
437             || changeAltitude || resolveCircularWait);
438 }
439
440 /***************************************************************************
441  * FGATCController
442  *
443  **************************************************************************/
444
445
446
447
448 FGATCController::FGATCController()
449 {
450     dt_count = 0;
451     available = true;
452     lastTransmission = 0;
453 }
454
455 string FGATCController::getGateName(FGAIAircraft * ref)
456 {
457     return ref->atGate();
458 }
459
460 void FGATCController::transmit(FGTrafficRecord * rec, AtcMsgId msgId,
461                                AtcMsgDir msgDir)
462 {
463     string sender, receiver;
464     int stationFreq = 0;
465     int taxiFreq = 0;
466     int freqId = 0;
467     string atisInformation;
468     string text;
469     string taxiFreqStr;
470     double heading = 0;
471     string activeRunway;
472     string fltType;
473     string rwyClass;
474     string SID;
475     string transponderCode;
476     FGAIFlightPlan *fp;
477     string fltRules;
478
479     //double commFreqD;
480     sender = rec->getAircraft()->getTrafficRef()->getCallSign();
481     //cerr << "transmitting for: " << sender << "Leg = " << rec->getLeg() << endl;
482     switch (rec->getLeg()) {
483     case 2:
484     case 3:
485         freqId = rec->getNextFrequency();
486         stationFreq =
487             rec->getAircraft()->getTrafficRef()->getDepartureAirport()->
488             getDynamics()->getGroundFrequency(rec->getLeg() + freqId);
489         taxiFreq =
490             rec->getAircraft()->getTrafficRef()->getDepartureAirport()->
491             getDynamics()->getGroundFrequency(3);
492         receiver =
493             rec->getAircraft()->getTrafficRef()->getDepartureAirport()->
494             getName() + "-Ground";
495         atisInformation =
496             rec->getAircraft()->getTrafficRef()->getDepartureAirport()->
497             getDynamics()->getAtisSequence();
498         break;
499     case 4:
500         receiver =
501             rec->getAircraft()->getTrafficRef()->getDepartureAirport()->
502             getName() + "-Tower";
503         break;
504     }
505     // Swap sender and receiver value in case of a ground to air transmission
506     if (msgDir == ATC_GROUND_TO_AIR) {
507         string tmp = sender;
508         sender = receiver;
509         receiver = tmp;
510     }
511     switch (msgId) {
512     case MSG_ANNOUNCE_ENGINE_START:
513         text = sender + ". Ready to Start up";
514         break;
515     case MSG_REQUEST_ENGINE_START:
516         text =
517             receiver + ", This is " + sender + ". Position " +
518             getGateName(rec->getAircraft()) + ". Information " +
519             atisInformation + ". " +
520             rec->getAircraft()->getTrafficRef()->getFlightRules() +
521             " to " +
522             rec->getAircraft()->getTrafficRef()->getArrivalAirport()->
523             getName() + ". Request start-up";
524         break;
525         // Acknowledge engine startup permission
526         // Assign departure runway
527         // Assign SID, if necessery (TODO)
528     case MSG_PERMIT_ENGINE_START:
529         taxiFreqStr = formatATCFrequency3_2(taxiFreq);
530
531         heading = rec->getAircraft()->getTrafficRef()->getCourse();
532         fltType = rec->getAircraft()->getTrafficRef()->getFlightType();
533         rwyClass =
534             rec->getAircraft()->GetFlightPlan()->
535             getRunwayClassFromTrafficType(fltType);
536
537         rec->getAircraft()->getTrafficRef()->getDepartureAirport()->
538             getDynamics()->getActiveRunway(rwyClass, 1, activeRunway,
539                                            heading);
540         rec->getAircraft()->GetFlightPlan()->setRunway(activeRunway);
541         fp = rec->getAircraft()->getTrafficRef()->getDepartureAirport()->
542             getDynamics()->getSID(activeRunway, heading);
543         rec->getAircraft()->GetFlightPlan()->setSID(fp);
544         if (fp) {
545             SID = fp->getName() + " departure";
546         } else {
547             SID = "fly runway heading ";
548         }
549         //snprintf(buffer, 7, "%3.2f", heading);
550         fltRules = rec->getAircraft()->getTrafficRef()->getFlightRules();
551         transponderCode = genTransponderCode(fltRules);
552         rec->getAircraft()->SetTransponderCode(transponderCode);
553         text =
554             receiver + ". Start-up approved. " + atisInformation +
555             " correct, runway " + activeRunway + ", " + SID + ", squawk " +
556             transponderCode + ". " +
557             "For push-back and taxi clearance call " + taxiFreqStr + ". " +
558             sender + " control.";
559         break;
560     case MSG_DENY_ENGINE_START:
561         text = receiver + ". Standby";
562         break;
563     case MSG_ACKNOWLEDGE_ENGINE_START:
564         fp = rec->getAircraft()->GetFlightPlan()->getSID();
565         if (fp) {
566             SID =
567                 rec->getAircraft()->GetFlightPlan()->getSID()->getName() +
568                 " departure";
569         } else {
570             SID = "fly runway heading ";
571         }
572         taxiFreqStr = formatATCFrequency3_2(taxiFreq);
573         activeRunway = rec->getAircraft()->GetFlightPlan()->getRunway();
574         transponderCode = rec->getAircraft()->GetTransponderCode();
575         text =
576             receiver + ". Start-up approved. " + atisInformation +
577             " correct, runway " + activeRunway + ", " + SID + ", squawk " +
578             transponderCode + ". " +
579             "For push-back and taxi clearance call " + taxiFreqStr + ". " +
580             sender;
581         break;
582     case MSG_ACKNOWLEDGE_SWITCH_GROUND_FREQUENCY:
583         taxiFreqStr = formatATCFrequency3_2(taxiFreq);
584         text = receiver + ". Switching to " + taxiFreqStr + ". " + sender;
585         break;
586     case MSG_INITIATE_CONTACT:
587         text = receiver + ". With you. " + sender;
588         break;
589     case MSG_ACKNOWLEDGE_INITIATE_CONTACT:
590         text = receiver + ". Roger. " + sender;
591         break;
592     case MSG_REQUEST_PUSHBACK_CLEARANCE:
593         text = receiver + ". Request push-back. " + sender;
594         break;
595     case MSG_PERMIT_PUSHBACK_CLEARANCE:
596         text = receiver + ". Push-back approved. " + sender;
597         break;
598     case MSG_HOLD_PUSHBACK_CLEARANCE:
599         text = receiver + ". Standby. " + sender;
600         break;
601     case MSG_REQUEST_TAXI_CLEARANCE:
602         text = receiver + ". Ready to Taxi. " + sender;
603         break;
604     case MSG_ISSUE_TAXI_CLEARANCE:
605         text = receiver + ". Cleared to taxi. " + sender;
606         break;
607     case MSG_ACKNOWLEDGE_TAXI_CLEARANCE:
608         text = receiver + ". Cleared to taxi. " + sender;
609         break;
610     case MSG_HOLD_POSITION:
611         text = receiver + ". Hold Position. " + sender;
612         break;
613     case MSG_ACKNOWLEDGE_HOLD_POSITION:
614         text = receiver + ". Holding Position. " + sender;
615         break;
616     case MSG_RESUME_TAXI:
617         text = receiver + ". Resume Taxiing. " + sender;
618         break;
619     case MSG_ACKNOWLEDGE_RESUME_TAXI:
620         text = receiver + ". Continuing Taxi. " + sender;
621         break;
622     default:
623         text = text + sender + ". Transmitting unknown Message";
624         break;
625     }
626     double onBoardRadioFreq0 =
627         fgGetDouble("/instrumentation/comm[0]/frequencies/selected-mhz");
628     double onBoardRadioFreq1 =
629         fgGetDouble("/instrumentation/comm[1]/frequencies/selected-mhz");
630     int onBoardRadioFreqI0 = (int) floor(onBoardRadioFreq0 * 100 + 0.5);
631     int onBoardRadioFreqI1 = (int) floor(onBoardRadioFreq1 * 100 + 0.5);
632     //cerr << "Using " << onBoardRadioFreq0 << ", " << onBoardRadioFreq1 << " and " << stationFreq << " for " << text << endl;
633
634     // Display ATC message only when one of the radios is tuned
635     // the relevant frequency.
636     // Note that distance attenuation is currently not yet implemented
637     if ((onBoardRadioFreqI0 == stationFreq)
638         || (onBoardRadioFreqI1 == stationFreq)) {
639         if (rec->allowTransmissions()) {
640             fgSetString("/sim/messages/atc", text.c_str());
641         }
642     }
643 }
644
645 string FGATCController::formatATCFrequency3_2(int freq)
646 {
647     char buffer[7];
648     snprintf(buffer, 7, "%3.2f", ((float) freq / 100.0));
649     return string(buffer);
650 }
651
652 // TODO: Set transponder codes according to real-world routes.
653 // The current version just returns a random string of four octal numbers. 
654 string FGATCController::genTransponderCode(string fltRules)
655 {
656     if (fltRules == "VFR") {
657         return string("1200");
658     } else {
659         char buffer[5];
660         snprintf(buffer, 5, "%d%d%d%d", rand() % 8, rand() % 8, rand() % 8,
661                  rand() % 8);
662         return string(buffer);
663     }
664 }
665
666 /***************************************************************************
667  * class FGTowerController
668  *
669  **************************************************************************/
670 FGTowerController::FGTowerController():
671 FGATCController()
672 {
673 }
674
675 // 
676 void FGTowerController::announcePosition(int id,
677                                          FGAIFlightPlan * intendedRoute,
678                                          int currentPosition, double lat,
679                                          double lon, double heading,
680                                          double speed, double alt,
681                                          double radius, int leg,
682                                          FGAIAircraft * ref)
683 {
684     TrafficVectorIterator i = activeTraffic.begin();
685     // Search whether the current id alread has an entry
686     // This might be faster using a map instead of a vector, but let's start by taking a safe route
687     if (activeTraffic.size()) {
688         //while ((i->getId() != id) && i != activeTraffic.end()) {
689         while (i != activeTraffic.end()) {
690             if (i->getId() == id) {
691                 break;
692             }
693             i++;
694         }
695     }
696     // Add a new TrafficRecord if no one exsists for this aircraft.
697     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
698         FGTrafficRecord rec;
699         rec.setId(id);
700
701         rec.setPositionAndHeading(lat, lon, heading, speed, alt);
702         rec.setRunway(intendedRoute->getRunway());
703         rec.setLeg(leg);
704         //rec.setCallSign(callsign);
705         rec.setAircraft(ref);
706         activeTraffic.push_back(rec);
707     } else {
708         i->setPositionAndHeading(lat, lon, heading, speed, alt);
709     }
710 }
711
712 void FGTowerController::update(int id, double lat, double lon,
713                                double heading, double speed, double alt,
714                                double dt)
715 {
716     TrafficVectorIterator i = activeTraffic.begin();
717     // Search whether the current id has an entry
718     // This might be faster using a map instead of a vector, but let's start by taking a safe route
719     TrafficVectorIterator current, closest;
720     if (activeTraffic.size()) {
721         //while ((i->getId() != id) && i != activeTraffic.end()) {
722         while (i != activeTraffic.end()) {
723             if (i->getId() == id) {
724                 break;
725             }
726             i++;
727         }
728     }
729 //    // update position of the current aircraft
730     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
731         SG_LOG(SG_GENERAL, SG_ALERT,
732                "AI error: updating aircraft without traffic record");
733     } else {
734         i->setPositionAndHeading(lat, lon, heading, speed, alt);
735         current = i;
736     }
737     setDt(getDt() + dt);
738
739 //    // see if we already have a clearance record for the currently active runway
740     ActiveRunwayVecIterator rwy = activeRunways.begin();
741     // again, a map might be more efficient here
742     if (activeRunways.size()) {
743         //while ((rwy->getRunwayName() != current->getRunway()) && (rwy != activeRunways.end())) {
744         while (rwy != activeRunways.end()) {
745             if (rwy->getRunwayName() == current->getRunway()) {
746                 break;
747             }
748             rwy++;
749         }
750     }
751     if (rwy == activeRunways.end()) {
752         ActiveRunway aRwy(current->getRunway(), id);
753         activeRunways.push_back(aRwy);  // Since there are no clearance records for this runway yet
754         current->setHoldPosition(false);        // Clear the current aircraft to continue
755     } else {
756         // Okay, we have a clearance record for this runway, so check
757         // whether the clearence ID matches that of the current aircraft
758         if (id == rwy->getCleared()) {
759             current->setHoldPosition(false);
760         } else {
761             current->setHoldPosition(true);
762         }
763     }
764 }
765
766
767 void FGTowerController::signOff(int id)
768 {
769     TrafficVectorIterator i = activeTraffic.begin();
770     // Search search if the current id alread has an entry
771     // This might be faster using a map instead of a vector, but let's start by taking a safe route
772     if (activeTraffic.size()) {
773         //while ((i->getId() != id) && i != activeTraffic.end()) {
774         while (i != activeTraffic.end()) {
775             if (i->getId() == id) {
776                 break;
777             }
778             i++;
779         }
780     }
781     // If this aircraft has left the runway, we can clear the departure record for this runway
782     ActiveRunwayVecIterator rwy = activeRunways.begin();
783     if (activeRunways.size()) {
784         //while ((rwy->getRunwayName() != i->getRunway()) && (rwy != activeRunways.end())) {
785         while (rwy != activeRunways.end()) {
786             if (rwy->getRunwayName() == i->getRunway()) {
787                 break;
788             }
789             rwy++;
790         }
791         if (rwy != activeRunways.end()) {
792             rwy = activeRunways.erase(rwy);
793         } else {
794             SG_LOG(SG_GENERAL, SG_ALERT,
795                    "AI error: Attempting to erase non-existing runway clearance record in FGTowerController::signoff");
796         }
797     }
798     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
799         SG_LOG(SG_GENERAL, SG_ALERT,
800                "AI error: Aircraft without traffic record is signing off from tower");
801     } else {
802         i = activeTraffic.erase(i);
803     }
804 }
805
806 // NOTE:
807 // IF WE MAKE TRAFFICRECORD A MEMBER OF THE BASE CLASS
808 // THE FOLLOWING THREE FUNCTIONS: SIGNOFF, HAS INSTRUCTION AND GETINSTRUCTION CAN 
809 // BECOME DEVIRTUALIZED AND BE A MEMBER OF THE BASE ATCCONTROLLER CLASS
810 // WHICH WOULD SIMPLIFY CODE MAINTENANCE.
811 // Note that this function is probably obsolete
812 bool FGTowerController::hasInstruction(int id)
813 {
814     TrafficVectorIterator i = activeTraffic.begin();
815     // Search search if the current id has an entry
816     // This might be faster using a map instead of a vector, but let's start by taking a safe route
817     if (activeTraffic.size()) {
818         //while ((i->getId() != id) && i != activeTraffic.end()) {
819         while (i != activeTraffic.end()) {
820             if (i->getId() == id) {
821                 break;
822             }
823             i++;
824         }
825     }
826     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
827         SG_LOG(SG_GENERAL, SG_ALERT,
828                "AI error: checking ATC instruction for aircraft without traffic record");
829     } else {
830         return i->hasInstruction();
831     }
832     return false;
833 }
834
835
836 FGATCInstruction FGTowerController::getInstruction(int id)
837 {
838     TrafficVectorIterator i = activeTraffic.begin();
839     // Search search if the current id has an entry
840     // This might be faster using a map instead of a vector, but let's start by taking a safe route
841     if (activeTraffic.size()) {
842         //while ((i->getId() != id) && i != activeTraffic.end()) {
843         while (i != activeTraffic.end()) {
844             if (i->getId() == id) {
845                 break;
846             }
847             i++;
848         }
849     }
850     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
851         SG_LOG(SG_GENERAL, SG_ALERT,
852                "AI error: requesting ATC instruction for aircraft without traffic record");
853     } else {
854         return i->getInstruction();
855     }
856     return FGATCInstruction();
857 }
858
859 /***************************************************************************
860  * class FGStartupController
861  *
862  **************************************************************************/
863 FGStartupController::FGStartupController():
864 FGATCController()
865 {
866 }
867
868 void FGStartupController::announcePosition(int id,
869                                            FGAIFlightPlan * intendedRoute,
870                                            int currentPosition, double lat,
871                                            double lon, double heading,
872                                            double speed, double alt,
873                                            double radius, int leg,
874                                            FGAIAircraft * ref)
875 {
876     TrafficVectorIterator i = activeTraffic.begin();
877     // Search whether the current id alread has an entry
878     // This might be faster using a map instead of a vector, but let's start by taking a safe route
879     if (activeTraffic.size()) {
880         //while ((i->getId() != id) && i != activeTraffic.end()) {
881         while (i != activeTraffic.end()) {
882             if (i->getId() == id) {
883                 break;
884             }
885             i++;
886         }
887     }
888     // Add a new TrafficRecord if no one exsists for this aircraft.
889     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
890         FGTrafficRecord rec;
891         rec.setId(id);
892
893         rec.setPositionAndHeading(lat, lon, heading, speed, alt);
894         rec.setRunway(intendedRoute->getRunway());
895         rec.setLeg(leg);
896         //rec.setCallSign(callsign);
897         rec.setAircraft(ref);
898         rec.setHoldPosition(true);
899         activeTraffic.push_back(rec);
900     } else {
901         i->setPositionAndHeading(lat, lon, heading, speed, alt);
902
903     }
904 }
905
906 // NOTE:
907 // IF WE MAKE TRAFFICRECORD A MEMBER OF THE BASE CLASS
908 // THE FOLLOWING THREE FUNCTIONS: SIGNOFF, HAS INSTRUCTION AND GETINSTRUCTION CAN 
909 // BECOME DEVIRTUALIZED AND BE A MEMBER OF THE BASE ATCCONTROLLER CLASS
910 // WHICH WOULD SIMPLIFY CODE MAINTENANCE.
911 // Note that this function is probably obsolete
912 bool FGStartupController::hasInstruction(int id)
913 {
914     TrafficVectorIterator i = activeTraffic.begin();
915     // Search search if the current id has an entry
916     // This might be faster using a map instead of a vector, but let's start by taking a safe route
917     if (activeTraffic.size()) {
918         //while ((i->getId() != id) && i != activeTraffic.end()) {
919         while (i != activeTraffic.end()) {
920             if (i->getId() == id) {
921                 break;
922             }
923             i++;
924         }
925     }
926     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
927         SG_LOG(SG_GENERAL, SG_ALERT,
928                "AI error: checking ATC instruction for aircraft without traffic record");
929     } else {
930         return i->hasInstruction();
931     }
932     return false;
933 }
934
935
936 FGATCInstruction FGStartupController::getInstruction(int id)
937 {
938     TrafficVectorIterator i = activeTraffic.begin();
939     // Search search if the current id has an entry
940     // This might be faster using a map instead of a vector, but let's start by taking a safe route
941     if (activeTraffic.size()) {
942         //while ((i->getId() != id) && i != activeTraffic.end()) {
943         while (i != activeTraffic.end()) {
944             if (i->getId() == id) {
945                 break;
946             }
947             i++;
948         }
949     }
950     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
951         SG_LOG(SG_GENERAL, SG_ALERT,
952                "AI error: requesting ATC instruction for aircraft without traffic record");
953     } else {
954         return i->getInstruction();
955     }
956     return FGATCInstruction();
957 }
958
959 void FGStartupController::signOff(int id)
960 {
961     TrafficVectorIterator i = activeTraffic.begin();
962     // Search search if the current id alread has an entry
963     // This might be faster using a map instead of a vector, but let's start by taking a safe route
964     if (activeTraffic.size()) {
965         //while ((i->getId() != id) && i != activeTraffic.end()) {
966         while (i != activeTraffic.end()) {
967             if (i->getId() == id) {
968                 break;
969             }
970             i++;
971         }
972     }
973     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
974         SG_LOG(SG_GENERAL, SG_ALERT,
975                "AI error: Aircraft without traffic record is signing off from tower");
976     } else {
977         i = activeTraffic.erase(i);
978     }
979 }
980
981 void FGStartupController::update(int id, double lat, double lon,
982                                  double heading, double speed, double alt,
983                                  double dt)
984 {
985     TrafficVectorIterator i = activeTraffic.begin();
986     // Search search if the current id has an entry
987     // This might be faster using a map instead of a vector, but let's start by taking a safe route
988     TrafficVectorIterator current, closest;
989     if (activeTraffic.size()) {
990         //while ((i->getId() != id) && i != activeTraffic.end()) {
991         while (i != activeTraffic.end()) {
992             if (i->getId() == id) {
993                 break;
994             }
995             i++;
996         }
997     }
998 //    // update position of the current aircraft
999     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1000         SG_LOG(SG_GENERAL, SG_ALERT,
1001                "AI error: updating aircraft without traffic record");
1002     } else {
1003         i->setPositionAndHeading(lat, lon, heading, speed, alt);
1004         current = i;
1005     }
1006     setDt(getDt() + dt);
1007
1008     int state = i->getState();
1009     time_t startTime =
1010         i->getAircraft()->getTrafficRef()->getDepartureTime();
1011     time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1012     //cerr << i->getAircraft()->getTrafficRef()->getCallSign() 
1013     //     << " is scheduled to depart in " << startTime-now << " seconds. Available = " << available
1014     //     << " at parking " << getGateName(i->getAircraft()) << endl;
1015
1016     if ((now - lastTransmission) > 3 + (rand() % 15)) {
1017         available = true;
1018     }
1019
1020     if ((state == 0) && available) {
1021         if (now > startTime) {
1022             //cerr << "Transmitting startup msg" << endl;
1023             transmit(&(*i), MSG_ANNOUNCE_ENGINE_START, ATC_AIR_TO_GROUND);
1024             i->updateState();
1025             lastTransmission = now;
1026             available = false;
1027         }
1028     }
1029     if ((state == 1) && available) {
1030         if (now > startTime + 60) {
1031             transmit(&(*i), MSG_REQUEST_ENGINE_START, ATC_AIR_TO_GROUND);
1032             i->updateState();
1033             lastTransmission = now;
1034             available = false;
1035         }
1036     }
1037     if ((state == 2) && available) {
1038         if (now > startTime + 80) {
1039             transmit(&(*i), MSG_PERMIT_ENGINE_START, ATC_GROUND_TO_AIR);
1040             i->updateState();
1041             lastTransmission = now;
1042             available = false;
1043         }
1044     }
1045     if ((state == 3) && available) {
1046         if (now > startTime + 100) {
1047             transmit(&(*i), MSG_ACKNOWLEDGE_ENGINE_START,
1048                      ATC_AIR_TO_GROUND);
1049             i->updateState();
1050             lastTransmission = now;
1051             available = false;
1052         }
1053     }
1054     // Note: The next four stages are only necessesary when Startup control is
1055     //  on a different frequency, compared to ground control
1056     if ((state == 4) && available) {
1057         if (now > startTime + 130) {
1058             transmit(&(*i), MSG_ACKNOWLEDGE_SWITCH_GROUND_FREQUENCY,
1059                      ATC_AIR_TO_GROUND);
1060             i->updateState();
1061             i->nextFrequency();
1062             lastTransmission = now;
1063             available = false;
1064         }
1065     }
1066     if ((state == 5) && available) {
1067         if (now > startTime + 140) {
1068             transmit(&(*i), MSG_INITIATE_CONTACT, ATC_AIR_TO_GROUND);
1069             i->updateState();
1070             lastTransmission = now;
1071             available = false;
1072         }
1073     }
1074     if ((state == 6) && available) {
1075         if (now > startTime + 150) {
1076             transmit(&(*i), MSG_ACKNOWLEDGE_INITIATE_CONTACT,
1077                      ATC_GROUND_TO_AIR);
1078             i->updateState();
1079             lastTransmission = now;
1080             available = false;
1081         }
1082     }
1083
1084     // TODO: Switch to APRON control and request pushback Clearance.
1085     // Get Push back clearance
1086     if ((state == 7) && available) {
1087         if (now > startTime + 180) {
1088             transmit(&(*i), MSG_REQUEST_PUSHBACK_CLEARANCE,
1089                      ATC_AIR_TO_GROUND);
1090             i->updateState();
1091             lastTransmission = now;
1092             available = false;
1093         }
1094     }
1095     if ((state == 8) && available) {
1096         if (now > startTime + 200) {
1097             if (i->pushBackAllowed()) {
1098                 i->allowRepeatedTransmissions();
1099                 transmit(&(*i), MSG_PERMIT_PUSHBACK_CLEARANCE,
1100                          ATC_GROUND_TO_AIR);
1101                 i->updateState();
1102             } else {
1103                 transmit(&(*i), MSG_HOLD_PUSHBACK_CLEARANCE,
1104                          ATC_GROUND_TO_AIR);
1105                 i->suppressRepeatedTransmissions();
1106             }
1107             lastTransmission = now;
1108             available = false;
1109         }
1110     }
1111     if ((state == 9) && available) {
1112         i->setHoldPosition(false);
1113     }
1114 }
1115
1116
1117 /***************************************************************************
1118  * class FGApproachController
1119  *
1120  **************************************************************************/
1121 FGApproachController::FGApproachController():
1122 FGATCController()
1123 {
1124 }
1125
1126 // 
1127 void FGApproachController::announcePosition(int id,
1128                                             FGAIFlightPlan * intendedRoute,
1129                                             int currentPosition,
1130                                             double lat, double lon,
1131                                             double heading, double speed,
1132                                             double alt, double radius,
1133                                             int leg, FGAIAircraft * ref)
1134 {
1135     TrafficVectorIterator i = activeTraffic.begin();
1136     // Search whether the current id alread has an entry
1137     // This might be faster using a map instead of a vector, but let's start by taking a safe route
1138     if (activeTraffic.size()) {
1139         //while ((i->getId() != id) && i != activeTraffic.end()) {
1140         while (i != activeTraffic.end()) {
1141             if (i->getId() == id) {
1142                 break;
1143             }
1144             i++;
1145         }
1146     }
1147     // Add a new TrafficRecord if no one exsists for this aircraft.
1148     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1149         FGTrafficRecord rec;
1150         rec.setId(id);
1151
1152         rec.setPositionAndHeading(lat, lon, heading, speed, alt);
1153         rec.setRunway(intendedRoute->getRunway());
1154         rec.setLeg(leg);
1155         //rec.setCallSign(callsign);
1156         rec.setAircraft(ref);
1157         activeTraffic.push_back(rec);
1158     } else {
1159         i->setPositionAndHeading(lat, lon, heading, speed, alt);
1160     }
1161 }
1162
1163 void FGApproachController::update(int id, double lat, double lon,
1164                                   double heading, double speed, double alt,
1165                                   double dt)
1166 {
1167     TrafficVectorIterator i = activeTraffic.begin();
1168     // Search search if the current id has an entry
1169     // This might be faster using a map instead of a vector, but let's start by taking a safe route
1170     TrafficVectorIterator current, closest;
1171     if (activeTraffic.size()) {
1172         //while ((i->getId() != id) && i != activeTraffic.end()) {
1173         while (i != activeTraffic.end()) {
1174             if (i->getId() == id) {
1175                 break;
1176             }
1177             i++;
1178         }
1179     }
1180 //    // update position of the current aircraft
1181     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1182         SG_LOG(SG_GENERAL, SG_ALERT,
1183                "AI error: updating aircraft without traffic record");
1184     } else {
1185         i->setPositionAndHeading(lat, lon, heading, speed, alt);
1186         current = i;
1187         //cerr << "ApproachController: checking for speed" << endl;
1188         time_t time_diff =
1189             current->getAircraft()->
1190             checkForArrivalTime(string("final001"));
1191         if (time_diff > 15) {
1192             current->setSpeedAdjustment(current->getAircraft()->
1193                                         getPerformance()->vDescent() *
1194                                         1.35);
1195         } else if (time_diff > 5) {
1196             current->setSpeedAdjustment(current->getAircraft()->
1197                                         getPerformance()->vDescent() *
1198                                         1.2);
1199         } else if (time_diff < -15) {
1200             current->setSpeedAdjustment(current->getAircraft()->
1201                                         getPerformance()->vDescent() *
1202                                         0.65);
1203         } else if (time_diff < -5) {
1204             current->setSpeedAdjustment(current->getAircraft()->
1205                                         getPerformance()->vDescent() *
1206                                         0.8);
1207         } else {
1208             current->clearSpeedAdjustment();
1209         }
1210         //current->setSpeedAdjustment(current->getAircraft()->getPerformance()->vDescent() + time_diff);
1211     }
1212     setDt(getDt() + dt);
1213 }
1214
1215 void FGApproachController::signOff(int id)
1216 {
1217     TrafficVectorIterator i = activeTraffic.begin();
1218     // Search search if the current id alread has an entry
1219     // This might be faster using a map instead of a vector, but let's start by taking a safe route
1220     if (activeTraffic.size()) {
1221         //while ((i->getId() != id) && i != activeTraffic.end()) {
1222         while (i != activeTraffic.end()) {
1223             if (i->getId() == id) {
1224                 break;
1225             }
1226             i++;
1227         }
1228     }
1229     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1230         SG_LOG(SG_GENERAL, SG_ALERT,
1231                "AI error: Aircraft without traffic record is signing off from approach");
1232     } else {
1233         i = activeTraffic.erase(i);
1234     }
1235 }
1236
1237
1238
1239
1240 bool FGApproachController::hasInstruction(int id)
1241 {
1242     TrafficVectorIterator i = activeTraffic.begin();
1243     // Search search if the current id has an entry
1244     // This might be faster using a map instead of a vector, but let's start by taking a safe route
1245     if (activeTraffic.size()) {
1246         //while ((i->getId() != id) && i != activeTraffic.end()) {
1247         while (i != activeTraffic.end()) {
1248             if (i->getId() == id) {
1249                 break;
1250             }
1251             i++;
1252         }
1253     }
1254     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1255         SG_LOG(SG_GENERAL, SG_ALERT,
1256                "AI error: checking ATC instruction for aircraft without traffic record");
1257     } else {
1258         return i->hasInstruction();
1259     }
1260     return false;
1261 }
1262
1263
1264 FGATCInstruction FGApproachController::getInstruction(int id)
1265 {
1266     TrafficVectorIterator i = activeTraffic.begin();
1267     // Search search if the current id has an entry
1268     // This might be faster using a map instead of a vector, but let's start by taking a safe route
1269     if (activeTraffic.size()) {
1270         //while ((i->getId() != id) && i != activeTraffic.end()) {
1271         while (i != activeTraffic.end()) {
1272             if (i->getId() == id) {
1273                 break;
1274             }
1275             i++;
1276         }
1277     }
1278     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1279         SG_LOG(SG_GENERAL, SG_ALERT,
1280                "AI error: requesting ATC instruction for aircraft without traffic record");
1281     } else {
1282         return i->getInstruction();
1283     }
1284     return FGATCInstruction();
1285 }
1286
1287
1288 ActiveRunway *FGApproachController::getRunway(string name)
1289 {
1290     ActiveRunwayVecIterator rwy = activeRunways.begin();
1291     if (activeRunways.size()) {
1292         while (rwy != activeRunways.end()) {
1293             if (rwy->getRunwayName() == name) {
1294                 break;
1295             }
1296             rwy++;
1297         }
1298     }
1299     if (rwy == activeRunways.end()) {
1300         ActiveRunway aRwy(name, 0);
1301         activeRunways.push_back(aRwy);
1302         rwy = activeRunways.end() - 1;
1303     }
1304     return &(*rwy);
1305 }