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-kts", true);
77 taxiRequestPending = false;
78 taxiRequestCleared = false;
80 clearedToLineUp = false;
81 clearedToTakeOff = false;
82 reportReadyForDeparture = false;
84 contactGround = false;
87 FGAILocalTraffic::~FGAILocalTraffic() {
91 // Get details of the active runway
92 // It is assumed that by the time this is called the tower control and airport code will have been set up.
93 void FGAILocalTraffic::GetRwyDetails() {
94 //cout << "GetRwyDetails called" << endl;
96 rwy.rwyID = tower->GetActiveRunway();
98 // Now we need to get the threshold position and rwy heading
100 SGPath path( globals->get_fg_root() );
101 path.append( "Airports" );
102 path.append( "runways.mk4" );
103 FGRunways runways( path.c_str() );
106 bool rwyGood = runways.search(airportID, rwy.rwyID, &runway);
108 // Get the threshold position
109 hdg = runway.heading; // TODO - check - is this our heading we are setting here, and if so should we be?
110 //cout << "hdg reset to " << hdg << '\n';
111 double other_way = hdg - 180.0;
112 while(other_way <= 0.0) {
116 // move to the +l end/center of the runway
117 //cout << "Runway center is at " << runway.lon << ", " << runway.lat << '\n';
118 Point3D origin = Point3D(runway.lon, runway.lat, aptElev);
119 Point3D ref = origin;
120 double tshlon, tshlat, tshr;
121 double tolon, tolat, tor;
122 rwy.length = runway.length * SG_FEET_TO_METER;
123 geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), other_way,
124 rwy.length / 2.0 - 25.0, &tshlat, &tshlon, &tshr );
125 geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), hdg,
126 rwy.length / 2.0 - 25.0, &tolat, &tolon, &tor );
127 // Note - 25 meters in from the runway end is a bit of a hack to put the plane ahead of the user.
128 // now copy what we need out of runway into rwy
129 rwy.threshold_pos = Point3D(tshlon, tshlat, aptElev);
130 Point3D takeoff_end = Point3D(tolon, tolat, aptElev);
131 //cout << "Threshold position = " << tshlon << ", " << tshlat << ", " << aptElev << '\n';
132 //cout << "Takeoff position = " << tolon << ", " << tolat << ", " << aptElev << '\n';
134 // Set the projection for the local area
135 ortho.Init(rwy.threshold_pos, rwy.hdg);
136 rwy.end1ortho = ortho.ConvertToLocal(rwy.threshold_pos); // should come out as zero
137 rwy.end2ortho = ortho.ConvertToLocal(takeoff_end);
139 SG_LOG(SG_ATC, SG_ALERT, "Help - can't get good runway in FGAILocalTraffic!!\n");
144 There are two possible scenarios during initialisation:
145 The first is that the user is flying towards the airport, and hence the traffic
146 could be initialised anywhere, as long as the AI planes are consistent with
148 The second is that the user has started the sim at or close to the airport, and
149 hence the traffic must be initialised with respect to the user as well as each other.
150 To a certain extent it's FGAIMgr that has to worry about this, but we need to provide
151 sufficient initialisation functionality within the plane classes to allow the manager
152 to initialy position them where and how required.
154 bool FGAILocalTraffic::Init(string ICAO, OperatingState initialState, PatternLeg initialLeg) {
155 //cout << "FGAILocalTraffic.Init(...) called" << endl;
156 // Hack alert - Hardwired path!!
157 string planepath = "Aircraft/c172/Models/c172-dpm.ac";
158 SGPath path = globals->get_fg_root();
159 path.append(planepath);
160 ssgBranch *model = sgLoad3DModel( path.str(),
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((string)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 // Check CTAF, unicom etc
192 //cout << "Unable to find airport details in FGAILocalTraffic::Init()\n";
195 // Get the airport elevation
196 aptElev = dclGetAirportElev(airportID.c_str()) * SG_FEET_TO_METER;
197 //cout << "Airport elev in AILocalTraffic = " << aptElev << '\n';
198 // WARNING - we use this elev for the whole airport - some assumptions in the code
199 // might fall down with very slopey airports.
201 //cout << "In Init(), initialState = " << initialState << endl;
202 operatingState = initialState;
203 switch(operatingState) {
205 ourGate = ground->GetGateNode();
206 if(ourGate == NULL) {
207 // Implies no available gates - what shall we do?
208 // For now just vanish the plane - possibly we can make this more elegant in the future
209 SG_LOG(SG_ATC, SG_ALERT, "No gate found by FGAILocalTraffic whilst attempting Init at " << airportID << '\n');
217 pos.setelev(aptElev);
218 hdg = ourGate->heading;
220 // Now we've set the position we can do the ground elev
221 elevInitGood = false;
228 // FIXME - implement this case properly
229 return(false); // remove this line when fixed!
232 // For now we'll always start the in_pattern case on the threshold ready to take-off
233 // since we've got the implementation for this case already.
234 // TODO - implement proper generic in_pattern startup.
236 // Get the active runway details (and copy them into rwy)
239 // Initial position on threshold for now
240 pos.setlat(rwy.threshold_pos.lat());
241 pos.setlon(rwy.threshold_pos.lon());
242 pos.setelev(rwy.threshold_pos.elev());
245 // Now we've set the position we can do the ground elev
246 // This might not always be necessary if we implement in-air start
247 elevInitGood = false;
257 circuitsToFly = 0; // ie just fly this circuit and then stop
259 // FIXME TODO - pattern direction is still hardwired
260 patternDirection = -1; // Left
261 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
262 if(rwy.rwyID.size() == 3) {
263 patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
266 operatingState = IN_PATTERN;
271 SG_LOG(SG_ATC, SG_ALERT, "Attempt to set unknown operating state in FGAILocalTraffic.Init(...)\n");
279 // Commands to do something from higher level logic
280 void FGAILocalTraffic::FlyCircuits(int numCircuits, bool tag) {
281 //cout << "FlyCircuits called" << endl;
283 switch(operatingState) {
285 circuitsToFly += numCircuits;
289 // For now we'll punt this and do nothing
292 circuitsToFly = numCircuits; // Note that one too many circuits gets flown because we only test and decrement circuitsToFly after landing
293 // thus flying one too many circuits. TODO - Need to sort this out better!
296 // Get the active runway details (and copy them into rwy)
299 // Get the takeoff node for the active runway, get a path to it and start taxiing
300 path = ground->GetPath(ourGate, rwy.rwyID);
301 if(path.size() < 2) {
302 // something has gone wrong
303 SG_LOG(SG_ATC, SG_ALERT, "Invalid path from gate to theshold in FGAILocalTraffic::FlyCircuits\n");
307 cout << "path returned was:" << endl;
308 for(unsigned int i=0; i<path.size(); ++i) {
309 switch(path[i]->struct_type) {
311 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
319 // pop the gate - we're here already!
320 path.erase(path.begin());
321 //path.erase(path.begin());
323 cout << "path after popping front is:" << endl;
324 for(unsigned int i=0; i<path.size(); ++i) {
325 switch(path[i]->struct_type) {
327 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
336 taxiState = TD_OUTBOUND;
339 // Maybe the below should be set when we get to the threshold and prepare for TO?
340 // FIXME TODO - pattern direction is still hardwired
341 patternDirection = -1; // Left
342 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
343 if(rwy.rwyID.size() == 3) {
344 patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
353 // Run the internal calculations
354 void FGAILocalTraffic::Update(double dt) {
355 //cout << "A" << flush;
356 double responseTime = 10.0; // seconds - this should get more sophisticated at some point
357 responseCounter += dt;
358 if((contactTower) && (responseCounter >= 8.0)) {
359 // Acknowledge request before changing frequency so it gets rendered if the user is on the same freq
360 string trns = "Tower ";
361 double f = globals->get_ATC_mgr()->GetFrequency(airportID, TOWER) / 100.0;
363 sprintf(buf, "%f", f);
366 trns += plane.callsign;
368 responseCounter = 0.0;
369 contactTower = false;
371 changeFreqType = TOWER;
374 if((changeFreq) && (responseCounter > 8.0)) {
375 switch(changeFreqType) {
377 freq = (double)tower->get_freq() / 100.0;
379 // Contact the tower, even if only virtually
381 tower->ContactAtHoldShort(plane, this, CIRCUIT);
384 freq = (double)ground->get_freq() / 100.0;
386 // And to avoid compiler warnings...
400 //cout << "." << flush;
402 switch(operatingState) {
404 //cout << "In IN_PATTERN\n";
405 if(!inAir) DoGroundElev();
407 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
408 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
409 //cout << "TAKEOFF_ROLL, POS = " << pos.lon() << ", " << pos.lat() << ", " << pos.elev() << '\n';
411 aip.setVisible(true);
412 //cout << "Making plane visible!\n";
416 FlyTrafficPattern(dt);
420 //cout << "In TAXIING\n";
421 //cout << "*" << flush;
424 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
425 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
427 aip.setVisible(true);
429 //cout << "Making plane visible!\n";
434 //cout << "," << flush;
435 if(!((holdingShort) && (!clearedToLineUp))) {
436 //cout << "|" << flush;
439 //cout << ";" << flush;
440 if((clearedToTakeOff) && (responseCounter >= 8.0)) {
441 // possible assumption that we're at the hold short here - may not always hold
442 // TODO - sort out the case where we're cleared to line-up first and then cleared to take-off on the rwy.
443 taxiState = TD_LINING_UP;
444 path = ground->GetPath(holdShortNode, rwy.rwyID);
446 cout << "path returned was:" << endl;
447 for(unsigned int i=0; i<path.size(); ++i) {
448 switch(path[i]->struct_type) {
450 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
458 clearedToTakeOff = false; // We *are* still cleared - this simply stops the response recurring!!
459 holdingShort = false;
460 string trns = "Cleared for take-off ";
461 trns += plane.callsign;
465 //cout << "^" << flush;
469 //cout << "In PARKED\n";
472 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
473 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
475 aip.setVisible(true);
477 //cout << "Making plane visible!\n";
483 if((taxiRequestPending) && (taxiRequestCleared)) {
484 //cout << "&" << flush;
485 // Get the active runway details (and copy them into rwy)
488 // Get the takeoff node for the active runway, get a path to it and start taxiing
489 path = ground->GetPathToHoldShort(ourGate, rwy.rwyID);
490 if(path.size() < 2) {
491 // something has gone wrong
492 SG_LOG(SG_ATC, SG_ALERT, "Invalid path from gate to theshold in FGAILocalTraffic::FlyCircuits\n");
496 cout << "path returned was:\n";
497 for(unsigned int i=0; i<path.size(); ++i) {
498 switch(path[i]->struct_type) {
500 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
508 path.erase(path.begin()); // pop the gate - we're here already!
509 taxiState = TD_OUTBOUND;
510 taxiRequestPending = false;
511 holdShortNode = (node*)(*(path.begin() + path.size()));
513 } else if(!taxiRequestPending) {
514 //cout << "(" << flush;
515 ground->RequestDeparture(plane, this);
516 // Do some communication
517 // airport name + tower + airplane callsign + location + request taxi for + operation type + ?
519 trns += tower->get_name();
521 trns += plane.callsign;
522 trns += " on apron parking request taxi for traffic pattern";
523 //cout << "trns = " << trns << endl;
525 taxiRequestCleared = false;
526 taxiRequestPending = true;
530 //cout << "!" << flush;
532 // Maybe the below should be set when we get to the threshold and prepare for TO?
533 // FIXME TODO - pattern direction is still hardwired
534 patternDirection = -1; // Left
535 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
536 if(rwy.rwyID.size() == 3) {
537 patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
541 //cout << ")" << flush;
546 //cout << "I " << flush;
549 void FGAILocalTraffic::RegisterTransmission(int code) {
551 case 1: // taxi request cleared
552 taxiRequestCleared = true;
553 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to taxi...");
555 case 2: // contact tower
558 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " told to contact tower...");
560 case 3: // Cleared to line up
562 clearedToLineUp = true;
563 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to line-up...");
565 case 4: // cleared to take-off
567 clearedToTakeOff = true;
568 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to take-off...");
575 // Fly a traffic pattern
576 // FIXME - far too much of the mechanics of turning, rolling, accellerating, descending etc is in here.
577 // Move it out to FGAIPlane and have FlyTrafficPattern just specify what to do, not the implementation.
578 void FGAILocalTraffic::FlyTrafficPattern(double dt) {
579 // Need to differentiate between in-air (IAS governed) and on-ground (vel governed)
580 // Take-off is an interesting case - we are on the ground but takeoff speed is IAS governed.
582 static bool transmitted = false; // FIXME - this is a hack
585 // Wind has two effects - a mechanical one in that IAS translates to a different vel, and the hdg != track,
586 // but also a piloting effect, in that the AI must be able to descend at a different rate in order to hit the threshold.
588 //cout << "dt = " << dt << '\n';
590 // ack - I can't remember how long a rate 1 turn is meant to take.
591 double turn_time = 60.0; // seconds - TODO - check this guess
592 double turn_circumference;
594 Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane
595 //cout << "runway elev = " << rwy.threshold_pos.elev() << ' ' << rwy.threshold_pos.elev() * SG_METER_TO_FEET << '\n';
596 //cout << "elev = " << pos.elev() << ' ' << pos.elev() * SG_METER_TO_FEET << '\n';
598 // HACK FOR TESTING - REMOVE
599 //cout << "Calling ExitRunway..." << endl;
600 //ExitRunway(orthopos);
605 double wind_from = wind_from_hdg->getDoubleValue();
606 double wind_speed = wind_speed_knots->getDoubleValue();
618 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
619 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
621 IAS = vel + (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
625 IAS = best_rate_of_climb_speed;
632 // Turn to crosswind if above 600ft AND if other traffic allows
633 // (decided in FGTower and accessed through GetCrosswindConstraint(...)).
634 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 600) {
636 if(tower->GetCrosswindConstraint(cc)) {
637 if(orthopos.y() > cc) {
638 cout << "Turning to crosswind, distance from threshold = " << orthopos.y() << '\n';
642 cout << "Turning to crosswind, distance from threshold = " << orthopos.y() << '\n';
646 // Need to check for levelling off in case we can't turn crosswind as soon
647 // as we would like due to other traffic.
648 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
651 IAS = 80.0; // FIXME - use smooth transistion to new speed and attitude.
655 track += (360.0 / turn_time) * dt * patternDirection;
656 Bank(25.0 * patternDirection);
657 if((track < (rwy.hdg - 89.0)) || (track > (rwy.hdg + 89.0))) {
663 track = rwy.hdg + (90.0 * patternDirection);
664 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
667 IAS = 80.0; // FIXME - use smooth transistion to new speed
669 // turn 1000m out for now, taking other traffic into accout
670 if(fabs(orthopos.x()) > 980) {
672 if(tower->GetDownwindConstraint(dd)) {
673 if(fabs(orthopos.x()) > fabs(dd)) {
674 cout << "Turning to downwind, distance from centerline = " << fabs(orthopos.x()) << '\n';
678 cout << "Turning to downwind, distance from centerline = " << fabs(orthopos.x()) << '\n';
684 track += (360.0 / turn_time) * dt * patternDirection;
685 Bank(25.0 * patternDirection);
686 // just in case we didn't make height on crosswind
687 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
690 IAS = 80.0; // FIXME - use smooth transistion to new speed
692 if((track < (rwy.hdg - 179.0)) || (track > (rwy.hdg + 179.0))) {
700 track = rwy.hdg - (180 * patternDirection); //should tend to bring track back into the 0->360 range
701 // just in case we didn't make height on crosswind
702 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
705 IAS = 90.0; // FIXME - use smooth transistion to new speed
707 if((orthopos.y() < 0) && (!transmitted)) {
708 TransmitPatternPositionReport();
711 if(orthopos.y() < -480) {
712 // FIXME - TODO - take tower baseleg constraint ie. other traffic, into account when calculating start of descent
713 slope = -4.0; // FIXME - calculate to descent at 500fpm and hit the threshold (taking wind into account as well!!)
717 if(orthopos.y() < -980) {
719 if(tower->GetDownwindConstraint(bb)) {
720 if(fabs(orthopos.y()) > fabs(bb)) {
721 cout << "Turning to base, distance from threshold = " << fabs(orthopos.y()) << '\n';
727 cout << "Turning to base, distance from threshold = " << fabs(orthopos.y()) << '\n';
735 track += (360.0 / turn_time) * dt * patternDirection;
736 Bank(25.0 * patternDirection);
737 if(fabs(rwy.hdg - track) < 91.0) {
744 TransmitPatternPositionReport();
747 track = rwy.hdg - (90 * patternDirection);
748 slope = -6.0; // FIXME - calculate to descent at 500fpm and hit the threshold
750 IAS = 70.0; // FIXME - slowdown gradually
751 // Try and arrange to turn nicely onto base
752 turn_circumference = IAS * 0.514444 * turn_time;
753 //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius
754 //We'll leave it as a hack with IAS for now but it needs revisiting.
756 turn_radius = turn_circumference / (2.0 * DCL_PI);
757 if(fabs(orthopos.x()) < (turn_radius + 50)) {
764 track += (360.0 / turn_time) * dt * patternDirection;
765 Bank(25.0 * patternDirection);
766 if(fabs(track - rwy.hdg) < 0.6) {
768 vel = nominal_final_speed;
774 TransmitPatternPositionReport();
777 // Try and track the extended centreline
778 track = rwy.hdg - (0.2 * orthopos.x());
779 //cout << "orthopos.x() = " << orthopos.x() << " hdg = " << hdg << '\n';
780 if(pos.elev() < (rwy.threshold_pos.elev()+20.0+wheelOffset)) {
781 DoGroundElev(); // Need to call it here expicitly on final since it's only called
782 // for us in update(...) when the inAir flag is false.
784 if(pos.elev() < (rwy.threshold_pos.elev()+10.0+wheelOffset)) {
785 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
786 if((aip.getSGLocation()->get_cur_elev_m() + wheelOffset) > pos.elev()) {
792 } // else need a fallback position based on arpt elev in case ground elev determination fails?
797 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
798 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
803 // FIXME - differentiate between touch and go and full stops
805 //cout << "Vel <= 15.0, circuitsToFly = " << circuitsToFly << endl;
806 if(circuitsToFly <= 0) {
807 //cout << "Calling ExitRunway..." << endl;
808 ExitRunway(orthopos);
811 //cout << "Taking off again..." << endl;
822 // FIXME - at the moment this is a bit screwy
823 // The velocity correction is applied based on the relative headings.
824 // Then the heading is changed based on the velocity.
825 // Which comes first, the chicken or the egg?
826 // Does it really matter?
828 // Apply wind to ground-relative velocity if in the air
829 vel = IAS - (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
830 //crab = f(track, wind, vel);
831 // The vector we need to fly is our desired vector minus the wind vector
832 // TODO - we probably ought to use plib's built in vector types and operations for this
833 // ie. There's almost *certainly* a better way to do this!
834 double gxx = vel * sin(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity x component wrt ground
835 double gyy = vel * cos(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity y component wrt ground
836 double wxx = wind_speed * sin((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS); // Wind velocity x component
837 double wyy = wind_speed * cos((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS); // Wind velocity y component
838 double axx = gxx - wxx; // Plane in-air velocity x component
839 double ayy = gyy - wyy; // Plane in-air velocity y component
840 // Now we want the angle between gxx and axx (which is the crab)
841 double maga = sqrt(axx*axx + ayy*ayy);
842 double magg = sqrt(gxx*gxx + gyy*gyy);
843 crab = acos((axx*gxx + ayy*gyy) / (maga * magg));
844 // At this point this works except we're getting the modulus of the angle
845 //cout << "crab = " << crab << '\n';
847 // Make sure both headings are in the 0->360 circle in order to get sane differences
848 dclBoundHeading(wind_from);
849 dclBoundHeading(track);
850 if(track > wind_from) {
851 if((track - wind_from) <= 180) {
855 if((wind_from - track) >= 180) {
859 } else { // on the ground - crab dosen't apply
864 dist = vel * 0.514444 * dt;
865 pos = dclUpdatePosition(pos, track, slope, dist);
868 void FGAILocalTraffic::TransmitPatternPositionReport(void) {
869 // airport name + "traffic" + airplane callsign + pattern direction + pattern leg + rwy + ?
872 trns += tower->get_name();
874 trns += plane.callsign;
875 if(patternDirection == 1) {
881 // We could probably get rid of this whole switch statement and just pass a string containing the leg from the FlyPattern function.
882 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?
884 // Fall through to CROSSWIND
885 case CROSSWIND: // I don't think this case will be used here but it can't hurt to leave it in
886 trns += "crosswind ";
889 // Fall through to DOWNWIND
894 // Fall through to BASE
899 // Fall through to FINAL
900 case FINAL: // maybe this should include long/short final if appropriate?
903 default: // Hopefully this won't be used
907 // FIXME - I've hardwired the runway call as well!! (We could work this out from rwy heading and mag deviation)
908 trns += ConvertRwyNumToSpokenString(1);
910 // And add the airport name again
911 trns += tower->get_name();
916 void FGAILocalTraffic::ExitRunway(Point3D orthopos) {
917 //cout << "In ExitRunway" << endl;
918 //cout << "Runway ID is " << rwy.ID << endl;
919 node_array_type exitNodes = ground->GetExits(rwy.rwyID); //I suppose we ought to have some fallback for rwy with no defined exits?
921 cout << "Node ID's of exits are ";
922 for(unsigned int i=0; i<exitNodes.size(); ++i) {
923 cout << exitNodes[i]->nodeID << ' ';
927 if(exitNodes.size()) {
928 //Find the next exit from orthopos.y
930 double dist = 100000; //ie. longer than any runway in existance
931 double backdist = 100000;
932 node_array_iterator nItr = exitNodes.begin();
933 node* rwyExit = *(exitNodes.begin());
934 //int gateID; //This might want to be more persistant at some point
935 while(nItr != exitNodes.end()) {
936 d = ortho.ConvertToLocal((*nItr)->pos).y() - ortho.ConvertToLocal(pos).y(); //FIXME - consider making orthopos a class variable
943 if(fabs(d) < backdist) {
945 //TODO - need some logic here that if we don't get a forward exit we turn round and store the backwards one
950 ourGate = ground->GetGateNode();
951 if(ourGate == NULL) {
952 // Implies no available gates - what shall we do?
953 // For now just vanish the plane - possibly we can make this more elegant in the future
954 SG_LOG(SG_ATC, SG_ALERT, "No gate found by FGAILocalTraffic whilst landing at " << airportID << '\n');
955 aip.setVisible(false);
956 operatingState = PARKED;
959 path = ground->GetPath(rwyExit, ourGate);
961 cout << "path returned was:" << endl;
962 for(unsigned int i=0; i<path.size(); ++i) {
963 switch(path[i]->struct_type) {
965 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
973 taxiState = TD_INBOUND;
976 // Something must have gone wrong with the ground network file - or there is only a rwy here and no exits defined
977 SG_LOG(SG_ATC, SG_ALERT, "No exits found by FGAILocalTraffic from runway " << rwy.rwyID << " at " << airportID << '\n');
978 // What shall we do - just remove the plane from sight?
979 aip.setVisible(false);
980 operatingState = PARKED;
984 // Set the class variable nextTaxiNode to the next node in the path
985 // and update taxiPathPos, the class variable path iterator position
986 // TODO - maybe should return error codes to the calling function if we fail here
987 void FGAILocalTraffic::GetNextTaxiNode() {
988 //cout << "GetNextTaxiNode called " << endl;
989 //cout << "taxiPathPos = " << taxiPathPos << endl;
990 ground_network_path_iterator pathItr = path.begin() + taxiPathPos;
991 if(pathItr == path.end()) {
992 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - no more nodes in path\n");
994 if((*pathItr)->struct_type == NODE) {
995 //cout << "ITS A NODE" << endl;
996 //*pathItr = new node;
997 nextTaxiNode = (node*)*pathItr;
1001 //cout << "ITS NOT A NODE" << endl;
1002 //The first item in found must have been an arc
1003 //Assume for now that it was straight
1006 if(pathItr == path.end()) {
1007 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - path ended with an arc\n");
1008 } else if((*pathItr)->struct_type == NODE) {
1009 nextTaxiNode = (node*)*pathItr;
1012 //OOPS - two non-nodes in a row - that shouldn't happen ATM
1013 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - two non-nodes in sequence\n");
1019 // StartTaxi - set up the taxiing state - call only at the start of taxiing
1020 void FGAILocalTraffic::StartTaxi() {
1021 //cout << "StartTaxi called" << endl;
1022 operatingState = TAXIING;
1025 //Set the desired heading
1026 //Assume we are aiming for first node on path
1027 //Eventually we may need to consider the fact that we might start on a curved arc and
1028 //not be able to head directly for the first node.
1029 GetNextTaxiNode(); // sets the class variable nextTaxiNode to the next taxi node!
1030 desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
1031 //cout << "First taxi heading is " << desiredTaxiHeading << endl;
1034 // speed in knots, headings in degrees, radius in meters.
1035 static double TaxiTurnTowardsHeading(double current_hdg, double desired_hdg, double speed, double radius, double dt) {
1036 // wrap heading - this prevents a logic bug where the plane would just go round in circles!!
1037 while(current_hdg < 0.0) {
1038 current_hdg += 360.0;
1040 while(current_hdg > 360.0) {
1041 current_hdg -= 360.0;
1043 if(fabs(current_hdg - desired_hdg) > 0.1) {
1044 // Which is the quickest direction to turn onto heading?
1045 if(desired_hdg > current_hdg) {
1046 if((desired_hdg - current_hdg) <= 180) {
1048 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1049 // TODO - check that increments are less than the delta that we check for the right direction
1050 // Probably need to reduce convergence speed as convergence is reached
1052 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1055 if((current_hdg - desired_hdg) <= 180) {
1057 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1058 // TODO - check that increments are less than the delta that we check for the right direction
1059 // Probably need to reduce convergence speed as convergence is reached
1061 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1065 return(current_hdg);
1068 void FGAILocalTraffic::Taxi(double dt) {
1069 //cout << "Taxi called" << endl;
1070 // Logic - if we are further away from next point than turn radius then head for it
1071 // If we have reached turning point then get next point and turn onto that heading
1072 // Look out for the finish!!
1074 //Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane
1075 desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
1077 bool lastNode = (taxiPathPos == path.size() ? true : false);
1079 //cout << "LAST NODE\n";
1082 // HACK ALERT! - for now we will taxi at constant speed for straights and turns
1084 // Remember that hdg is always equal to track when taxiing so we don't have to consider them both
1085 double dist_to_go = dclGetHorizontalSeparation(pos, nextTaxiNode->pos); // we may be able to do this more cheaply using orthopos
1086 //cout << "dist_to_go = " << dist_to_go << endl;
1087 if((nextTaxiNode->type == GATE) && (dist_to_go <= 0.1)) {
1088 // This might be more robust to outward paths starting with a gate if we check for either
1089 // last node or TD_INBOUND ?
1091 operatingState = PARKED;
1092 } else if(((dist_to_go > taxiTurnRadius) || (nextTaxiNode->type == GATE)) && (!liningUp)){
1093 // if the turn radius is r, and speed is s, then in a time dt we turn through
1094 // ((s.dt)/(PI.r)) x 180 degrees
1095 // or alternatively (s.dt)/r radians
1096 //cout << "hdg = " << hdg << " desired taxi heading = " << desiredTaxiHeading << '\n';
1097 hdg = TaxiTurnTowardsHeading(hdg, desiredTaxiHeading, nominalTaxiSpeed, taxiTurnRadius, dt);
1098 double vel = nominalTaxiSpeed;
1099 //cout << "vel = " << vel << endl;
1100 double dist = vel * 0.514444 * dt;
1101 //cout << "dist = " << dist << endl;
1103 //cout << "track = " << track << endl;
1105 pos = dclUpdatePosition(pos, track, slope, dist);
1106 //cout << "Updated position...\n";
1107 if(aip.getSGLocation()->get_cur_elev_m() > -9990) {
1108 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
1109 } // else don't change the elev until we get a valid ground elev again!
1110 } else if(lastNode) {
1111 if(taxiState == TD_LINING_UP) {
1112 if((!liningUp) && (dist_to_go <= taxiTurnRadius)) {
1116 hdg = TaxiTurnTowardsHeading(hdg, rwy.hdg, nominalTaxiSpeed, taxiTurnRadius, dt);
1117 double vel = nominalTaxiSpeed;
1118 //cout << "vel = " << vel << endl;
1119 double dist = vel * 0.514444 * dt;
1120 //cout << "dist = " << dist << endl;
1122 //cout << "track = " << track << endl;
1124 pos = dclUpdatePosition(pos, track, slope, dist);
1125 //cout << "Updated position...\n";
1126 if(aip.getSGLocation()->get_cur_elev_m() > -9990) {
1127 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
1128 } // else don't change the elev until we get a valid ground elev again!
1129 if(fabs(hdg - rwy.hdg) <= 1.0) {
1130 operatingState = IN_PATTERN;
1136 } else if(taxiState == TD_OUTBOUND) {
1137 // Pause awaiting further instructions
1138 // and for now assume we've reached the hold-short node
1139 holdingShort = true;
1140 } // else at the moment assume TD_INBOUND always ends in a gate in which case we can ignore it
1142 // Time to turn (we've already checked it's not the end we're heading for).
1143 // set the target node to be the next node which will prompt automatically turning onto
1144 // the right heading in the stuff above, with the usual provisos applied.
1146 // For now why not just recursively call this function?
1152 // Warning - ground elev determination is CPU intensive
1153 // Either this function or the logic of how often it is called
1154 // will almost certainly change.
1155 void FGAILocalTraffic::DoGroundElev() {
1157 // It would be nice if we could set the correct tile center here in order to get a correct
1158 // answer with one call to the function, but what I tried in the two commented-out lines
1159 // below only intermittently worked, and I haven't quite groked why yet.
1160 //SGBucket buck(pos.lon(), pos.lat());
1161 //aip.getSGLocation()->set_tile_center(Point3D(buck.get_center_lon(), buck.get_center_lat(), 0.0));
1163 double visibility_meters = fgGetDouble("/environment/visibility-m");
1164 //globals->get_tile_mgr()->prep_ssg_nodes( acmodel_location,
1165 globals->get_tile_mgr()->prep_ssg_nodes( aip.getSGLocation(), visibility_meters );
1166 Point3D scenery_center = globals->get_scenery()->get_center();
1167 globals->get_tile_mgr()->update( aip.getSGLocation(), visibility_meters, (aip.getSGLocation())->get_absolute_view_pos( scenery_center ) );
1168 // save results of update in SGLocation for fdm...
1170 //if ( globals->get_scenery()->get_cur_elev() > -9990 ) {
1171 // acmodel_location->
1172 // set_cur_elev_m( globals->get_scenery()->get_cur_elev() );
1175 // The need for this here means that at least 2 consecutive passes are needed :-(
1176 aip.getSGLocation()->set_tile_center( globals->get_scenery()->get_next_center() );
1178 //cout << "Transform Elev is " << globals->get_scenery()->get_cur_elev() << '\n';
1179 aip.getSGLocation()->set_cur_elev_m(globals->get_scenery()->get_cur_elev());
1180 //return(globals->get_scenery()->get_cur_elev());