1 // FGAILocalTraffic - AIEntity derived class with enough logic to
2 // fly and interact with the traffic pattern.
4 // Written by David Luff, started March 2002.
6 // Copyright (C) 2002 David C. Luff - david.luff@nottingham.ac.uk
8 // This program is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU General Public License as
10 // published by the Free Software Foundation; either version 2 of the
11 // License, or (at your option) any later version.
13 // This program is distributed in the hope that it will be useful, but
14 // WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 // General Public License for more details.
18 // You should have received a copy of the GNU General Public License
19 // along with this program; if not, write to the Free Software
20 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
22 /*==========================================================
26 Should get pattern direction from tower.
28 Need to continually monitor and adjust deviation from glideslope
29 during descent to avoid occasionally landing short or long.
31 ============================================================*/
37 #include <simgear/scene/model/location.hxx>
39 #include <Airports/runways.hxx>
40 #include <Main/globals.hxx>
41 #include <Scenery/scenery.hxx>
42 #include <Scenery/tilemgr.hxx>
43 #include <simgear/math/point3d.hxx>
44 #include <simgear/math/sg_geodesy.hxx>
45 #include <simgear/misc/sg_path.hxx>
52 #include "AILocalTraffic.hxx"
53 #include "ATCutils.hxx"
55 FGAILocalTraffic::FGAILocalTraffic() {
56 ATC = globals->get_ATC_mgr();
58 // TODO - unhardwire this - possibly let the AI manager set the callsign
59 plane.callsign = "Trainer-two-five-charlie";
60 plane.type = GA_SINGLE;
66 //Hardwire initialisation for now - a lot of this should be read in from config eventually
68 best_rate_of_climb_speed = 70.0;
70 //nominal_climb_speed;
72 //nominal_circuit_speed;
75 nominal_descent_rate = 500.0;
76 nominal_final_speed = 65.0;
77 //nominal_approach_speed;
78 //stall_speed_landing_config;
79 nominalTaxiSpeed = 7.5;
81 wheelOffset = 1.45; // Warning - hardwired to the C172 - we need to read this in from file.
83 // Init the property nodes
84 wind_from_hdg = fgGetNode("/environment/wind-from-heading-deg", true);
85 wind_speed_knots = fgGetNode("/environment/wind-speed-kt", true);
88 taxiRequestPending = false;
89 taxiRequestCleared = false;
91 clearedToLineUp = false;
92 clearedToTakeOff = false;
93 reportReadyForDeparture = false;
95 contactGround = false;
98 targetDescentRate = 0.0;
100 goAroundCalled = false;
103 FGAILocalTraffic::~FGAILocalTraffic() {
107 // Get details of the active runway
108 // It is assumed that by the time this is called the tower control and airport code will have been set up.
109 void FGAILocalTraffic::GetRwyDetails() {
110 //cout << "GetRwyDetails called" << endl;
112 rwy.rwyID = tower->GetActiveRunway();
114 // Now we need to get the threshold position and rwy heading
117 bool rwyGood = globals->get_runways()->search(airportID, rwy.rwyID,
120 // Get the threshold position
121 hdg = runway.heading; // TODO - check - is this our heading we are setting here, and if so should we be?
122 //cout << "hdg reset to " << hdg << '\n';
123 double other_way = hdg - 180.0;
124 while(other_way <= 0.0) {
128 // move to the +l end/center of the runway
129 //cout << "Runway center is at " << runway.lon << ", " << runway.lat << '\n';
130 Point3D origin = Point3D(runway.lon, runway.lat, aptElev);
131 Point3D ref = origin;
132 double tshlon, tshlat, tshr;
133 double tolon, tolat, tor;
134 rwy.length = runway.length * SG_FEET_TO_METER;
135 geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), other_way,
136 rwy.length / 2.0 - 25.0, &tshlat, &tshlon, &tshr );
137 geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), hdg,
138 rwy.length / 2.0 - 25.0, &tolat, &tolon, &tor );
139 // Note - 25 meters in from the runway end is a bit of a hack to put the plane ahead of the user.
140 // now copy what we need out of runway into rwy
141 rwy.threshold_pos = Point3D(tshlon, tshlat, aptElev);
142 Point3D takeoff_end = Point3D(tolon, tolat, aptElev);
143 //cout << "Threshold position = " << tshlon << ", " << tshlat << ", " << aptElev << '\n';
144 //cout << "Takeoff position = " << tolon << ", " << tolat << ", " << aptElev << '\n';
146 // Set the projection for the local area
147 ortho.Init(rwy.threshold_pos, rwy.hdg);
148 rwy.end1ortho = ortho.ConvertToLocal(rwy.threshold_pos); // should come out as zero
149 rwy.end2ortho = ortho.ConvertToLocal(takeoff_end);
151 SG_LOG(SG_ATC, SG_ALERT, "Help - can't get good runway in FGAILocalTraffic!!\n");
157 There are two possible scenarios during initialisation:
158 The first is that the user is flying towards the airport, and hence the traffic
159 could be initialised anywhere, as long as the AI planes are consistent with
161 The second is that the user has started the sim at or close to the airport, and
162 hence the traffic must be initialised with respect to the user as well as each other.
163 To a certain extent it's FGAIMgr that has to worry about this, but we need to provide
164 sufficient initialisation functionality within the plane classes to allow the manager
165 to initially position them where and how required.
167 bool FGAILocalTraffic::Init(string ICAO, OperatingState initialState, PatternLeg initialLeg) {
168 //cout << "FGAILocalTraffic.Init(...) called" << endl;
169 // Hack alert - Hardwired path!!
170 string planepath = "Aircraft/c172/Models/c172-dpm.ac";
171 ssgBranch *model = sgLoad3DModel( globals->get_fg_root(),
173 globals->get_props(),
174 globals->get_sim_time_sec() );
176 aip.setVisible(false); // This will be set to true once a valid ground elevation has been determined
177 globals->get_scenery()->get_scene_graph()->addKid(aip.getSceneGraph());
179 // Find the tower frequency - this is dependent on the ATC system being initialised before the AI system
182 if(ATC->GetAirportATCDetails(airportID, &a)) {
183 if(a.tower_freq) { // Has a tower
184 tower = (FGTower*)ATC->GetATCPointer(airportID, TOWER); // Maybe need some error checking here
186 // Something has gone wrong - abort or carry on with un-towered operation?
189 freq = (double)tower->get_freq() / 100.0;
190 ground = tower->GetGroundPtr();
192 // Something has gone wrong :-(
193 SG_LOG(SG_ATC, SG_ALERT, "ERROR - can't get a ground pointer from tower control in FGAILocalTraffic::Init() :-(");
195 } else if((initialState == PARKED) || (initialState == TAXIING)) {
196 freq = (double)ground->get_freq() / 100.0;
198 //cout << "AILocalTraffic freq is " << freq << '\n';
200 // TODO - Check CTAF, unicom etc
203 //cout << "Unable to find airport details in FGAILocalTraffic::Init()\n";
206 // Get the active runway details (and copy them into rwy)
209 // Get the airport elevation
210 aptElev = dclGetAirportElev(airportID.c_str()) * SG_FEET_TO_METER;
211 //cout << "Airport elev in AILocalTraffic = " << aptElev << '\n';
212 // WARNING - we use this elev for the whole airport - some assumptions in the code
213 // might fall down with very slopey airports.
215 //cout << "In Init(), initialState = " << initialState << endl;
216 operatingState = initialState;
217 switch(operatingState) {
219 tuned_station = ground;
220 ourGate = ground->GetGateNode();
221 if(ourGate == NULL) {
222 // Implies no available gates - what shall we do?
223 // For now just vanish the plane - possibly we can make this more elegant in the future
224 SG_LOG(SG_ATC, SG_ALERT, "No gate found by FGAILocalTraffic whilst attempting Init at " << airportID << '\n');
232 pos.setelev(aptElev);
233 hdg = ourGate->heading;
235 // Now we've set the position we can do the ground elev
236 elevInitGood = false;
243 tuned_station = ground;
244 // FIXME - implement this case properly
245 return(false); // remove this line when fixed!
248 // For now we'll always start the in_pattern case on the threshold ready to take-off
249 // since we've got the implementation for this case already.
250 // TODO - implement proper generic in_pattern startup.
252 // 18/10/03 - adding the ability to start on downwind (mainly to speed testing of the go-around code!!)
254 //cout << "Starting in pattern...\n";
256 tuned_station = tower;
258 circuitsToFly = 0; // ie just fly this circuit and then stop
260 // FIXME TODO - pattern direction is still hardwired
261 patternDirection = -1; // Left
262 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
263 if(rwy.rwyID.size() == 3) {
264 patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
267 if(initialLeg == DOWNWIND) {
268 pos = ortho.ConvertFromLocal(Point3D(1000*patternDirection, 800, 0.0));
269 pos.setelev(rwy.threshold_pos.elev() + 1000 * SG_FEET_TO_METER);
270 hdg = rwy.hdg + 180.0;
272 elevInitGood = false;
274 track = rwy.hdg - (180 * patternDirection); //should tend to bring track back into the 0->360 range
280 aip.setVisible(true);
281 tower->RegisterAIPlane(plane, this, CIRCUIT, DOWNWIND);
283 // Default to initial position on threshold for now
284 pos.setlat(rwy.threshold_pos.lat());
285 pos.setlon(rwy.threshold_pos.lon());
286 pos.setelev(rwy.threshold_pos.elev());
289 // Now we've set the position we can do the ground elev
290 // This might not always be necessary if we implement in-air start
291 elevInitGood = false;
302 operatingState = IN_PATTERN;
307 SG_LOG(SG_ATC, SG_ALERT, "Attempt to set unknown operating state in FGAILocalTraffic.Init(...)\n");
316 // Return what type of landing we're doing on this circuit
317 LandingType FGAILocalTraffic::GetLandingOption() {
318 //cout << "circuitsToFly = " << circuitsToFly << '\n';
320 return(touchAndGo ? TOUCH_AND_GO : STOP_AND_GO);
327 // Commands to do something from higher level logic
328 void FGAILocalTraffic::FlyCircuits(int numCircuits, bool tag) {
329 //cout << "FlyCircuits called" << endl;
331 switch(operatingState) {
333 circuitsToFly += numCircuits;
337 // TODO - For now we'll punt this and do nothing
340 circuitsToFly = numCircuits; // Note that one too many circuits gets flown because we only test and decrement circuitsToFly after landing
341 // thus flying one too many circuits. TODO - Need to sort this out better!
347 // Run the internal calculations
348 void FGAILocalTraffic::Update(double dt) {
349 //cout << "A" << flush;
350 //double responseTime = 10.0; // seconds - this should get more sophisticated at some point
351 responseCounter += dt;
352 if((contactTower) && (responseCounter >= 8.0)) {
353 // Acknowledge request before changing frequency so it gets rendered if the user is on the same freq
354 string trns = "Tower ";
355 double f = globals->get_ATC_mgr()->GetFrequency(airportID, TOWER) / 100.0;
357 sprintf(buf, "%.2f", f);
360 trns += plane.callsign;
361 pending_transmission = trns;
362 ConditionalTransmit(30.0);
363 responseCounter = 0.0;
364 contactTower = false;
366 changeFreqType = TOWER;
369 if((changeFreq) && (responseCounter > 8.0)) {
370 switch(changeFreqType) {
372 tuned_station = tower;
373 freq = (double)tower->get_freq() / 100.0;
375 // Contact the tower, even if only virtually
377 pending_transmission = plane.callsign;
378 pending_transmission += " at hold short for runway ";
379 pending_transmission += ConvertRwyNumToSpokenString(rwy.rwyID);
380 pending_transmission += " traffic pattern ";
382 pending_transmission += ConvertNumToSpokenDigits(circuitsToFly + 1);
383 pending_transmission += " circuits touch and go";
385 pending_transmission += " one circuit to full stop";
390 tuned_station = ground;
391 freq = (double)ground->get_freq() / 100.0;
393 // And to avoid compiler warnings...
394 case APPROACH: break;
397 case DEPARTURE: break;
402 //cout << "." << flush;
404 switch(operatingState) {
406 //cout << "In IN_PATTERN\n";
410 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
411 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
412 //cout << "TAKEOFF_ROLL, POS = " << pos.lon() << ", " << pos.lat() << ", " << pos.elev() << '\n';
414 aip.setVisible(true);
415 //cout << "Making plane visible!\n";
420 FlyTrafficPattern(dt);
424 //cout << "In TAXIING\n";
425 //cout << "*" << flush;
428 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
429 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
431 aip.setVisible(true);
433 //cout << "Making plane visible!\n";
438 //cout << "," << flush;
439 if(!((holdingShort) && (!clearedToLineUp))) {
440 //cout << "|" << flush;
443 //cout << ";" << flush;
444 if((clearedToTakeOff) && (responseCounter >= 8.0)) {
445 // possible assumption that we're at the hold short here - may not always hold
446 // TODO - sort out the case where we're cleared to line-up first and then cleared to take-off on the rwy.
447 taxiState = TD_LINING_UP;
448 path = ground->GetPath(holdShortNode, rwy.rwyID);
450 cout << "path returned was:" << endl;
451 for(unsigned int i=0; i<path.size(); ++i) {
452 switch(path[i]->struct_type) {
454 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
462 clearedToTakeOff = false; // We *are* still cleared - this simply stops the response recurring!!
463 holdingShort = false;
464 string trns = "Cleared for take-off ";
465 trns += plane.callsign;
466 pending_transmission = trns;
470 //cout << "^" << flush;
474 //cout << "In PARKED\n";
477 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
478 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
480 aip.setVisible(true);
482 //cout << "Making plane visible!\n";
488 if((taxiRequestPending) && (taxiRequestCleared)) {
489 //cout << "&" << flush;
490 // Get the active runway details (in case they've changed since init)
493 // Get the takeoff node for the active runway, get a path to it and start taxiing
494 path = ground->GetPathToHoldShort(ourGate, rwy.rwyID);
495 if(path.size() < 2) {
496 // something has gone wrong
497 SG_LOG(SG_ATC, SG_ALERT, "Invalid path from gate to theshold in FGAILocalTraffic::FlyCircuits\n");
501 cout << "path returned was:\n";
502 for(unsigned int i=0; i<path.size(); ++i) {
503 switch(path[i]->struct_type) {
505 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
513 path.erase(path.begin()); // pop the gate - we're here already!
514 taxiState = TD_OUTBOUND;
515 taxiRequestPending = false;
516 holdShortNode = (node*)(*(path.begin() + path.size()));
518 } else if(!taxiRequestPending) {
519 //cout << "(" << flush;
520 // Do some communication
521 // airport name + tower + airplane callsign + location + request taxi for + operation type + ?
523 trns += tower->get_name();
525 trns += plane.callsign;
526 trns += " on apron parking request taxi for traffic pattern";
527 //cout << "trns = " << trns << endl;
528 pending_transmission = trns;
530 taxiRequestCleared = false;
531 taxiRequestPending = true;
535 //cout << "!" << flush;
537 // Maybe the below should be set when we get to the threshold and prepare for TO?
538 // FIXME TODO - pattern direction is still hardwired
539 patternDirection = -1; // Left
540 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
541 if(rwy.rwyID.size() == 3) {
542 patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
546 //cout << ")" << flush;
551 //cout << "I " << flush;
553 // Convienience output for AI debugging user the property logger
554 fgSetDouble("/AI/Local1/ortho-x", (ortho.ConvertToLocal(pos)).x());
555 fgSetDouble("/AI/Local1/ortho-y", (ortho.ConvertToLocal(pos)).y());
556 fgSetDouble("/AI/Local1/elev", pos.elev() * SG_METER_TO_FEET);
558 // And finally, call parent for transmission rendering
559 FGAIPlane::Update(dt);
562 void FGAILocalTraffic::RegisterTransmission(int code) {
564 case 1: // taxi request cleared
565 taxiRequestCleared = true;
566 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to taxi...");
568 case 2: // contact tower
571 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " told to contact tower...");
573 case 3: // Cleared to line up
575 clearedToLineUp = true;
576 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to line-up...");
578 case 4: // cleared to take-off
580 clearedToTakeOff = true;
581 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to take-off...");
583 case 13: // Go around!
586 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " told to go-around!!");
593 // Fly a traffic pattern
594 // FIXME - far too much of the mechanics of turning, rolling, accellerating, descending etc is in here.
595 // Move it out to FGAIPlane and have FlyTrafficPattern just specify what to do, not the implementation.
596 void FGAILocalTraffic::FlyTrafficPattern(double dt) {
597 // Need to differentiate between in-air (IAS governed) and on-ground (vel governed)
598 // Take-off is an interesting case - we are on the ground but takeoff speed is IAS governed.
600 static bool transmitted = false; // FIXME - this is a hack
603 // Wind has two effects - a mechanical one in that IAS translates to a different vel, and the hdg != track,
604 // but also a piloting effect, in that the AI must be able to descend at a different rate in order to hit the threshold.
606 //cout << "dt = " << dt << '\n';
608 // ack - I can't remember how long a rate 1 turn is meant to take.
609 double turn_time = 60.0; // seconds - TODO - check this guess
610 double turn_circumference;
612 Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane
613 //cout << "runway elev = " << rwy.threshold_pos.elev() << ' ' << rwy.threshold_pos.elev() * SG_METER_TO_FEET << '\n';
614 //cout << "elev = " << pos.elev() << ' ' << pos.elev() * SG_METER_TO_FEET << '\n';
616 // HACK FOR TESTING - REMOVE
617 //cout << "Calling ExitRunway..." << endl;
618 //ExitRunway(orthopos);
623 double wind_from = wind_from_hdg->getDoubleValue();
624 double wind_speed = wind_speed_knots->getDoubleValue();
636 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
637 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
639 IAS = vel + (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
643 IAS = best_rate_of_climb_speed;
645 slope = 6.0; // Reduced it slightly since it's climbing a lot steeper than I can in the JSBSim C172.
651 // Turn to crosswind if above 700ft AND if other traffic allows
652 // (decided in FGTower and accessed through GetCrosswindConstraint(...)).
653 // According to AIM, traffic should climb to within 300ft of pattern altitude before commencing crosswind turn.
654 // TODO - At hot 'n high airports this may be 500ft AGL though - need to make this a variable.
655 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 700) {
657 if(tower->GetCrosswindConstraint(cc)) {
658 if(orthopos.y() > cc) {
659 cout << "Turning to crosswind, distance from threshold = " << orthopos.y() << '\n';
662 } else if(orthopos.y() > 1500.0) { // Added this constraint as a hack to prevent turning too early when going around.
663 // TODO - We should be doing it as a distance from takeoff end, not theshold end though.
664 cout << "Turning to crosswind, distance from threshold = " << orthopos.y() << '\n';
668 // Need to check for levelling off in case we can't turn crosswind as soon
669 // as we would like due to other traffic.
670 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
673 IAS = 80.0; // FIXME - use smooth transistion to new speed and attitude.
675 if(goAround && !goAroundCalled) {
676 if(responseCounter > 5.5) {
677 pending_transmission = plane.callsign;
678 pending_transmission += " going around";
680 goAroundCalled = true;
685 track += (360.0 / turn_time) * dt * patternDirection;
686 Bank(25.0 * patternDirection);
687 if((track < (rwy.hdg - 89.0)) || (track > (rwy.hdg + 89.0))) {
694 track = rwy.hdg + (90.0 * patternDirection);
695 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
698 IAS = 80.0; // FIXME - use smooth transistion to new speed
700 // turn 1000m out for now, taking other traffic into accout
701 if(fabs(orthopos.x()) > 980) {
703 if(tower->GetDownwindConstraint(dd)) {
704 if(fabs(orthopos.x()) > fabs(dd)) {
705 cout << "Turning to downwind, distance from centerline = " << fabs(orthopos.x()) << '\n';
709 cout << "Turning to downwind, distance from centerline = " << fabs(orthopos.x()) << '\n';
715 track += (360.0 / turn_time) * dt * patternDirection;
716 Bank(25.0 * patternDirection);
717 // just in case we didn't make height on crosswind
718 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
721 IAS = 80.0; // FIXME - use smooth transistion to new speed
723 if((track < (rwy.hdg - 179.0)) || (track > (rwy.hdg + 179.0))) {
731 track = rwy.hdg - (180 * patternDirection); //should tend to bring track back into the 0->360 range
732 // just in case we didn't make height on crosswind
733 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
736 IAS = 90.0; // FIXME - use smooth transistion to new speed
738 if((orthopos.y() < 0) && (!transmitted)) {
739 TransmitPatternPositionReport();
742 if((orthopos.y() < -100) && (!descending)) {
743 // Maybe we should think about when to start descending.
744 // For now we're assuming that we aim to follow the same glidepath regardless of wind.
747 CalculateSoD((tower->GetBaseConstraint(d1) ? d1 : -1000.0), (tower->GetDownwindConstraint(d2) ? d2 : 1000.0 * patternDirection), (patternDirection ? true : false));
748 if(SoD.leg == DOWNWIND) {
749 descending = (orthopos.y() < SoD.y ? true : false);
754 slope = -5.5; // FIXME - calculate to descent at 500fpm and hit the desired point on the runway (taking wind into account as well!!)
759 // Try and arrange to turn nicely onto base
760 turn_circumference = IAS * 0.514444 * turn_time;
761 //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius
762 //We'll leave it as a hack with IAS for now but it needs revisiting.
763 turn_radius = turn_circumference / (2.0 * DCL_PI);
764 if(orthopos.y() < -1000.0 + turn_radius) {
765 //if(orthopos.y() < -980) {
767 if(tower->GetBaseConstraint(bb)) {
768 if(fabs(orthopos.y()) > fabs(bb)) {
769 cout << "Turning to base, distance from threshold = " << fabs(orthopos.y()) << '\n';
775 cout << "Turning to base, distance from threshold = " << fabs(orthopos.y()) << '\n';
783 track += (360.0 / turn_time) * dt * patternDirection;
784 Bank(25.0 * patternDirection);
785 if(fabs(rwy.hdg - track) < 91.0) {
792 TransmitPatternPositionReport();
798 // Make downwind leg position artifically large to avoid any chance of SoD being returned as
799 // on downwind when we are already on base.
800 CalculateSoD((tower->GetBaseConstraint(d1) ? d1 : -1000.0), (10000.0 * patternDirection), (patternDirection ? true : false));
801 if(SoD.leg == BASE) {
802 descending = (fabs(orthopos.y()) < fabs(SoD.y) ? true : false);
807 slope = -5.5; // FIXME - calculate to descent at 500fpm and hit the threshold (taking wind into account as well!!)
812 track = rwy.hdg - (90 * patternDirection);
814 // Try and arrange to turn nicely onto final
815 turn_circumference = IAS * 0.514444 * turn_time;
816 //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius
817 //We'll leave it as a hack with IAS for now but it needs revisiting.
818 turn_radius = turn_circumference / (2.0 * DCL_PI);
819 if(fabs(orthopos.x()) < (turn_radius + 50)) {
826 track += (360.0 / turn_time) * dt * patternDirection;
827 Bank(25.0 * patternDirection);
828 if(fabs(track - rwy.hdg) < 0.6) {
830 vel = nominal_final_speed;
834 if(goAround && responseCounter > 2.0) {
837 IAS = best_rate_of_climb_speed;
838 slope = 5.0; // A bit less steep than the initial climbout.
840 goAroundCalled = false;
845 TransmitPatternPositionReport();
849 // Make base leg position artifically large to avoid any chance of SoD being returned as
850 // on base or downwind when we are already on final.
851 CalculateSoD(-10000.0, (1000.0 * patternDirection), (patternDirection ? true : false));
852 if(SoD.leg == FINAL) {
853 descending = (fabs(orthopos.y()) < fabs(SoD.y) ? true : false);
858 slope = -5.5; // FIXME - calculate to descent at 500fpm and hit the threshold (taking wind into account as well!!)
862 // Try and track the extended centreline
863 track = rwy.hdg - (0.2 * orthopos.x());
864 //cout << "orthopos.x() = " << orthopos.x() << " hdg = " << hdg << '\n';
865 if(pos.elev() < (rwy.threshold_pos.elev()+20.0+wheelOffset)) {
866 DoGroundElev(); // Need to call it here expicitly on final since it's only called
867 // for us in update(...) when the inAir flag is false.
869 if(pos.elev() < (rwy.threshold_pos.elev()+10.0+wheelOffset)) {
870 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
871 if((aip.getSGLocation()->get_cur_elev_m() + wheelOffset) > pos.elev()) {
877 } // else need a fallback position based on arpt elev in case ground elev determination fails?
883 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
884 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
889 // FIXME - differentiate between touch and go and full stops
891 //cout << "Vel <= 15.0, circuitsToFly = " << circuitsToFly << endl;
892 if(circuitsToFly <= 0) {
893 //cout << "Calling ExitRunway..." << endl;
894 ExitRunway(orthopos);
897 //cout << "Taking off again..." << endl;
908 // FIXME - at the moment this is a bit screwy
909 // The velocity correction is applied based on the relative headings.
910 // Then the heading is changed based on the velocity.
911 // Which comes first, the chicken or the egg?
912 // Does it really matter?
914 // Apply wind to ground-relative velocity if in the air
915 vel = IAS - (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
916 //crab = f(track, wind, vel);
917 // The vector we need to fly is our desired vector minus the wind vector
918 // TODO - we probably ought to use plib's built in vector types and operations for this
919 // ie. There's almost *certainly* a better way to do this!
920 double gxx = vel * sin(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity x component wrt ground
921 double gyy = vel * cos(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity y component wrt ground
922 double wxx = wind_speed * sin((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS); // Wind velocity x component
923 double wyy = wind_speed * cos((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS); // Wind velocity y component
924 double axx = gxx - wxx; // Plane in-air velocity x component
925 double ayy = gyy - wyy; // Plane in-air velocity y component
926 // Now we want the angle between gxx and axx (which is the crab)
927 double maga = sqrt(axx*axx + ayy*ayy);
928 double magg = sqrt(gxx*gxx + gyy*gyy);
929 crab = acos((axx*gxx + ayy*gyy) / (maga * magg));
930 // At this point this works except we're getting the modulus of the angle
931 //cout << "crab = " << crab << '\n';
933 // Make sure both headings are in the 0->360 circle in order to get sane differences
934 dclBoundHeading(wind_from);
935 dclBoundHeading(track);
936 if(track > wind_from) {
937 if((track - wind_from) <= 180) {
941 if((wind_from - track) >= 180) {
945 } else { // on the ground - crab dosen't apply
950 dist = vel * 0.514444 * dt;
951 pos = dclUpdatePosition(pos, track, slope, dist);
954 // Pattern direction is true for right, false for left
955 void FGAILocalTraffic::CalculateSoD(double base_leg_pos, double downwind_leg_pos, bool pattern_direction) {
956 // For now we'll ignore wind and hardwire the glide angle.
957 double ga = 5.5; //degrees
958 double pa = 1000.0 * SG_FEET_TO_METER; // pattern altitude in meters
959 // FIXME - get glideslope angle and pattern altitude agl from airport details if available
961 // For convienience, we'll have +ve versions of the input distances
962 double blp = fabs(base_leg_pos);
963 double dlp = fabs(downwind_leg_pos);
965 //double turn_allowance = 150.0; // Approximate distance in meters that a 90deg corner is shortened by turned in a light plane.
967 double stod = pa / tan(ga * DCL_DEGREES_TO_RADIANS); // distance in meters from touchdown point to start descent
968 //cout << "Descent to start = " << stod << " meters out\n";
969 if(stod < blp) { // Start descending on final
973 } else if(stod < (blp + dlp)) { // Start descending on base leg
976 SoD.x = (pattern_direction ? (stod - dlp) : (stod - dlp) * -1.0);
977 } else { // Start descending on downwind leg
979 SoD.x = (pattern_direction ? dlp : dlp * -1.0);
980 SoD.y = (blp - (stod - (blp + dlp))) * -1.0;
984 void FGAILocalTraffic::TransmitPatternPositionReport(void) {
985 // airport name + "traffic" + airplane callsign + pattern direction + pattern leg + rwy + ?
989 trns += tower->get_name();
991 trns += plane.callsign;
992 if(patternDirection == 1) {
998 // We could probably get rid of this whole switch statement and just pass a string containing the leg from the FlyPattern function.
999 switch(leg) { // We'll assume that transmissions in turns are intended for next leg - do pilots ever call out that they are in the turn?
1001 // Fall through to CROSSWIND
1002 case CROSSWIND: // I don't think this case will be used here but it can't hurt to leave it in
1003 trns += "crosswind ";
1006 // Fall through to DOWNWIND
1008 trns += "downwind ";
1012 // Fall through to BASE
1017 // Fall through to FINAL
1018 case FINAL: // maybe this should include long/short final if appropriate?
1022 default: // Hopefully this won't be used
1026 trns += ConvertRwyNumToSpokenString(rwy.rwyID);
1028 // And add the airport name again
1029 trns += tower->get_name();
1031 pending_transmission = trns; // FIXME - make up pending_transmission natively
1032 ConditionalTransmit(90.0, code); // Assume a report of this leg will be invalid if we can't transmit within a minute and a half.
1036 // TODO - Really should enumerate these coded values.
1037 void FGAILocalTraffic::ProcessCallback(int code) {
1038 // 1 - Request Departure from ground
1039 // 2 - Report at hold short
1040 // 10 - report crosswind
1041 // 11 - report downwind
1043 // 13 - report final
1045 ground->RequestDeparture(plane, this);
1046 } else if(code == 2) {
1047 tower->ContactAtHoldShort(plane, this, CIRCUIT);
1048 } else if(code == 11) {
1049 tower->ReportDownwind(plane.callsign);
1050 } else if(code == 13) {
1051 tower->ReportFinal(plane.callsign);
1055 void FGAILocalTraffic::ExitRunway(Point3D orthopos) {
1056 //cout << "In ExitRunway" << endl;
1057 //cout << "Runway ID is " << rwy.ID << endl;
1058 node_array_type exitNodes = ground->GetExits(rwy.rwyID); //I suppose we ought to have some fallback for rwy with no defined exits?
1060 cout << "Node ID's of exits are ";
1061 for(unsigned int i=0; i<exitNodes.size(); ++i) {
1062 cout << exitNodes[i]->nodeID << ' ';
1066 if(exitNodes.size()) {
1067 //Find the next exit from orthopos.y
1069 double dist = 100000; //ie. longer than any runway in existance
1070 double backdist = 100000;
1071 node_array_iterator nItr = exitNodes.begin();
1072 node* rwyExit = *(exitNodes.begin());
1073 //int gateID; //This might want to be more persistant at some point
1074 while(nItr != exitNodes.end()) {
1075 d = ortho.ConvertToLocal((*nItr)->pos).y() - ortho.ConvertToLocal(pos).y(); //FIXME - consider making orthopos a class variable
1082 if(fabs(d) < backdist) {
1084 //TODO - need some logic here that if we don't get a forward exit we turn round and store the backwards one
1089 ourGate = ground->GetGateNode();
1090 if(ourGate == NULL) {
1091 // Implies no available gates - what shall we do?
1092 // For now just vanish the plane - possibly we can make this more elegant in the future
1093 SG_LOG(SG_ATC, SG_ALERT, "No gate found by FGAILocalTraffic whilst landing at " << airportID << '\n');
1094 aip.setVisible(false);
1095 operatingState = PARKED;
1098 path = ground->GetPath(rwyExit, ourGate);
1100 cout << "path returned was:" << endl;
1101 for(unsigned int i=0; i<path.size(); ++i) {
1102 switch(path[i]->struct_type) {
1104 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
1112 taxiState = TD_INBOUND;
1115 // Something must have gone wrong with the ground network file - or there is only a rwy here and no exits defined
1116 SG_LOG(SG_ATC, SG_ALERT, "No exits found by FGAILocalTraffic from runway " << rwy.rwyID << " at " << airportID << '\n');
1117 // What shall we do - just remove the plane from sight?
1118 aip.setVisible(false);
1119 operatingState = PARKED;
1123 // Set the class variable nextTaxiNode to the next node in the path
1124 // and update taxiPathPos, the class variable path iterator position
1125 // TODO - maybe should return error codes to the calling function if we fail here
1126 void FGAILocalTraffic::GetNextTaxiNode() {
1127 //cout << "GetNextTaxiNode called " << endl;
1128 //cout << "taxiPathPos = " << taxiPathPos << endl;
1129 ground_network_path_iterator pathItr = path.begin() + taxiPathPos;
1130 if(pathItr == path.end()) {
1131 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - no more nodes in path\n");
1133 if((*pathItr)->struct_type == NODE) {
1134 //cout << "ITS A NODE" << endl;
1135 //*pathItr = new node;
1136 nextTaxiNode = (node*)*pathItr;
1140 //cout << "ITS NOT A NODE" << endl;
1141 //The first item in found must have been an arc
1142 //Assume for now that it was straight
1145 if(pathItr == path.end()) {
1146 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - path ended with an arc\n");
1147 } else if((*pathItr)->struct_type == NODE) {
1148 nextTaxiNode = (node*)*pathItr;
1151 //OOPS - two non-nodes in a row - that shouldn't happen ATM
1152 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - two non-nodes in sequence\n");
1158 // StartTaxi - set up the taxiing state - call only at the start of taxiing
1159 void FGAILocalTraffic::StartTaxi() {
1160 //cout << "StartTaxi called" << endl;
1161 operatingState = TAXIING;
1164 //Set the desired heading
1165 //Assume we are aiming for first node on path
1166 //Eventually we may need to consider the fact that we might start on a curved arc and
1167 //not be able to head directly for the first node.
1168 GetNextTaxiNode(); // sets the class variable nextTaxiNode to the next taxi node!
1169 desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
1170 //cout << "First taxi heading is " << desiredTaxiHeading << endl;
1173 // speed in knots, headings in degrees, radius in meters.
1174 static double TaxiTurnTowardsHeading(double current_hdg, double desired_hdg, double speed, double radius, double dt) {
1175 // wrap heading - this prevents a logic bug where the plane would just go round in circles!!
1176 while(current_hdg < 0.0) {
1177 current_hdg += 360.0;
1179 while(current_hdg > 360.0) {
1180 current_hdg -= 360.0;
1182 if(fabs(current_hdg - desired_hdg) > 0.1) {
1183 // Which is the quickest direction to turn onto heading?
1184 if(desired_hdg > current_hdg) {
1185 if((desired_hdg - current_hdg) <= 180) {
1187 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1188 // TODO - check that increments are less than the delta that we check for the right direction
1189 // Probably need to reduce convergence speed as convergence is reached
1191 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1194 if((current_hdg - desired_hdg) <= 180) {
1196 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1197 // TODO - check that increments are less than the delta that we check for the right direction
1198 // Probably need to reduce convergence speed as convergence is reached
1200 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1204 return(current_hdg);
1207 void FGAILocalTraffic::Taxi(double dt) {
1208 //cout << "Taxi called" << endl;
1209 // Logic - if we are further away from next point than turn radius then head for it
1210 // If we have reached turning point then get next point and turn onto that heading
1211 // Look out for the finish!!
1213 //Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane
1214 desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
1216 bool lastNode = (taxiPathPos == path.size() ? true : false);
1218 //cout << "LAST NODE\n";
1221 // HACK ALERT! - for now we will taxi at constant speed for straights and turns
1223 // Remember that hdg is always equal to track when taxiing so we don't have to consider them both
1224 double dist_to_go = dclGetHorizontalSeparation(pos, nextTaxiNode->pos); // we may be able to do this more cheaply using orthopos
1225 //cout << "dist_to_go = " << dist_to_go << endl;
1226 if((nextTaxiNode->type == GATE) && (dist_to_go <= 0.1)) {
1227 // This might be more robust to outward paths starting with a gate if we check for either
1228 // last node or TD_INBOUND ?
1230 operatingState = PARKED;
1231 } else if(((dist_to_go > taxiTurnRadius) || (nextTaxiNode->type == GATE)) && (!liningUp)){
1232 // if the turn radius is r, and speed is s, then in a time dt we turn through
1233 // ((s.dt)/(PI.r)) x 180 degrees
1234 // or alternatively (s.dt)/r radians
1235 //cout << "hdg = " << hdg << " desired taxi heading = " << desiredTaxiHeading << '\n';
1236 hdg = TaxiTurnTowardsHeading(hdg, desiredTaxiHeading, nominalTaxiSpeed, taxiTurnRadius, dt);
1237 double vel = nominalTaxiSpeed;
1238 //cout << "vel = " << vel << endl;
1239 double dist = vel * 0.514444 * dt;
1240 //cout << "dist = " << dist << endl;
1242 //cout << "track = " << track << endl;
1244 pos = dclUpdatePosition(pos, track, slope, dist);
1245 //cout << "Updated position...\n";
1246 if(aip.getSGLocation()->get_cur_elev_m() > -9990) {
1247 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
1248 } // else don't change the elev until we get a valid ground elev again!
1249 } else if(lastNode) {
1250 if(taxiState == TD_LINING_UP) {
1251 if((!liningUp) && (dist_to_go <= taxiTurnRadius)) {
1255 hdg = TaxiTurnTowardsHeading(hdg, rwy.hdg, nominalTaxiSpeed, taxiTurnRadius, dt);
1256 double vel = nominalTaxiSpeed;
1257 //cout << "vel = " << vel << endl;
1258 double dist = vel * 0.514444 * dt;
1259 //cout << "dist = " << dist << endl;
1261 //cout << "track = " << track << endl;
1263 pos = dclUpdatePosition(pos, track, slope, dist);
1264 //cout << "Updated position...\n";
1265 if(aip.getSGLocation()->get_cur_elev_m() > -9990) {
1266 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
1267 } // else don't change the elev until we get a valid ground elev again!
1268 if(fabs(hdg - rwy.hdg) <= 1.0) {
1269 operatingState = IN_PATTERN;
1275 } else if(taxiState == TD_OUTBOUND) {
1276 // Pause awaiting further instructions
1277 // and for now assume we've reached the hold-short node
1278 holdingShort = true;
1279 } // else at the moment assume TD_INBOUND always ends in a gate in which case we can ignore it
1281 // Time to turn (we've already checked it's not the end we're heading for).
1282 // set the target node to be the next node which will prompt automatically turning onto
1283 // the right heading in the stuff above, with the usual provisos applied.
1285 // For now why not just recursively call this function?
1291 // Warning - ground elev determination is CPU intensive
1292 // Either this function or the logic of how often it is called
1293 // will almost certainly change.
1294 void FGAILocalTraffic::DoGroundElev() {
1296 // It would be nice if we could set the correct tile center here in order to get a correct
1297 // answer with one call to the function, but what I tried in the two commented-out lines
1298 // below only intermittently worked, and I haven't quite groked why yet.
1299 //SGBucket buck(pos.lon(), pos.lat());
1300 //aip.getSGLocation()->set_tile_center(Point3D(buck.get_center_lon(), buck.get_center_lat(), 0.0));
1302 double visibility_meters = fgGetDouble("/environment/visibility-m");
1303 //globals->get_tile_mgr()->prep_ssg_nodes( acmodel_location,
1304 globals->get_tile_mgr()->prep_ssg_nodes( aip.getSGLocation(), visibility_meters );
1305 Point3D scenery_center = globals->get_scenery()->get_center();
1306 globals->get_tile_mgr()->update( aip.getSGLocation(), visibility_meters, (aip.getSGLocation())->get_absolute_view_pos( scenery_center ) );
1307 // save results of update in SGLocation for fdm...
1309 //if ( globals->get_scenery()->get_cur_elev() > -9990 ) {
1310 // acmodel_location->
1311 // set_cur_elev_m( globals->get_scenery()->get_cur_elev() );
1314 // The need for this here means that at least 2 consecutive passes are needed :-(
1315 aip.getSGLocation()->set_tile_center( globals->get_scenery()->get_next_center() );
1317 //cout << "Transform Elev is " << globals->get_scenery()->get_cur_elev() << '\n';
1318 aip.getSGLocation()->set_cur_elev_m(globals->get_scenery()->get_cur_elev());
1319 //return(globals->get_scenery()->get_cur_elev());