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.
26 #include <simgear/scene/model/location.hxx>
28 #include <Airports/runways.hxx>
29 #include <Main/globals.hxx>
30 #include <Scenery/scenery.hxx>
31 #include <Scenery/tilemgr.hxx>
32 #include <simgear/math/point3d.hxx>
33 #include <simgear/math/sg_geodesy.hxx>
34 #include <simgear/misc/sg_path.hxx>
41 #include "AILocalTraffic.hxx"
42 #include "ATCutils.hxx"
44 FGAILocalTraffic::FGAILocalTraffic() {
45 ATC = globals->get_ATC_mgr();
47 // TODO - unhardwire this - possibly let the AI manager set the callsign
48 plane.callsign = "Trainer-two-five-charlie";
49 plane.type = GA_SINGLE;
55 //Hardwire initialisation for now - a lot of this should be read in from config eventually
57 best_rate_of_climb_speed = 70.0;
59 //nominal_climb_speed;
61 //nominal_circuit_speed;
64 nominal_descent_rate = 500.0;
65 nominal_final_speed = 65.0;
66 //nominal_approach_speed;
67 //stall_speed_landing_config;
68 nominalTaxiSpeed = 7.5;
70 wheelOffset = 1.45; // Warning - hardwired to the C172 - we need to read this in from file.
72 // Init the property nodes
73 wind_from_hdg = fgGetNode("/environment/wind-from-heading-deg", true);
74 wind_speed_knots = fgGetNode("/environment/wind-speed-kt", true);
77 taxiRequestPending = false;
78 taxiRequestCleared = false;
80 clearedToLineUp = false;
81 clearedToTakeOff = false;
82 reportReadyForDeparture = false;
84 contactGround = false;
87 targetDescentRate = 0.0;
89 goAroundCalled = false;
92 FGAILocalTraffic::~FGAILocalTraffic() {
96 // Get details of the active runway
97 // It is assumed that by the time this is called the tower control and airport code will have been set up.
98 void FGAILocalTraffic::GetRwyDetails() {
99 //cout << "GetRwyDetails called" << endl;
101 rwy.rwyID = tower->GetActiveRunway();
103 // Now we need to get the threshold position and rwy heading
106 bool rwyGood = globals->get_runways()->search(airportID, rwy.rwyID,
109 // Get the threshold position
110 hdg = runway.heading; // TODO - check - is this our heading we are setting here, and if so should we be?
111 //cout << "hdg reset to " << hdg << '\n';
112 double other_way = hdg - 180.0;
113 while(other_way <= 0.0) {
117 // move to the +l end/center of the runway
118 //cout << "Runway center is at " << runway.lon << ", " << runway.lat << '\n';
119 Point3D origin = Point3D(runway.lon, runway.lat, aptElev);
120 Point3D ref = origin;
121 double tshlon, tshlat, tshr;
122 double tolon, tolat, tor;
123 rwy.length = runway.length * SG_FEET_TO_METER;
124 geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), other_way,
125 rwy.length / 2.0 - 25.0, &tshlat, &tshlon, &tshr );
126 geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), hdg,
127 rwy.length / 2.0 - 25.0, &tolat, &tolon, &tor );
128 // Note - 25 meters in from the runway end is a bit of a hack to put the plane ahead of the user.
129 // now copy what we need out of runway into rwy
130 rwy.threshold_pos = Point3D(tshlon, tshlat, aptElev);
131 Point3D takeoff_end = Point3D(tolon, tolat, aptElev);
132 //cout << "Threshold position = " << tshlon << ", " << tshlat << ", " << aptElev << '\n';
133 //cout << "Takeoff position = " << tolon << ", " << tolat << ", " << aptElev << '\n';
135 // Set the projection for the local area
136 ortho.Init(rwy.threshold_pos, rwy.hdg);
137 rwy.end1ortho = ortho.ConvertToLocal(rwy.threshold_pos); // should come out as zero
138 rwy.end2ortho = ortho.ConvertToLocal(takeoff_end);
140 SG_LOG(SG_ATC, SG_ALERT, "Help - can't get good runway in FGAILocalTraffic!!\n");
146 There are two possible scenarios during initialisation:
147 The first is that the user is flying towards the airport, and hence the traffic
148 could be initialised anywhere, as long as the AI planes are consistent with
150 The second is that the user has started the sim at or close to the airport, and
151 hence the traffic must be initialised with respect to the user as well as each other.
152 To a certain extent it's FGAIMgr that has to worry about this, but we need to provide
153 sufficient initialisation functionality within the plane classes to allow the manager
154 to initially position them where and how required.
156 bool FGAILocalTraffic::Init(string ICAO, OperatingState initialState, PatternLeg initialLeg) {
157 //cout << "FGAILocalTraffic.Init(...) called" << endl;
158 // Hack alert - Hardwired path!!
159 string planepath = "Aircraft/c172/Models/c172-dpm.ac";
160 ssgBranch *model = sgLoad3DModel( globals->get_fg_root(),
162 globals->get_props(),
163 globals->get_sim_time_sec() );
165 aip.setVisible(false); // This will be set to true once a valid ground elevation has been determined
166 globals->get_scenery()->get_scene_graph()->addKid(aip.getSceneGraph());
168 // Find the tower frequency - this is dependent on the ATC system being initialised before the AI system
171 if(ATC->GetAirportATCDetails(airportID, &a)) {
172 if(a.tower_freq) { // Has a tower
173 tower = (FGTower*)ATC->GetATCPointer(airportID, TOWER); // Maybe need some error checking here
175 // Something has gone wrong - abort or carry on with un-towered operation?
178 freq = (double)tower->get_freq() / 100.0;
179 ground = tower->GetGroundPtr();
181 // Something has gone wrong :-(
182 SG_LOG(SG_ATC, SG_ALERT, "ERROR - can't get a ground pointer from tower control in FGAILocalTraffic::Init() :-(");
184 } else if((initialState == PARKED) || (initialState == TAXIING)) {
185 freq = (double)ground->get_freq() / 100.0;
187 //cout << "AILocalTraffic freq is " << freq << '\n';
189 // TODO - Check CTAF, unicom etc
192 //cout << "Unable to find airport details in FGAILocalTraffic::Init()\n";
195 // Get the active runway details (and copy them into rwy)
198 // Get the airport elevation
199 aptElev = dclGetAirportElev(airportID.c_str()) * SG_FEET_TO_METER;
200 //cout << "Airport elev in AILocalTraffic = " << aptElev << '\n';
201 // WARNING - we use this elev for the whole airport - some assumptions in the code
202 // might fall down with very slopey airports.
204 //cout << "In Init(), initialState = " << initialState << endl;
205 operatingState = initialState;
206 switch(operatingState) {
208 tuned_station = ground;
209 ourGate = ground->GetGateNode();
210 if(ourGate == NULL) {
211 // Implies no available gates - what shall we do?
212 // For now just vanish the plane - possibly we can make this more elegant in the future
213 SG_LOG(SG_ATC, SG_ALERT, "No gate found by FGAILocalTraffic whilst attempting Init at " << airportID << '\n');
221 pos.setelev(aptElev);
222 hdg = ourGate->heading;
224 // Now we've set the position we can do the ground elev
225 elevInitGood = false;
232 tuned_station = ground;
233 // FIXME - implement this case properly
234 return(false); // remove this line when fixed!
237 // For now we'll always start the in_pattern case on the threshold ready to take-off
238 // since we've got the implementation for this case already.
239 // TODO - implement proper generic in_pattern startup.
241 // 18/10/03 - adding the ability to start on downwind (mainly to speed testing of the go-around code!!)
243 //cout << "Starting in pattern...\n";
245 tuned_station = tower;
247 circuitsToFly = 0; // ie just fly this circuit and then stop
249 // FIXME TODO - pattern direction is still hardwired
250 patternDirection = -1; // Left
251 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
252 if(rwy.rwyID.size() == 3) {
253 patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
256 if(initialLeg == DOWNWIND) {
257 pos = ortho.ConvertFromLocal(Point3D(1000*patternDirection, 800, 0.0));
258 pos.setelev(rwy.threshold_pos.elev() + 1000 * SG_FEET_TO_METER);
259 hdg = rwy.hdg + 180.0;
261 elevInitGood = false;
263 track = rwy.hdg - (180 * patternDirection); //should tend to bring track back into the 0->360 range
269 aip.setVisible(true);
270 tower->RegisterAIPlane(plane, this, CIRCUIT, DOWNWIND);
272 // Default to initial position on threshold for now
273 pos.setlat(rwy.threshold_pos.lat());
274 pos.setlon(rwy.threshold_pos.lon());
275 pos.setelev(rwy.threshold_pos.elev());
278 // Now we've set the position we can do the ground elev
279 // This might not always be necessary if we implement in-air start
280 elevInitGood = false;
291 operatingState = IN_PATTERN;
296 SG_LOG(SG_ATC, SG_ALERT, "Attempt to set unknown operating state in FGAILocalTraffic.Init(...)\n");
305 // Return what type of landing we're doing on this circuit
306 LandingType FGAILocalTraffic::GetLandingOption() {
307 //cout << "circuitsToFly = " << circuitsToFly << '\n';
309 return(touchAndGo ? TOUCH_AND_GO : STOP_AND_GO);
316 // Commands to do something from higher level logic
317 void FGAILocalTraffic::FlyCircuits(int numCircuits, bool tag) {
318 //cout << "FlyCircuits called" << endl;
320 switch(operatingState) {
322 circuitsToFly += numCircuits;
326 // TODO - For now we'll punt this and do nothing
329 circuitsToFly = numCircuits; // Note that one too many circuits gets flown because we only test and decrement circuitsToFly after landing
330 // thus flying one too many circuits. TODO - Need to sort this out better!
336 // Run the internal calculations
337 void FGAILocalTraffic::Update(double dt) {
338 //cout << "A" << flush;
339 //double responseTime = 10.0; // seconds - this should get more sophisticated at some point
340 responseCounter += dt;
341 if((contactTower) && (responseCounter >= 8.0)) {
342 // Acknowledge request before changing frequency so it gets rendered if the user is on the same freq
343 string trns = "Tower ";
344 double f = globals->get_ATC_mgr()->GetFrequency(airportID, TOWER) / 100.0;
346 sprintf(buf, "%.2f", f);
349 trns += plane.callsign;
350 pending_transmission = trns;
351 ConditionalTransmit(30.0);
352 responseCounter = 0.0;
353 contactTower = false;
355 changeFreqType = TOWER;
358 if((changeFreq) && (responseCounter > 8.0)) {
359 switch(changeFreqType) {
361 tuned_station = tower;
362 freq = (double)tower->get_freq() / 100.0;
364 // Contact the tower, even if only virtually
366 pending_transmission = plane.callsign;
367 pending_transmission += " at hold short for runway ";
368 pending_transmission += ConvertRwyNumToSpokenString(rwy.rwyID);
369 pending_transmission += " traffic pattern ";
371 pending_transmission += ConvertNumToSpokenDigits(circuitsToFly + 1);
372 pending_transmission += " circuits touch and go";
374 pending_transmission += " one circuit to full stop";
379 tuned_station = ground;
380 freq = (double)ground->get_freq() / 100.0;
382 // And to avoid compiler warnings...
383 case APPROACH: break;
386 case DEPARTURE: break;
391 //cout << "." << flush;
393 switch(operatingState) {
395 //cout << "In IN_PATTERN\n";
399 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
400 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
401 //cout << "TAKEOFF_ROLL, POS = " << pos.lon() << ", " << pos.lat() << ", " << pos.elev() << '\n';
403 aip.setVisible(true);
404 //cout << "Making plane visible!\n";
409 FlyTrafficPattern(dt);
413 //cout << "In TAXIING\n";
414 //cout << "*" << flush;
417 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
418 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
420 aip.setVisible(true);
422 //cout << "Making plane visible!\n";
427 //cout << "," << flush;
428 if(!((holdingShort) && (!clearedToLineUp))) {
429 //cout << "|" << flush;
432 //cout << ";" << flush;
433 if((clearedToTakeOff) && (responseCounter >= 8.0)) {
434 // possible assumption that we're at the hold short here - may not always hold
435 // TODO - sort out the case where we're cleared to line-up first and then cleared to take-off on the rwy.
436 taxiState = TD_LINING_UP;
437 path = ground->GetPath(holdShortNode, rwy.rwyID);
439 cout << "path returned was:" << endl;
440 for(unsigned int i=0; i<path.size(); ++i) {
441 switch(path[i]->struct_type) {
443 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
451 clearedToTakeOff = false; // We *are* still cleared - this simply stops the response recurring!!
452 holdingShort = false;
453 string trns = "Cleared for take-off ";
454 trns += plane.callsign;
455 pending_transmission = trns;
459 //cout << "^" << flush;
463 //cout << "In PARKED\n";
466 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
467 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
469 aip.setVisible(true);
471 //cout << "Making plane visible!\n";
477 if((taxiRequestPending) && (taxiRequestCleared)) {
478 //cout << "&" << flush;
479 // Get the active runway details (in case they've changed since init)
482 // Get the takeoff node for the active runway, get a path to it and start taxiing
483 path = ground->GetPathToHoldShort(ourGate, rwy.rwyID);
484 if(path.size() < 2) {
485 // something has gone wrong
486 SG_LOG(SG_ATC, SG_ALERT, "Invalid path from gate to theshold in FGAILocalTraffic::FlyCircuits\n");
490 cout << "path returned was:\n";
491 for(unsigned int i=0; i<path.size(); ++i) {
492 switch(path[i]->struct_type) {
494 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
502 path.erase(path.begin()); // pop the gate - we're here already!
503 taxiState = TD_OUTBOUND;
504 taxiRequestPending = false;
505 holdShortNode = (node*)(*(path.begin() + path.size()));
507 } else if(!taxiRequestPending) {
508 //cout << "(" << flush;
509 // Do some communication
510 // airport name + tower + airplane callsign + location + request taxi for + operation type + ?
512 trns += tower->get_name();
514 trns += plane.callsign;
515 trns += " on apron parking request taxi for traffic pattern";
516 //cout << "trns = " << trns << endl;
517 pending_transmission = trns;
519 taxiRequestCleared = false;
520 taxiRequestPending = true;
524 //cout << "!" << flush;
526 // Maybe the below should be set when we get to the threshold and prepare for TO?
527 // FIXME TODO - pattern direction is still hardwired
528 patternDirection = -1; // Left
529 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
530 if(rwy.rwyID.size() == 3) {
531 patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
535 //cout << ")" << flush;
540 //cout << "I " << flush;
542 // Convienience output for AI debugging user the property logger
543 fgSetDouble("/AI/Local1/ortho-x", (ortho.ConvertToLocal(pos)).x());
544 fgSetDouble("/AI/Local1/ortho-y", (ortho.ConvertToLocal(pos)).y());
545 fgSetDouble("/AI/Local1/elev", pos.elev() * SG_METER_TO_FEET);
547 // And finally, call parent for transmission rendering
548 FGAIPlane::Update(dt);
551 void FGAILocalTraffic::RegisterTransmission(int code) {
553 case 1: // taxi request cleared
554 taxiRequestCleared = true;
555 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to taxi...");
557 case 2: // contact tower
560 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " told to contact tower...");
562 case 3: // Cleared to line up
564 clearedToLineUp = true;
565 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to line-up...");
567 case 4: // cleared to take-off
569 clearedToTakeOff = true;
570 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to take-off...");
572 case 13: // Go around!
575 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " told to go-around!!");
582 // Fly a traffic pattern
583 // FIXME - far too much of the mechanics of turning, rolling, accellerating, descending etc is in here.
584 // Move it out to FGAIPlane and have FlyTrafficPattern just specify what to do, not the implementation.
585 void FGAILocalTraffic::FlyTrafficPattern(double dt) {
586 // Need to differentiate between in-air (IAS governed) and on-ground (vel governed)
587 // Take-off is an interesting case - we are on the ground but takeoff speed is IAS governed.
589 static bool transmitted = false; // FIXME - this is a hack
592 // Wind has two effects - a mechanical one in that IAS translates to a different vel, and the hdg != track,
593 // but also a piloting effect, in that the AI must be able to descend at a different rate in order to hit the threshold.
595 //cout << "dt = " << dt << '\n';
597 // ack - I can't remember how long a rate 1 turn is meant to take.
598 double turn_time = 60.0; // seconds - TODO - check this guess
599 double turn_circumference;
601 Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane
602 //cout << "runway elev = " << rwy.threshold_pos.elev() << ' ' << rwy.threshold_pos.elev() * SG_METER_TO_FEET << '\n';
603 //cout << "elev = " << pos.elev() << ' ' << pos.elev() * SG_METER_TO_FEET << '\n';
605 // HACK FOR TESTING - REMOVE
606 //cout << "Calling ExitRunway..." << endl;
607 //ExitRunway(orthopos);
612 double wind_from = wind_from_hdg->getDoubleValue();
613 double wind_speed = wind_speed_knots->getDoubleValue();
625 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
626 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
628 IAS = vel + (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
632 IAS = best_rate_of_climb_speed;
634 slope = 6.0; // Reduced it slightly since it's climbing a lot steeper than I can in the JSBSim C172.
640 // Turn to crosswind if above 700ft AND if other traffic allows
641 // (decided in FGTower and accessed through GetCrosswindConstraint(...)).
642 // According to AIM, traffic should climb to within 300ft of pattern altitude before commencing crosswind turn.
643 // TODO - At hot 'n high airports this may be 500ft AGL though - need to make this a variable.
644 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 700) {
646 if(tower->GetCrosswindConstraint(cc)) {
647 if(orthopos.y() > cc) {
648 cout << "Turning to crosswind, distance from threshold = " << orthopos.y() << '\n';
651 } else if(orthopos.y() > 1500.0) { // Added this constraint as a hack to prevent turning too early when going around.
652 // TODO - We should be doing it as a distance from takeoff end, not theshold end though.
653 cout << "Turning to crosswind, distance from threshold = " << orthopos.y() << '\n';
657 // Need to check for levelling off in case we can't turn crosswind as soon
658 // as we would like due to other traffic.
659 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
662 IAS = 80.0; // FIXME - use smooth transistion to new speed and attitude.
664 if(goAround && !goAroundCalled) {
665 if(responseCounter > 5.5) {
666 pending_transmission = plane.callsign;
667 pending_transmission += " going around";
669 goAroundCalled = true;
674 track += (360.0 / turn_time) * dt * patternDirection;
675 Bank(25.0 * patternDirection);
676 if((track < (rwy.hdg - 89.0)) || (track > (rwy.hdg + 89.0))) {
683 track = rwy.hdg + (90.0 * patternDirection);
684 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
687 IAS = 80.0; // FIXME - use smooth transistion to new speed
689 // turn 1000m out for now, taking other traffic into accout
690 if(fabs(orthopos.x()) > 980) {
692 if(tower->GetDownwindConstraint(dd)) {
693 if(fabs(orthopos.x()) > fabs(dd)) {
694 cout << "Turning to downwind, distance from centerline = " << fabs(orthopos.x()) << '\n';
698 cout << "Turning to downwind, distance from centerline = " << fabs(orthopos.x()) << '\n';
704 track += (360.0 / turn_time) * dt * patternDirection;
705 Bank(25.0 * patternDirection);
706 // just in case we didn't make height on crosswind
707 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
710 IAS = 80.0; // FIXME - use smooth transistion to new speed
712 if((track < (rwy.hdg - 179.0)) || (track > (rwy.hdg + 179.0))) {
720 track = rwy.hdg - (180 * patternDirection); //should tend to bring track back into the 0->360 range
721 // just in case we didn't make height on crosswind
722 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
725 IAS = 90.0; // FIXME - use smooth transistion to new speed
727 if((orthopos.y() < 0) && (!transmitted)) {
728 TransmitPatternPositionReport();
731 if((orthopos.y() < -100) && (!descending)) {
732 // Maybe we should think about when to start descending.
733 // For now we're assuming that we aim to follow the same glidepath regardless of wind.
736 CalculateSoD((tower->GetBaseConstraint(d1) ? d1 : -1000.0), (tower->GetDownwindConstraint(d2) ? d2 : 1000.0 * patternDirection), (patternDirection ? true : false));
737 if(SoD.leg == DOWNWIND) {
738 descending = (orthopos.y() < SoD.y ? true : false);
743 slope = -5.5; // FIXME - calculate to descent at 500fpm and hit the desired point on the runway (taking wind into account as well!!)
748 // Try and arrange to turn nicely onto base
749 turn_circumference = IAS * 0.514444 * turn_time;
750 //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius
751 //We'll leave it as a hack with IAS for now but it needs revisiting.
752 turn_radius = turn_circumference / (2.0 * DCL_PI);
753 if(orthopos.y() < -1000.0 + turn_radius) {
754 //if(orthopos.y() < -980) {
756 if(tower->GetBaseConstraint(bb)) {
757 if(fabs(orthopos.y()) > fabs(bb)) {
758 cout << "Turning to base, distance from threshold = " << fabs(orthopos.y()) << '\n';
764 cout << "Turning to base, distance from threshold = " << fabs(orthopos.y()) << '\n';
772 track += (360.0 / turn_time) * dt * patternDirection;
773 Bank(25.0 * patternDirection);
774 if(fabs(rwy.hdg - track) < 91.0) {
781 TransmitPatternPositionReport();
787 // Make downwind leg position artifically large to avoid any chance of SoD being returned as
788 // on downwind when we are already on base.
789 CalculateSoD((tower->GetBaseConstraint(d1) ? d1 : -1000.0), (10000.0 * patternDirection), (patternDirection ? true : false));
790 if(SoD.leg == BASE) {
791 descending = (fabs(orthopos.y()) < fabs(SoD.y) ? true : false);
796 slope = -5.5; // FIXME - calculate to descent at 500fpm and hit the threshold (taking wind into account as well!!)
801 track = rwy.hdg - (90 * patternDirection);
803 // Try and arrange to turn nicely onto final
804 turn_circumference = IAS * 0.514444 * turn_time;
805 //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius
806 //We'll leave it as a hack with IAS for now but it needs revisiting.
807 turn_radius = turn_circumference / (2.0 * DCL_PI);
808 if(fabs(orthopos.x()) < (turn_radius + 50)) {
815 track += (360.0 / turn_time) * dt * patternDirection;
816 Bank(25.0 * patternDirection);
817 if(fabs(track - rwy.hdg) < 0.6) {
819 vel = nominal_final_speed;
823 if(goAround && responseCounter > 2.0) {
826 IAS = best_rate_of_climb_speed;
827 slope = 5.0; // A bit less steep than the initial climbout.
829 goAroundCalled = false;
834 TransmitPatternPositionReport();
838 // Make base leg position artifically large to avoid any chance of SoD being returned as
839 // on base or downwind when we are already on final.
840 CalculateSoD(-10000.0, (1000.0 * patternDirection), (patternDirection ? true : false));
841 if(SoD.leg == FINAL) {
842 descending = (fabs(orthopos.y()) < fabs(SoD.y) ? true : false);
847 slope = -5.5; // FIXME - calculate to descent at 500fpm and hit the threshold (taking wind into account as well!!)
851 // Try and track the extended centreline
852 track = rwy.hdg - (0.2 * orthopos.x());
853 //cout << "orthopos.x() = " << orthopos.x() << " hdg = " << hdg << '\n';
854 if(pos.elev() < (rwy.threshold_pos.elev()+20.0+wheelOffset)) {
855 DoGroundElev(); // Need to call it here expicitly on final since it's only called
856 // for us in update(...) when the inAir flag is false.
858 if(pos.elev() < (rwy.threshold_pos.elev()+10.0+wheelOffset)) {
859 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
860 if((aip.getSGLocation()->get_cur_elev_m() + wheelOffset) > pos.elev()) {
866 } // else need a fallback position based on arpt elev in case ground elev determination fails?
872 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
873 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
878 // FIXME - differentiate between touch and go and full stops
880 //cout << "Vel <= 15.0, circuitsToFly = " << circuitsToFly << endl;
881 if(circuitsToFly <= 0) {
882 //cout << "Calling ExitRunway..." << endl;
883 ExitRunway(orthopos);
886 //cout << "Taking off again..." << endl;
897 // FIXME - at the moment this is a bit screwy
898 // The velocity correction is applied based on the relative headings.
899 // Then the heading is changed based on the velocity.
900 // Which comes first, the chicken or the egg?
901 // Does it really matter?
903 // Apply wind to ground-relative velocity if in the air
904 vel = IAS - (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
905 //crab = f(track, wind, vel);
906 // The vector we need to fly is our desired vector minus the wind vector
907 // TODO - we probably ought to use plib's built in vector types and operations for this
908 // ie. There's almost *certainly* a better way to do this!
909 double gxx = vel * sin(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity x component wrt ground
910 double gyy = vel * cos(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity y component wrt ground
911 double wxx = wind_speed * sin((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS); // Wind velocity x component
912 double wyy = wind_speed * cos((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS); // Wind velocity y component
913 double axx = gxx - wxx; // Plane in-air velocity x component
914 double ayy = gyy - wyy; // Plane in-air velocity y component
915 // Now we want the angle between gxx and axx (which is the crab)
916 double maga = sqrt(axx*axx + ayy*ayy);
917 double magg = sqrt(gxx*gxx + gyy*gyy);
918 crab = acos((axx*gxx + ayy*gyy) / (maga * magg));
919 // At this point this works except we're getting the modulus of the angle
920 //cout << "crab = " << crab << '\n';
922 // Make sure both headings are in the 0->360 circle in order to get sane differences
923 dclBoundHeading(wind_from);
924 dclBoundHeading(track);
925 if(track > wind_from) {
926 if((track - wind_from) <= 180) {
930 if((wind_from - track) >= 180) {
934 } else { // on the ground - crab dosen't apply
939 dist = vel * 0.514444 * dt;
940 pos = dclUpdatePosition(pos, track, slope, dist);
943 // Pattern direction is true for right, false for left
944 void FGAILocalTraffic::CalculateSoD(double base_leg_pos, double downwind_leg_pos, bool pattern_direction) {
945 // For now we'll ignore wind and hardwire the glide angle.
946 double ga = 5.5; //degrees
947 double pa = 1000.0 * SG_FEET_TO_METER; // pattern altitude in meters
948 // FIXME - get glideslope angle and pattern altitude agl from airport details if available
950 // For convienience, we'll have +ve versions of the input distances
951 double blp = fabs(base_leg_pos);
952 double dlp = fabs(downwind_leg_pos);
954 //double turn_allowance = 150.0; // Approximate distance in meters that a 90deg corner is shortened by turned in a light plane.
956 double stod = pa / tan(ga * DCL_DEGREES_TO_RADIANS); // distance in meters from touchdown point to start descent
957 //cout << "Descent to start = " << stod << " meters out\n";
958 if(stod < blp) { // Start descending on final
962 } else if(stod < (blp + dlp)) { // Start descending on base leg
965 SoD.x = (pattern_direction ? (stod - dlp) : (stod - dlp) * -1.0);
966 } else { // Start descending on downwind leg
968 SoD.x = (pattern_direction ? dlp : dlp * -1.0);
969 SoD.y = (blp - (stod - (blp + dlp))) * -1.0;
973 void FGAILocalTraffic::TransmitPatternPositionReport(void) {
974 // airport name + "traffic" + airplane callsign + pattern direction + pattern leg + rwy + ?
978 trns += tower->get_name();
980 trns += plane.callsign;
981 if(patternDirection == 1) {
987 // We could probably get rid of this whole switch statement and just pass a string containing the leg from the FlyPattern function.
988 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?
990 // Fall through to CROSSWIND
991 case CROSSWIND: // I don't think this case will be used here but it can't hurt to leave it in
992 trns += "crosswind ";
995 // Fall through to DOWNWIND
1001 // Fall through to BASE
1006 // Fall through to FINAL
1007 case FINAL: // maybe this should include long/short final if appropriate?
1011 default: // Hopefully this won't be used
1015 trns += ConvertRwyNumToSpokenString(rwy.rwyID);
1017 // And add the airport name again
1018 trns += tower->get_name();
1020 pending_transmission = trns; // FIXME - make up pending_transmission natively
1021 ConditionalTransmit(90.0, code); // Assume a report of this leg will be invalid if we can't transmit within a minute and a half.
1025 // TODO - Really should enumerate these coded values.
1026 void FGAILocalTraffic::ProcessCallback(int code) {
1027 // 1 - Request Departure from ground
1028 // 2 - Report at hold short
1029 // 10 - report crosswind
1030 // 11 - report downwind
1032 // 13 - report final
1034 ground->RequestDeparture(plane, this);
1035 } else if(code == 2) {
1036 tower->ContactAtHoldShort(plane, this, CIRCUIT);
1037 } else if(code == 11) {
1038 tower->ReportDownwind(plane.callsign);
1039 } else if(code == 13) {
1040 tower->ReportFinal(plane.callsign);
1044 void FGAILocalTraffic::ExitRunway(Point3D orthopos) {
1045 //cout << "In ExitRunway" << endl;
1046 //cout << "Runway ID is " << rwy.ID << endl;
1047 node_array_type exitNodes = ground->GetExits(rwy.rwyID); //I suppose we ought to have some fallback for rwy with no defined exits?
1049 cout << "Node ID's of exits are ";
1050 for(unsigned int i=0; i<exitNodes.size(); ++i) {
1051 cout << exitNodes[i]->nodeID << ' ';
1055 if(exitNodes.size()) {
1056 //Find the next exit from orthopos.y
1058 double dist = 100000; //ie. longer than any runway in existance
1059 double backdist = 100000;
1060 node_array_iterator nItr = exitNodes.begin();
1061 node* rwyExit = *(exitNodes.begin());
1062 //int gateID; //This might want to be more persistant at some point
1063 while(nItr != exitNodes.end()) {
1064 d = ortho.ConvertToLocal((*nItr)->pos).y() - ortho.ConvertToLocal(pos).y(); //FIXME - consider making orthopos a class variable
1071 if(fabs(d) < backdist) {
1073 //TODO - need some logic here that if we don't get a forward exit we turn round and store the backwards one
1078 ourGate = ground->GetGateNode();
1079 if(ourGate == NULL) {
1080 // Implies no available gates - what shall we do?
1081 // For now just vanish the plane - possibly we can make this more elegant in the future
1082 SG_LOG(SG_ATC, SG_ALERT, "No gate found by FGAILocalTraffic whilst landing at " << airportID << '\n');
1083 aip.setVisible(false);
1084 operatingState = PARKED;
1087 path = ground->GetPath(rwyExit, ourGate);
1089 cout << "path returned was:" << endl;
1090 for(unsigned int i=0; i<path.size(); ++i) {
1091 switch(path[i]->struct_type) {
1093 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
1101 taxiState = TD_INBOUND;
1104 // Something must have gone wrong with the ground network file - or there is only a rwy here and no exits defined
1105 SG_LOG(SG_ATC, SG_ALERT, "No exits found by FGAILocalTraffic from runway " << rwy.rwyID << " at " << airportID << '\n');
1106 // What shall we do - just remove the plane from sight?
1107 aip.setVisible(false);
1108 operatingState = PARKED;
1112 // Set the class variable nextTaxiNode to the next node in the path
1113 // and update taxiPathPos, the class variable path iterator position
1114 // TODO - maybe should return error codes to the calling function if we fail here
1115 void FGAILocalTraffic::GetNextTaxiNode() {
1116 //cout << "GetNextTaxiNode called " << endl;
1117 //cout << "taxiPathPos = " << taxiPathPos << endl;
1118 ground_network_path_iterator pathItr = path.begin() + taxiPathPos;
1119 if(pathItr == path.end()) {
1120 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - no more nodes in path\n");
1122 if((*pathItr)->struct_type == NODE) {
1123 //cout << "ITS A NODE" << endl;
1124 //*pathItr = new node;
1125 nextTaxiNode = (node*)*pathItr;
1129 //cout << "ITS NOT A NODE" << endl;
1130 //The first item in found must have been an arc
1131 //Assume for now that it was straight
1134 if(pathItr == path.end()) {
1135 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - path ended with an arc\n");
1136 } else if((*pathItr)->struct_type == NODE) {
1137 nextTaxiNode = (node*)*pathItr;
1140 //OOPS - two non-nodes in a row - that shouldn't happen ATM
1141 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - two non-nodes in sequence\n");
1147 // StartTaxi - set up the taxiing state - call only at the start of taxiing
1148 void FGAILocalTraffic::StartTaxi() {
1149 //cout << "StartTaxi called" << endl;
1150 operatingState = TAXIING;
1153 //Set the desired heading
1154 //Assume we are aiming for first node on path
1155 //Eventually we may need to consider the fact that we might start on a curved arc and
1156 //not be able to head directly for the first node.
1157 GetNextTaxiNode(); // sets the class variable nextTaxiNode to the next taxi node!
1158 desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
1159 //cout << "First taxi heading is " << desiredTaxiHeading << endl;
1162 // speed in knots, headings in degrees, radius in meters.
1163 static double TaxiTurnTowardsHeading(double current_hdg, double desired_hdg, double speed, double radius, double dt) {
1164 // wrap heading - this prevents a logic bug where the plane would just go round in circles!!
1165 while(current_hdg < 0.0) {
1166 current_hdg += 360.0;
1168 while(current_hdg > 360.0) {
1169 current_hdg -= 360.0;
1171 if(fabs(current_hdg - desired_hdg) > 0.1) {
1172 // Which is the quickest direction to turn onto heading?
1173 if(desired_hdg > current_hdg) {
1174 if((desired_hdg - current_hdg) <= 180) {
1176 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1177 // TODO - check that increments are less than the delta that we check for the right direction
1178 // Probably need to reduce convergence speed as convergence is reached
1180 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1183 if((current_hdg - desired_hdg) <= 180) {
1185 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1186 // TODO - check that increments are less than the delta that we check for the right direction
1187 // Probably need to reduce convergence speed as convergence is reached
1189 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1193 return(current_hdg);
1196 void FGAILocalTraffic::Taxi(double dt) {
1197 //cout << "Taxi called" << endl;
1198 // Logic - if we are further away from next point than turn radius then head for it
1199 // If we have reached turning point then get next point and turn onto that heading
1200 // Look out for the finish!!
1202 //Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane
1203 desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
1205 bool lastNode = (taxiPathPos == path.size() ? true : false);
1207 //cout << "LAST NODE\n";
1210 // HACK ALERT! - for now we will taxi at constant speed for straights and turns
1212 // Remember that hdg is always equal to track when taxiing so we don't have to consider them both
1213 double dist_to_go = dclGetHorizontalSeparation(pos, nextTaxiNode->pos); // we may be able to do this more cheaply using orthopos
1214 //cout << "dist_to_go = " << dist_to_go << endl;
1215 if((nextTaxiNode->type == GATE) && (dist_to_go <= 0.1)) {
1216 // This might be more robust to outward paths starting with a gate if we check for either
1217 // last node or TD_INBOUND ?
1219 operatingState = PARKED;
1220 } else if(((dist_to_go > taxiTurnRadius) || (nextTaxiNode->type == GATE)) && (!liningUp)){
1221 // if the turn radius is r, and speed is s, then in a time dt we turn through
1222 // ((s.dt)/(PI.r)) x 180 degrees
1223 // or alternatively (s.dt)/r radians
1224 //cout << "hdg = " << hdg << " desired taxi heading = " << desiredTaxiHeading << '\n';
1225 hdg = TaxiTurnTowardsHeading(hdg, desiredTaxiHeading, nominalTaxiSpeed, taxiTurnRadius, dt);
1226 double vel = nominalTaxiSpeed;
1227 //cout << "vel = " << vel << endl;
1228 double dist = vel * 0.514444 * dt;
1229 //cout << "dist = " << dist << endl;
1231 //cout << "track = " << track << endl;
1233 pos = dclUpdatePosition(pos, track, slope, dist);
1234 //cout << "Updated position...\n";
1235 if(aip.getSGLocation()->get_cur_elev_m() > -9990) {
1236 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
1237 } // else don't change the elev until we get a valid ground elev again!
1238 } else if(lastNode) {
1239 if(taxiState == TD_LINING_UP) {
1240 if((!liningUp) && (dist_to_go <= taxiTurnRadius)) {
1244 hdg = TaxiTurnTowardsHeading(hdg, rwy.hdg, nominalTaxiSpeed, taxiTurnRadius, dt);
1245 double vel = nominalTaxiSpeed;
1246 //cout << "vel = " << vel << endl;
1247 double dist = vel * 0.514444 * dt;
1248 //cout << "dist = " << dist << endl;
1250 //cout << "track = " << track << endl;
1252 pos = dclUpdatePosition(pos, track, slope, dist);
1253 //cout << "Updated position...\n";
1254 if(aip.getSGLocation()->get_cur_elev_m() > -9990) {
1255 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
1256 } // else don't change the elev until we get a valid ground elev again!
1257 if(fabs(hdg - rwy.hdg) <= 1.0) {
1258 operatingState = IN_PATTERN;
1264 } else if(taxiState == TD_OUTBOUND) {
1265 // Pause awaiting further instructions
1266 // and for now assume we've reached the hold-short node
1267 holdingShort = true;
1268 } // else at the moment assume TD_INBOUND always ends in a gate in which case we can ignore it
1270 // Time to turn (we've already checked it's not the end we're heading for).
1271 // set the target node to be the next node which will prompt automatically turning onto
1272 // the right heading in the stuff above, with the usual provisos applied.
1274 // For now why not just recursively call this function?
1280 // Warning - ground elev determination is CPU intensive
1281 // Either this function or the logic of how often it is called
1282 // will almost certainly change.
1283 void FGAILocalTraffic::DoGroundElev() {
1285 // It would be nice if we could set the correct tile center here in order to get a correct
1286 // answer with one call to the function, but what I tried in the two commented-out lines
1287 // below only intermittently worked, and I haven't quite groked why yet.
1288 //SGBucket buck(pos.lon(), pos.lat());
1289 //aip.getSGLocation()->set_tile_center(Point3D(buck.get_center_lon(), buck.get_center_lat(), 0.0));
1291 double visibility_meters = fgGetDouble("/environment/visibility-m");
1292 //globals->get_tile_mgr()->prep_ssg_nodes( acmodel_location,
1293 globals->get_tile_mgr()->prep_ssg_nodes( aip.getSGLocation(), visibility_meters );
1294 Point3D scenery_center = globals->get_scenery()->get_center();
1295 globals->get_tile_mgr()->update( aip.getSGLocation(), visibility_meters, (aip.getSGLocation())->get_absolute_view_pos( scenery_center ) );
1296 // save results of update in SGLocation for fdm...
1298 //if ( globals->get_scenery()->get_cur_elev() > -9990 ) {
1299 // acmodel_location->
1300 // set_cur_elev_m( globals->get_scenery()->get_cur_elev() );
1303 // The need for this here means that at least 2 consecutive passes are needed :-(
1304 aip.getSGLocation()->set_tile_center( globals->get_scenery()->get_next_center() );
1306 //cout << "Transform Elev is " << globals->get_scenery()->get_cur_elev() << '\n';
1307 aip.getSGLocation()->set_cur_elev_m(globals->get_scenery()->get_cur_elev());
1308 //return(globals->get_scenery()->get_cur_elev());