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 <Main/globals.hxx>
27 #include <Main/location.hxx>
28 #include <Scenery/scenery.hxx>
29 #include <Scenery/tilemgr.hxx>
30 #include <simgear/math/point3d.hxx>
31 #include <simgear/math/sg_geodesy.hxx>
32 #include <simgear/misc/sg_path.hxx>
39 #include "AILocalTraffic.hxx"
40 #include "ATCutils.hxx"
42 FGAILocalTraffic::FGAILocalTraffic() {
43 //Hardwire initialisation for now - a lot of this should be read in from config eventually
45 best_rate_of_climb_speed = 70.0;
47 //nominal_climb_speed;
49 //nominal_circuit_speed;
52 nominal_descent_rate = 500.0;
53 nominal_final_speed = 65.0;
54 //nominal_approach_speed;
55 //stall_speed_landing_config;
56 nominalTaxiSpeed = 8.0;
58 wheelOffset = 1.45; // Warning - hardwired to the C172 - we need to read this in from file.
59 // Init the property nodes
60 wind_from_hdg = fgGetNode("/environment/wind-from-heading-deg", true);
61 wind_speed_knots = fgGetNode("/environment/wind-speed-kts", true);
65 FGAILocalTraffic::~FGAILocalTraffic() {
68 void FGAILocalTraffic::Init() {
69 // Hack alert - Hardwired path!!
70 string planepath = "Aircraft/c172/Models/c172-dpm.ac";
71 SGPath path = globals->get_fg_root();
72 path.append(planepath);
73 aip.init(planepath.c_str());
75 globals->get_scenery()->get_scene_graph()->addKid(aip.getSceneGraph());
76 // is it OK to leave it like this until the first time transform is called?
77 // Really ought to be started in a parking space unless otherwise specified?
79 // Find the tower frequency - this is dependent on the ATC system being initialised before the AI system
80 // FIXME - ATM this is hardwired.
83 if(globals->get_ATC_mgr()->GetAirportATCDetails((string)airportID, &a)) {
84 if(a.tower_freq) { // Has a tower
85 tower = (FGTower*)globals->get_ATC_mgr()->GetATCPointer((string)airportID, TOWER); // Maybe need some error checking here
86 freq = (double)tower->get_freq() / 100.0;
87 //cout << "***********************************AILocalTraffic freq = " << freq << '\n';
89 // Check CTAF, unicom etc
92 //cout << "Unable to find airport details in FGAILocalTraffic::Init()\n";
95 // Initiallise the FGAirportData structure
96 // This needs a complete overhaul soon - what happens if we have 2 AI planes at same airport - they don't both need a structure
97 // This needs to be handled by the ATC manager or similar so only one set of physical data per airport is instantiated
98 // ie. TODO TODO FIXME FIXME
103 // Commands to do something from higher level logic
104 void FGAILocalTraffic::FlyCircuits(int numCircuits, bool tag) {
105 circuitsToFly += numCircuits - 1; // Hack (-1) because we only test and decrement circuitsToFly after landing
106 // thus flying one to many circuits. TODO - Need to sort this out better!
109 //At the moment we'll assume that we are always finished previous circuits when called,
110 //And just teleport to the threshold to start.
111 //This is a hack though, we need to check where we are and taxi out if appropriate.
112 operatingState = IN_PATTERN;
114 // Hardwire to KEMT for now
115 // Hardwired points at each end of KEMT runway
116 Point3D P010(-118.037483, 34.081358, 296 * SG_FEET_TO_METER);
117 Point3D P190(-118.032308, 34.090456, 299.395263 * SG_FEET_TO_METER);
119 bool d010 = true; // use this to change the hardwired runway direction
121 rwy.threshold_pos = P010;
123 rwy.hdg = 25.32; //from default.apt
125 patternDirection = -1; // Left
126 pos.setelev(rwy.threshold_pos.elev() + (-8.5 * SG_FEET_TO_METER)); // This is a complete hack - the rendered runway takes the underlying scenery elev rather than the published runway elev so I should use height above terrain or something.
128 rwy.threshold_pos = P190;
132 patternDirection = 1; // Right
133 pos.setelev(rwy.threshold_pos.elev() + (-0.0 * SG_FEET_TO_METER)); // This is a complete hack - the rendered runway takes the underlying scenery elev rather than the published runway elev so I should use height above terrain or something.
136 //rwy.threshold_pos.setlat(34.081358);
137 //rwy.threshold_pos.setlon(-118.037483);
138 //rwy.mag_hdg = 12.0;
139 //rwy.mag_var = 14.0;
140 //rwy.hdg = rwy.mag_hdg + rwy.mag_var;
141 //rwy.threshold_pos.setelev(296 * SG_FEET_TO_METER);
143 // Initial position on threshold for now
144 // TODO - check wind / default runway
145 pos.setlat(rwy.threshold_pos.lat());
146 pos.setlon(rwy.threshold_pos.lon());
155 // Set the projection for the local area
156 ortho.Init(rwy.threshold_pos, rwy.hdg);
157 rwy.end1ortho = ortho.ConvertToLocal(rwy.threshold_pos); // should come out as zero
158 // Hardwire to KEMT for now
159 rwy.end2ortho = ortho.ConvertToLocal(takeoff_end);
160 //cout << "*********************************************************************************\n";
161 //cout << "*********************************************************************************\n";
162 //cout << "*********************************************************************************\n";
163 //cout << "end1ortho = " << rwy.end1ortho << '\n';
164 //cout << "end2ortho = " << rwy.end2ortho << '\n'; // end2ortho.x() should be zero or thereabouts
169 // Run the internal calculations
170 void FGAILocalTraffic::Update(double dt) {
171 switch(operatingState) {
173 FlyTrafficPattern(dt);
189 // Fly a traffic pattern
190 // FIXME - far too much of the mechanics of turning, rolling, accellerating, descending etc is in here.
191 // Move it out to FGAIPlane and have FlyTrafficPattern just specify what to do, not the implementation.
192 void FGAILocalTraffic::FlyTrafficPattern(double dt) {
193 // Need to differentiate between in-air (IAS governed) and on-ground (vel governed)
194 // Take-off is an interesting case - we are on the ground but takeoff speed is IAS governed.
195 bool inAir = true; // FIXME - possibly make into a class variable
197 static bool transmitted = false; // FIXME - this is a hack
200 // Wind has two effects - a mechanical one in that IAS translates to a different vel, and the hdg != track,
201 // but also a piloting effect, in that the AI must be able to descend at a different rate in order to hit the threshold.
203 //cout << "dt = " << dt << '\n';
205 // ack - I can't remember how long a rate 1 turn is meant to take.
206 double turn_time = 60.0; // seconds - TODO - check this guess
207 double turn_circumference;
209 Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane
210 //cout << "runway elev = " << rwy.threshold_pos.elev() << ' ' << rwy.threshold_pos.elev() * SG_METER_TO_FEET << '\n';
211 //cout << "elev = " << pos.elev() << ' ' << pos.elev() * SG_METER_TO_FEET << '\n';
213 // HACK FOR TESTING - REMOVE
214 //cout << "Calling ExitRunway..." << endl;
215 //ExitRunway(orthopos);
220 double wind_from = wind_from_hdg->getDoubleValue();
221 double wind_speed = wind_speed_knots->getDoubleValue();
231 IAS = vel + (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
235 IAS = best_rate_of_climb_speed;
241 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 600) {
246 track += (360.0 / turn_time) * dt * patternDirection;
247 Bank(25.0 * patternDirection);
248 if((track < (rwy.hdg - 89.0)) || (track > (rwy.hdg + 89.0))) {
254 track = rwy.hdg + (90.0 * patternDirection);
255 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
258 IAS = 80.0; // FIXME - use smooth transistion to new speed
260 // turn 1000m out for now
261 if(fabs(orthopos.x()) > 980) {
266 track += (360.0 / turn_time) * dt * patternDirection;
267 Bank(25.0 * patternDirection);
268 // just in case we didn't make height on crosswind
269 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
272 IAS = 80.0; // FIXME - use smooth transistion to new speed
274 if((track < (rwy.hdg - 179.0)) || (track > (rwy.hdg + 179.0))) {
282 track = rwy.hdg - (180 * patternDirection); //should tend to bring track back into the 0->360 range
283 // just in case we didn't make height on crosswind
284 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
287 IAS = 90.0; // FIXME - use smooth transistion to new speed
289 if((orthopos.y() < 0) && (!transmitted)) {
290 TransmitPatternPositionReport();
293 if(orthopos.y() < -480) {
294 slope = -4.0; // FIXME - calculate to descent at 500fpm and hit the threshold (taking wind into account as well!!)
298 if(orthopos.y() < -980) {
306 track += (360.0 / turn_time) * dt * patternDirection;
307 Bank(25.0 * patternDirection);
308 if(fabs(rwy.hdg - track) < 91.0) {
315 TransmitPatternPositionReport();
318 track = rwy.hdg - (90 * patternDirection);
319 slope = -6.0; // FIXME - calculate to descent at 500fpm and hit the threshold
321 IAS = 70.0; // FIXME - slowdown gradually
322 // Try and arrange to turn nicely onto base
323 turn_circumference = IAS * 0.514444 * turn_time;
324 //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius
325 //We'll leave it as a hack with IAS for now but it needs revisiting.
327 turn_radius = turn_circumference / (2.0 * DCL_PI);
328 if(fabs(orthopos.x()) < (turn_radius + 50)) {
335 track += (360.0 / turn_time) * dt * patternDirection;
336 Bank(25.0 * patternDirection);
337 if(fabs(track - rwy.hdg) < 0.6) {
339 vel = nominal_final_speed;
345 TransmitPatternPositionReport();
348 // Try and track the extended centreline
349 track = rwy.hdg - (0.2 * orthopos.x());
350 //cout << "orthopos.x() = " << orthopos.x() << " hdg = " << hdg << '\n';
351 if(pos.elev() <= rwy.threshold_pos.elev()) {
352 pos.setelev(rwy.threshold_pos.elev());// + (-8.5 * SG_FEET_TO_METER)); // This is a complete hack - the rendered runway takes the underlying scenery elev rather than the published runway elev so I should use height above terrain or something.
361 double dveldt = -5.0;
363 // FIXME - differentiate between touch and go and full stops
365 //cout << "Vel <= 15.0, circuitsToFly = " << circuitsToFly << endl;
366 if(circuitsToFly <= 0) {
367 //cout << "Calling ExitRunway..." << endl;
368 ExitRunway(orthopos);
371 //cout << "Taking off again..." << endl;
380 // FIXME - at the moment this is a bit screwy
381 // The velocity correction is applied based on the relative headings.
382 // Then the heading is changed based on the velocity.
383 // Which comes first, the chicken or the egg?
384 // Does it really matter?
386 // Apply wind to ground-relative velocity if in the air
387 vel = IAS - (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
388 //crab = f(track, wind, vel);
389 // The vector we need to fly is our desired vector minus the wind vector
390 // TODO - we probably ought to use plib's built in vector types and operations for this
391 // ie. There's almost *certainly* a better way to do this!
392 double gxx = vel * sin(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity x component wrt ground
393 double gyy = vel * cos(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity y component wrt ground
394 double wxx = wind_speed * sin((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS); // Wind velocity x component
395 double wyy = wind_speed * cos((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS); // Wind velocity y component
396 double axx = gxx - wxx; // Plane in-air velocity x component
397 double ayy = gyy - wyy; // Plane in-air velocity y component
398 // Now we want the angle between gxx and axx (which is the crab)
399 double maga = sqrt(axx*axx + ayy*ayy);
400 double magg = sqrt(gxx*gxx + gyy*gyy);
401 crab = acos((axx*gxx + ayy*gyy) / (maga * magg));
402 // At this point this works except we're getting the modulus of the angle
403 //cout << "crab = " << crab << '\n';
405 // Make sure both headings are in the 0->360 circle in order to get sane differences
406 dclBoundHeading(wind_from);
407 dclBoundHeading(track);
408 if(track > wind_from) {
409 if((track - wind_from) <= 180) {
413 if((wind_from - track) >= 180) {
417 } else { // on the ground - crab dosen't apply
422 dist = vel * 0.514444 * dt;
423 pos = dclUpdatePosition(pos, track, slope, dist);
426 void FGAILocalTraffic::TransmitPatternPositionReport(void) {
427 // airport name + "traffic" + airplane callsign + pattern direction + pattern leg + rwy + ?
430 trns += tower->get_name();
432 // FIXME - add the callsign to the class variables
433 trns += "Trainer-two-five-charlie ";
434 if(patternDirection == 1) {
440 // We could probably get rid of this whole switch statement and just pass a string containing the leg from the FlyPattern function.
441 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?
443 // Fall through to CROSSWIND
444 case CROSSWIND: // I don't think this case will be used here but it can't hurt to leave it in
445 trns += "crosswind ";
448 // Fall through to DOWNWIND
453 // Fall through to BASE
458 // Fall through to FINAL
459 case FINAL: // maybe this should include long/short final if appropriate?
462 default: // Hopefully this won't be used
466 // FIXME - I've hardwired the runway call as well!! (We could work this out from rwy heading and mag deviation)
467 trns += ConvertRwyNumToSpokenString(1);
469 // And add the airport name again
470 trns += tower->get_name();
475 void FGAILocalTraffic::ExitRunway(Point3D orthopos) {
476 //cout << "In ExitRunway" << endl;
477 //cout << "Runway ID is " << rwy.ID << endl;
478 node_array_type exitNodes = airport.GetExits(rwy.ID); //I suppose we ought to have some fallback for rwy with no defined exits?
480 cout << "Node ID's of exits are ";
481 for(unsigned int i=0; i<exitNodes.size(); ++i) {
482 cout << exitNodes[i]->nodeID << ' ';
486 if(exitNodes.size()) {
487 //Find the next exit from orthopos.y
489 double dist = 100000; //ie. longer than any runway in existance
490 double backdist = 100000;
491 node_array_iterator nItr = exitNodes.begin();
492 node* rwyExit = *(exitNodes.begin());
493 //int gateID; //This might want to be more persistant at some point
494 while(nItr != exitNodes.end()) {
495 d = ortho.ConvertToLocal((*nItr)->pos).y() - ortho.ConvertToLocal(pos).y(); //FIXME - consider making orthopos a class variable
502 if(fabs(d) < backdist) {
504 //TODO - need some logic here that if we don't get a forward exit we turn round and store the backwards one
509 in_dest = airport.GetGateNode();
510 if(in_dest == NULL) {
511 // Implies no available gates - what shall we do?
512 // For now just vanish the plane - possibly we can make this more elegant in the future
513 SG_LOG(SG_GENERAL, SG_ALERT, "No gate found by FGAILocalTraffic whilst landing at " << airportID << '\n');
514 aip.setVisible(false);
515 operatingState = PARKED;
518 path = airport.GetPath(rwyExit, in_dest);
520 cout << "path returned was:" << endl;
521 for(unsigned int i=0; i<path.size(); ++i) {
522 switch(path[i]->struct_type) {
524 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
532 taxiState = TD_INBOUND;
535 // Something must have gone wrong with the ground network file - or there is only a rwy here and no exits defined
536 SG_LOG(SG_GENERAL, SG_ALERT, "No exits found by FGAILocalTraffic from runway " << rwy.ID << " at " << airportID << '\n');
537 // What shall we do - just remove the plane from sight?
538 aip.setVisible(false);
539 operatingState = PARKED;
543 // Set the class variable nextTaxiNode to the next node in the path
544 // and update taxiPathPos, the class variable path iterator position
545 // TODO - maybe should return error codes to the calling function if we fail here
546 void FGAILocalTraffic::GetNextTaxiNode() {
547 //cout << "GetNextTaxiNode called " << endl;
548 //cout << "taxiPathPos = " << taxiPathPos << endl;
549 ground_network_path_iterator pathItr = path.begin() + taxiPathPos;
550 if(pathItr == path.end()) {
551 SG_LOG(SG_GENERAL, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - no more nodes in path\n");
553 if((*pathItr)->struct_type == NODE) {
554 //cout << "ITS A NODE" << endl;
555 //*pathItr = new node;
556 nextTaxiNode = (node*)*pathItr;
560 //cout << "ITS NOT A NODE" << endl;
561 //The first item in found must have been an arc
562 //Assume for now that it was straight
565 if(pathItr == path.end()) {
566 SG_LOG(SG_GENERAL, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - path ended with an arc\n");
567 } else if((*pathItr)->struct_type == NODE) {
568 nextTaxiNode = (node*)*pathItr;
571 //OOPS - two non-nodes in a row - that shouldn't happen ATM
572 SG_LOG(SG_GENERAL, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - two non-nodes in sequence\n");
578 // StartTaxi - set up the taxiing state - call only at the start of taxiing
579 void FGAILocalTraffic::StartTaxi() {
580 //cout << "StartTaxi called" << endl;
581 operatingState = TAXIING;
584 //Set the desired heading
585 //Assume we are aiming for first node on path
586 //Eventually we may need to consider the fact that we might start on a curved arc and
587 //not be able to head directly for the first node.
588 GetNextTaxiNode(); // sets the class variable nextTaxiNode to the next taxi node!
589 desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
590 //cout << "First taxi heading is " << desiredTaxiHeading << endl;
593 void FGAILocalTraffic::Taxi(double dt) {
594 //cout << "Taxi called" << endl;
595 // Logic - if we are further away from next point than turn radius then head for it
596 // If we have reached turning point then get next point and turn onto that heading
597 // Look out for the finish!!
599 //Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane
600 desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
602 // HACK ALERT! - for now we will taxi at constant speed for straights and turns
604 // Remember that hdg is always equal to track when taxiing so we don't have to consider them both
605 double dist_to_go = dclGetHorizontalSeparation(pos, nextTaxiNode->pos); // we may be able to do this more cheaply using orthopos
606 //cout << "dist_to_go = " << dist_to_go << endl;
607 if((nextTaxiNode->type == GATE) && (dist_to_go <= 0.1)) {
611 operatingState = PARKED;
612 } else if((dist_to_go > taxiTurnRadius) || (nextTaxiNode->type == GATE)) {
613 // if the turn radius is r, and speed is s, then in a time dt we turn through
614 // ((s.dt)/(PI.r)) x 180 degrees
615 // or alternatively (s.dt)/r radians
616 //cout << "hdg = " << hdg << " desired taxi heading = " << desiredTaxiHeading << '\n';
617 if(fabs(hdg - desiredTaxiHeading) > 0.1) {
618 // Which is the quickest direction to turn onto heading?
619 if(desiredTaxiHeading > hdg) {
620 if((desiredTaxiHeading - hdg) <= 180) {
622 hdg += ((nominalTaxiSpeed * 0.514444 * dt) / (taxiTurnRadius * DCL_PI)) * 180.0;
623 // TODO - check that increments are less than the delta that we check for the right direction
624 // Probably need to reduce convergence speed as convergence is reached
626 hdg -= ((nominalTaxiSpeed * 0.514444 * dt) / (taxiTurnRadius * DCL_PI)) * 180.0;
629 if((hdg - desiredTaxiHeading) <= 180) {
631 hdg -= ((nominalTaxiSpeed * 0.514444 * dt) / (taxiTurnRadius * DCL_PI)) * 180.0;
632 // TODO - check that increments are less than the delta that we check for the right direction
633 // Probably need to reduce convergence speed as convergence is reached
635 hdg += ((nominalTaxiSpeed * 0.514444 * dt) / (taxiTurnRadius * DCL_PI)) * 180.0;
639 double vel = nominalTaxiSpeed;
640 //cout << "vel = " << vel << endl;
641 double dist = vel * 0.514444 * dt;
642 //cout << "dist = " << dist << endl;
644 //cout << "track = " << track << endl;
646 pos = dclUpdatePosition(pos, track, slope, dist);
647 //cout << "Updated position...\n";
648 if(aip.getFGLocation()->get_cur_elev_m() > -9990) {
649 pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
650 } // else don't change the elev until we get a valid ground elev again!
652 // Time to turn (we've already checked it's not the end we're heading for).
653 // set the target node to be the next node which will prompt automatically turning onto
654 // the right heading in the stuff above, with the usual provisos applied.
656 // For now why not just recursively call this function?
662 // Warning - ground elev determination is CPU intensive
663 // Either this function or the logic of how often it is called
664 // will almost certainly change.
665 void FGAILocalTraffic::DoGroundElev() {
667 // It would be nice if we could set the correct tile center here in order to get a correct
668 // answer with one call to the function, but what I tried in the two commented-out lines
669 // below only intermittently worked, and I haven't quite groked why yet.
670 //SGBucket buck(pos.lon(), pos.lat());
671 //aip.getFGLocation()->set_tile_center(Point3D(buck.get_center_lon(), buck.get_center_lat(), 0.0));
673 double visibility_meters = fgGetDouble("/environment/visibility-m");
674 //globals->get_tile_mgr()->prep_ssg_nodes( acmodel_location,
675 globals->get_tile_mgr()->prep_ssg_nodes( aip.getFGLocation(), visibility_meters );
676 globals->get_tile_mgr()->update( aip.getFGLocation(), visibility_meters, (aip.getFGLocation())->get_absolute_view_pos() );
677 // save results of update in FGLocation for fdm...
679 //if ( globals->get_scenery()->get_cur_elev() > -9990 ) {
680 // acmodel_location->
681 // set_cur_elev_m( globals->get_scenery()->get_cur_elev() );
684 // The need for this here means that at least 2 consecutive passes are needed :-(
685 aip.getFGLocation()->set_tile_center( globals->get_scenery()->get_next_center() );
687 //cout << "Transform Elev is " << globals->get_scenery()->get_cur_elev() << '\n';
688 aip.getFGLocation()->set_cur_elev_m(globals->get_scenery()->get_cur_elev());
689 //return(globals->get_scenery()->get_cur_elev());