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 = 8.0;
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;
90 FGAILocalTraffic::~FGAILocalTraffic() {
94 // Get details of the active runway
95 // It is assumed that by the time this is called the tower control and airport code will have been set up.
96 void FGAILocalTraffic::GetRwyDetails() {
97 //cout << "GetRwyDetails called" << endl;
99 rwy.rwyID = tower->GetActiveRunway();
101 // Now we need to get the threshold position and rwy heading
104 bool rwyGood = globals->get_runways()->search(airportID, rwy.rwyID,
107 // Get the threshold position
108 hdg = runway.heading; // TODO - check - is this our heading we are setting here, and if so should we be?
109 //cout << "hdg reset to " << hdg << '\n';
110 double other_way = hdg - 180.0;
111 while(other_way <= 0.0) {
115 // move to the +l end/center of the runway
116 //cout << "Runway center is at " << runway.lon << ", " << runway.lat << '\n';
117 Point3D origin = Point3D(runway.lon, runway.lat, aptElev);
118 Point3D ref = origin;
119 double tshlon, tshlat, tshr;
120 double tolon, tolat, tor;
121 rwy.length = runway.length * SG_FEET_TO_METER;
122 geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), other_way,
123 rwy.length / 2.0 - 25.0, &tshlat, &tshlon, &tshr );
124 geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), hdg,
125 rwy.length / 2.0 - 25.0, &tolat, &tolon, &tor );
126 // Note - 25 meters in from the runway end is a bit of a hack to put the plane ahead of the user.
127 // now copy what we need out of runway into rwy
128 rwy.threshold_pos = Point3D(tshlon, tshlat, aptElev);
129 Point3D takeoff_end = Point3D(tolon, tolat, aptElev);
130 //cout << "Threshold position = " << tshlon << ", " << tshlat << ", " << aptElev << '\n';
131 //cout << "Takeoff position = " << tolon << ", " << tolat << ", " << aptElev << '\n';
133 // Set the projection for the local area
134 ortho.Init(rwy.threshold_pos, rwy.hdg);
135 rwy.end1ortho = ortho.ConvertToLocal(rwy.threshold_pos); // should come out as zero
136 rwy.end2ortho = ortho.ConvertToLocal(takeoff_end);
138 SG_LOG(SG_ATC, SG_ALERT, "Help - can't get good runway in FGAILocalTraffic!!\n");
143 There are two possible scenarios during initialisation:
144 The first is that the user is flying towards the airport, and hence the traffic
145 could be initialised anywhere, as long as the AI planes are consistent with
147 The second is that the user has started the sim at or close to the airport, and
148 hence the traffic must be initialised with respect to the user as well as each other.
149 To a certain extent it's FGAIMgr that has to worry about this, but we need to provide
150 sufficient initialisation functionality within the plane classes to allow the manager
151 to initialy position them where and how required.
153 bool FGAILocalTraffic::Init(string ICAO, OperatingState initialState, PatternLeg initialLeg) {
154 //cout << "FGAILocalTraffic.Init(...) called" << endl;
155 // Hack alert - Hardwired path!!
156 string planepath = "Aircraft/c172/Models/c172-dpm.ac";
157 SGPath path = globals->get_fg_root();
158 path.append(planepath);
159 ssgBranch *model = sgLoad3DModel( globals->get_fg_root(),
161 globals->get_props(),
162 globals->get_sim_time_sec() );
164 aip.setVisible(false); // This will be set to true once a valid ground elevation has been determined
165 globals->get_scenery()->get_scene_graph()->addKid(aip.getSceneGraph());
167 // Find the tower frequency - this is dependent on the ATC system being initialised before the AI system
170 if(ATC->GetAirportATCDetails(airportID, &a)) {
171 if(a.tower_freq) { // Has a tower
172 tower = (FGTower*)ATC->GetATCPointer((string)airportID, TOWER); // Maybe need some error checking here
174 // Something has gone wrong - abort or carry on with un-towered operation?
177 freq = (double)tower->get_freq() / 100.0;
178 ground = tower->GetGroundPtr();
180 // Something has gone wrong :-(
181 SG_LOG(SG_ATC, SG_ALERT, "ERROR - can't get a ground pointer from tower control in FGAILocalTraffic::Init() :-(");
183 } else if((initialState == PARKED) || (initialState == TAXIING)) {
184 freq = (double)ground->get_freq() / 100.0;
186 //cout << "AILocalTraffic freq is " << freq << '\n';
188 // Check CTAF, unicom etc
191 //cout << "Unable to find airport details in FGAILocalTraffic::Init()\n";
194 // Get the airport elevation
195 aptElev = dclGetAirportElev(airportID.c_str()) * SG_FEET_TO_METER;
196 //cout << "Airport elev in AILocalTraffic = " << aptElev << '\n';
197 // WARNING - we use this elev for the whole airport - some assumptions in the code
198 // might fall down with very slopey airports.
200 //cout << "In Init(), initialState = " << initialState << endl;
201 operatingState = initialState;
202 switch(operatingState) {
204 ourGate = ground->GetGateNode();
205 if(ourGate == NULL) {
206 // Implies no available gates - what shall we do?
207 // For now just vanish the plane - possibly we can make this more elegant in the future
208 SG_LOG(SG_ATC, SG_ALERT, "No gate found by FGAILocalTraffic whilst attempting Init at " << airportID << '\n');
216 pos.setelev(aptElev);
217 hdg = ourGate->heading;
219 // Now we've set the position we can do the ground elev
220 elevInitGood = false;
227 // FIXME - implement this case properly
228 return(false); // remove this line when fixed!
231 // For now we'll always start the in_pattern case on the threshold ready to take-off
232 // since we've got the implementation for this case already.
233 // TODO - implement proper generic in_pattern startup.
235 // Get the active runway details (and copy them into rwy)
238 // Initial position on threshold for now
239 pos.setlat(rwy.threshold_pos.lat());
240 pos.setlon(rwy.threshold_pos.lon());
241 pos.setelev(rwy.threshold_pos.elev());
244 // Now we've set the position we can do the ground elev
245 // This might not always be necessary if we implement in-air start
246 elevInitGood = false;
256 circuitsToFly = 0; // ie just fly this circuit and then stop
258 // FIXME TODO - pattern direction is still hardwired
259 patternDirection = -1; // Left
260 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
261 if(rwy.rwyID.size() == 3) {
262 patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
265 operatingState = IN_PATTERN;
270 SG_LOG(SG_ATC, SG_ALERT, "Attempt to set unknown operating state in FGAILocalTraffic.Init(...)\n");
278 // Commands to do something from higher level logic
279 void FGAILocalTraffic::FlyCircuits(int numCircuits, bool tag) {
280 //cout << "FlyCircuits called" << endl;
282 switch(operatingState) {
284 circuitsToFly += numCircuits;
288 // For now we'll punt this and do nothing
291 circuitsToFly = numCircuits; // Note that one too many circuits gets flown because we only test and decrement circuitsToFly after landing
292 // thus flying one too many circuits. TODO - Need to sort this out better!
295 // Get the active runway details (and copy them into rwy)
298 // Get the takeoff node for the active runway, get a path to it and start taxiing
299 path = ground->GetPath(ourGate, rwy.rwyID);
300 if(path.size() < 2) {
301 // something has gone wrong
302 SG_LOG(SG_ATC, SG_ALERT, "Invalid path from gate to theshold in FGAILocalTraffic::FlyCircuits\n");
306 cout << "path returned was:" << endl;
307 for(unsigned int i=0; i<path.size(); ++i) {
308 switch(path[i]->struct_type) {
310 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
318 // pop the gate - we're here already!
319 path.erase(path.begin());
320 //path.erase(path.begin());
322 cout << "path after popping front is:" << endl;
323 for(unsigned int i=0; i<path.size(); ++i) {
324 switch(path[i]->struct_type) {
326 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
335 taxiState = TD_OUTBOUND;
338 // Maybe the below should be set when we get to the threshold and prepare for TO?
339 // FIXME TODO - pattern direction is still hardwired
340 patternDirection = -1; // Left
341 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
342 if(rwy.rwyID.size() == 3) {
343 patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
352 // Run the internal calculations
353 void FGAILocalTraffic::Update(double dt) {
354 //cout << "A" << flush;
355 double responseTime = 10.0; // seconds - this should get more sophisticated at some point
356 responseCounter += dt;
357 if((contactTower) && (responseCounter >= 8.0)) {
358 // Acknowledge request before changing frequency so it gets rendered if the user is on the same freq
359 string trns = "Tower ";
360 double f = globals->get_ATC_mgr()->GetFrequency(airportID, TOWER) / 100.0;
362 sprintf(buf, "%f", f);
365 trns += plane.callsign;
367 responseCounter = 0.0;
368 contactTower = false;
370 changeFreqType = TOWER;
373 if((changeFreq) && (responseCounter > 8.0)) {
374 switch(changeFreqType) {
376 freq = (double)tower->get_freq() / 100.0;
378 // Contact the tower, even if only virtually
380 tower->ContactAtHoldShort(plane, this, CIRCUIT);
383 freq = (double)ground->get_freq() / 100.0;
385 // And to avoid compiler warnings...
399 //cout << "." << flush;
401 switch(operatingState) {
403 //cout << "In IN_PATTERN\n";
404 if(!inAir) DoGroundElev();
406 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
407 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
408 //cout << "TAKEOFF_ROLL, POS = " << pos.lon() << ", " << pos.lat() << ", " << pos.elev() << '\n';
410 aip.setVisible(true);
411 //cout << "Making plane visible!\n";
415 FlyTrafficPattern(dt);
419 //cout << "In TAXIING\n";
420 //cout << "*" << flush;
423 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
424 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
426 aip.setVisible(true);
428 //cout << "Making plane visible!\n";
433 //cout << "," << flush;
434 if(!((holdingShort) && (!clearedToLineUp))) {
435 //cout << "|" << flush;
438 //cout << ";" << flush;
439 if((clearedToTakeOff) && (responseCounter >= 8.0)) {
440 // possible assumption that we're at the hold short here - may not always hold
441 // TODO - sort out the case where we're cleared to line-up first and then cleared to take-off on the rwy.
442 taxiState = TD_LINING_UP;
443 path = ground->GetPath(holdShortNode, rwy.rwyID);
445 cout << "path returned was:" << endl;
446 for(unsigned int i=0; i<path.size(); ++i) {
447 switch(path[i]->struct_type) {
449 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
457 clearedToTakeOff = false; // We *are* still cleared - this simply stops the response recurring!!
458 holdingShort = false;
459 string trns = "Cleared for take-off ";
460 trns += plane.callsign;
464 //cout << "^" << flush;
468 //cout << "In PARKED\n";
471 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
472 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
474 aip.setVisible(true);
476 //cout << "Making plane visible!\n";
482 if((taxiRequestPending) && (taxiRequestCleared)) {
483 //cout << "&" << flush;
484 // Get the active runway details (and copy them into rwy)
487 // Get the takeoff node for the active runway, get a path to it and start taxiing
488 path = ground->GetPathToHoldShort(ourGate, rwy.rwyID);
489 if(path.size() < 2) {
490 // something has gone wrong
491 SG_LOG(SG_ATC, SG_ALERT, "Invalid path from gate to theshold in FGAILocalTraffic::FlyCircuits\n");
495 cout << "path returned was:\n";
496 for(unsigned int i=0; i<path.size(); ++i) {
497 switch(path[i]->struct_type) {
499 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
507 path.erase(path.begin()); // pop the gate - we're here already!
508 taxiState = TD_OUTBOUND;
509 taxiRequestPending = false;
510 holdShortNode = (node*)(*(path.begin() + path.size()));
512 } else if(!taxiRequestPending) {
513 //cout << "(" << flush;
514 ground->RequestDeparture(plane, this);
515 // Do some communication
516 // airport name + tower + airplane callsign + location + request taxi for + operation type + ?
518 trns += tower->get_name();
520 trns += plane.callsign;
521 trns += " on apron parking request taxi for traffic pattern";
522 //cout << "trns = " << trns << endl;
524 taxiRequestCleared = false;
525 taxiRequestPending = true;
529 //cout << "!" << flush;
531 // Maybe the below should be set when we get to the threshold and prepare for TO?
532 // FIXME TODO - pattern direction is still hardwired
533 patternDirection = -1; // Left
534 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
535 if(rwy.rwyID.size() == 3) {
536 patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
540 //cout << ")" << flush;
545 //cout << "I " << flush;
547 // Convienience output for AI debugging user the property logger
548 fgSetDouble("/AI/Local1/ortho-x", (ortho.ConvertToLocal(pos)).x());
549 fgSetDouble("/AI/Local1/ortho-y", (ortho.ConvertToLocal(pos)).y());
550 fgSetDouble("/AI/Local1/elev", pos.elev() * SG_METER_TO_FEET);
553 void FGAILocalTraffic::RegisterTransmission(int code) {
555 case 1: // taxi request cleared
556 taxiRequestCleared = true;
557 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to taxi...");
559 case 2: // contact tower
562 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " told to contact tower...");
564 case 3: // Cleared to line up
566 clearedToLineUp = true;
567 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to line-up...");
569 case 4: // cleared to take-off
571 clearedToTakeOff = true;
572 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to take-off...");
579 // Fly a traffic pattern
580 // FIXME - far too much of the mechanics of turning, rolling, accellerating, descending etc is in here.
581 // Move it out to FGAIPlane and have FlyTrafficPattern just specify what to do, not the implementation.
582 void FGAILocalTraffic::FlyTrafficPattern(double dt) {
583 // Need to differentiate between in-air (IAS governed) and on-ground (vel governed)
584 // Take-off is an interesting case - we are on the ground but takeoff speed is IAS governed.
586 static bool transmitted = false; // FIXME - this is a hack
589 // Wind has two effects - a mechanical one in that IAS translates to a different vel, and the hdg != track,
590 // but also a piloting effect, in that the AI must be able to descend at a different rate in order to hit the threshold.
592 //cout << "dt = " << dt << '\n';
594 // ack - I can't remember how long a rate 1 turn is meant to take.
595 double turn_time = 60.0; // seconds - TODO - check this guess
596 double turn_circumference;
598 Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane
599 //cout << "runway elev = " << rwy.threshold_pos.elev() << ' ' << rwy.threshold_pos.elev() * SG_METER_TO_FEET << '\n';
600 //cout << "elev = " << pos.elev() << ' ' << pos.elev() * SG_METER_TO_FEET << '\n';
602 // HACK FOR TESTING - REMOVE
603 //cout << "Calling ExitRunway..." << endl;
604 //ExitRunway(orthopos);
609 double wind_from = wind_from_hdg->getDoubleValue();
610 double wind_speed = wind_speed_knots->getDoubleValue();
622 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
623 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
625 IAS = vel + (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
629 IAS = best_rate_of_climb_speed;
631 slope = 6.0; // Reduced it slightly since it's climbing a lot steeper than I can in the JSBSim C172.
637 // Turn to crosswind if above 600ft AND if other traffic allows
638 // (decided in FGTower and accessed through GetCrosswindConstraint(...)).
639 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 600) {
641 if(tower->GetCrosswindConstraint(cc)) {
642 if(orthopos.y() > cc) {
643 cout << "Turning to crosswind, distance from threshold = " << orthopos.y() << '\n';
647 cout << "Turning to crosswind, distance from threshold = " << orthopos.y() << '\n';
651 // Need to check for levelling off in case we can't turn crosswind as soon
652 // as we would like due to other traffic.
653 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
656 IAS = 80.0; // FIXME - use smooth transistion to new speed and attitude.
660 track += (360.0 / turn_time) * dt * patternDirection;
661 Bank(25.0 * patternDirection);
662 if((track < (rwy.hdg - 89.0)) || (track > (rwy.hdg + 89.0))) {
668 track = rwy.hdg + (90.0 * patternDirection);
669 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
672 IAS = 80.0; // FIXME - use smooth transistion to new speed
674 // turn 1000m out for now, taking other traffic into accout
675 if(fabs(orthopos.x()) > 980) {
677 if(tower->GetDownwindConstraint(dd)) {
678 if(fabs(orthopos.x()) > fabs(dd)) {
679 cout << "Turning to downwind, distance from centerline = " << fabs(orthopos.x()) << '\n';
683 cout << "Turning to downwind, distance from centerline = " << fabs(orthopos.x()) << '\n';
689 track += (360.0 / turn_time) * dt * patternDirection;
690 Bank(25.0 * patternDirection);
691 // just in case we didn't make height on crosswind
692 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
695 IAS = 80.0; // FIXME - use smooth transistion to new speed
697 if((track < (rwy.hdg - 179.0)) || (track > (rwy.hdg + 179.0))) {
705 track = rwy.hdg - (180 * patternDirection); //should tend to bring track back into the 0->360 range
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 = 90.0; // FIXME - use smooth transistion to new speed
712 if((orthopos.y() < 0) && (!transmitted)) {
713 TransmitPatternPositionReport();
716 if((orthopos.y() < -100) && (!descending)) {
717 // Maybe we should think about when to start descending.
718 // For now we're assuming that we aim to follow the same glidepath regardless of wind.
721 CalculateSoD((tower->GetBaseConstraint(d1) ? d1 : -1000.0), (tower->GetDownwindConstraint(d2) ? d2 : 1000.0 * patternDirection), (patternDirection ? true : false));
722 if(SoD.leg == DOWNWIND) {
723 descending = (orthopos.y() < SoD.y ? true : false);
728 slope = -5.5; // FIXME - calculate to descent at 500fpm and hit the desired point on the runway (taking wind into account as well!!)
733 // Try and arrange to turn nicely onto base
734 turn_circumference = IAS * 0.514444 * turn_time;
735 //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius
736 //We'll leave it as a hack with IAS for now but it needs revisiting.
737 turn_radius = turn_circumference / (2.0 * DCL_PI);
738 if(orthopos.y() < -1000.0 + turn_radius) {
739 //if(orthopos.y() < -980) {
741 if(tower->GetBaseConstraint(bb)) {
742 if(fabs(orthopos.y()) > fabs(bb)) {
743 cout << "Turning to base, distance from threshold = " << fabs(orthopos.y()) << '\n';
749 cout << "Turning to base, distance from threshold = " << fabs(orthopos.y()) << '\n';
757 track += (360.0 / turn_time) * dt * patternDirection;
758 Bank(25.0 * patternDirection);
759 if(fabs(rwy.hdg - track) < 91.0) {
766 TransmitPatternPositionReport();
772 // Make downwind leg position artifically large to avoid any chance of SoD being returned as
773 // on downwind when we are already on base.
774 CalculateSoD((tower->GetBaseConstraint(d1) ? d1 : -1000.0), (10000.0 * patternDirection), (patternDirection ? true : false));
775 if(SoD.leg == BASE) {
776 descending = (fabs(orthopos.y()) < fabs(SoD.y) ? true : false);
781 slope = -5.5; // FIXME - calculate to descent at 500fpm and hit the threshold (taking wind into account as well!!)
786 track = rwy.hdg - (90 * patternDirection);
788 // Try and arrange to turn nicely onto final
789 turn_circumference = IAS * 0.514444 * turn_time;
790 //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius
791 //We'll leave it as a hack with IAS for now but it needs revisiting.
792 turn_radius = turn_circumference / (2.0 * DCL_PI);
793 if(fabs(orthopos.x()) < (turn_radius + 50)) {
800 track += (360.0 / turn_time) * dt * patternDirection;
801 Bank(25.0 * patternDirection);
802 if(fabs(track - rwy.hdg) < 0.6) {
804 vel = nominal_final_speed;
810 TransmitPatternPositionReport();
814 // Make base leg position artifically large to avoid any chance of SoD being returned as
815 // on base or downwind when we are already on final.
816 CalculateSoD(-10000.0, (1000.0 * patternDirection), (patternDirection ? true : false));
817 if(SoD.leg == FINAL) {
818 descending = (fabs(orthopos.y()) < fabs(SoD.y) ? true : false);
823 slope = -5.5; // FIXME - calculate to descent at 500fpm and hit the threshold (taking wind into account as well!!)
827 // Try and track the extended centreline
828 track = rwy.hdg - (0.2 * orthopos.x());
829 //cout << "orthopos.x() = " << orthopos.x() << " hdg = " << hdg << '\n';
830 if(pos.elev() < (rwy.threshold_pos.elev()+20.0+wheelOffset)) {
831 DoGroundElev(); // Need to call it here expicitly on final since it's only called
832 // for us in update(...) when the inAir flag is false.
834 if(pos.elev() < (rwy.threshold_pos.elev()+10.0+wheelOffset)) {
835 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
836 if((aip.getSGLocation()->get_cur_elev_m() + wheelOffset) > pos.elev()) {
842 } // else need a fallback position based on arpt elev in case ground elev determination fails?
847 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
848 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
853 // FIXME - differentiate between touch and go and full stops
855 //cout << "Vel <= 15.0, circuitsToFly = " << circuitsToFly << endl;
856 if(circuitsToFly <= 0) {
857 //cout << "Calling ExitRunway..." << endl;
858 ExitRunway(orthopos);
861 //cout << "Taking off again..." << endl;
872 // FIXME - at the moment this is a bit screwy
873 // The velocity correction is applied based on the relative headings.
874 // Then the heading is changed based on the velocity.
875 // Which comes first, the chicken or the egg?
876 // Does it really matter?
878 // Apply wind to ground-relative velocity if in the air
879 vel = IAS - (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
880 //crab = f(track, wind, vel);
881 // The vector we need to fly is our desired vector minus the wind vector
882 // TODO - we probably ought to use plib's built in vector types and operations for this
883 // ie. There's almost *certainly* a better way to do this!
884 double gxx = vel * sin(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity x component wrt ground
885 double gyy = vel * cos(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity y component wrt ground
886 double wxx = wind_speed * sin((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS); // Wind velocity x component
887 double wyy = wind_speed * cos((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS); // Wind velocity y component
888 double axx = gxx - wxx; // Plane in-air velocity x component
889 double ayy = gyy - wyy; // Plane in-air velocity y component
890 // Now we want the angle between gxx and axx (which is the crab)
891 double maga = sqrt(axx*axx + ayy*ayy);
892 double magg = sqrt(gxx*gxx + gyy*gyy);
893 crab = acos((axx*gxx + ayy*gyy) / (maga * magg));
894 // At this point this works except we're getting the modulus of the angle
895 //cout << "crab = " << crab << '\n';
897 // Make sure both headings are in the 0->360 circle in order to get sane differences
898 dclBoundHeading(wind_from);
899 dclBoundHeading(track);
900 if(track > wind_from) {
901 if((track - wind_from) <= 180) {
905 if((wind_from - track) >= 180) {
909 } else { // on the ground - crab dosen't apply
914 dist = vel * 0.514444 * dt;
915 pos = dclUpdatePosition(pos, track, slope, dist);
918 // Pattern direction is true for right, false for left
919 void FGAILocalTraffic::CalculateSoD(double base_leg_pos, double downwind_leg_pos, bool pattern_direction) {
920 // For now we'll ignore wind and hardwire the glide angle.
921 double ga = 5.5; //degrees
922 double pa = 1000.0 * SG_FEET_TO_METER; // pattern altitude in meters
923 // FIXME - get glideslope angle and pattern altitude agl from airport details if available
925 // For convienience, we'll have +ve versions of the input distances
926 double blp = fabs(base_leg_pos);
927 double dlp = fabs(downwind_leg_pos);
929 //double turn_allowance = 150.0; // Approximate distance in meters that a 90deg corner is shortened by turned in a light plane.
931 double stod = pa / tan(ga * DCL_DEGREES_TO_RADIANS); // distance in meters from touchdown point to start descent
932 cout << "Descent to start = " << stod << " meters out\n";
933 if(stod < blp) { // Start descending on final
937 } else if(stod < (blp + dlp)) { // Start descending on base leg
940 SoD.x = (pattern_direction ? (stod - dlp) : (stod - dlp) * -1.0);
941 } else { // Start descending on downwind leg
943 SoD.x = (pattern_direction ? dlp : dlp * -1.0);
944 SoD.y = (blp - (stod - (blp + dlp))) * -1.0;
948 void FGAILocalTraffic::TransmitPatternPositionReport(void) {
949 // airport name + "traffic" + airplane callsign + pattern direction + pattern leg + rwy + ?
952 trns += tower->get_name();
954 trns += plane.callsign;
955 if(patternDirection == 1) {
961 // We could probably get rid of this whole switch statement and just pass a string containing the leg from the FlyPattern function.
962 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?
964 // Fall through to CROSSWIND
965 case CROSSWIND: // I don't think this case will be used here but it can't hurt to leave it in
966 trns += "crosswind ";
969 // Fall through to DOWNWIND
974 // Fall through to BASE
979 // Fall through to FINAL
980 case FINAL: // maybe this should include long/short final if appropriate?
983 default: // Hopefully this won't be used
987 // FIXME - I've hardwired the runway call as well!! (We could work this out from rwy heading and mag deviation)
988 trns += ConvertRwyNumToSpokenString(1);
990 // And add the airport name again
991 trns += tower->get_name();
996 void FGAILocalTraffic::ExitRunway(Point3D orthopos) {
997 //cout << "In ExitRunway" << endl;
998 //cout << "Runway ID is " << rwy.ID << endl;
999 node_array_type exitNodes = ground->GetExits(rwy.rwyID); //I suppose we ought to have some fallback for rwy with no defined exits?
1001 cout << "Node ID's of exits are ";
1002 for(unsigned int i=0; i<exitNodes.size(); ++i) {
1003 cout << exitNodes[i]->nodeID << ' ';
1007 if(exitNodes.size()) {
1008 //Find the next exit from orthopos.y
1010 double dist = 100000; //ie. longer than any runway in existance
1011 double backdist = 100000;
1012 node_array_iterator nItr = exitNodes.begin();
1013 node* rwyExit = *(exitNodes.begin());
1014 //int gateID; //This might want to be more persistant at some point
1015 while(nItr != exitNodes.end()) {
1016 d = ortho.ConvertToLocal((*nItr)->pos).y() - ortho.ConvertToLocal(pos).y(); //FIXME - consider making orthopos a class variable
1023 if(fabs(d) < backdist) {
1025 //TODO - need some logic here that if we don't get a forward exit we turn round and store the backwards one
1030 ourGate = ground->GetGateNode();
1031 if(ourGate == NULL) {
1032 // Implies no available gates - what shall we do?
1033 // For now just vanish the plane - possibly we can make this more elegant in the future
1034 SG_LOG(SG_ATC, SG_ALERT, "No gate found by FGAILocalTraffic whilst landing at " << airportID << '\n');
1035 aip.setVisible(false);
1036 operatingState = PARKED;
1039 path = ground->GetPath(rwyExit, ourGate);
1041 cout << "path returned was:" << endl;
1042 for(unsigned int i=0; i<path.size(); ++i) {
1043 switch(path[i]->struct_type) {
1045 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
1053 taxiState = TD_INBOUND;
1056 // Something must have gone wrong with the ground network file - or there is only a rwy here and no exits defined
1057 SG_LOG(SG_ATC, SG_ALERT, "No exits found by FGAILocalTraffic from runway " << rwy.rwyID << " at " << airportID << '\n');
1058 // What shall we do - just remove the plane from sight?
1059 aip.setVisible(false);
1060 operatingState = PARKED;
1064 // Set the class variable nextTaxiNode to the next node in the path
1065 // and update taxiPathPos, the class variable path iterator position
1066 // TODO - maybe should return error codes to the calling function if we fail here
1067 void FGAILocalTraffic::GetNextTaxiNode() {
1068 //cout << "GetNextTaxiNode called " << endl;
1069 //cout << "taxiPathPos = " << taxiPathPos << endl;
1070 ground_network_path_iterator pathItr = path.begin() + taxiPathPos;
1071 if(pathItr == path.end()) {
1072 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - no more nodes in path\n");
1074 if((*pathItr)->struct_type == NODE) {
1075 //cout << "ITS A NODE" << endl;
1076 //*pathItr = new node;
1077 nextTaxiNode = (node*)*pathItr;
1081 //cout << "ITS NOT A NODE" << endl;
1082 //The first item in found must have been an arc
1083 //Assume for now that it was straight
1086 if(pathItr == path.end()) {
1087 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - path ended with an arc\n");
1088 } else if((*pathItr)->struct_type == NODE) {
1089 nextTaxiNode = (node*)*pathItr;
1092 //OOPS - two non-nodes in a row - that shouldn't happen ATM
1093 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - two non-nodes in sequence\n");
1099 // StartTaxi - set up the taxiing state - call only at the start of taxiing
1100 void FGAILocalTraffic::StartTaxi() {
1101 //cout << "StartTaxi called" << endl;
1102 operatingState = TAXIING;
1105 //Set the desired heading
1106 //Assume we are aiming for first node on path
1107 //Eventually we may need to consider the fact that we might start on a curved arc and
1108 //not be able to head directly for the first node.
1109 GetNextTaxiNode(); // sets the class variable nextTaxiNode to the next taxi node!
1110 desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
1111 //cout << "First taxi heading is " << desiredTaxiHeading << endl;
1114 // speed in knots, headings in degrees, radius in meters.
1115 static double TaxiTurnTowardsHeading(double current_hdg, double desired_hdg, double speed, double radius, double dt) {
1116 // wrap heading - this prevents a logic bug where the plane would just go round in circles!!
1117 while(current_hdg < 0.0) {
1118 current_hdg += 360.0;
1120 while(current_hdg > 360.0) {
1121 current_hdg -= 360.0;
1123 if(fabs(current_hdg - desired_hdg) > 0.1) {
1124 // Which is the quickest direction to turn onto heading?
1125 if(desired_hdg > current_hdg) {
1126 if((desired_hdg - current_hdg) <= 180) {
1128 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1129 // TODO - check that increments are less than the delta that we check for the right direction
1130 // Probably need to reduce convergence speed as convergence is reached
1132 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1135 if((current_hdg - desired_hdg) <= 180) {
1137 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1138 // TODO - check that increments are less than the delta that we check for the right direction
1139 // Probably need to reduce convergence speed as convergence is reached
1141 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1145 return(current_hdg);
1148 void FGAILocalTraffic::Taxi(double dt) {
1149 //cout << "Taxi called" << endl;
1150 // Logic - if we are further away from next point than turn radius then head for it
1151 // If we have reached turning point then get next point and turn onto that heading
1152 // Look out for the finish!!
1154 //Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane
1155 desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
1157 bool lastNode = (taxiPathPos == path.size() ? true : false);
1159 //cout << "LAST NODE\n";
1162 // HACK ALERT! - for now we will taxi at constant speed for straights and turns
1164 // Remember that hdg is always equal to track when taxiing so we don't have to consider them both
1165 double dist_to_go = dclGetHorizontalSeparation(pos, nextTaxiNode->pos); // we may be able to do this more cheaply using orthopos
1166 //cout << "dist_to_go = " << dist_to_go << endl;
1167 if((nextTaxiNode->type == GATE) && (dist_to_go <= 0.1)) {
1168 // This might be more robust to outward paths starting with a gate if we check for either
1169 // last node or TD_INBOUND ?
1171 operatingState = PARKED;
1172 } else if(((dist_to_go > taxiTurnRadius) || (nextTaxiNode->type == GATE)) && (!liningUp)){
1173 // if the turn radius is r, and speed is s, then in a time dt we turn through
1174 // ((s.dt)/(PI.r)) x 180 degrees
1175 // or alternatively (s.dt)/r radians
1176 //cout << "hdg = " << hdg << " desired taxi heading = " << desiredTaxiHeading << '\n';
1177 hdg = TaxiTurnTowardsHeading(hdg, desiredTaxiHeading, nominalTaxiSpeed, taxiTurnRadius, dt);
1178 double vel = nominalTaxiSpeed;
1179 //cout << "vel = " << vel << endl;
1180 double dist = vel * 0.514444 * dt;
1181 //cout << "dist = " << dist << endl;
1183 //cout << "track = " << track << endl;
1185 pos = dclUpdatePosition(pos, track, slope, dist);
1186 //cout << "Updated position...\n";
1187 if(aip.getSGLocation()->get_cur_elev_m() > -9990) {
1188 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
1189 } // else don't change the elev until we get a valid ground elev again!
1190 } else if(lastNode) {
1191 if(taxiState == TD_LINING_UP) {
1192 if((!liningUp) && (dist_to_go <= taxiTurnRadius)) {
1196 hdg = TaxiTurnTowardsHeading(hdg, rwy.hdg, nominalTaxiSpeed, taxiTurnRadius, dt);
1197 double vel = nominalTaxiSpeed;
1198 //cout << "vel = " << vel << endl;
1199 double dist = vel * 0.514444 * dt;
1200 //cout << "dist = " << dist << endl;
1202 //cout << "track = " << track << endl;
1204 pos = dclUpdatePosition(pos, track, slope, dist);
1205 //cout << "Updated position...\n";
1206 if(aip.getSGLocation()->get_cur_elev_m() > -9990) {
1207 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
1208 } // else don't change the elev until we get a valid ground elev again!
1209 if(fabs(hdg - rwy.hdg) <= 1.0) {
1210 operatingState = IN_PATTERN;
1216 } else if(taxiState == TD_OUTBOUND) {
1217 // Pause awaiting further instructions
1218 // and for now assume we've reached the hold-short node
1219 holdingShort = true;
1220 } // else at the moment assume TD_INBOUND always ends in a gate in which case we can ignore it
1222 // Time to turn (we've already checked it's not the end we're heading for).
1223 // set the target node to be the next node which will prompt automatically turning onto
1224 // the right heading in the stuff above, with the usual provisos applied.
1226 // For now why not just recursively call this function?
1232 // Warning - ground elev determination is CPU intensive
1233 // Either this function or the logic of how often it is called
1234 // will almost certainly change.
1235 void FGAILocalTraffic::DoGroundElev() {
1237 // It would be nice if we could set the correct tile center here in order to get a correct
1238 // answer with one call to the function, but what I tried in the two commented-out lines
1239 // below only intermittently worked, and I haven't quite groked why yet.
1240 //SGBucket buck(pos.lon(), pos.lat());
1241 //aip.getSGLocation()->set_tile_center(Point3D(buck.get_center_lon(), buck.get_center_lat(), 0.0));
1243 double visibility_meters = fgGetDouble("/environment/visibility-m");
1244 //globals->get_tile_mgr()->prep_ssg_nodes( acmodel_location,
1245 globals->get_tile_mgr()->prep_ssg_nodes( aip.getSGLocation(), visibility_meters );
1246 Point3D scenery_center = globals->get_scenery()->get_center();
1247 globals->get_tile_mgr()->update( aip.getSGLocation(), visibility_meters, (aip.getSGLocation())->get_absolute_view_pos( scenery_center ) );
1248 // save results of update in SGLocation for fdm...
1250 //if ( globals->get_scenery()->get_cur_elev() > -9990 ) {
1251 // acmodel_location->
1252 // set_cur_elev_m( globals->get_scenery()->get_cur_elev() );
1255 // The need for this here means that at least 2 consecutive passes are needed :-(
1256 aip.getSGLocation()->set_tile_center( globals->get_scenery()->get_next_center() );
1258 //cout << "Transform Elev is " << globals->get_scenery()->get_cur_elev() << '\n';
1259 aip.getSGLocation()->set_cur_elev_m(globals->get_scenery()->get_cur_elev());
1260 //return(globals->get_scenery()->get_cur_elev());