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.
22 #include <Main/globals.hxx>
23 #include <Main/location.hxx>
24 #include <Scenery/scenery.hxx>
25 #include <simgear/math/point3d.hxx>
26 #include <simgear/math/sg_geodesy.hxx>
27 #include <simgear/misc/sg_path.hxx>
34 #include "AILocalTraffic.hxx"
35 #include "ATCutils.hxx"
37 FGAILocalTraffic::FGAILocalTraffic() {
38 //Hardwire initialisation for now - a lot of this should be read in from config eventually
40 best_rate_of_climb_speed = 70.0;
42 //nominal_climb_speed;
44 //nominal_circuit_speed;
47 nominal_descent_rate = 500.0;
48 nominal_final_speed = 65.0;
49 //nominal_approach_speed;
50 //stall_speed_landing_config;
52 wind_speed_knots = 0.0;
55 FGAILocalTraffic::~FGAILocalTraffic() {
58 void FGAILocalTraffic::Init() {
59 // Hack alert - Hardwired path!!
60 string planepath = "Aircraft/c172/Models/c172-dpm.ac";
61 SGPath path = globals->get_fg_root();
62 path.append(planepath);
63 aip.init(planepath.c_str());
65 globals->get_scenery()->get_scene_graph()->addKid(aip.getSceneGraph());
68 //#define DCL_KPAO true
70 // Hardwire to KEMT for now
71 // Hardwired points at each end of KEMT runway
72 Point3D P010(-118.037483, 34.081358, 296 * SG_FEET_TO_METER);
73 Point3D P190(-118.032308, 34.090456, 299.395263 * SG_FEET_TO_METER);
75 bool d010 = true; // use this to change the hardwired runway direction
77 rwy.threshold_pos = P010;
79 rwy.hdg = 25.32; //from default.apt
80 patternDirection = -1; // Left
81 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.
83 rwy.threshold_pos = P190;
86 patternDirection = 1; // Right
87 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.
90 //KPAO - might be a better choice since its in the default scenery
91 //Hardwire it to the default (no wind) direction
92 Point3D threshold_end(-122.1124358, 37.45848783, 6.8 * SG_FEET_TO_METER); // These positions are from airnav.com and don't quite seem to correspond with the sim scenery
93 Point3D takeoff_end(-122.1176522, 37.463752, 6.7 * SG_FEET_TO_METER);
94 rwy.threshold_pos = threshold_end;
96 patternDirection = 1; // Right
97 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.
100 //rwy.threshold_pos.setlat(34.081358);
101 //rwy.threshold_pos.setlon(-118.037483);
102 //rwy.mag_hdg = 12.0;
103 //rwy.mag_var = 14.0;
104 //rwy.hdg = rwy.mag_hdg + rwy.mag_var;
105 //rwy.threshold_pos.setelev(296 * SG_FEET_TO_METER);
107 // Initial position on threshold for now
108 // TODO - check wind / default runway
109 pos.setlat(rwy.threshold_pos.lat());
110 pos.setlon(rwy.threshold_pos.lon());
119 // Now set the position of the plane and then re-get the elevation!! (Didn't work - elev always returned as zero) :-(
120 //aip.setPosition(pos.lon(), pos.lat(), pos.elev() * SG_METER_TO_FEET);
121 //cout << "*********************** elev in FGAILocalTraffic = " << aip.getFGLocation()->get_cur_elev_m() << '\n';
123 // Activate the tower - this is dependent on the ATC system being initialised before the AI system
125 if(globals->get_ATC_mgr()->GetAirportATCDetails((string)"KEMT", &a)) {
126 if(a.tower_freq) { // Has a tower
127 tower = (FGTower*)globals->get_ATC_mgr()->GetATCPointer((string)"KEMT", TOWER); // Maybe need some error checking here
128 freq = (double)tower->get_freq() / 100.0;
129 //cout << "***********************************AILocalTraffic freq = " << freq << '\n';
131 // Check CTAF, unicom etc
134 cout << "Unable to find airport details in FGAILocalTraffic::Init()\n";
137 // Set the projection for the local area
138 ortho.Init(rwy.threshold_pos, rwy.hdg);
139 rwy.end1ortho = ortho.ConvertToLocal(rwy.threshold_pos); // should come out as zero
140 // Hardwire to KEMT for now
141 rwy.end2ortho = ortho.ConvertToLocal(takeoff_end);
142 //cout << "*********************************************************************************\n";
143 //cout << "*********************************************************************************\n";
144 //cout << "*********************************************************************************\n";
145 //cout << "end1ortho = " << rwy.end1ortho << '\n';
146 //cout << "end2ortho = " << rwy.end2ortho << '\n'; // end2ortho.x() should be zero or thereabouts
151 // Run the internal calculations
152 void FGAILocalTraffic::Update(double dt) {
153 std::cout << "In FGAILocalTraffic::Update\n";
154 // Hardwire flying traffic pattern for now - eventually also needs to be able to taxi to and from runway and GA parking area.
155 FlyTrafficPattern(dt);
157 //cout << "elev in FGAILocalTraffic = " << aip.getFGLocation()->get_cur_elev_m() << '\n';
158 // This should become if(the plane has moved) then Transform()
161 // Fly a traffic pattern
162 // FIXME - far too much of the mechanics of turning, rolling, accellerating, descending etc is in here.
163 // Move it out to FGAIPlane and have FlyTrafficPattern just specify what to do, not the implementation.
164 void FGAILocalTraffic::FlyTrafficPattern(double dt) {
165 // Need to differentiate between in-air (IAS governed) and on-ground (vel governed)
166 // Take-off is an interesting case - we are on the ground but takeoff speed is IAS governed.
167 bool inAir = true; // FIXME - possibly make into a class variable
169 static bool transmitted = false; // FIXME - this is a hack
172 // Wind has two effects - a mechanical one in that IAS translates to a different vel, and the hdg != track,
173 // but also a piloting effect, in that the AI must be able to descend at a different rate in order to hit the threshold.
175 //cout << "dt = " << dt << '\n';
177 // ack - I can't remember how long a rate 1 turn is meant to take.
178 double turn_time = 60.0; // seconds - TODO - check this guess
179 double turn_circumference;
181 Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane
182 //cout << "runway elev = " << rwy.threshold_pos.elev() << ' ' << rwy.threshold_pos.elev() * SG_METER_TO_FEET << '\n';
183 //cout << "elev = " << pos.elev() << ' ' << pos.elev() * SG_METER_TO_FEET << '\n';
192 IAS = vel + (cos((hdg - wind_from_hdg) * DCL_DEGREES_TO_RADIANS) * wind_speed_knots);
196 IAS = best_rate_of_climb_speed;
202 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 600) {
207 track += (360.0 / turn_time) * dt * patternDirection;
208 Bank(25.0 * patternDirection);
209 if((track < (rwy.hdg - 89.0)) || (track > (rwy.hdg + 89.0))) {
215 track = rwy.hdg + (90.0 * patternDirection);
216 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
219 IAS = 80.0; // FIXME - use smooth transistion to new speed
221 // turn 1000m out for now
222 if(fabs(orthopos.x()) > 980) {
227 track += (360.0 / turn_time) * dt * patternDirection;
228 Bank(25.0 * patternDirection);
229 // just in case we didn't make height on crosswind
230 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
233 IAS = 80.0; // FIXME - use smooth transistion to new speed
235 if((track < (rwy.hdg - 179.0)) || (track > (rwy.hdg + 179.0))) {
243 track = rwy.hdg - (180 * patternDirection); //should tend to bring track back into the 0->360 range
244 // just in case we didn't make height on crosswind
245 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
248 IAS = 90.0; // FIXME - use smooth transistion to new speed
250 if((orthopos.y() < 0) && (!transmitted)) {
251 TransmitPatternPositionReport();
254 if(orthopos.y() < -480) {
255 slope = -4.0; // FIXME - calculate to descent at 500fpm and hit the threshold (taking wind into account as well!!)
259 if(orthopos.y() < -980) {
267 track += (360.0 / turn_time) * dt * patternDirection;
268 Bank(25.0 * patternDirection);
269 if(fabs(rwy.hdg - track) < 91.0) {
276 TransmitPatternPositionReport();
279 track = rwy.hdg - (90 * patternDirection);
280 slope = -6.0; // FIXME - calculate to descent at 500fpm and hit the threshold
282 IAS = 70.0; // FIXME - slowdown gradually
283 // Try and arrange to turn nicely onto base
284 turn_circumference = IAS * 0.514444 * turn_time;
285 //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius
286 //We'll leave it as a hack with IAS for now but it needs revisiting.
288 turn_radius = turn_circumference / (2.0 * DCL_PI);
289 if(fabs(orthopos.x()) < (turn_radius + 50)) {
296 track += (360.0 / turn_time) * dt * patternDirection;
297 Bank(25.0 * patternDirection);
298 if(fabs(track - rwy.hdg) < 0.6) {
300 vel = nominal_final_speed;
306 TransmitPatternPositionReport();
309 // Try and track the extended centreline
310 track = rwy.hdg - (0.2 * orthopos.x());
311 //cout << "orthopos.x() = " << orthopos.x() << " hdg = " << hdg << '\n';
312 if(pos.elev() <= rwy.threshold_pos.elev()) {
313 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.
322 double dveldt = -5.0;
330 yaw = 0.0; //yaw = f(track, wind);
332 // Apply wind to ground-relative velocity if in the air
334 vel = IAS - (cos((hdg - wind_from_hdg) * DCL_DEGREES_TO_RADIANS) * wind_speed_knots);
336 dist = vel * 0.514444 * dt;
337 pos = dclUpdatePosition(pos, track, slope, dist);
340 void FGAILocalTraffic::TransmitPatternPositionReport(void) {
341 // airport name + "traffic" + airplane callsign + pattern direction + pattern leg + rwy + ?
344 trns += tower->get_name();
346 // FIXME - add the callsign to the class variables
347 trns += "Trainer-two-five-charlie ";
348 if(patternDirection == 1) {
354 // We could probably get rid of this whole switch statement and just pass a string containing the leg from the FlyPattern function.
355 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?
357 // Fall through to CROSSWIND
358 case CROSSWIND: // I don't think this case will be used here but it can't hurt to leave it in
359 trns += "crosswind ";
362 // Fall through to DOWNWIND
367 // Fall through to BASE
372 // Fall through to FINAL
373 case FINAL: // maybe this should include long/short final if appropriate?
376 default: // Hopefully this won't be used
380 // FIXME - I've hardwired the runway call as well!! (We could work this out from rwy heading and mag deviation)
381 trns += convertNumToSpokenString(1);
383 // And add the airport name again
384 trns += tower->get_name();