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 "AIGAVFRTraffic.hxx"
43 #include "ATCutils.hxx"
45 FGAIGAVFRTraffic::FGAIGAVFRTraffic() {
46 ATC = globals->get_ATC_mgr();
47 _towerContactedIncoming = false;
48 _clearedStraightIn = false;
49 _clearedDownwindEntry = false;
52 _downwindEntry = false;
59 _cruise_climb_ias = 90.0;
61 patternDirection = -1.0;
63 // TESTING - REMOVE OR COMMENT OUT BEFORE COMMIT!!!
64 //_towerContactPrinted = false;
67 FGAIGAVFRTraffic::~FGAIGAVFRTraffic() {
70 // We should never need to Init FGAIGAVFRTraffic in the pattern since that implies arrivel
71 // and we can just use an FGAILocalTraffic instance for that instead.
73 // Init en-route to destID at point pt.
74 // TODO - no idea what to do if pt is above planes ceiling due mountains!!
75 bool FGAIGAVFRTraffic::Init(Point3D pt, string destID, const string& callsign) {
76 FGAILocalTraffic::Init(callsign, destID, EN_ROUTE);
77 // TODO FIXME - to get up and running we're going to ignore elev and get FGAIMgr to
78 // pass in known good values for the test location. Need to fix this!!! (or at least canonically decide who has responsibility for setting elev).
82 _destPos = dclGetAirportPos(destID); // TODO - check if we are within the tower catchment area already.
83 _cruise_alt = (_destPos.elev() + 2500.0) * SG_FEET_TO_METER; // TODO look at terrain elevation as well
84 _pos.setelev(_cruise_alt);
85 // initially set waypoint as airport location
87 // Set the initial track
88 track = GetHeadingFromTo(_pos, _wp);
89 // And set the plane to keep following it.
90 SetTrack(GetHeadingFromTo(_pos, _wp));
94 // TODO - set climbout if altitude is below normal cruising altitude?
96 // Assume it's OK to set the plane visible
97 _aip.setVisible(true);
98 //cout << "Setting visible true\n";
103 // Init at srcID to fly to destID
104 bool FGAIGAVFRTraffic::Init(string srcID, string destID, const string& callsign, OperatingState state) {
106 FGAILocalTraffic::Init(callsign, srcID, PARKED);
110 void FGAIGAVFRTraffic::Update(double dt) {
112 //cout << "_enroute\n";
113 //cout << "e" << flush;
115 //cout << "f" << flush;
117 //cout << "g" << flush;
118 FGAIPlane::Update(dt);
119 //cout << "h" << flush;
120 responseCounter += dt;
122 // we shouldn't really need this since there's a LOD of 10K on the whole plane anyway I think.
123 // There are two _aip.setVisible statements set when _local = true that can be removed if the below is removed.
124 if(dclGetHorizontalSeparation(_pos, Point3D(fgGetDouble("/position/longitude-deg"), fgGetDouble("/position/latitude-deg"), 0.0)) > 8000) _aip.setVisible(false);
125 else _aip.setVisible(true);
129 //cout << "_local\n";
130 FGAILocalTraffic::Update(dt);
134 void FGAIGAVFRTraffic::FlyPlane(double dt) {
136 // Check whether to level off
137 if(_pos.elev() >= _cruise_alt) {
140 IAS = _cruise_ias; // FIXME - use smooth transistion to new speed and attitude.
145 IAS = _cruise_climb_ias;
150 if(dclGetHorizontalSeparation(_destPos, _pos) / 1600.0 < 8.1) {
151 if(!_towerContactPrinted) {
152 if(airportID == "KSQL") {
153 cout << "****************************************************************\n";
154 cout << "++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n";
155 cout << "****************************************************************\n";
157 _towerContactPrinted = true;
162 // if distance to destination is less than 6 - 9 miles contact tower
163 // and prepare to become _incoming after response.
164 // Possibly check whether to start descent before this?
165 //cout << "." << flush;
166 //cout << "sep = " << dclGetHorizontalSeparation(_destPos, _pos) / 1600.0 << '\n';
167 if(dclGetHorizontalSeparation(_destPos, _pos) / 1600.0 < 8.0) {
168 //cout << "-" << flush;
169 if(!_towerContactedIncoming) {
170 //cout << "_" << flush;
171 GetAirportDetails(airportID);
172 //cout << "L" << flush;
174 freq = (double)tower->get_freq() / 100.0;
175 tuned_station = tower;
177 freq = 122.8; // TODO - need to get the correct CTAF/Unicom frequency if no tower
178 tuned_station = NULL;
180 //cout << "freq = " << freq << endl;
181 GetRwyDetails(airportID);
182 //"@AP Tower @CS @MI miles @CD of the airport for full stop with the ATIS"
183 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
184 if(rwy.rwyID.size() == 3) {
185 patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
188 pending_transmission = tower->get_name();
189 pending_transmission += " Tower ";
191 pending_transmission = "Traffic ";
192 // TODO - find some way of getting uncontrolled airport name
194 pending_transmission += plane.callsign;
196 int dist_miles = (int)dclGetHorizontalSeparation(_pos, _destPos) / 1600;
197 //sprintf(buf, " %i ", dist_miles);
198 pending_transmission += " ";
199 pending_transmission += ConvertNumToSpokenDigits(dist_miles);
200 if(dist_miles > 1) pending_transmission += " miles ";
201 else pending_transmission += " mile ";
202 pending_transmission += GetCompassDirection(GetHeadingFromTo(_destPos, _pos));
203 pending_transmission += " of the airport for full stop with the ATIS";
204 //cout << pending_transmission << endl;
205 Transmit(14); // 14 is the callback code, NOT the timeout!
207 _towerContactedIncoming = true;
209 //cout << "?" << flush;
210 if(_clearedStraightIn && responseCounter > 5.5) {
211 //cout << "5 " << flush;
212 _clearedStraightIn = false;
215 _wp = GetPatternApproachPos();
216 //_hdg = GetHeadingFromTo(_pos, _wp); // TODO - turn properly!
217 SetTrack(GetHeadingFromTo(_pos, _wp));
218 slope = atan((_wp.elev() - _pos.elev()) / dclGetHorizontalSeparation(_wp, _pos)) * DCL_RADIANS_TO_DEGREES;
219 double thesh_offset = 0.0;
220 Point3D opos = ortho.ConvertToLocal(_pos);
221 double angToApt = atan((_pos.elev() - dclGetAirportElev(airportID)) / (opos.y() - thesh_offset)) * DCL_RADIANS_TO_DEGREES;
222 //cout << "angToApt = " << angToApt << ' ';
223 slope = (angToApt > -5.0 ? 0.0 : angToApt);
224 //cout << "slope = " << slope << '\n';
225 pending_transmission = "Straight-in ";
226 pending_transmission += ConvertRwyNumToSpokenString(rwy.rwyID);
227 pending_transmission += " ";
228 pending_transmission += plane.callsign;
229 //cout << pending_transmission << '\n';
230 ConditionalTransmit(4);
231 } else if(_clearedDownwindEntry && responseCounter > 5.5) {
232 //cout << "6" << flush;
233 _clearedDownwindEntry = false;
234 _downwindEntry = true;
236 _wp = GetPatternApproachPos();
237 SetTrack(GetHeadingFromTo(_pos, _wp));
238 slope = atan((_wp.elev() - _pos.elev()) / dclGetHorizontalSeparation(_wp, _pos)) * DCL_RADIANS_TO_DEGREES;
239 //cout << "slope = " << slope << '\n';
240 pending_transmission = "Report ";
241 pending_transmission += (patternDirection == 1 ? "right downwind " : "left downwind ");
242 pending_transmission += ConvertRwyNumToSpokenString(rwy.rwyID);
243 pending_transmission += " ";
244 pending_transmission += plane.callsign;
245 //cout << pending_transmission << '\n';
246 ConditionalTransmit(4);
249 if(_pos.elev() < (dclGetAirportElev(airportID) + (1000.0 * SG_FEET_TO_METER))) slope = 0.0;
253 //cout << "i" << '\n';
254 Point3D orthopos = ortho.ConvertToLocal(_pos);
255 // TODO - Check whether to start descent
256 // become _local after the 3 mile report.
257 if(_pos.elev() < (dclGetAirportElev(airportID) + (1000.0 * SG_FEET_TO_METER))) slope = 0.0;
258 // TODO - work out why I needed to add the above line to stop the plane going underground!!!
259 // (Although it's worth leaving it in as a robustness check anyway).
261 //cout << "A " << flush;
262 if(fabs(orthopos.x()) < 10.0 && !_established) {
265 //cout << "Established at " << orthopos << '\n';
267 double thesh_offset = 30.0;
268 //cout << "orthopos.y = " << orthopos.y() << " alt = " << _pos.elev() - dclGetAirportElev(airportID) << '\n';
269 if(_established && (orthopos.y() > -5400.0)) {
270 slope = atan((_pos.elev() - dclGetAirportElev(airportID)) / (orthopos.y() - thesh_offset)) * DCL_RADIANS_TO_DEGREES;
271 //cout << "slope0 = " << slope << '\n';
273 //cout << "slope1 = " << slope << '\n';
274 if(slope > -5.5) slope = 0.0; // ie we're too low.
275 //cout << "slope2 = " << slope << '\n';
276 slope += 0.001; // To avoid yo-yoing with the above.
277 //if(_established && (orthopos.y() > -5400.0)) slope = -5.5;
278 if(_established && (orthopos.y() > -4800.0)) {
279 pending_transmission = "3 mile final Runway ";
280 pending_transmission += ConvertRwyNumToSpokenString(rwy.rwyID);
281 pending_transmission += " ";
282 pending_transmission += plane.callsign;
283 //cout << pending_transmission << '\n';
284 ConditionalTransmit(35);
286 _aip.setVisible(true); // HACK
288 StraightInEntry(true);
290 } else if(_downwindEntry) {
291 //cout << "B" << flush;
293 //cout << "C" << flush;
295 if(fabs(_hdg - (rwy.hdg + 180)) < 2.0) { // TODO - use track instead of _hdg?
296 //cout << "Going Local...\n";
299 _aip.setVisible(true); // HACK
306 if(fabs(orthopos.x() - (patternDirection == 1 ? 1000 : -1000)) < (_e45 ? 175 : 550)) { // Caution - hardwired turn clearances.
307 //cout << "_turning...\n";
309 SetTrack(rwy.hdg + 180.0);
310 } // TODO - need to check for other traffic in the pattern and enter much more integilently than that!!!
312 //cout << "D" << flush;
313 //cout << '\n' << dclGetHorizontalSeparation(_wp, _pos) << '\n';
314 //cout << ortho.ConvertToLocal(_pos);
315 //cout << ortho.ConvertToLocal(_wp);
316 if(dclGetHorizontalSeparation(_wp, _pos) < 100.0) {
317 pending_transmission = "2 miles out for ";
318 pending_transmission += (patternDirection == 1 ? "right " : "left ");
319 pending_transmission += "downwind Runway ";
320 pending_transmission += ConvertRwyNumToSpokenString(rwy.rwyID);
321 pending_transmission += " ";
322 pending_transmission += plane.callsign;
323 //cout << pending_transmission << '\n';
324 // TODO - are we at pattern altitude??
326 ConditionalTransmit(30);
328 SetTrack(patternDirection == 1 ? rwy.hdg - 135.0 : rwy.hdg + 135.0);
330 SetTrack(patternDirection == 1 ? rwy.hdg + 90.0 : rwy.hdg - 90.0);
332 //if(_hdg < 0.0) _hdg += 360.0;
335 SetTrack(GetHeadingFromTo(_pos, _wp));
343 // FIXME - lots of hackery in the next six lines!!!!
344 //double track = _hdg;
345 double crab = 0.0; // This is a placeholder for when we take wind into account.
347 double vel = _cruise_ias;
348 double dist = vel * 0.514444 * dt;
349 _pos = dclUpdatePosition(_pos, track, slope, dist);
352 void FGAIGAVFRTraffic::RegisterTransmission(int code) {
354 case 1: // taxi request cleared
355 FGAILocalTraffic::RegisterTransmission(code);
357 case 2: // contact tower
358 FGAILocalTraffic::RegisterTransmission(code);
360 case 3: // Cleared to line up
361 FGAILocalTraffic::RegisterTransmission(code);
363 case 4: // cleared to take-off
364 FGAILocalTraffic::RegisterTransmission(code);
366 case 5: // contact ground
367 FGAILocalTraffic::RegisterTransmission(code);
369 case 6: // taxi to the GA parking
370 FGAILocalTraffic::RegisterTransmission(code);
372 case 7: // Cleared to land
373 FGAILocalTraffic::RegisterTransmission(code);
375 case 13: // Go around!
376 FGAILocalTraffic::RegisterTransmission(code);
378 case 14: // VFR approach for straight-in
380 _clearedStraightIn = true;
382 case 15: // VFR approach for downwind entry
384 _clearedDownwindEntry = true;
387 SG_LOG(SG_ATC, SG_WARN, "FGAIGAVFRTraffic::RegisterTransmission(...) called with unknown code " << code);
388 FGAILocalTraffic::RegisterTransmission(code);
394 // TODO - Really should enumerate these coded values.
395 void FGAIGAVFRTraffic::ProcessCallback(int code) {
396 // 1 - Request Departure from ground
397 // 2 - Report at hold short
398 // 10 - report crosswind
399 // 11 - report downwind
402 // 14 - Contact Tower for VFR arrival
405 FGAILocalTraffic::ProcessCallback(code);
406 } else if(code == 14) {
408 tower->VFRArrivalContact(plane, this, FULL_STOP);
410 // TODO else possibly announce arrival intentions at uncontrolled airport?
411 } else if(code == 99) {
412 // Might handle this different in future - hence separated from the other codes to pass to AILocalTraffic.
413 FGAILocalTraffic::ProcessCallback(code);
417 // Return an appropriate altitude to fly at based on the desired altitude and direction
418 // whilst respecting the quadrangle rule.
419 int FGAIGAVFRTraffic::GetQuadrangleAltitude(int dir, int des_alt) {
421 // TODO - implement me!
424 // Calculates the position needed to set up for either pattern entry or straight in approach.
425 // Currently returns one of three positions dependent on initial position wrt threshold of active rwy.
426 // 1/ A few miles out on extended centreline for straight-in.
427 // 2/ At an appropriate point on circuit side of rwy for a 45deg entry to downwind.
428 // 3/ At and appropriate point on non-circuit side of rwy at take-off end for perpendicular entry to circuit overflying end-of-rwy.
429 Point3D FGAIGAVFRTraffic::GetPatternApproachPos() {
431 //cout << "Calculating pattern approach pos for " << plane.callsign << '\n';
432 Point3D orthopos = ortho.ConvertToLocal(_pos);
434 //cout << "patternDirection = " << patternDirection << '\n';
435 if(orthopos.y() >= -1000.0) { // Note that this has to be set the same as the calculation in tower.cxx - at the moment approach type is not transmitted properly between the two.
436 //cout << "orthopos.x = " << orthopos.x() << '\n';
437 if((orthopos.x() * patternDirection) > 0.0) { // 45 deg entry
438 tmp.setx(2000 * patternDirection);
439 tmp.sety((rwy.end2ortho.y() / 2.0) + 2000);
440 tmp.setelev(dclGetAirportElev(airportID) + (1000 * SG_FEET_TO_METER));
442 //cout << "45 deg entry... ";
444 tmp.setx(1000 * patternDirection * -1);
445 tmp.sety(rwy.end2ortho.y());
446 tmp.setelev(dclGetAirportElev(airportID) + (1000 * SG_FEET_TO_METER));
448 //cout << "90 deg entry... ";
453 tmp.setelev((5400.0 / 6.0) + dclGetAirportElev(airportID) + 10.0);
454 //cout << "Straight in... ";
456 //cout << "Waypoint is " << tmp << '\n';
457 //cout << ortho.ConvertFromLocal(tmp) << '\n';
460 return ortho.ConvertFromLocal(tmp);