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 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
101 bool rwyGood = globals->get_runways()->search(airportID, rwy.rwyID,
104 // Get the threshold position
105 hdg = runway.heading; // TODO - check - is this our heading we are setting here, and if so should we be?
106 //cout << "hdg reset to " << hdg << '\n';
107 double other_way = hdg - 180.0;
108 while(other_way <= 0.0) {
112 // move to the +l end/center of the runway
113 //cout << "Runway center is at " << runway.lon << ", " << runway.lat << '\n';
114 Point3D origin = Point3D(runway.lon, runway.lat, aptElev);
115 Point3D ref = origin;
116 double tshlon, tshlat, tshr;
117 double tolon, tolat, tor;
118 rwy.length = runway.length * SG_FEET_TO_METER;
119 geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), other_way,
120 rwy.length / 2.0 - 25.0, &tshlat, &tshlon, &tshr );
121 geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), hdg,
122 rwy.length / 2.0 - 25.0, &tolat, &tolon, &tor );
123 // Note - 25 meters in from the runway end is a bit of a hack to put the plane ahead of the user.
124 // now copy what we need out of runway into rwy
125 rwy.threshold_pos = Point3D(tshlon, tshlat, aptElev);
126 Point3D takeoff_end = Point3D(tolon, tolat, aptElev);
127 //cout << "Threshold position = " << tshlon << ", " << tshlat << ", " << aptElev << '\n';
128 //cout << "Takeoff position = " << tolon << ", " << tolat << ", " << aptElev << '\n';
130 // Set the projection for the local area
131 ortho.Init(rwy.threshold_pos, rwy.hdg);
132 rwy.end1ortho = ortho.ConvertToLocal(rwy.threshold_pos); // should come out as zero
133 rwy.end2ortho = ortho.ConvertToLocal(takeoff_end);
135 SG_LOG(SG_ATC, SG_ALERT, "Help - can't get good runway in FGAILocalTraffic!!\n");
140 There are two possible scenarios during initialisation:
141 The first is that the user is flying towards the airport, and hence the traffic
142 could be initialised anywhere, as long as the AI planes are consistent with
144 The second is that the user has started the sim at or close to the airport, and
145 hence the traffic must be initialised with respect to the user as well as each other.
146 To a certain extent it's FGAIMgr that has to worry about this, but we need to provide
147 sufficient initialisation functionality within the plane classes to allow the manager
148 to initialy position them where and how required.
150 bool FGAILocalTraffic::Init(string ICAO, OperatingState initialState, PatternLeg initialLeg) {
151 //cout << "FGAILocalTraffic.Init(...) called" << endl;
152 // Hack alert - Hardwired path!!
153 string planepath = "Aircraft/c172/Models/c172-dpm.ac";
154 SGPath path = globals->get_fg_root();
155 path.append(planepath);
156 ssgBranch *model = sgLoad3DModel( path.str(),
158 globals->get_props(),
159 globals->get_sim_time_sec() );
161 aip.setVisible(false); // This will be set to true once a valid ground elevation has been determined
162 globals->get_scenery()->get_scene_graph()->addKid(aip.getSceneGraph());
164 // Find the tower frequency - this is dependent on the ATC system being initialised before the AI system
167 if(ATC->GetAirportATCDetails(airportID, &a)) {
168 if(a.tower_freq) { // Has a tower
169 tower = (FGTower*)ATC->GetATCPointer((string)airportID, TOWER); // Maybe need some error checking here
171 // Something has gone wrong - abort or carry on with un-towered operation?
174 freq = (double)tower->get_freq() / 100.0;
175 ground = tower->GetGroundPtr();
177 // Something has gone wrong :-(
178 SG_LOG(SG_ATC, SG_ALERT, "ERROR - can't get a ground pointer from tower control in FGAILocalTraffic::Init() :-(");
180 } else if((initialState == PARKED) || (initialState == TAXIING)) {
181 freq = (double)ground->get_freq() / 100.0;
183 //cout << "AILocalTraffic freq is " << freq << '\n';
185 // Check CTAF, unicom etc
188 //cout << "Unable to find airport details in FGAILocalTraffic::Init()\n";
191 // Get the airport elevation
192 aptElev = dclGetAirportElev(airportID.c_str()) * SG_FEET_TO_METER;
193 //cout << "Airport elev in AILocalTraffic = " << aptElev << '\n';
194 // WARNING - we use this elev for the whole airport - some assumptions in the code
195 // might fall down with very slopey airports.
197 //cout << "In Init(), initialState = " << initialState << endl;
198 operatingState = initialState;
199 switch(operatingState) {
201 ourGate = ground->GetGateNode();
202 if(ourGate == NULL) {
203 // Implies no available gates - what shall we do?
204 // For now just vanish the plane - possibly we can make this more elegant in the future
205 SG_LOG(SG_ATC, SG_ALERT, "No gate found by FGAILocalTraffic whilst attempting Init at " << airportID << '\n');
213 pos.setelev(aptElev);
214 hdg = ourGate->heading;
216 // Now we've set the position we can do the ground elev
217 elevInitGood = false;
224 // FIXME - implement this case properly
225 return(false); // remove this line when fixed!
228 // For now we'll always start the in_pattern case on the threshold ready to take-off
229 // since we've got the implementation for this case already.
230 // TODO - implement proper generic in_pattern startup.
232 // Get the active runway details (and copy them into rwy)
235 // Initial position on threshold for now
236 pos.setlat(rwy.threshold_pos.lat());
237 pos.setlon(rwy.threshold_pos.lon());
238 pos.setelev(rwy.threshold_pos.elev());
241 // Now we've set the position we can do the ground elev
242 // This might not always be necessary if we implement in-air start
243 elevInitGood = false;
253 circuitsToFly = 0; // ie just fly this circuit and then stop
255 // FIXME TODO - pattern direction is still hardwired
256 patternDirection = -1; // Left
257 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
258 if(rwy.rwyID.size() == 3) {
259 patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
262 operatingState = IN_PATTERN;
267 SG_LOG(SG_ATC, SG_ALERT, "Attempt to set unknown operating state in FGAILocalTraffic.Init(...)\n");
275 // Commands to do something from higher level logic
276 void FGAILocalTraffic::FlyCircuits(int numCircuits, bool tag) {
277 //cout << "FlyCircuits called" << endl;
279 switch(operatingState) {
281 circuitsToFly += numCircuits;
285 // For now we'll punt this and do nothing
288 circuitsToFly = numCircuits; // Note that one too many circuits gets flown because we only test and decrement circuitsToFly after landing
289 // thus flying one too many circuits. TODO - Need to sort this out better!
292 // Get the active runway details (and copy them into rwy)
295 // Get the takeoff node for the active runway, get a path to it and start taxiing
296 path = ground->GetPath(ourGate, rwy.rwyID);
297 if(path.size() < 2) {
298 // something has gone wrong
299 SG_LOG(SG_ATC, SG_ALERT, "Invalid path from gate to theshold in FGAILocalTraffic::FlyCircuits\n");
303 cout << "path returned was:" << endl;
304 for(unsigned int i=0; i<path.size(); ++i) {
305 switch(path[i]->struct_type) {
307 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
315 // pop the gate - we're here already!
316 path.erase(path.begin());
317 //path.erase(path.begin());
319 cout << "path after popping front is:" << endl;
320 for(unsigned int i=0; i<path.size(); ++i) {
321 switch(path[i]->struct_type) {
323 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
332 taxiState = TD_OUTBOUND;
335 // Maybe the below should be set when we get to the threshold and prepare for TO?
336 // FIXME TODO - pattern direction is still hardwired
337 patternDirection = -1; // Left
338 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
339 if(rwy.rwyID.size() == 3) {
340 patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
349 // Run the internal calculations
350 void FGAILocalTraffic::Update(double dt) {
351 //cout << "A" << flush;
352 double responseTime = 10.0; // seconds - this should get more sophisticated at some point
353 responseCounter += dt;
354 if((contactTower) && (responseCounter >= 8.0)) {
355 // Acknowledge request before changing frequency so it gets rendered if the user is on the same freq
356 string trns = "Tower ";
357 double f = globals->get_ATC_mgr()->GetFrequency(airportID, TOWER) / 100.0;
359 sprintf(buf, "%f", f);
362 trns += plane.callsign;
364 responseCounter = 0.0;
365 contactTower = false;
367 changeFreqType = TOWER;
370 if((changeFreq) && (responseCounter > 8.0)) {
371 switch(changeFreqType) {
373 freq = (double)tower->get_freq() / 100.0;
375 // Contact the tower, even if only virtually
377 tower->ContactAtHoldShort(plane, this, CIRCUIT);
380 freq = (double)ground->get_freq() / 100.0;
382 // And to avoid compiler warnings...
396 //cout << "." << flush;
398 switch(operatingState) {
400 //cout << "In IN_PATTERN\n";
401 if(!inAir) DoGroundElev();
403 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
404 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
405 //cout << "TAKEOFF_ROLL, POS = " << pos.lon() << ", " << pos.lat() << ", " << pos.elev() << '\n';
407 aip.setVisible(true);
408 //cout << "Making plane visible!\n";
412 FlyTrafficPattern(dt);
416 //cout << "In TAXIING\n";
417 //cout << "*" << flush;
420 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
421 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
423 aip.setVisible(true);
425 //cout << "Making plane visible!\n";
430 //cout << "," << flush;
431 if(!((holdingShort) && (!clearedToLineUp))) {
432 //cout << "|" << flush;
435 //cout << ";" << flush;
436 if((clearedToTakeOff) && (responseCounter >= 8.0)) {
437 // possible assumption that we're at the hold short here - may not always hold
438 // TODO - sort out the case where we're cleared to line-up first and then cleared to take-off on the rwy.
439 taxiState = TD_LINING_UP;
440 path = ground->GetPath(holdShortNode, rwy.rwyID);
442 cout << "path returned was:" << endl;
443 for(unsigned int i=0; i<path.size(); ++i) {
444 switch(path[i]->struct_type) {
446 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
454 clearedToTakeOff = false; // We *are* still cleared - this simply stops the response recurring!!
455 holdingShort = false;
456 string trns = "Cleared for take-off ";
457 trns += plane.callsign;
461 //cout << "^" << flush;
465 //cout << "In PARKED\n";
468 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
469 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
471 aip.setVisible(true);
473 //cout << "Making plane visible!\n";
479 if((taxiRequestPending) && (taxiRequestCleared)) {
480 //cout << "&" << flush;
481 // Get the active runway details (and copy them into rwy)
484 // Get the takeoff node for the active runway, get a path to it and start taxiing
485 path = ground->GetPathToHoldShort(ourGate, rwy.rwyID);
486 if(path.size() < 2) {
487 // something has gone wrong
488 SG_LOG(SG_ATC, SG_ALERT, "Invalid path from gate to theshold in FGAILocalTraffic::FlyCircuits\n");
492 cout << "path returned was:\n";
493 for(unsigned int i=0; i<path.size(); ++i) {
494 switch(path[i]->struct_type) {
496 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
504 path.erase(path.begin()); // pop the gate - we're here already!
505 taxiState = TD_OUTBOUND;
506 taxiRequestPending = false;
507 holdShortNode = (node*)(*(path.begin() + path.size()));
509 } else if(!taxiRequestPending) {
510 //cout << "(" << flush;
511 ground->RequestDeparture(plane, this);
512 // Do some communication
513 // airport name + tower + airplane callsign + location + request taxi for + operation type + ?
515 trns += tower->get_name();
517 trns += plane.callsign;
518 trns += " on apron parking request taxi for traffic pattern";
519 //cout << "trns = " << trns << endl;
521 taxiRequestCleared = false;
522 taxiRequestPending = true;
526 //cout << "!" << flush;
528 // Maybe the below should be set when we get to the threshold and prepare for TO?
529 // FIXME TODO - pattern direction is still hardwired
530 patternDirection = -1; // Left
531 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
532 if(rwy.rwyID.size() == 3) {
533 patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
537 //cout << ")" << flush;
542 //cout << "I " << flush;
545 void FGAILocalTraffic::RegisterTransmission(int code) {
547 case 1: // taxi request cleared
548 taxiRequestCleared = true;
549 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to taxi...");
551 case 2: // contact tower
554 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " told to contact tower...");
556 case 3: // Cleared to line up
558 clearedToLineUp = true;
559 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to line-up...");
561 case 4: // cleared to take-off
563 clearedToTakeOff = true;
564 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to take-off...");
571 // Fly a traffic pattern
572 // FIXME - far too much of the mechanics of turning, rolling, accellerating, descending etc is in here.
573 // Move it out to FGAIPlane and have FlyTrafficPattern just specify what to do, not the implementation.
574 void FGAILocalTraffic::FlyTrafficPattern(double dt) {
575 // Need to differentiate between in-air (IAS governed) and on-ground (vel governed)
576 // Take-off is an interesting case - we are on the ground but takeoff speed is IAS governed.
578 static bool transmitted = false; // FIXME - this is a hack
581 // Wind has two effects - a mechanical one in that IAS translates to a different vel, and the hdg != track,
582 // but also a piloting effect, in that the AI must be able to descend at a different rate in order to hit the threshold.
584 //cout << "dt = " << dt << '\n';
586 // ack - I can't remember how long a rate 1 turn is meant to take.
587 double turn_time = 60.0; // seconds - TODO - check this guess
588 double turn_circumference;
590 Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane
591 //cout << "runway elev = " << rwy.threshold_pos.elev() << ' ' << rwy.threshold_pos.elev() * SG_METER_TO_FEET << '\n';
592 //cout << "elev = " << pos.elev() << ' ' << pos.elev() * SG_METER_TO_FEET << '\n';
594 // HACK FOR TESTING - REMOVE
595 //cout << "Calling ExitRunway..." << endl;
596 //ExitRunway(orthopos);
601 double wind_from = wind_from_hdg->getDoubleValue();
602 double wind_speed = wind_speed_knots->getDoubleValue();
614 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
615 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
617 IAS = vel + (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
621 IAS = best_rate_of_climb_speed;
628 // Turn to crosswind if above 600ft AND if other traffic allows
629 // (decided in FGTower and accessed through GetCrosswindConstraint(...)).
630 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 600) {
632 if(tower->GetCrosswindConstraint(cc)) {
633 if(orthopos.y() > cc) {
634 cout << "Turning to crosswind, distance from threshold = " << orthopos.y() << '\n';
638 cout << "Turning to crosswind, distance from threshold = " << orthopos.y() << '\n';
642 // Need to check for levelling off in case we can't turn crosswind as soon
643 // as we would like due to other traffic.
644 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
647 IAS = 80.0; // FIXME - use smooth transistion to new speed and attitude.
651 track += (360.0 / turn_time) * dt * patternDirection;
652 Bank(25.0 * patternDirection);
653 if((track < (rwy.hdg - 89.0)) || (track > (rwy.hdg + 89.0))) {
659 track = rwy.hdg + (90.0 * patternDirection);
660 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
663 IAS = 80.0; // FIXME - use smooth transistion to new speed
665 // turn 1000m out for now, taking other traffic into accout
666 if(fabs(orthopos.x()) > 980) {
668 if(tower->GetDownwindConstraint(dd)) {
669 if(fabs(orthopos.x()) > fabs(dd)) {
670 cout << "Turning to downwind, distance from centerline = " << fabs(orthopos.x()) << '\n';
674 cout << "Turning to downwind, distance from centerline = " << fabs(orthopos.x()) << '\n';
680 track += (360.0 / turn_time) * dt * patternDirection;
681 Bank(25.0 * patternDirection);
682 // just in case we didn't make height on crosswind
683 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
686 IAS = 80.0; // FIXME - use smooth transistion to new speed
688 if((track < (rwy.hdg - 179.0)) || (track > (rwy.hdg + 179.0))) {
696 track = rwy.hdg - (180 * patternDirection); //should tend to bring track back into the 0->360 range
697 // just in case we didn't make height on crosswind
698 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
701 IAS = 90.0; // FIXME - use smooth transistion to new speed
703 if((orthopos.y() < 0) && (!transmitted)) {
704 TransmitPatternPositionReport();
707 if(orthopos.y() < -480) {
708 // FIXME - TODO - take tower baseleg constraint ie. other traffic, into account when calculating start of descent
709 slope = -4.0; // FIXME - calculate to descent at 500fpm and hit the threshold (taking wind into account as well!!)
713 if(orthopos.y() < -980) {
715 if(tower->GetDownwindConstraint(bb)) {
716 if(fabs(orthopos.y()) > fabs(bb)) {
717 cout << "Turning to base, distance from threshold = " << fabs(orthopos.y()) << '\n';
723 cout << "Turning to base, distance from threshold = " << fabs(orthopos.y()) << '\n';
731 track += (360.0 / turn_time) * dt * patternDirection;
732 Bank(25.0 * patternDirection);
733 if(fabs(rwy.hdg - track) < 91.0) {
740 TransmitPatternPositionReport();
743 track = rwy.hdg - (90 * patternDirection);
744 slope = -6.0; // FIXME - calculate to descent at 500fpm and hit the threshold
746 IAS = 70.0; // FIXME - slowdown gradually
747 // Try and arrange to turn nicely onto base
748 turn_circumference = IAS * 0.514444 * turn_time;
749 //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius
750 //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(fabs(orthopos.x()) < (turn_radius + 50)) {
760 track += (360.0 / turn_time) * dt * patternDirection;
761 Bank(25.0 * patternDirection);
762 if(fabs(track - rwy.hdg) < 0.6) {
764 vel = nominal_final_speed;
770 TransmitPatternPositionReport();
773 // Try and track the extended centreline
774 track = rwy.hdg - (0.2 * orthopos.x());
775 //cout << "orthopos.x() = " << orthopos.x() << " hdg = " << hdg << '\n';
776 if(pos.elev() < (rwy.threshold_pos.elev()+20.0+wheelOffset)) {
777 DoGroundElev(); // Need to call it here expicitly on final since it's only called
778 // for us in update(...) when the inAir flag is false.
780 if(pos.elev() < (rwy.threshold_pos.elev()+10.0+wheelOffset)) {
781 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
782 if((aip.getSGLocation()->get_cur_elev_m() + wheelOffset) > pos.elev()) {
788 } // else need a fallback position based on arpt elev in case ground elev determination fails?
793 if(aip.getSGLocation()->get_cur_elev_m() > -9990.0) {
794 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
799 // FIXME - differentiate between touch and go and full stops
801 //cout << "Vel <= 15.0, circuitsToFly = " << circuitsToFly << endl;
802 if(circuitsToFly <= 0) {
803 //cout << "Calling ExitRunway..." << endl;
804 ExitRunway(orthopos);
807 //cout << "Taking off again..." << endl;
818 // FIXME - at the moment this is a bit screwy
819 // The velocity correction is applied based on the relative headings.
820 // Then the heading is changed based on the velocity.
821 // Which comes first, the chicken or the egg?
822 // Does it really matter?
824 // Apply wind to ground-relative velocity if in the air
825 vel = IAS - (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
826 //crab = f(track, wind, vel);
827 // The vector we need to fly is our desired vector minus the wind vector
828 // TODO - we probably ought to use plib's built in vector types and operations for this
829 // ie. There's almost *certainly* a better way to do this!
830 double gxx = vel * sin(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity x component wrt ground
831 double gyy = vel * cos(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity y component wrt ground
832 double wxx = wind_speed * sin((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS); // Wind velocity x component
833 double wyy = wind_speed * cos((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS); // Wind velocity y component
834 double axx = gxx - wxx; // Plane in-air velocity x component
835 double ayy = gyy - wyy; // Plane in-air velocity y component
836 // Now we want the angle between gxx and axx (which is the crab)
837 double maga = sqrt(axx*axx + ayy*ayy);
838 double magg = sqrt(gxx*gxx + gyy*gyy);
839 crab = acos((axx*gxx + ayy*gyy) / (maga * magg));
840 // At this point this works except we're getting the modulus of the angle
841 //cout << "crab = " << crab << '\n';
843 // Make sure both headings are in the 0->360 circle in order to get sane differences
844 dclBoundHeading(wind_from);
845 dclBoundHeading(track);
846 if(track > wind_from) {
847 if((track - wind_from) <= 180) {
851 if((wind_from - track) >= 180) {
855 } else { // on the ground - crab dosen't apply
860 dist = vel * 0.514444 * dt;
861 pos = dclUpdatePosition(pos, track, slope, dist);
864 void FGAILocalTraffic::TransmitPatternPositionReport(void) {
865 // airport name + "traffic" + airplane callsign + pattern direction + pattern leg + rwy + ?
868 trns += tower->get_name();
870 trns += plane.callsign;
871 if(patternDirection == 1) {
877 // We could probably get rid of this whole switch statement and just pass a string containing the leg from the FlyPattern function.
878 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?
880 // Fall through to CROSSWIND
881 case CROSSWIND: // I don't think this case will be used here but it can't hurt to leave it in
882 trns += "crosswind ";
885 // Fall through to DOWNWIND
890 // Fall through to BASE
895 // Fall through to FINAL
896 case FINAL: // maybe this should include long/short final if appropriate?
899 default: // Hopefully this won't be used
903 // FIXME - I've hardwired the runway call as well!! (We could work this out from rwy heading and mag deviation)
904 trns += ConvertRwyNumToSpokenString(1);
906 // And add the airport name again
907 trns += tower->get_name();
912 void FGAILocalTraffic::ExitRunway(Point3D orthopos) {
913 //cout << "In ExitRunway" << endl;
914 //cout << "Runway ID is " << rwy.ID << endl;
915 node_array_type exitNodes = ground->GetExits(rwy.rwyID); //I suppose we ought to have some fallback for rwy with no defined exits?
917 cout << "Node ID's of exits are ";
918 for(unsigned int i=0; i<exitNodes.size(); ++i) {
919 cout << exitNodes[i]->nodeID << ' ';
923 if(exitNodes.size()) {
924 //Find the next exit from orthopos.y
926 double dist = 100000; //ie. longer than any runway in existance
927 double backdist = 100000;
928 node_array_iterator nItr = exitNodes.begin();
929 node* rwyExit = *(exitNodes.begin());
930 //int gateID; //This might want to be more persistant at some point
931 while(nItr != exitNodes.end()) {
932 d = ortho.ConvertToLocal((*nItr)->pos).y() - ortho.ConvertToLocal(pos).y(); //FIXME - consider making orthopos a class variable
939 if(fabs(d) < backdist) {
941 //TODO - need some logic here that if we don't get a forward exit we turn round and store the backwards one
946 ourGate = ground->GetGateNode();
947 if(ourGate == NULL) {
948 // Implies no available gates - what shall we do?
949 // For now just vanish the plane - possibly we can make this more elegant in the future
950 SG_LOG(SG_ATC, SG_ALERT, "No gate found by FGAILocalTraffic whilst landing at " << airportID << '\n');
951 aip.setVisible(false);
952 operatingState = PARKED;
955 path = ground->GetPath(rwyExit, ourGate);
957 cout << "path returned was:" << endl;
958 for(unsigned int i=0; i<path.size(); ++i) {
959 switch(path[i]->struct_type) {
961 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
969 taxiState = TD_INBOUND;
972 // Something must have gone wrong with the ground network file - or there is only a rwy here and no exits defined
973 SG_LOG(SG_ATC, SG_ALERT, "No exits found by FGAILocalTraffic from runway " << rwy.rwyID << " at " << airportID << '\n');
974 // What shall we do - just remove the plane from sight?
975 aip.setVisible(false);
976 operatingState = PARKED;
980 // Set the class variable nextTaxiNode to the next node in the path
981 // and update taxiPathPos, the class variable path iterator position
982 // TODO - maybe should return error codes to the calling function if we fail here
983 void FGAILocalTraffic::GetNextTaxiNode() {
984 //cout << "GetNextTaxiNode called " << endl;
985 //cout << "taxiPathPos = " << taxiPathPos << endl;
986 ground_network_path_iterator pathItr = path.begin() + taxiPathPos;
987 if(pathItr == path.end()) {
988 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - no more nodes in path\n");
990 if((*pathItr)->struct_type == NODE) {
991 //cout << "ITS A NODE" << endl;
992 //*pathItr = new node;
993 nextTaxiNode = (node*)*pathItr;
997 //cout << "ITS NOT A NODE" << endl;
998 //The first item in found must have been an arc
999 //Assume for now that it was straight
1002 if(pathItr == path.end()) {
1003 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - path ended with an arc\n");
1004 } else if((*pathItr)->struct_type == NODE) {
1005 nextTaxiNode = (node*)*pathItr;
1008 //OOPS - two non-nodes in a row - that shouldn't happen ATM
1009 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - two non-nodes in sequence\n");
1015 // StartTaxi - set up the taxiing state - call only at the start of taxiing
1016 void FGAILocalTraffic::StartTaxi() {
1017 //cout << "StartTaxi called" << endl;
1018 operatingState = TAXIING;
1021 //Set the desired heading
1022 //Assume we are aiming for first node on path
1023 //Eventually we may need to consider the fact that we might start on a curved arc and
1024 //not be able to head directly for the first node.
1025 GetNextTaxiNode(); // sets the class variable nextTaxiNode to the next taxi node!
1026 desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
1027 //cout << "First taxi heading is " << desiredTaxiHeading << endl;
1030 // speed in knots, headings in degrees, radius in meters.
1031 static double TaxiTurnTowardsHeading(double current_hdg, double desired_hdg, double speed, double radius, double dt) {
1032 // wrap heading - this prevents a logic bug where the plane would just go round in circles!!
1033 while(current_hdg < 0.0) {
1034 current_hdg += 360.0;
1036 while(current_hdg > 360.0) {
1037 current_hdg -= 360.0;
1039 if(fabs(current_hdg - desired_hdg) > 0.1) {
1040 // Which is the quickest direction to turn onto heading?
1041 if(desired_hdg > current_hdg) {
1042 if((desired_hdg - current_hdg) <= 180) {
1044 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1045 // TODO - check that increments are less than the delta that we check for the right direction
1046 // Probably need to reduce convergence speed as convergence is reached
1048 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1051 if((current_hdg - desired_hdg) <= 180) {
1053 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1054 // TODO - check that increments are less than the delta that we check for the right direction
1055 // Probably need to reduce convergence speed as convergence is reached
1057 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1061 return(current_hdg);
1064 void FGAILocalTraffic::Taxi(double dt) {
1065 //cout << "Taxi called" << endl;
1066 // Logic - if we are further away from next point than turn radius then head for it
1067 // If we have reached turning point then get next point and turn onto that heading
1068 // Look out for the finish!!
1070 //Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane
1071 desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
1073 bool lastNode = (taxiPathPos == path.size() ? true : false);
1075 //cout << "LAST NODE\n";
1078 // HACK ALERT! - for now we will taxi at constant speed for straights and turns
1080 // Remember that hdg is always equal to track when taxiing so we don't have to consider them both
1081 double dist_to_go = dclGetHorizontalSeparation(pos, nextTaxiNode->pos); // we may be able to do this more cheaply using orthopos
1082 //cout << "dist_to_go = " << dist_to_go << endl;
1083 if((nextTaxiNode->type == GATE) && (dist_to_go <= 0.1)) {
1084 // This might be more robust to outward paths starting with a gate if we check for either
1085 // last node or TD_INBOUND ?
1087 operatingState = PARKED;
1088 } else if(((dist_to_go > taxiTurnRadius) || (nextTaxiNode->type == GATE)) && (!liningUp)){
1089 // if the turn radius is r, and speed is s, then in a time dt we turn through
1090 // ((s.dt)/(PI.r)) x 180 degrees
1091 // or alternatively (s.dt)/r radians
1092 //cout << "hdg = " << hdg << " desired taxi heading = " << desiredTaxiHeading << '\n';
1093 hdg = TaxiTurnTowardsHeading(hdg, desiredTaxiHeading, nominalTaxiSpeed, taxiTurnRadius, dt);
1094 double vel = nominalTaxiSpeed;
1095 //cout << "vel = " << vel << endl;
1096 double dist = vel * 0.514444 * dt;
1097 //cout << "dist = " << dist << endl;
1099 //cout << "track = " << track << endl;
1101 pos = dclUpdatePosition(pos, track, slope, dist);
1102 //cout << "Updated position...\n";
1103 if(aip.getSGLocation()->get_cur_elev_m() > -9990) {
1104 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
1105 } // else don't change the elev until we get a valid ground elev again!
1106 } else if(lastNode) {
1107 if(taxiState == TD_LINING_UP) {
1108 if((!liningUp) && (dist_to_go <= taxiTurnRadius)) {
1112 hdg = TaxiTurnTowardsHeading(hdg, rwy.hdg, nominalTaxiSpeed, taxiTurnRadius, dt);
1113 double vel = nominalTaxiSpeed;
1114 //cout << "vel = " << vel << endl;
1115 double dist = vel * 0.514444 * dt;
1116 //cout << "dist = " << dist << endl;
1118 //cout << "track = " << track << endl;
1120 pos = dclUpdatePosition(pos, track, slope, dist);
1121 //cout << "Updated position...\n";
1122 if(aip.getSGLocation()->get_cur_elev_m() > -9990) {
1123 pos.setelev(aip.getSGLocation()->get_cur_elev_m() + wheelOffset);
1124 } // else don't change the elev until we get a valid ground elev again!
1125 if(fabs(hdg - rwy.hdg) <= 1.0) {
1126 operatingState = IN_PATTERN;
1132 } else if(taxiState == TD_OUTBOUND) {
1133 // Pause awaiting further instructions
1134 // and for now assume we've reached the hold-short node
1135 holdingShort = true;
1136 } // else at the moment assume TD_INBOUND always ends in a gate in which case we can ignore it
1138 // Time to turn (we've already checked it's not the end we're heading for).
1139 // set the target node to be the next node which will prompt automatically turning onto
1140 // the right heading in the stuff above, with the usual provisos applied.
1142 // For now why not just recursively call this function?
1148 // Warning - ground elev determination is CPU intensive
1149 // Either this function or the logic of how often it is called
1150 // will almost certainly change.
1151 void FGAILocalTraffic::DoGroundElev() {
1153 // It would be nice if we could set the correct tile center here in order to get a correct
1154 // answer with one call to the function, but what I tried in the two commented-out lines
1155 // below only intermittently worked, and I haven't quite groked why yet.
1156 //SGBucket buck(pos.lon(), pos.lat());
1157 //aip.getSGLocation()->set_tile_center(Point3D(buck.get_center_lon(), buck.get_center_lat(), 0.0));
1159 double visibility_meters = fgGetDouble("/environment/visibility-m");
1160 //globals->get_tile_mgr()->prep_ssg_nodes( acmodel_location,
1161 globals->get_tile_mgr()->prep_ssg_nodes( aip.getSGLocation(), visibility_meters );
1162 Point3D scenery_center = globals->get_scenery()->get_center();
1163 globals->get_tile_mgr()->update( aip.getSGLocation(), visibility_meters, (aip.getSGLocation())->get_absolute_view_pos( scenery_center ) );
1164 // save results of update in SGLocation for fdm...
1166 //if ( globals->get_scenery()->get_cur_elev() > -9990 ) {
1167 // acmodel_location->
1168 // set_cur_elev_m( globals->get_scenery()->get_cur_elev() );
1171 // The need for this here means that at least 2 consecutive passes are needed :-(
1172 aip.getSGLocation()->set_tile_center( globals->get_scenery()->get_next_center() );
1174 //cout << "Transform Elev is " << globals->get_scenery()->get_cur_elev() << '\n';
1175 aip.getSGLocation()->set_cur_elev_m(globals->get_scenery()->get_cur_elev());
1176 //return(globals->get_scenery()->get_cur_elev());