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 <Airports/runways.hxx>
27 #include <Main/globals.hxx>
28 #include <Main/location.hxx>
29 #include <Scenery/scenery.hxx>
30 #include <Scenery/tilemgr.hxx>
31 #include <simgear/math/point3d.hxx>
32 #include <simgear/math/sg_geodesy.hxx>
33 #include <simgear/misc/sg_path.hxx>
40 #include "AILocalTraffic.hxx"
41 #include "ATCutils.hxx"
43 FGAILocalTraffic::FGAILocalTraffic() {
48 //Hardwire initialisation for now - a lot of this should be read in from config eventually
50 best_rate_of_climb_speed = 70.0;
52 //nominal_climb_speed;
54 //nominal_circuit_speed;
57 nominal_descent_rate = 500.0;
58 nominal_final_speed = 65.0;
59 //nominal_approach_speed;
60 //stall_speed_landing_config;
61 nominalTaxiSpeed = 8.0;
63 wheelOffset = 1.45; // Warning - hardwired to the C172 - we need to read this in from file.
65 // Init the property nodes
66 wind_from_hdg = fgGetNode("/environment/wind-from-heading-deg", true);
67 wind_speed_knots = fgGetNode("/environment/wind-speed-kts", true);
72 FGAILocalTraffic::~FGAILocalTraffic() {
76 // Get details of the active runway
77 // This is a private internal function and it is assumed that by the
78 // time it is called the tower control and airport code will have been set up.
79 void FGAILocalTraffic::GetRwyDetails() {
80 //cout << "GetRwyDetails called" << endl;
82 // Based on the airport-id and wind get the active runway
83 SGPath path( globals->get_fg_root() );
84 path.append( "Airports" );
85 path.append( "runways.mk4" );
86 FGRunways runways( path.c_str() );
89 double hdg = wind_from_hdg->getDoubleValue();
90 double speed = wind_speed_knots->getDoubleValue();
91 hdg = (speed == 0.0 ? 270.0 : hdg);
92 //cout << "Heading = " << hdg << '\n';
95 bool rwyGood = runways.search(airportID, int(hdg), &runway);
97 // Get the threshold position
99 //cout << "hdg reset to " << hdg << '\n';
100 double other_way = hdg - 180.0;
101 while(other_way <= 0.0) {
105 // move to the +l end/center of the runway
106 //cout << "Runway center is at " << runway.lon << ", " << runway.lat << '\n';
107 Point3D origin = Point3D(runway.lon, runway.lat, aptElev);
108 Point3D ref = origin;
109 double tshlon, tshlat, tshr;
110 double tolon, tolat, tor;
111 rwy.length = runway.length * SG_FEET_TO_METER;
112 geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), other_way,
113 rwy.length / 2.0 - 25.0, &tshlat, &tshlon, &tshr );
114 geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), hdg,
115 rwy.length / 2.0 - 25.0, &tolat, &tolon, &tor );
116 // Note - 25 meters in from the runway end is a bit of a hack to put the plane ahead of the user.
117 // now copy what we need out of runway into rwy
118 rwy.threshold_pos = Point3D(tshlon, tshlat, aptElev);
119 Point3D takeoff_end = Point3D(tolon, tolat, aptElev);
120 //cout << "Threshold position = " << tshlon << ", " << tshlat << ", " << aptElev << '\n';
121 //cout << "Takeoff position = " << tolon << ", " << tolat << ", " << aptElev << '\n';
123 rwy.rwyID = runway.rwy_no;
124 // Set the projection for the local area
125 ortho.Init(rwy.threshold_pos, rwy.hdg);
126 rwy.end1ortho = ortho.ConvertToLocal(rwy.threshold_pos); // should come out as zero
127 rwy.end2ortho = ortho.ConvertToLocal(takeoff_end);
128 rwy.mag_var = 14.0; // TODO - remove this last hardwired bit!!
129 //(Although I don't think we even use the magvar any more?)
130 rwy.mag_hdg = rwy.hdg - rwy.mag_var;
131 rwy.ID = (int)rwy.mag_hdg / 10;
132 //cout << "rwy.ID = " << rwy.ID << '\n';
134 SG_LOG(SG_GENERAL, SG_ALERT, "Help - can't get good runway in FGAILocalTraffic!!\n");
139 There are two possible scenarios during initialisation:
140 The first is that the user is flying towards the airport, and hence the traffic
141 could be initialised anywhere, as long as the AI planes are consistent with
143 The second is that the user has started the sim at or close to the airport, and
144 hence the traffic must be initialised with respect to the user as well as each other.
145 To a certain extent it's FGAIMgr that has to worry about this, but we need to provide
146 sufficient initialisation functionality within the plane classes to allow the manager
147 to initialy position them where and how required.
149 bool FGAILocalTraffic::Init(string ICAO, OperatingState initialState, PatternLeg initialLeg) {
150 //cout << "FGAILocalTraffic.Init(...) called" << endl;
151 // Hack alert - Hardwired path!!
152 string planepath = "Aircraft/c172/Models/c172-dpm.ac";
153 SGPath path = globals->get_fg_root();
154 path.append(planepath);
155 aip.init(planepath.c_str());
156 aip.setVisible(false); // This will be set to true once a valid ground elevation has been determined
157 globals->get_scenery()->get_scene_graph()->addKid(aip.getSceneGraph());
159 // Find the tower frequency - this is dependent on the ATC system being initialised before the AI system
162 if(globals->get_ATC_mgr()->GetAirportATCDetails(airportID, &a)) {
163 if(a.tower_freq) { // Has a tower
164 tower = (FGTower*)globals->get_ATC_mgr()->GetATCPointer((string)airportID, TOWER); // Maybe need some error checking here
165 freq = (double)tower->get_freq() / 100.0;
166 //cout << "***********************************AILocalTraffic freq = " << freq << '\n';
168 // Check CTAF, unicom etc
171 //cout << "Unable to find airport details in FGAILocalTraffic::Init()\n";
174 // Initialise the relevant FGGround
175 // This needs a complete overhaul soon - what happens if we have 2 AI planes at same airport - they don't both need a structure
176 // This needs to be handled by the ATC manager or similar so only one set of physical data per airport is instantiated
177 // ie. TODO TODO FIXME FIXME
180 // Get the airport elevation
181 aptElev = dclGetAirportElev(airportID.c_str()) * SG_FEET_TO_METER;
182 //cout << "Airport elev in AILocalTraffic = " << aptElev << '\n';
183 // WARNING - we use this elev for the whole airport - some assumptions in the code
184 // might fall down with very slopey airports.
186 //cout << "In Init(), initialState = " << initialState << '\n';
187 operatingState = initialState;
188 switch(operatingState) {
190 ourGate = airport.GetGateNode();
191 if(ourGate == NULL) {
192 // Implies no available gates - what shall we do?
193 // For now just vanish the plane - possibly we can make this more elegant in the future
194 SG_LOG(SG_GENERAL, SG_ALERT, "No gate found by FGAILocalTraffic whilst attempting Init at " << airportID << '\n');
202 pos.setelev(aptElev);
203 hdg = ourGate->heading;
205 // Now we've set the position we can do the ground elev
206 elevInitGood = false;
213 // FIXME - implement this case properly
214 return(false); // remove this line when fixed!
217 // For now we'll always start the in_pattern case on the threshold ready to take-off
218 // since we've got the implementation for this case already.
219 // TODO - implement proper generic in_pattern startup.
221 // Get the active runway details (and copy them into rwy)
224 // Initial position on threshold for now
225 pos.setlat(rwy.threshold_pos.lat());
226 pos.setlon(rwy.threshold_pos.lon());
227 pos.setelev(rwy.threshold_pos.elev());
230 // Now we've set the position we can do the ground elev
231 // This might not always be necessary if we implement in-air start
232 elevInitGood = false;
242 circuitsToFly = 0; // ie just fly this circuit and then stop
244 // FIXME TODO - pattern direction is still hardwired
245 patternDirection = -1; // Left
246 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
247 if(rwy.rwyID.size() == 3) {
248 patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
251 operatingState = IN_PATTERN;
256 SG_LOG(SG_GENERAL, SG_ALERT, "Attempt to set unknown operating state in FGAILocalTraffic.Init(...)\n");
264 // Commands to do something from higher level logic
265 void FGAILocalTraffic::FlyCircuits(int numCircuits, bool tag) {
266 //cout << "FlyCircuits called" << endl;
268 switch(operatingState) {
270 circuitsToFly += numCircuits;
274 // For now we'll punt this and do nothing
277 circuitsToFly = numCircuits - 1; // Hack (-1) because we only test and decrement circuitsToFly after landing
278 // thus flying one too many circuits. TODO - Need to sort this out better!
281 // Get the active runway details (and copy them into rwy)
284 // Get the takeoff node for the active runway, get a path to it and start taxiing
285 path = airport.GetPath(ourGate, rwy.rwyID);
286 if(path.size() < 2) {
287 // something has gone wrong
288 SG_LOG(SG_GENERAL, SG_ALERT, "Invalid path from gate to theshold in FGAILocalTraffic::FlyCircuits\n");
292 cout << "path returned was:" << endl;
293 for(unsigned int i=0; i<path.size(); ++i) {
294 switch(path[i]->struct_type) {
296 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
304 // pop the gate - we're here already!
305 path.erase(path.begin());
306 //path.erase(path.begin());
308 cout << "path after popping front is:" << endl;
309 for(unsigned int i=0; i<path.size(); ++i) {
310 switch(path[i]->struct_type) {
312 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
321 taxiState = TD_OUTBOUND;
324 // Maybe the below should be set when we get to the threshold and prepare for TO?
325 // FIXME TODO - pattern direction is still hardwired
326 patternDirection = -1; // Left
327 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
328 if(rwy.rwyID.size() == 3) {
329 patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
337 // Run the internal calculations
338 void FGAILocalTraffic::Update(double dt) {
339 switch(operatingState) {
341 //cout << "In IN_PATTERN\n";
342 if(!inAir) DoGroundElev();
344 if(aip.getFGLocation()->get_cur_elev_m() > -9990.0) {
345 pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
346 //cout << "TAKEOFF_ROLL, POS = " << pos.lon() << ", " << pos.lat() << ", " << pos.elev() << '\n';
348 aip.setVisible(true);
349 //cout << "Making plane visible!\n";
353 FlyTrafficPattern(dt);
357 //cout << "In TAXIING\n";
360 if(aip.getFGLocation()->get_cur_elev_m() > -9990.0) {
361 pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
363 aip.setVisible(true);
365 //cout << "Making plane visible!\n";
374 //cout << "In PARKED\n";
377 if(aip.getFGLocation()->get_cur_elev_m() > -9990.0) {
378 pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
380 aip.setVisible(true);
382 //cout << "Making plane visible!\n";
394 // Fly a traffic pattern
395 // FIXME - far too much of the mechanics of turning, rolling, accellerating, descending etc is in here.
396 // Move it out to FGAIPlane and have FlyTrafficPattern just specify what to do, not the implementation.
397 void FGAILocalTraffic::FlyTrafficPattern(double dt) {
398 // Need to differentiate between in-air (IAS governed) and on-ground (vel governed)
399 // Take-off is an interesting case - we are on the ground but takeoff speed is IAS governed.
401 static bool transmitted = false; // FIXME - this is a hack
404 // Wind has two effects - a mechanical one in that IAS translates to a different vel, and the hdg != track,
405 // but also a piloting effect, in that the AI must be able to descend at a different rate in order to hit the threshold.
407 //cout << "dt = " << dt << '\n';
409 // ack - I can't remember how long a rate 1 turn is meant to take.
410 double turn_time = 60.0; // seconds - TODO - check this guess
411 double turn_circumference;
413 Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane
414 //cout << "runway elev = " << rwy.threshold_pos.elev() << ' ' << rwy.threshold_pos.elev() * SG_METER_TO_FEET << '\n';
415 //cout << "elev = " << pos.elev() << ' ' << pos.elev() * SG_METER_TO_FEET << '\n';
417 // HACK FOR TESTING - REMOVE
418 //cout << "Calling ExitRunway..." << endl;
419 //ExitRunway(orthopos);
424 double wind_from = wind_from_hdg->getDoubleValue();
425 double wind_speed = wind_speed_knots->getDoubleValue();
435 if(aip.getFGLocation()->get_cur_elev_m() > -9990.0) {
436 pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
438 IAS = vel + (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
442 IAS = best_rate_of_climb_speed;
449 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 600) {
454 track += (360.0 / turn_time) * dt * patternDirection;
455 Bank(25.0 * patternDirection);
456 if((track < (rwy.hdg - 89.0)) || (track > (rwy.hdg + 89.0))) {
462 track = rwy.hdg + (90.0 * patternDirection);
463 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
466 IAS = 80.0; // FIXME - use smooth transistion to new speed
468 // turn 1000m out for now
469 if(fabs(orthopos.x()) > 980) {
474 track += (360.0 / turn_time) * dt * patternDirection;
475 Bank(25.0 * patternDirection);
476 // just in case we didn't make height on crosswind
477 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
480 IAS = 80.0; // FIXME - use smooth transistion to new speed
482 if((track < (rwy.hdg - 179.0)) || (track > (rwy.hdg + 179.0))) {
490 track = rwy.hdg - (180 * patternDirection); //should tend to bring track back into the 0->360 range
491 // just in case we didn't make height on crosswind
492 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
495 IAS = 90.0; // FIXME - use smooth transistion to new speed
497 if((orthopos.y() < 0) && (!transmitted)) {
498 TransmitPatternPositionReport();
501 if(orthopos.y() < -480) {
502 slope = -4.0; // FIXME - calculate to descent at 500fpm and hit the threshold (taking wind into account as well!!)
506 if(orthopos.y() < -980) {
514 track += (360.0 / turn_time) * dt * patternDirection;
515 Bank(25.0 * patternDirection);
516 if(fabs(rwy.hdg - track) < 91.0) {
523 TransmitPatternPositionReport();
526 track = rwy.hdg - (90 * patternDirection);
527 slope = -6.0; // FIXME - calculate to descent at 500fpm and hit the threshold
529 IAS = 70.0; // FIXME - slowdown gradually
530 // Try and arrange to turn nicely onto base
531 turn_circumference = IAS * 0.514444 * turn_time;
532 //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius
533 //We'll leave it as a hack with IAS for now but it needs revisiting.
535 turn_radius = turn_circumference / (2.0 * DCL_PI);
536 if(fabs(orthopos.x()) < (turn_radius + 50)) {
543 track += (360.0 / turn_time) * dt * patternDirection;
544 Bank(25.0 * patternDirection);
545 if(fabs(track - rwy.hdg) < 0.6) {
547 vel = nominal_final_speed;
553 TransmitPatternPositionReport();
556 // Try and track the extended centreline
557 track = rwy.hdg - (0.2 * orthopos.x());
558 //cout << "orthopos.x() = " << orthopos.x() << " hdg = " << hdg << '\n';
559 if(pos.elev() < (rwy.threshold_pos.elev()+20.0+wheelOffset)) {
560 DoGroundElev(); // Need to call it here expicitly on final since it's only called
561 // for us in update(...) when the inAir flag is false.
563 if(pos.elev() < (rwy.threshold_pos.elev()+10.0+wheelOffset)) {
564 if(aip.getFGLocation()->get_cur_elev_m() > -9990.0) {
565 if((aip.getFGLocation()->get_cur_elev_m() + wheelOffset) > pos.elev()) {
571 } // else need a fallback position based on arpt elev in case ground elev determination fails?
576 if(aip.getFGLocation()->get_cur_elev_m() > -9990.0) {
577 pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
580 double dveldt = -5.0;
582 // FIXME - differentiate between touch and go and full stops
584 //cout << "Vel <= 15.0, circuitsToFly = " << circuitsToFly << endl;
585 if(circuitsToFly <= 0) {
586 //cout << "Calling ExitRunway..." << endl;
587 ExitRunway(orthopos);
590 //cout << "Taking off again..." << endl;
599 // FIXME - at the moment this is a bit screwy
600 // The velocity correction is applied based on the relative headings.
601 // Then the heading is changed based on the velocity.
602 // Which comes first, the chicken or the egg?
603 // Does it really matter?
605 // Apply wind to ground-relative velocity if in the air
606 vel = IAS - (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
607 //crab = f(track, wind, vel);
608 // The vector we need to fly is our desired vector minus the wind vector
609 // TODO - we probably ought to use plib's built in vector types and operations for this
610 // ie. There's almost *certainly* a better way to do this!
611 double gxx = vel * sin(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity x component wrt ground
612 double gyy = vel * cos(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity y component wrt ground
613 double wxx = wind_speed * sin((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS); // Wind velocity x component
614 double wyy = wind_speed * cos((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS); // Wind velocity y component
615 double axx = gxx - wxx; // Plane in-air velocity x component
616 double ayy = gyy - wyy; // Plane in-air velocity y component
617 // Now we want the angle between gxx and axx (which is the crab)
618 double maga = sqrt(axx*axx + ayy*ayy);
619 double magg = sqrt(gxx*gxx + gyy*gyy);
620 crab = acos((axx*gxx + ayy*gyy) / (maga * magg));
621 // At this point this works except we're getting the modulus of the angle
622 //cout << "crab = " << crab << '\n';
624 // Make sure both headings are in the 0->360 circle in order to get sane differences
625 dclBoundHeading(wind_from);
626 dclBoundHeading(track);
627 if(track > wind_from) {
628 if((track - wind_from) <= 180) {
632 if((wind_from - track) >= 180) {
636 } else { // on the ground - crab dosen't apply
641 dist = vel * 0.514444 * dt;
642 pos = dclUpdatePosition(pos, track, slope, dist);
645 void FGAILocalTraffic::TransmitPatternPositionReport(void) {
646 // airport name + "traffic" + airplane callsign + pattern direction + pattern leg + rwy + ?
649 trns += tower->get_name();
651 // FIXME - add the callsign to the class variables
652 trns += "Trainer-two-five-charlie ";
653 if(patternDirection == 1) {
659 // We could probably get rid of this whole switch statement and just pass a string containing the leg from the FlyPattern function.
660 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?
662 // Fall through to CROSSWIND
663 case CROSSWIND: // I don't think this case will be used here but it can't hurt to leave it in
664 trns += "crosswind ";
667 // Fall through to DOWNWIND
672 // Fall through to BASE
677 // Fall through to FINAL
678 case FINAL: // maybe this should include long/short final if appropriate?
681 default: // Hopefully this won't be used
685 // FIXME - I've hardwired the runway call as well!! (We could work this out from rwy heading and mag deviation)
686 trns += ConvertRwyNumToSpokenString(1);
688 // And add the airport name again
689 trns += tower->get_name();
694 void FGAILocalTraffic::ExitRunway(Point3D orthopos) {
695 //cout << "In ExitRunway" << endl;
696 //cout << "Runway ID is " << rwy.ID << endl;
697 node_array_type exitNodes = airport.GetExits(rwy.ID); //I suppose we ought to have some fallback for rwy with no defined exits?
699 cout << "Node ID's of exits are ";
700 for(unsigned int i=0; i<exitNodes.size(); ++i) {
701 cout << exitNodes[i]->nodeID << ' ';
705 if(exitNodes.size()) {
706 //Find the next exit from orthopos.y
708 double dist = 100000; //ie. longer than any runway in existance
709 double backdist = 100000;
710 node_array_iterator nItr = exitNodes.begin();
711 node* rwyExit = *(exitNodes.begin());
712 //int gateID; //This might want to be more persistant at some point
713 while(nItr != exitNodes.end()) {
714 d = ortho.ConvertToLocal((*nItr)->pos).y() - ortho.ConvertToLocal(pos).y(); //FIXME - consider making orthopos a class variable
721 if(fabs(d) < backdist) {
723 //TODO - need some logic here that if we don't get a forward exit we turn round and store the backwards one
728 ourGate = airport.GetGateNode();
729 if(ourGate == NULL) {
730 // Implies no available gates - what shall we do?
731 // For now just vanish the plane - possibly we can make this more elegant in the future
732 SG_LOG(SG_GENERAL, SG_ALERT, "No gate found by FGAILocalTraffic whilst landing at " << airportID << '\n');
733 aip.setVisible(false);
734 operatingState = PARKED;
737 path = airport.GetPath(rwyExit, ourGate);
739 cout << "path returned was:" << endl;
740 for(unsigned int i=0; i<path.size(); ++i) {
741 switch(path[i]->struct_type) {
743 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
751 taxiState = TD_INBOUND;
754 // Something must have gone wrong with the ground network file - or there is only a rwy here and no exits defined
755 SG_LOG(SG_GENERAL, SG_ALERT, "No exits found by FGAILocalTraffic from runway " << rwy.ID << " at " << airportID << '\n');
756 // What shall we do - just remove the plane from sight?
757 aip.setVisible(false);
758 operatingState = PARKED;
762 // Set the class variable nextTaxiNode to the next node in the path
763 // and update taxiPathPos, the class variable path iterator position
764 // TODO - maybe should return error codes to the calling function if we fail here
765 void FGAILocalTraffic::GetNextTaxiNode() {
766 //cout << "GetNextTaxiNode called " << endl;
767 //cout << "taxiPathPos = " << taxiPathPos << endl;
768 ground_network_path_iterator pathItr = path.begin() + taxiPathPos;
769 if(pathItr == path.end()) {
770 SG_LOG(SG_GENERAL, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - no more nodes in path\n");
772 if((*pathItr)->struct_type == NODE) {
773 //cout << "ITS A NODE" << endl;
774 //*pathItr = new node;
775 nextTaxiNode = (node*)*pathItr;
779 //cout << "ITS NOT A NODE" << endl;
780 //The first item in found must have been an arc
781 //Assume for now that it was straight
784 if(pathItr == path.end()) {
785 SG_LOG(SG_GENERAL, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - path ended with an arc\n");
786 } else if((*pathItr)->struct_type == NODE) {
787 nextTaxiNode = (node*)*pathItr;
790 //OOPS - two non-nodes in a row - that shouldn't happen ATM
791 SG_LOG(SG_GENERAL, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - two non-nodes in sequence\n");
797 // StartTaxi - set up the taxiing state - call only at the start of taxiing
798 void FGAILocalTraffic::StartTaxi() {
799 //cout << "StartTaxi called" << endl;
800 operatingState = TAXIING;
803 //Set the desired heading
804 //Assume we are aiming for first node on path
805 //Eventually we may need to consider the fact that we might start on a curved arc and
806 //not be able to head directly for the first node.
807 GetNextTaxiNode(); // sets the class variable nextTaxiNode to the next taxi node!
808 desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
809 //cout << "First taxi heading is " << desiredTaxiHeading << endl;
812 // speed in knots, headings in degrees, radius in meters.
813 static double TaxiTurnTowardsHeading(double current_hdg, double desired_hdg, double speed, double radius, double dt) {
814 // wrap heading - this prevents a logic bug where the plane would just go round in circles!!
815 while(current_hdg < 0.0) {
816 current_hdg += 360.0;
818 while(current_hdg > 360.0) {
819 current_hdg -= 360.0;
821 if(fabs(current_hdg - desired_hdg) > 0.1) {
822 // Which is the quickest direction to turn onto heading?
823 if(desired_hdg > current_hdg) {
824 if((desired_hdg - current_hdg) <= 180) {
826 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
827 // TODO - check that increments are less than the delta that we check for the right direction
828 // Probably need to reduce convergence speed as convergence is reached
830 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
833 if((current_hdg - desired_hdg) <= 180) {
835 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
836 // TODO - check that increments are less than the delta that we check for the right direction
837 // Probably need to reduce convergence speed as convergence is reached
839 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
846 void FGAILocalTraffic::Taxi(double dt) {
847 //cout << "Taxi called" << endl;
848 // Logic - if we are further away from next point than turn radius then head for it
849 // If we have reached turning point then get next point and turn onto that heading
850 // Look out for the finish!!
852 //Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane
853 desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
855 bool lastNode = (taxiPathPos == path.size() ? true : false);
857 //cout << "LAST NODE\n";
860 // HACK ALERT! - for now we will taxi at constant speed for straights and turns
862 // Remember that hdg is always equal to track when taxiing so we don't have to consider them both
863 double dist_to_go = dclGetHorizontalSeparation(pos, nextTaxiNode->pos); // we may be able to do this more cheaply using orthopos
864 //cout << "dist_to_go = " << dist_to_go << endl;
865 if((nextTaxiNode->type == GATE) && (dist_to_go <= 0.1)) {
866 // This might be more robust to outward paths starting with a gate if we check for either
867 // last node or TD_INBOUND ?
869 operatingState = PARKED;
870 } else if(((dist_to_go > taxiTurnRadius) || (nextTaxiNode->type == GATE)) && (!liningUp)){
871 // if the turn radius is r, and speed is s, then in a time dt we turn through
872 // ((s.dt)/(PI.r)) x 180 degrees
873 // or alternatively (s.dt)/r radians
874 //cout << "hdg = " << hdg << " desired taxi heading = " << desiredTaxiHeading << '\n';
875 hdg = TaxiTurnTowardsHeading(hdg, desiredTaxiHeading, nominalTaxiSpeed, taxiTurnRadius, dt);
876 double vel = nominalTaxiSpeed;
877 //cout << "vel = " << vel << endl;
878 double dist = vel * 0.514444 * dt;
879 //cout << "dist = " << dist << endl;
881 //cout << "track = " << track << endl;
883 pos = dclUpdatePosition(pos, track, slope, dist);
884 //cout << "Updated position...\n";
885 if(aip.getFGLocation()->get_cur_elev_m() > -9990) {
886 pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
887 } // else don't change the elev until we get a valid ground elev again!
888 } else if(lastNode) {
889 if(taxiState == TD_OUTBOUND) {
890 if((!liningUp) && (dist_to_go <= taxiTurnRadius)) {
894 hdg = TaxiTurnTowardsHeading(hdg, rwy.hdg, nominalTaxiSpeed, taxiTurnRadius, dt);
895 double vel = nominalTaxiSpeed;
896 //cout << "vel = " << vel << endl;
897 double dist = vel * 0.514444 * dt;
898 //cout << "dist = " << dist << endl;
900 //cout << "track = " << track << endl;
902 pos = dclUpdatePosition(pos, track, slope, dist);
903 //cout << "Updated position...\n";
904 if(aip.getFGLocation()->get_cur_elev_m() > -9990) {
905 pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
906 } // else don't change the elev until we get a valid ground elev again!
907 if(fabs(hdg - rwy.hdg) <= 1.0) {
908 operatingState = IN_PATTERN;
914 } // else at the moment assume TD_INBOUND always ends in a gate in which case we can ignore it
916 // Time to turn (we've already checked it's not the end we're heading for).
917 // set the target node to be the next node which will prompt automatically turning onto
918 // the right heading in the stuff above, with the usual provisos applied.
920 // For now why not just recursively call this function?
926 // Warning - ground elev determination is CPU intensive
927 // Either this function or the logic of how often it is called
928 // will almost certainly change.
929 void FGAILocalTraffic::DoGroundElev() {
931 // It would be nice if we could set the correct tile center here in order to get a correct
932 // answer with one call to the function, but what I tried in the two commented-out lines
933 // below only intermittently worked, and I haven't quite groked why yet.
934 //SGBucket buck(pos.lon(), pos.lat());
935 //aip.getFGLocation()->set_tile_center(Point3D(buck.get_center_lon(), buck.get_center_lat(), 0.0));
937 double visibility_meters = fgGetDouble("/environment/visibility-m");
938 //globals->get_tile_mgr()->prep_ssg_nodes( acmodel_location,
939 globals->get_tile_mgr()->prep_ssg_nodes( aip.getFGLocation(), visibility_meters );
940 globals->get_tile_mgr()->update( aip.getFGLocation(), visibility_meters, (aip.getFGLocation())->get_absolute_view_pos() );
941 // save results of update in FGLocation for fdm...
943 //if ( globals->get_scenery()->get_cur_elev() > -9990 ) {
944 // acmodel_location->
945 // set_cur_elev_m( globals->get_scenery()->get_cur_elev() );
948 // The need for this here means that at least 2 consecutive passes are needed :-(
949 aip.getFGLocation()->set_tile_center( globals->get_scenery()->get_next_center() );
951 //cout << "Transform Elev is " << globals->get_scenery()->get_cur_elev() << '\n';
952 aip.getFGLocation()->set_cur_elev_m(globals->get_scenery()->get_cur_elev());
953 //return(globals->get_scenery()->get_cur_elev());