]> git.mxchange.org Git - flightgear.git/blob - src/ATC/AILocalTraffic.cxx
Set the initial log-level back to info
[flightgear.git] / src / ATC / AILocalTraffic.cxx
1 // FGAILocalTraffic - AIEntity derived class with enough logic to
2 // fly and interact with the traffic pattern.
3 //
4 // Written by David Luff, started March 2002.
5 //
6 // Copyright (C) 2002  David C. Luff - david.luff@nottingham.ac.uk
7 //
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.
12 //
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.
17 //
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.
21
22 #ifdef HAVE_CONFIG_H
23 #  include <config.h>
24 #endif
25
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>
34 #include <string>
35 #include <math.h>
36
37 SG_USING_STD(string);
38
39 #include "ATCmgr.hxx"
40 #include "AILocalTraffic.hxx"
41 #include "ATCutils.hxx"
42
43 FGAILocalTraffic::FGAILocalTraffic() {
44         ATC = globals->get_ATC_mgr();
45         
46         // TODO - unhardwire this - possibly let the AI manager set the callsign
47         plane.callsign = "Trainer-two-five-charlie";
48         plane.type = GA_SINGLE;
49         
50         roll = 0.0;
51         pitch = 0.0;
52         hdg = 270.0;
53         
54         //Hardwire initialisation for now - a lot of this should be read in from config eventually
55         Vr = 70.0;
56         best_rate_of_climb_speed = 70.0;
57         //best_rate_of_climb;
58         //nominal_climb_speed;
59         //nominal_climb_rate;
60         //nominal_circuit_speed;
61         //min_circuit_speed;
62         //max_circuit_speed;
63         nominal_descent_rate = 500.0;
64         nominal_final_speed = 65.0;
65         //nominal_approach_speed;
66         //stall_speed_landing_config;
67         nominalTaxiSpeed = 8.0;
68         taxiTurnRadius = 8.0;
69         wheelOffset = 1.45;     // Warning - hardwired to the C172 - we need to read this in from file.
70         elevInitGood = false;
71         // Init the property nodes
72         wind_from_hdg = fgGetNode("/environment/wind-from-heading-deg", true);
73         wind_speed_knots = fgGetNode("/environment/wind-speed-kts", true);
74         circuitsToFly = 0;
75         liningUp = false;
76         taxiRequestPending = false;
77         taxiRequestCleared = false;
78         holdingShort = false;
79         clearedToLineUp = false;
80         clearedToTakeOff = false;
81         reportReadyForDeparture = false;
82         contactTower = false;
83         contactGround = false;
84 }
85
86 FGAILocalTraffic::~FGAILocalTraffic() {
87 }
88
89
90 // Get details of the active runway
91 // It is assumed that by the time this is called the tower control and airport code will have been set up.
92 void FGAILocalTraffic::GetRwyDetails() {
93         //cout << "GetRwyDetails called" << endl;
94         
95         rwy.rwyID = tower->GetActiveRunway();
96         
97         // Now we need to get the threshold position and rwy heading
98         
99         SGPath path( globals->get_fg_root() );
100         path.append( "Airports" );
101         path.append( "runways.mk4" );
102         FGRunways runways( path.c_str() );
103
104         FGRunway runway;
105         bool rwyGood = runways.search(airportID, rwy.rwyID, &runway);
106         if(rwyGood) {
107                 // Get the threshold position
108         hdg = runway.heading;   // TODO - check - is this our heading we are setting here, and if so should we be?
109                 //cout << "hdg reset to " << hdg << '\n';
110                 double other_way = hdg - 180.0;
111                 while(other_way <= 0.0) {
112                         other_way += 360.0;
113                 }
114
115         // move to the +l end/center of the runway
116                 //cout << "Runway center is at " << runway.lon << ", " << runway.lat << '\n';
117         Point3D origin = Point3D(runway.lon, runway.lat, aptElev);
118                 Point3D ref = origin;
119         double tshlon, tshlat, tshr;
120                 double tolon, tolat, tor;
121                 rwy.length = runway.length * SG_FEET_TO_METER;
122         geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), other_way, 
123                                 rwy.length / 2.0 - 25.0, &tshlat, &tshlon, &tshr );
124         geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), hdg, 
125                                 rwy.length / 2.0 - 25.0, &tolat, &tolon, &tor );
126                 // Note - 25 meters in from the runway end is a bit of a hack to put the plane ahead of the user.
127                 // now copy what we need out of runway into rwy
128         rwy.threshold_pos = Point3D(tshlon, tshlat, aptElev);
129                 Point3D takeoff_end = Point3D(tolon, tolat, aptElev);
130                 //cout << "Threshold position = " << tshlon << ", " << tshlat << ", " << aptElev << '\n';
131                 //cout << "Takeoff position = " << tolon << ", " << tolat << ", " << aptElev << '\n';
132                 rwy.hdg = hdg;
133                 // Set the projection for the local area
134                 ortho.Init(rwy.threshold_pos, rwy.hdg); 
135                 rwy.end1ortho = ortho.ConvertToLocal(rwy.threshold_pos);        // should come out as zero
136                 rwy.end2ortho = ortho.ConvertToLocal(takeoff_end);
137         } else {
138                 SG_LOG(SG_ATC, SG_ALERT, "Help  - can't get good runway in FGAILocalTraffic!!\n");
139         }
140 }
141
142 /* 
143 There are two possible scenarios during initialisation:
144 The first is that the user is flying towards the airport, and hence the traffic
145 could be initialised anywhere, as long as the AI planes are consistent with
146 each other.
147 The second is that the user has started the sim at or close to the airport, and
148 hence the traffic must be initialised with respect to the user as well as each other.
149 To a certain extent it's FGAIMgr that has to worry about this, but we need to provide
150 sufficient initialisation functionality within the plane classes to allow the manager
151 to initialy position them where and how required.
152 */
153 bool FGAILocalTraffic::Init(string ICAO, OperatingState initialState, PatternLeg initialLeg) {
154         //cout << "FGAILocalTraffic.Init(...) called" << endl;
155         // Hack alert - Hardwired path!!
156         string planepath = "Aircraft/c172/Models/c172-dpm.ac";
157         SGPath path = globals->get_fg_root();
158         path.append(planepath);
159         aip.init(planepath.c_str());
160         aip.setVisible(false);          // This will be set to true once a valid ground elevation has been determined
161         globals->get_scenery()->get_scene_graph()->addKid(aip.getSceneGraph());
162         
163         // Find the tower frequency - this is dependent on the ATC system being initialised before the AI system
164         airportID = ICAO;
165         AirportATC a;
166         if(ATC->GetAirportATCDetails(airportID, &a)) {
167                 if(a.tower_freq) {      // Has a tower
168                         tower = (FGTower*)ATC->GetATCPointer((string)airportID, TOWER); // Maybe need some error checking here
169                         if(tower == NULL) {
170                                 // Something has gone wrong - abort or carry on with un-towered operation?
171                                 return(false);
172                         }
173                         freq = (double)tower->get_freq() / 100.0;
174                         ground = tower->GetGroundPtr();
175                         if(ground == NULL) {
176                                 // Something has gone wrong :-(
177                                 SG_LOG(SG_ATC, SG_ALERT, "ERROR - can't get a ground pointer from tower control in FGAILocalTraffic::Init() :-(");
178                                 return(false);
179                         } else if((initialState == PARKED) || (initialState == TAXIING)) {
180                                 freq = (double)ground->get_freq() / 100.0;
181                         }
182                         //cout << "AILocalTraffic freq is " << freq << '\n';
183                 } else {
184                         // Check CTAF, unicom etc
185                 }
186         } else {
187                 //cout << "Unable to find airport details in FGAILocalTraffic::Init()\n";
188         }
189
190         // Get the airport elevation
191         aptElev = dclGetAirportElev(airportID.c_str()) * SG_FEET_TO_METER;
192         //cout << "Airport elev in AILocalTraffic = " << aptElev << '\n';
193         // WARNING - we use this elev for the whole airport - some assumptions in the code 
194         // might fall down with very slopey airports.
195
196         //cout << "In Init(), initialState = " << initialState << endl;
197         operatingState = initialState;
198         switch(operatingState) {
199         case PARKED:
200                 ourGate = ground->GetGateNode();
201                 if(ourGate == NULL) {
202                         // Implies no available gates - what shall we do?
203                         // For now just vanish the plane - possibly we can make this more elegant in the future
204                         SG_LOG(SG_ATC, SG_ALERT, "No gate found by FGAILocalTraffic whilst attempting Init at " << airportID << '\n');
205                         return(false);
206                 }
207                 pitch = 0.0;
208                 roll = 0.0;
209                 vel = 0.0;
210                 slope = 0.0;
211                 pos = ourGate->pos;
212                 pos.setelev(aptElev);
213                 hdg = ourGate->heading;
214                 
215                 // Now we've set the position we can do the ground elev
216                 elevInitGood = false;
217                 inAir = false;
218                 DoGroundElev();
219                 
220                 Transform();
221                 break;
222         case TAXIING:
223                 // FIXME - implement this case properly
224                 return(false);  // remove this line when fixed!
225                 break;
226         case IN_PATTERN:
227                 // For now we'll always start the in_pattern case on the threshold ready to take-off
228                 // since we've got the implementation for this case already.
229                 // TODO - implement proper generic in_pattern startup.
230                 
231                 // Get the active runway details (and copy them into rwy)
232                 GetRwyDetails();
233
234                 // Initial position on threshold for now
235                 pos.setlat(rwy.threshold_pos.lat());
236                 pos.setlon(rwy.threshold_pos.lon());
237                 pos.setelev(rwy.threshold_pos.elev());
238                 hdg = rwy.hdg;
239                 
240                 // Now we've set the position we can do the ground elev
241                 // This might not always be necessary if we implement in-air start
242                 elevInitGood = false;
243                 inAir = false;
244                 DoGroundElev();
245                 
246                 pitch = 0.0;
247                 roll = 0.0;
248                 leg = TAKEOFF_ROLL;
249                 vel = 0.0;
250                 slope = 0.0;
251                 
252                 circuitsToFly = 0;              // ie just fly this circuit and then stop
253                 touchAndGo = false;
254                 // FIXME TODO - pattern direction is still hardwired
255                 patternDirection = -1;          // Left
256                 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
257                 if(rwy.rwyID.size() == 3) {
258                         patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
259                 }
260                 
261                 operatingState = IN_PATTERN;
262                 
263                 Transform();
264                 break;
265         default:
266                 SG_LOG(SG_ATC, SG_ALERT, "Attempt to set unknown operating state in FGAILocalTraffic.Init(...)\n");
267                 return(false);
268         }
269         
270         
271         return(true);
272 }
273
274 // Commands to do something from higher level logic
275 void FGAILocalTraffic::FlyCircuits(int numCircuits, bool tag) {
276         //cout << "FlyCircuits called" << endl;
277         
278         switch(operatingState) {
279         case IN_PATTERN:
280                 circuitsToFly += numCircuits;
281                 return;
282                 break;
283         case TAXIING:
284                 // For now we'll punt this and do nothing
285                 break;
286         case PARKED:
287                 circuitsToFly = numCircuits;    // Note that one too many circuits gets flown because we only test and decrement circuitsToFly after landing
288                                                                                 // thus flying one too many circuits.  TODO - Need to sort this out better!
289                 touchAndGo = tag;
290 #if 0   
291                 // Get the active runway details (and copy them into rwy)
292                 GetRwyDetails();
293                 
294                 // Get the takeoff node for the active runway, get a path to it and start taxiing
295                 path = ground->GetPath(ourGate, rwy.rwyID);
296                 if(path.size() < 2) {
297                         // something has gone wrong
298                         SG_LOG(SG_ATC, SG_ALERT, "Invalid path from gate to theshold in FGAILocalTraffic::FlyCircuits\n");
299                         return;
300                 }
301                 /*
302                 cout << "path returned was:" << endl;
303                 for(unsigned int i=0; i<path.size(); ++i) {
304                         switch(path[i]->struct_type) {
305                         case NODE:
306                                 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
307                                 break;
308                         case ARC:
309                                 cout << "ARC\n";
310                                 break;
311                         }
312                 }
313                 */
314                 // pop the gate - we're here already!
315                 path.erase(path.begin());
316                 //path.erase(path.begin());
317                 /*
318                 cout << "path after popping front is:" << endl;
319                 for(unsigned int i=0; i<path.size(); ++i) {
320                         switch(path[i]->struct_type) {
321                         case NODE:
322                                 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
323                                 break;
324                         case ARC:
325                                 cout << "ARC\n";
326                                 break;
327                         }
328                 }
329                 */
330                 
331                 taxiState = TD_OUTBOUND;
332                 StartTaxi();
333                 
334                 // Maybe the below should be set when we get to the threshold and prepare for TO?
335                 // FIXME TODO - pattern direction is still hardwired
336                 patternDirection = -1;          // Left
337                 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
338                 if(rwy.rwyID.size() == 3) {
339                         patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
340                 }
341
342                 Transform();
343 #endif
344                 break;
345         }
346 }   
347
348 // Run the internal calculations
349 void FGAILocalTraffic::Update(double dt) {
350         //cout << "A" << flush;
351         double responseTime = 10.0;             // seconds - this should get more sophisticated at some point
352         responseCounter += dt;
353         if((contactTower) && (responseCounter >= 8.0)) {
354                 // Acknowledge request before changing frequency so it gets rendered if the user is on the same freq
355                 string trns = "Tower ";
356                 double f = globals->get_ATC_mgr()->GetFrequency(airportID, TOWER) / 100.0;      
357                 char buf[10];
358                 sprintf(buf, "%f", f);
359                 trns += buf;
360                 trns += " ";
361                 trns += plane.callsign;
362                 Transmit(trns);
363                 responseCounter = 0.0;
364                 contactTower = false;
365                 changeFreq = true;
366                 changeFreqType = TOWER;
367         }
368         
369         if((changeFreq) && (responseCounter > 8.0)) {
370                 switch(changeFreqType) {
371                 case TOWER:
372                         freq = (double)tower->get_freq() / 100.0;
373                         //Transmit("DING!");
374                         // Contact the tower, even if only virtually
375                         changeFreq = false;
376                         tower->ContactAtHoldShort(plane, this, CIRCUIT);
377                         break;
378                 case GROUND:
379                         freq = (double)ground->get_freq() / 100.0;
380                         break;
381                 // And to avoid compiler warnings...
382                 case APPROACH:
383                         break;
384                 case ATIS:
385                         break;
386                 case ENROUTE:
387                         break;
388                 case DEPARTURE:
389                         break;
390                 case INVALID:
391                         break;
392                 }
393         }
394         
395         //cout << "." << flush;
396                 
397         switch(operatingState) {
398         case IN_PATTERN:
399                 //cout << "In IN_PATTERN\n";
400                 if(!inAir) DoGroundElev();
401                 if(!elevInitGood) {
402                         if(aip.getFGLocation()->get_cur_elev_m() > -9990.0) {
403                                 pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
404                                 //cout << "TAKEOFF_ROLL, POS = " << pos.lon() << ", " << pos.lat() << ", " << pos.elev() << '\n';
405                                 //Transform();
406                                 aip.setVisible(true);
407                                 //cout << "Making plane visible!\n";
408                                 elevInitGood = true;
409                         }
410                 }
411                 FlyTrafficPattern(dt);
412                 Transform();
413                 break;
414         case TAXIING:
415                 //cout << "In TAXIING\n";
416                 //cout << "*" << flush;
417                 if(!elevInitGood) {
418                         //DoGroundElev();
419                         if(aip.getFGLocation()->get_cur_elev_m() > -9990.0) {
420                                 pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
421                                 //Transform();
422                                 aip.setVisible(true);
423                                 //Transform();
424                                 //cout << "Making plane visible!\n";
425                                 elevInitGood = true;
426                         }
427                 }
428                 DoGroundElev();
429                 //cout << "," << flush;
430                 if(!((holdingShort) && (!clearedToLineUp))) {
431                         //cout << "|" << flush;
432                         Taxi(dt);
433                 }
434                 //cout << ";" << flush;
435                 if((clearedToTakeOff) && (responseCounter >= 8.0)) {
436                         // possible assumption that we're at the hold short here - may not always hold
437                         // TODO - sort out the case where we're cleared to line-up first and then cleared to take-off on the rwy.
438                         taxiState = TD_LINING_UP;
439                         path = ground->GetPath(holdShortNode, rwy.rwyID);
440                         /*
441                         cout << "path returned was:" << endl;
442                         for(unsigned int i=0; i<path.size(); ++i) {
443                                 switch(path[i]->struct_type) {
444                                         case NODE:
445                                         cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
446                                         break;
447                                         case ARC:
448                                         cout << "ARC\n";
449                                         break;
450                                 }
451                         }
452                         */
453                         clearedToTakeOff = false;       // We *are* still cleared - this simply stops the response recurring!!
454                         holdingShort = false;
455                         string trns = "Cleared for take-off ";
456                         trns += plane.callsign;
457                         Transmit(trns);
458                         StartTaxi();
459                 }
460                 //cout << "^" << flush;
461                 Transform();
462                 break;
463                 case PARKED:
464                 //cout << "In PARKED\n";
465                 if(!elevInitGood) {
466                         DoGroundElev();
467                         if(aip.getFGLocation()->get_cur_elev_m() > -9990.0) {
468                                 pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
469                                 //Transform();
470                                 aip.setVisible(true);
471                                 //Transform();
472                                 //cout << "Making plane visible!\n";
473                                 elevInitGood = true;
474                         }
475                 }
476                 
477                 if(circuitsToFly) {
478                         if((taxiRequestPending) && (taxiRequestCleared)) {
479                                 //cout << "&" << flush;
480                                 // Get the active runway details (and copy them into rwy)
481                                 GetRwyDetails();
482                                 
483                                 // Get the takeoff node for the active runway, get a path to it and start taxiing
484                                 path = ground->GetPathToHoldShort(ourGate, rwy.rwyID);
485                                 if(path.size() < 2) {
486                                         // something has gone wrong
487                                         SG_LOG(SG_ATC, SG_ALERT, "Invalid path from gate to theshold in FGAILocalTraffic::FlyCircuits\n");
488                                         return;
489                                 }
490                                 /*
491                                 cout << "path returned was:\n";
492                                 for(unsigned int i=0; i<path.size(); ++i) {
493                                         switch(path[i]->struct_type) {
494                                                 case NODE:
495                                                 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
496                                                 break;
497                                                 case ARC:
498                                                 cout << "ARC\n";
499                                                 break;
500                                         }
501                                 }
502                                 */
503                                 path.erase(path.begin());       // pop the gate - we're here already!
504                                 taxiState = TD_OUTBOUND;
505                                 taxiRequestPending = false;
506                                 holdShortNode = (node*)(*(path.begin() + path.size()));
507                                 StartTaxi();
508                         } else if(!taxiRequestPending) {
509                                 //cout << "(" << flush;
510                                 ground->RequestDeparture(plane, this);
511                                 // Do some communication
512                                 // airport name + tower + airplane callsign + location + request taxi for + operation type + ?
513                                 string trns = "";
514                                 trns += tower->get_name();
515                                 trns += " tower ";
516                                 trns += plane.callsign;
517                                 trns += " on apron parking request taxi for traffic pattern";
518                                 //cout << "trns = " << trns << endl;
519                                 Transmit(trns);
520                                 taxiRequestCleared = false;
521                                 taxiRequestPending = true;
522                         }
523                 }
524                 
525                 //cout << "!" << flush;
526                                 
527                 // Maybe the below should be set when we get to the threshold and prepare for TO?
528                 // FIXME TODO - pattern direction is still hardwired
529                 patternDirection = -1;          // Left
530                 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
531                 if(rwy.rwyID.size() == 3) {
532                         patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
533                 }               
534                 // Do nothing
535                 Transform();
536                 //cout << ")" << flush;
537                 break;
538         default:
539                 break;
540         }
541         //cout << "I " << flush;
542 }
543
544 void FGAILocalTraffic::RegisterTransmission(int code) {
545         switch(code) {
546         case 1: // taxi request cleared
547                 taxiRequestCleared = true;
548                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to taxi...");
549                 break;
550         case 2: // contact tower
551                 responseCounter = 0;
552                 contactTower = true;
553                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " told to contact tower...");
554                 break;
555         case 3: // Cleared to line up
556                 responseCounter = 0;
557                 clearedToLineUp = true;
558                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to line-up...");
559                 break;
560         case 4: // cleared to take-off
561                 responseCounter = 0;
562                 clearedToTakeOff = true;
563                 SG_LOG(SG_ATC, SG_INFO, "AI local traffic " << plane.callsign << " cleared to take-off...");
564                 break;
565         default:
566                 break;
567         }
568 }
569
570 // Fly a traffic pattern
571 // FIXME - far too much of the mechanics of turning, rolling, accellerating, descending etc is in here.
572 //         Move it out to FGAIPlane and have FlyTrafficPattern just specify what to do, not the implementation.
573 void FGAILocalTraffic::FlyTrafficPattern(double dt) {
574         // Need to differentiate between in-air (IAS governed) and on-ground (vel governed)
575         // Take-off is an interesting case - we are on the ground but takeoff speed is IAS governed.
576         
577         static bool transmitted = false;        // FIXME - this is a hack
578
579         // WIND
580         // Wind has two effects - a mechanical one in that IAS translates to a different vel, and the hdg != track,
581         // but also a piloting effect, in that the AI must be able to descend at a different rate in order to hit the threshold.
582         
583         //cout << "dt = " << dt << '\n';
584         double dist = 0;
585         // ack - I can't remember how long a rate 1 turn is meant to take.
586         double turn_time = 60.0;        // seconds - TODO - check this guess
587         double turn_circumference;
588         double turn_radius;
589         Point3D orthopos = ortho.ConvertToLocal(pos);   // ortho position of the plane
590         //cout << "runway elev = " << rwy.threshold_pos.elev() << ' ' << rwy.threshold_pos.elev() * SG_METER_TO_FEET << '\n';
591         //cout << "elev = " << pos.elev() << ' ' << pos.elev() * SG_METER_TO_FEET << '\n';
592
593         // HACK FOR TESTING - REMOVE
594         //cout << "Calling ExitRunway..." << endl;
595         //ExitRunway(orthopos);
596         //return;
597         // END HACK
598         
599         //wind
600         double wind_from = wind_from_hdg->getDoubleValue();
601         double wind_speed = wind_speed_knots->getDoubleValue();
602
603         switch(leg) {
604         case TAKEOFF_ROLL:
605                 //inAir = false;
606                 track = rwy.hdg;
607                 if(vel < 80.0) {
608                         double dveldt = 5.0;
609                         vel += dveldt * dt;
610                 }
611                 if(aip.getFGLocation()->get_cur_elev_m() > -9990.0) {
612                         pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
613                 }
614                 IAS = vel + (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
615                 if(IAS >= 70) {
616                         leg = CLIMBOUT;
617                         pitch = 10.0;
618                         IAS = best_rate_of_climb_speed;
619                         slope = 7.0;
620                         inAir = true;
621                 }
622                 break;
623         case CLIMBOUT:
624                 track = rwy.hdg;
625                 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 600) {
626                         cout << "Turning to crosswind, distance from threshold = " << orthopos.y() << '\n'; 
627                         leg = TURN1;
628                 }
629                 break;
630         case TURN1:
631                 track += (360.0 / turn_time) * dt * patternDirection;
632                 Bank(25.0 * patternDirection);
633                 if((track < (rwy.hdg - 89.0)) || (track > (rwy.hdg + 89.0))) {
634                         leg = CROSSWIND;
635                 }
636                 break;
637         case CROSSWIND:
638                 LevelWings();
639                 track = rwy.hdg + (90.0 * patternDirection);
640                 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
641                         slope = 0.0;
642                         pitch = 0.0;
643                         IAS = 80.0;             // FIXME - use smooth transistion to new speed
644                 }
645                 // turn 1000m out for now
646                 if(fabs(orthopos.x()) > 980) {
647                         leg = TURN2;
648                 }
649                 break;
650         case TURN2:
651                 track += (360.0 / turn_time) * dt * patternDirection;
652                 Bank(25.0 * patternDirection);
653                 // just in case we didn't make height on crosswind
654                 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
655                         slope = 0.0;
656                         pitch = 0.0;
657                         IAS = 80.0;             // FIXME - use smooth transistion to new speed
658                 }
659                 if((track < (rwy.hdg - 179.0)) || (track > (rwy.hdg + 179.0))) {
660                         leg = DOWNWIND;
661                         transmitted = false;
662                         //roll = 0.0;
663                 }
664                 break;
665         case DOWNWIND:
666                 LevelWings();
667                 track = rwy.hdg - (180 * patternDirection);     //should tend to bring track back into the 0->360 range
668                 // just in case we didn't make height on crosswind
669                 if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
670                         slope = 0.0;
671                         pitch = 0.0;
672                         IAS = 90.0;             // FIXME - use smooth transistion to new speed
673                 }
674                 if((orthopos.y() < 0) && (!transmitted)) {
675                         TransmitPatternPositionReport();
676                         transmitted = true;
677                 }
678                 if(orthopos.y() < -480) {
679                         slope = -4.0;   // FIXME - calculate to descent at 500fpm and hit the threshold (taking wind into account as well!!)
680                         pitch = -3.0;
681                         IAS = 85.0;
682                 }
683                 if(orthopos.y() < -980) {
684                         //roll = -20;
685                         leg = TURN3;
686                         transmitted = false;
687                         IAS = 80.0;
688                 }
689                 break;
690         case TURN3:
691                 track += (360.0 / turn_time) * dt * patternDirection;
692                 Bank(25.0 * patternDirection);
693                 if(fabs(rwy.hdg - track) < 91.0) {
694                         leg = BASE;
695                 }
696                 break;
697         case BASE:
698                 LevelWings();
699                 if(!transmitted) {
700                         TransmitPatternPositionReport();
701                         transmitted = true;
702                 }
703                 track = rwy.hdg - (90 * patternDirection);
704                 slope = -6.0;   // FIXME - calculate to descent at 500fpm and hit the threshold
705                 pitch = -4.0;
706                 IAS = 70.0;     // FIXME - slowdown gradually
707                 // Try and arrange to turn nicely onto base
708                 turn_circumference = IAS * 0.514444 * turn_time;        
709                 //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius
710                 //We'll leave it as a hack with IAS for now but it needs revisiting.
711                 
712                 turn_radius = turn_circumference / (2.0 * DCL_PI);
713                 if(fabs(orthopos.x()) < (turn_radius + 50)) {
714                         leg = TURN4;
715                         transmitted = false;
716                         //roll = -20;
717                 }
718                 break;
719         case TURN4:
720                 track += (360.0 / turn_time) * dt * patternDirection;
721                 Bank(25.0 * patternDirection);
722                 if(fabs(track - rwy.hdg) < 0.6) {
723                         leg = FINAL;
724                         vel = nominal_final_speed;
725                 }
726                 break;
727         case FINAL:
728                 LevelWings();
729                 if(!transmitted) {
730                         TransmitPatternPositionReport();
731                         transmitted = true;
732                 }
733                 // Try and track the extended centreline
734                 track = rwy.hdg - (0.2 * orthopos.x());
735                 //cout << "orthopos.x() = " << orthopos.x() << " hdg = " << hdg << '\n';
736                 if(pos.elev() < (rwy.threshold_pos.elev()+20.0+wheelOffset)) {
737                         DoGroundElev(); // Need to call it here expicitly on final since it's only called
738                                         // for us in update(...) when the inAir flag is false.
739                 }
740                 if(pos.elev() < (rwy.threshold_pos.elev()+10.0+wheelOffset)) {
741                         if(aip.getFGLocation()->get_cur_elev_m() > -9990.0) {
742                                 if((aip.getFGLocation()->get_cur_elev_m() + wheelOffset) > pos.elev()) {
743                                         slope = 0.0;
744                                         pitch = 0.0;
745                                         leg = LANDING_ROLL;
746                                         inAir = false;
747                                 }
748                         }       // else need a fallback position based on arpt elev in case ground elev determination fails?
749                 }
750                 break;
751         case LANDING_ROLL:
752                 //inAir = false;
753                 if(aip.getFGLocation()->get_cur_elev_m() > -9990.0) {
754                         pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
755                 }
756                 track = rwy.hdg;
757                 double dveldt = -5.0;
758                 vel += dveldt * dt;
759                 // FIXME - differentiate between touch and go and full stops
760                 if(vel <= 15.0) {
761                         //cout << "Vel <= 15.0, circuitsToFly = " << circuitsToFly << endl;
762                         if(circuitsToFly <= 0) {
763                                 //cout << "Calling ExitRunway..." << endl;
764                                 ExitRunway(orthopos);
765                                 return;
766                         } else {
767                                 //cout << "Taking off again..." << endl;
768                                 leg = TAKEOFF_ROLL;
769                                 --circuitsToFly;
770                         }
771                 }
772                 break;
773     }
774
775         if(inAir) {
776                 // FIXME - at the moment this is a bit screwy
777                 // The velocity correction is applied based on the relative headings.
778                 // Then the heading is changed based on the velocity.
779                 // Which comes first, the chicken or the egg?
780                 // Does it really matter?
781                 
782                 // Apply wind to ground-relative velocity if in the air
783                 vel = IAS - (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
784                 //crab = f(track, wind, vel);
785                 // The vector we need to fly is our desired vector minus the wind vector
786                 // TODO - we probably ought to use plib's built in vector types and operations for this
787                 // ie.  There's almost *certainly* a better way to do this!
788                 double gxx = vel * sin(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity x component wrt ground
789                 double gyy = vel * cos(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity y component wrt ground
790                 double wxx = wind_speed * sin((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS);    // Wind velocity x component
791                 double wyy = wind_speed * cos((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS);    // Wind velocity y component
792                 double axx = gxx - wxx; // Plane in-air velocity x component
793                 double ayy = gyy - wyy; // Plane in-air velocity y component
794                 // Now we want the angle between gxx and axx (which is the crab)
795                 double maga = sqrt(axx*axx + ayy*ayy);
796                 double magg = sqrt(gxx*gxx + gyy*gyy);
797                 crab = acos((axx*gxx + ayy*gyy) / (maga * magg));
798                 // At this point this works except we're getting the modulus of the angle
799                 //cout << "crab = " << crab << '\n';
800                 
801                 // Make sure both headings are in the 0->360 circle in order to get sane differences
802                 dclBoundHeading(wind_from);
803                 dclBoundHeading(track);
804                 if(track > wind_from) {
805                         if((track - wind_from) <= 180) {
806                                 crab *= -1.0;
807                         }
808                 } else {
809                         if((wind_from - track) >= 180) {
810                                 crab *= -1.0;
811                         }
812                 }
813         } else {        // on the ground - crab dosen't apply
814                 crab = 0.0;
815         }
816         
817         hdg = track + crab;
818         dist = vel * 0.514444 * dt;
819         pos = dclUpdatePosition(pos, track, slope, dist);
820 }
821
822 void FGAILocalTraffic::TransmitPatternPositionReport(void) {
823         // airport name + "traffic" + airplane callsign + pattern direction + pattern leg + rwy + ?
824         string trns = "";
825         
826         trns += tower->get_name();
827         trns += " Traffic ";
828         trns += plane.callsign;
829         if(patternDirection == 1) {
830                 trns += " right ";
831         } else {
832                 trns += " left ";
833         }
834         
835         // We could probably get rid of this whole switch statement and just pass a string containing the leg from the FlyPattern function.
836         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?
837         case TURN1:
838                 // Fall through to CROSSWIND
839         case CROSSWIND: // I don't think this case will be used here but it can't hurt to leave it in
840                 trns += "crosswind ";
841                 break;
842         case TURN2:
843                 // Fall through to DOWNWIND
844         case DOWNWIND:
845                 trns += "downwind ";
846                 break;
847         case TURN3:
848                 // Fall through to BASE
849         case BASE:
850                 trns += "base ";
851                 break;
852         case TURN4:
853                 // Fall through to FINAL
854         case FINAL:             // maybe this should include long/short final if appropriate?
855                 trns += "final ";
856                 break;
857         default:                // Hopefully this won't be used
858                 trns += "pattern ";
859                 break;
860         }
861         // FIXME - I've hardwired the runway call as well!! (We could work this out from rwy heading and mag deviation)
862         trns += ConvertRwyNumToSpokenString(1);
863         
864         // And add the airport name again
865         trns += tower->get_name();
866         
867         Transmit(trns);
868 }
869
870 void FGAILocalTraffic::ExitRunway(Point3D orthopos) {
871         //cout << "In ExitRunway" << endl;
872         //cout << "Runway ID is " << rwy.ID << endl;
873         node_array_type exitNodes = ground->GetExits(rwy.rwyID);        //I suppose we ought to have some fallback for rwy with no defined exits?
874         /*
875         cout << "Node ID's of exits are ";
876         for(unsigned int i=0; i<exitNodes.size(); ++i) {
877                 cout << exitNodes[i]->nodeID << ' ';
878         }
879         cout << endl;
880         */
881         if(exitNodes.size()) {
882                 //Find the next exit from orthopos.y
883                 double d;
884                 double dist = 100000;   //ie. longer than any runway in existance
885                 double backdist = 100000;
886                 node_array_iterator nItr = exitNodes.begin();
887                 node* rwyExit = *(exitNodes.begin());
888                 //int gateID;           //This might want to be more persistant at some point
889                 while(nItr != exitNodes.end()) {
890                         d = ortho.ConvertToLocal((*nItr)->pos).y() - ortho.ConvertToLocal(pos).y();     //FIXME - consider making orthopos a class variable
891                         if(d > 0.0) {
892                                 if(d < dist) {
893                                         dist = d;
894                                         rwyExit = *nItr;
895                                 }
896                         } else {
897                                 if(fabs(d) < backdist) {
898                                         backdist = d;
899                                         //TODO - need some logic here that if we don't get a forward exit we turn round and store the backwards one
900                                 }
901                         }
902                         ++nItr;
903                 }
904                 ourGate = ground->GetGateNode();
905                 if(ourGate == NULL) {
906                         // Implies no available gates - what shall we do?
907                         // For now just vanish the plane - possibly we can make this more elegant in the future
908                         SG_LOG(SG_ATC, SG_ALERT, "No gate found by FGAILocalTraffic whilst landing at " << airportID << '\n');
909                         aip.setVisible(false);
910                         operatingState = PARKED;
911                         return;
912                 }
913                 path = ground->GetPath(rwyExit, ourGate);
914                 /*
915                 cout << "path returned was:" << endl;
916                 for(unsigned int i=0; i<path.size(); ++i) {
917                         switch(path[i]->struct_type) {
918                         case NODE:
919                                 cout << "NODE " << ((node*)(path[i]))->nodeID << endl;
920                                 break;
921                         case ARC:
922                                 cout << "ARC\n";
923                                 break;
924                         }
925                 }
926                 */
927                 taxiState = TD_INBOUND;
928                 StartTaxi();
929         } else {
930                 // Something must have gone wrong with the ground network file - or there is only a rwy here and no exits defined
931                 SG_LOG(SG_ATC, SG_ALERT, "No exits found by FGAILocalTraffic from runway " << rwy.rwyID << " at " << airportID << '\n');
932                 // What shall we do - just remove the plane from sight?
933                 aip.setVisible(false);
934                 operatingState = PARKED;
935         }
936 }
937
938 // Set the class variable nextTaxiNode to the next node in the path
939 // and update taxiPathPos, the class variable path iterator position
940 // TODO - maybe should return error codes to the calling function if we fail here
941 void FGAILocalTraffic::GetNextTaxiNode() {
942         //cout << "GetNextTaxiNode called " << endl;
943         //cout << "taxiPathPos = " << taxiPathPos << endl;
944         ground_network_path_iterator pathItr = path.begin() + taxiPathPos;
945         if(pathItr == path.end()) {
946                 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - no more nodes in path\n");
947         } else {
948                 if((*pathItr)->struct_type == NODE) {
949                         //cout << "ITS A NODE" << endl;
950                         //*pathItr = new node;
951                         nextTaxiNode = (node*)*pathItr;
952                         ++taxiPathPos;
953                         //delete pathItr;
954                 } else {
955                         //cout << "ITS NOT A NODE" << endl;
956                         //The first item in found must have been an arc
957                         //Assume for now that it was straight
958                         pathItr++;
959                         taxiPathPos++;
960                         if(pathItr == path.end()) {
961                                 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - path ended with an arc\n");
962                         } else if((*pathItr)->struct_type == NODE) {
963                                 nextTaxiNode = (node*)*pathItr;
964                                 ++taxiPathPos;
965                         } else {
966                                 //OOPS - two non-nodes in a row - that shouldn't happen ATM
967                                 SG_LOG(SG_ATC, SG_ALERT, "ERROR IN AILocalTraffic::GetNextTaxiNode - two non-nodes in sequence\n");
968                         }
969                 }
970         }
971 }           
972
973 // StartTaxi - set up the taxiing state - call only at the start of taxiing
974 void FGAILocalTraffic::StartTaxi() {
975         //cout << "StartTaxi called" << endl;
976         operatingState = TAXIING;
977         taxiPathPos = 0;
978         
979         //Set the desired heading
980         //Assume we are aiming for first node on path
981         //Eventually we may need to consider the fact that we might start on a curved arc and
982         //not be able to head directly for the first node.
983         GetNextTaxiNode();      // sets the class variable nextTaxiNode to the next taxi node!
984         desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
985         //cout << "First taxi heading is " << desiredTaxiHeading << endl;
986 }
987
988 // speed in knots, headings in degrees, radius in meters.
989 static double TaxiTurnTowardsHeading(double current_hdg, double desired_hdg, double speed, double radius, double dt) {
990         // wrap heading - this prevents a logic bug where the plane would just go round in circles!!
991         while(current_hdg < 0.0) {
992                 current_hdg += 360.0;
993         }
994         while(current_hdg > 360.0) {
995                 current_hdg -= 360.0;
996         }
997         if(fabs(current_hdg - desired_hdg) > 0.1) {
998                 // Which is the quickest direction to turn onto heading?
999                 if(desired_hdg > current_hdg) {
1000                         if((desired_hdg - current_hdg) <= 180) {
1001                                 // turn right
1002                                 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1003                                 // TODO - check that increments are less than the delta that we check for the right direction
1004                                 // Probably need to reduce convergence speed as convergence is reached
1005                         } else {
1006                                 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;   
1007                         }
1008                 } else {
1009                         if((current_hdg - desired_hdg) <= 180) {
1010                                 // turn left
1011                                 current_hdg -= ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;
1012                                 // TODO - check that increments are less than the delta that we check for the right direction
1013                                 // Probably need to reduce convergence speed as convergence is reached
1014                         } else {
1015                                 current_hdg += ((speed * 0.514444 * dt) / (radius * DCL_PI)) * 180.0;   
1016                         }
1017                 }               
1018         }
1019         return(current_hdg);
1020 }
1021
1022 void FGAILocalTraffic::Taxi(double dt) {
1023         //cout << "Taxi called" << endl;
1024         // Logic - if we are further away from next point than turn radius then head for it
1025         // If we have reached turning point then get next point and turn onto that heading
1026         // Look out for the finish!!
1027
1028         //Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane
1029         desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
1030         
1031         bool lastNode = (taxiPathPos == path.size() ? true : false);
1032         if(lastNode) {
1033                 //cout << "LAST NODE\n";
1034         }
1035
1036         // HACK ALERT! - for now we will taxi at constant speed for straights and turns
1037         
1038         // Remember that hdg is always equal to track when taxiing so we don't have to consider them both
1039         double dist_to_go = dclGetHorizontalSeparation(pos, nextTaxiNode->pos); // we may be able to do this more cheaply using orthopos
1040         //cout << "dist_to_go = " << dist_to_go << endl;
1041         if((nextTaxiNode->type == GATE) && (dist_to_go <= 0.1)) {
1042                 // This might be more robust to outward paths starting with a gate if we check for either
1043                 // last node or TD_INBOUND ?
1044                 // park up
1045                 operatingState = PARKED;
1046         } else if(((dist_to_go > taxiTurnRadius) || (nextTaxiNode->type == GATE)) && (!liningUp)){
1047                 // if the turn radius is r, and speed is s, then in a time dt we turn through
1048                 // ((s.dt)/(PI.r)) x 180 degrees
1049                 // or alternatively (s.dt)/r radians
1050                 //cout << "hdg = " << hdg << " desired taxi heading = " << desiredTaxiHeading << '\n';
1051                 hdg = TaxiTurnTowardsHeading(hdg, desiredTaxiHeading, nominalTaxiSpeed, taxiTurnRadius, dt);
1052                 double vel = nominalTaxiSpeed;
1053                 //cout << "vel = " << vel << endl;
1054                 double dist = vel * 0.514444 * dt;
1055                 //cout << "dist = " << dist << endl;
1056                 double track = hdg;
1057                 //cout << "track = " << track << endl;
1058                 double slope = 0.0;
1059                 pos = dclUpdatePosition(pos, track, slope, dist);
1060                 //cout << "Updated position...\n";
1061                 if(aip.getFGLocation()->get_cur_elev_m() > -9990) {
1062                         pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
1063                 } // else don't change the elev until we get a valid ground elev again!
1064         } else if(lastNode) {
1065                 if(taxiState == TD_LINING_UP) {
1066                         if((!liningUp) && (dist_to_go <= taxiTurnRadius)) {
1067                                 liningUp = true;
1068                         }
1069                         if(liningUp) {
1070                                 hdg = TaxiTurnTowardsHeading(hdg, rwy.hdg, nominalTaxiSpeed, taxiTurnRadius, dt);
1071                                 double vel = nominalTaxiSpeed;
1072                                 //cout << "vel = " << vel << endl;
1073                                 double dist = vel * 0.514444 * dt;
1074                                 //cout << "dist = " << dist << endl;
1075                                 double track = hdg;
1076                                 //cout << "track = " << track << endl;
1077                                 double slope = 0.0;
1078                                 pos = dclUpdatePosition(pos, track, slope, dist);
1079                                 //cout << "Updated position...\n";
1080                                 if(aip.getFGLocation()->get_cur_elev_m() > -9990) {
1081                                         pos.setelev(aip.getFGLocation()->get_cur_elev_m() + wheelOffset);
1082                                 } // else don't change the elev until we get a valid ground elev again!
1083                                 if(fabs(hdg - rwy.hdg) <= 1.0) {
1084                                         operatingState = IN_PATTERN;
1085                                         leg = TAKEOFF_ROLL;
1086                                         inAir = false;
1087                                         liningUp = false;
1088                                 }
1089                         }
1090                 } else if(taxiState == TD_OUTBOUND) {
1091                         // Pause awaiting further instructions
1092                         // and for now assume we've reached the hold-short node
1093                         holdingShort = true;
1094                 } // else at the moment assume TD_INBOUND always ends in a gate in which case we can ignore it
1095         } else {
1096                 // Time to turn (we've already checked it's not the end we're heading for).
1097                 // set the target node to be the next node which will prompt automatically turning onto
1098                 // the right heading in the stuff above, with the usual provisos applied.
1099                 GetNextTaxiNode();
1100                 // For now why not just recursively call this function?
1101                 Taxi(dt);
1102         }
1103 }
1104
1105
1106 // Warning - ground elev determination is CPU intensive
1107 // Either this function or the logic of how often it is called
1108 // will almost certainly change.
1109 void FGAILocalTraffic::DoGroundElev() {
1110
1111         // It would be nice if we could set the correct tile center here in order to get a correct
1112         // answer with one call to the function, but what I tried in the two commented-out lines
1113         // below only intermittently worked, and I haven't quite groked why yet.
1114         //SGBucket buck(pos.lon(), pos.lat());
1115         //aip.getFGLocation()->set_tile_center(Point3D(buck.get_center_lon(), buck.get_center_lat(), 0.0));
1116         
1117         double visibility_meters = fgGetDouble("/environment/visibility-m");
1118         //globals->get_tile_mgr()->prep_ssg_nodes( acmodel_location,
1119         globals->get_tile_mgr()->prep_ssg_nodes( aip.getFGLocation(),   visibility_meters );
1120         globals->get_tile_mgr()->update( aip.getFGLocation(), visibility_meters, (aip.getFGLocation())->get_absolute_view_pos() );
1121         // save results of update in FGLocation for fdm...
1122         
1123         //if ( globals->get_scenery()->get_cur_elev() > -9990 ) {
1124         //      acmodel_location->
1125         //      set_cur_elev_m( globals->get_scenery()->get_cur_elev() );
1126         //}
1127         
1128         // The need for this here means that at least 2 consecutive passes are needed :-(
1129         aip.getFGLocation()->set_tile_center( globals->get_scenery()->get_next_center() );
1130         
1131         //cout << "Transform Elev is " << globals->get_scenery()->get_cur_elev() << '\n';
1132         aip.getFGLocation()->set_cur_elev_m(globals->get_scenery()->get_cur_elev());
1133         //return(globals->get_scenery()->get_cur_elev());
1134 }
1135