]> git.mxchange.org Git - flightgear.git/blob - src/ATC/approach.cxx
ignore resets for now because every z/Z key press would trigger a call to NOAA. We...
[flightgear.git] / src / ATC / approach.cxx
1 // FGApproach - a class to provide approach control at larger airports.
2 //
3 // Written by Alexander Kappes, started March 2002.
4 //
5 // Copyright (C) 2002  Alexander Kappes
6 //
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
11 //
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
15 // General Public License for more details.
16 //
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
20
21 #include "approach.hxx"
22 #include "transmission.hxx"
23 #include "transmissionlist.hxx"
24 #include "ATCdisplay.hxx"
25 #include "ATCDialog.hxx"
26
27 #include <Airports/runways.hxx>
28 #include <simgear/math/polar3d.hxx>
29 #include <simgear/misc/sg_path.hxx>
30
31 #ifdef FG_WEATHERCM
32 # include <WeatherCM/FGLocalWeatherDatabase.h>
33 #else
34 # include <Environment/environment_mgr.hxx>
35 # include <Environment/environment.hxx>
36 #endif
37
38
39 #include <GUI/gui.h>
40
41 //Constructor
42 FGApproach::FGApproach(){
43   comm1_node = fgGetNode("/radios/comm[0]/frequencies/selected-mhz", true);
44   comm2_node = fgGetNode("/radios/comm[1]/frequencies/selected-mhz", true);
45   
46   _type = APPROACH;
47
48   num_planes = 0;
49   lon_node   = fgGetNode("/position/longitude-deg", true);
50   lat_node   = fgGetNode("/position/latitude-deg", true);
51   elev_node  = fgGetNode("/position/altitude-ft", true);
52   hdg_node   = fgGetNode("/orientation/heading-deg", true);
53   speed_node = fgGetNode("/velocities/airspeed-kt", true);
54   etime_node = fgGetNode("/sim/time/elapsed-ms", true);
55
56   first = true;
57   active_runway = "";
58   int i;
59   for ( i=0; i<max_planes; i++) {
60     planes[i].contact   = 0;
61     planes[i].wpn       = 0;
62     planes[i].dnwp      = -999.;
63     planes[i].on_crs    = true;
64     planes[i].turn_rate = 10.0;
65     planes[i].desc_rate = 1000.0;
66     planes[i].clmb_rate = 500.0;
67     planes[i].tlm       = 0.0;
68     planes[i].lmc.c1    = 0;
69     planes[i].lmc.c2    = 0;
70     planes[i].lmc.c3    = -1;
71     planes[i].wp_change = false;
72   }
73 }
74
75 //Destructor
76 FGApproach::~FGApproach(){
77 }
78
79 void FGApproach::Init() {
80   display = false;
81 }
82
83
84
85 // ============================================================================
86 // the main update function
87 // ============================================================================
88 void FGApproach::Update(double dt) {
89         
90         const int max_trans = 20;
91         FGTransmission tmissions[max_trans];
92         int    wpn;
93         atc_type station = APPROACH;
94         TransCode code;
95         TransPar TPar;
96         int    i,j;
97         //double course, d, 
98         double adif, datp;
99         //char   buf[10];
100         string message;
101         //static string atcmsg1[10];
102         //static string atcmsg2[10];
103         string mentry;
104         string transm;
105         TransPar tpars;
106         //static bool TransDisplayed = false;
107         
108         update_plane_dat();
109         if ( active_runway == "" ) get_active_runway();
110         
111         double comm1_freq = comm1_node->getDoubleValue();
112         
113         //bool DisplayTransmissions = true;
114         
115         for (i=0; i<num_planes; i++) {
116                 if ( planes[i].ident == "Player") { 
117                         station = APPROACH;
118                         tpars.station = name;
119                         tpars.callsign = "Player";
120                         tpars.airport = ident;
121                         
122                         //cout << "ident = " << ident << " name = " << name << '\n';
123                         
124                         int num_trans = 0;
125                         // is the frequency of the station tuned in?
126                         if ( freq == (int)(comm1_freq*100.0 + 0.5) ) {
127                                 current_transmissionlist->query_station( station, tmissions, max_trans, num_trans );
128                                 // loop over all transmissions for station
129                                 for ( j=0; j<=num_trans-1; j++ ) {
130                                         code = tmissions[j].get_code();
131                                         //cout << "code is " << code.c1 << "  " << code.c2 << "  " << code.c3 << '\n';
132                                         // select proper transmissions
133                                         if(code.c3 != 2) {    // DCL - hack to prevent request crossing airspace being displayed since this isn't implemented yet.
134                                             if ( ( code.c2 == -1 && planes[i].lmc.c3 == 0 ) || 
135                                                     ( code.c1 == 0  && code.c2 == planes[i].lmc.c2 ) ) {
136                                                     mentry = current_transmissionlist->gen_text(station, code, tpars, false);
137                                                     transm = current_transmissionlist->gen_text(station, code, tpars, true);
138                                                     // is the transmission already registered?
139                                                     if (!current_atcdialog->trans_reg( ident, transm, APPROACH )) {
140                                                             current_atcdialog->add_entry( ident, transm, mentry, APPROACH, 0 );
141                                                     }
142                                             }
143                                         }
144                                 }
145                         }
146                 }
147         }
148         
149         for ( i=0; i<num_planes; i++ ) {
150                 //cout << "TPar.airport = " << TPar.airport << " TPar.station = " << TPar.station << " TPar.callsign = " << TPar.callsign << '\n';
151                 //if ( planes[i].ident == TPar.callsign && name == TPar.airport && TPar.station == "approach" ) {
152                         //if ( TPar.request && TPar.intention == "landing" && ident == TPar.intid) {
153                         if(planes[i].ident == "Player" && fgGetBool("/sim/atc/opt0")) {
154                                 //cout << "Landing requested\n";
155                                 fgSetBool("/sim/atc/opt0", false);
156                                 planes[i].wpn = 0; 
157                                 // ===========================
158                                 // === calculate waypoints ===
159                                 // ===========================
160                                 calc_wp( i );  
161                                 update_param( i );
162                                 wpn = planes[i].wpn-1;
163                                 planes[i].aalt = planes[i].wpts[wpn-1][2];
164                                 planes[i].ahdg = planes[i].wpts[wpn][4];
165                                 
166                                 // generate the message
167                                 code.c1 = 1;
168                                 code.c2 = 1;
169                                 code.c3 = 0;
170                                 adif = angle_diff_deg( planes[i].hdg, planes[i].ahdg );
171                                 tpars.station = name;
172                                 tpars.callsign = "Player";
173                                 if ( adif < 0 ) tpars.tdir = 1;
174                                 else            tpars.tdir = 2;
175                                 tpars.heading = planes[i].ahdg;
176                                 if      (planes[i].alt-planes[i].aalt > 100.0)  tpars.VDir = 1;
177                                 else if (planes[i].alt-planes[i].aalt < -100.0) tpars.VDir = 3;
178                                 else tpars.VDir = 2;
179                                 tpars.alt = planes[i].aalt;
180                                 message = current_transmissionlist->gen_text(station, code, tpars, true );
181                                 //cout << message << '\n';
182                                 globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
183                                 planes[i].lmc = code;
184                                 planes[i].tlm = etime_node->getDoubleValue();
185                                 planes[i].on_crs = true;
186                                 planes[i].contact = 1;
187                         }
188                 //}
189                 
190                 //if(1) {
191                 if ( planes[i].contact == 1 ) {
192                         // =========================
193                         // === update parameters ===
194                         // =========================
195                         update_param( i );
196                         //cout << planes[i].brg << " " << planes[i].dist << " " << planes[i].wpts[wpn+1][0] 
197                         //<< " " << planes[i].wpts[wpn+1][1] << " " << planes[i].wpts[wpn+1][4] 
198                         //cout << wpn << " distance to current course = " << planes[i].dcc << endl;
199                         //cout << etime_node->getDoubleValue() << endl;
200                         
201                         // =========================
202                         // === reached waypoint? ===
203                         // =========================
204                         wpn = planes[i].wpn-2;
205                         adif = angle_diff_deg( planes[i].hdg, planes[i].wpts[wpn][4] ) 
206                         * SGD_DEGREES_TO_RADIANS;
207                         datp = 2*sin(fabs(adif)/2.0)*sin(fabs(adif)/2.0) *
208                                planes[i].spd/3600. * planes[i].turn_rate + 
209                                planes[i].spd/3600. * 3.0;
210                         //cout << adif/SGD_DEGREES_TO_RADIANS << " " 
211                         //     << datp << " " << planes[i].dnc << " " << planes[i].dcc <<endl;
212                         if ( fabs(planes[i].dnc) < datp ) {
213                         //if ( fabs(planes[i].dnc) < 0.3 && planes[i].dnwp < 1.0 ) {
214                                 //cout << "Reached next waypoint!\n";
215                                 planes[i].wpn -= 1;
216                                 wpn = planes[i].wpn-1;
217                                 planes[i].ahdg = planes[i].wpts[wpn][4];
218                                 planes[i].aalt = planes[i].wpts[wpn-1][2];
219                                 planes[i].wp_change = true;
220                                 
221                                 // generate the message
222                                 adif = angle_diff_deg( planes[i].hdg, planes[i].ahdg );
223                                 tpars.station = name;
224                                 tpars.callsign = "Player";
225                                 if ( adif < 0 ) tpars.tdir = 1;
226                                 else            tpars.tdir = 2;
227                                 tpars.heading = planes[i].ahdg;
228                                 
229                                 if ( wpn-1 != 0) { 
230                                         code.c1 = 1;
231                                         code.c2 = 1;
232                                         code.c3 = 0;
233                                         if      (planes[i].alt-planes[i].aalt > 100.0)  tpars.VDir = 1;
234                                         else if (planes[i].alt-planes[i].aalt < -100.0) tpars.VDir = 3;
235                                         else tpars.VDir = 2;
236                                         tpars.alt = planes[i].aalt;
237                                         message = current_transmissionlist->gen_text(station, code, tpars, true );
238                                         //cout << "Approach transmitting...\n";
239                                         //cout << message << endl;
240                                         globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
241                                         
242                                 }
243                                 else {
244                                         code.c1 = 1;
245                                         code.c2 = 3;
246                                         code.c3 = 0;
247                                         tpars.runway = active_runway;
248                                         message = current_transmissionlist->gen_text(station, code, tpars, true);
249                                         //cout << "Approach transmitting 2 ...\n";
250                                         //cout << message << endl;
251                                         globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
252                                 }
253                                 planes[i].lmc = code;
254                                 planes[i].tlm = etime_node->getDoubleValue();
255                                 planes[i].on_crs = true;
256                                 
257                                 update_param( i );
258                         }
259                         
260                         // =========================
261                         // === come off course ? ===
262                         // =========================
263                         if ( fabs(planes[i].dcc) > 1.0 && 
264                            ( !planes[i].wp_change || etime_node->getDoubleValue() - planes[i].tlm > tbm ) ) {
265                                 //cout << "Off course!\n";
266                                 if ( planes[i].on_crs ) {
267                                         if ( planes[i].dcc < 0) {
268                                                 planes[i].ahdg += 30.0;
269                                         }
270                                         else {
271                                                 planes[i].ahdg -= 30.0;
272                                         }
273                                         if (planes[i].ahdg > 360.0) planes[i].ahdg -= 360.0;
274                                         else if (planes[i].ahdg < 0.0) planes[i].ahdg += 360.0;
275                                 }
276                                 //cout << planes[i].on_crs << " " 
277                                 //     << angle_diff_deg( planes[i].hdg, planes[i].ahdg) << " "
278                                 //     << etime_node->getDoubleValue() << " "
279                                 //     << planes[i].tlm << endl;
280                                 // generate the message
281                                 if ( planes[i].on_crs || 
282                                    ( fabs(angle_diff_deg( planes[i].hdg, planes[i].ahdg )) >  30.0  && 
283                                     etime_node->getDoubleValue() - planes[i].tlm > tbm) ) {
284                                         // generate the message
285                                         code.c1 = 1;
286                                         code.c2 = 4;
287                                         code.c3 = 0;
288                                         adif = angle_diff_deg( planes[i].hdg, planes[i].ahdg );
289                                         tpars.station = name;
290                                         tpars.callsign = "Player";
291                                         tpars.miles   = fabs(planes[i].dcc);
292                                         if ( adif < 0 ) tpars.tdir = 1;
293                                         else            tpars.tdir = 2;
294                                         tpars.heading = planes[i].ahdg;
295                                         message = current_transmissionlist->gen_text(station, code, tpars, true);
296                                         //cout << "Approach transmitting 3 ...\n";
297                                         //cout << message << '\n';
298                                         globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
299                                         planes[i].lmc = code;
300                                         planes[i].tlm = etime_node->getDoubleValue();
301                                 }
302                                 
303                                 planes[i].on_crs = false;
304                         }
305                         else if ( !planes[i].on_crs ) {
306                                 //cout << "Off course 2!\n";
307                                 wpn = planes[i].wpn-1;
308                                 adif = angle_diff_deg( planes[i].hdg, planes[i].wpts[wpn][4] ) 
309                                        * SGD_DEGREES_TO_RADIANS;
310                                 datp = 2*sin(fabs(adif)/2.0)*sin(fabs(adif)/2.0) *
311                                 planes[i].spd/3600. * planes[i].turn_rate + 
312                                 planes[i].spd/3600. * 3.0;
313                                 if ( fabs(planes[i].dcc) < datp ) { 
314                                         planes[i].ahdg = fabs(planes[i].wpts[wpn][4]);
315                                         
316                                         // generate the message
317                                         code.c1 = 1;
318                                         code.c2 = 2;
319                                         code.c3 = 0;
320                                         tpars.station = name;
321                                         tpars.callsign = "Player";
322                                         if ( adif < 0 ) tpars.tdir = 1;
323                                         else            tpars.tdir = 2;
324                                         tpars.heading = planes[i].ahdg;
325                                         message = current_transmissionlist->gen_text(station, code, tpars, true);
326                                         //cout << "Approach transmitting 4 ...\n";
327                                         //cout << message << '\n';
328                                         globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
329                                         planes[i].lmc = code;
330                                         planes[i].tlm = etime_node->getDoubleValue();
331                                         
332                                         planes[i].on_crs = true;          
333                                 } 
334                         }
335                         else if ( planes[i].wp_change  ) {
336                                 planes[i].wp_change = false;
337                         }
338                         
339                         // ===================================================================
340                         // === Less than two minutes away from touchdown? -> Contact Tower ===
341                         // ===================================================================
342                         if ( planes[i].wpn == 2 && planes[i].dnwp < planes[i].spd/60.*2.0 ) {
343                                 
344                                 double freq = 121.95;   // Hardwired - FIXME
345                                 // generate message
346                                 code.c1 = 1;
347                                 code.c2 = 5;
348                                 code.c3 = 0;
349                                 tpars.station = name;
350                                 tpars.callsign = "Player";
351                                 tpars.freq    = freq;
352                                 message = current_transmissionlist->gen_text(station, code, tpars, true);
353                                 //cout << "Approach transmitting 5 ...\n";
354                                 //cout << message << '\n';
355                                 globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
356                                 planes[i].lmc = code;
357                                 planes[i].tlm = etime_node->getDoubleValue();
358                                 
359                                 planes[i].contact = 2;
360                         }
361                 }
362         }
363 }
364
365
366 // ============================================================================
367 // update course parameters
368 // ============================================================================
369 void FGApproach::update_param( const int &i ) {
370   
371   double course, d;
372
373   int wpn = planes[i].wpn-1;            // this is the current waypoint
374
375   planes[i].dcc  = calc_psl_dist(planes[i].brg, planes[i].dist,
376                                  planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],
377                                  planes[i].wpts[wpn][4]);
378   planes[i].dnc  = calc_psl_dist(planes[i].brg, planes[i].dist,
379                                  planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
380                                  planes[i].wpts[wpn-1][4]);
381   calc_hd_course_dist(planes[i].brg, planes[i].dist, 
382                       planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
383                       &course, &d);
384   planes[i].dnwp = d;
385
386 }
387
388 // ============================================================================
389 // smallest difference between two angles in degree
390 // difference is negative if a1 > a2 and positive if a2 > a1
391 // ===========================================================================
392 double FGApproach::angle_diff_deg( const double &a1, const double &a2) {
393   
394   double a3 = a2 - a1;
395   if (a3 < 180.0) a3 += 360.0;
396   if (a3 > 180.0) a3 -= 360.0;
397
398   return a3;
399 }
400
401 // ============================================================================
402 // calculate waypoints
403 // ============================================================================
404 void FGApproach::calc_wp( const int &i ) {
405         
406         int j;
407         double course, d, cd, a1;
408         
409         int wpn = planes[i].wpn;
410         // waypoint 0: Threshold of active runway
411         calc_gc_course_dist(Point3D(lon*SGD_DEGREES_TO_RADIANS, lat*SGD_DEGREES_TO_RADIANS, 0.0),
412                             Point3D(active_rw_lon*SGD_DEGREES_TO_RADIANS,active_rw_lat*SGD_DEGREES_TO_RADIANS, 0.0 ),
413                             &course, &d);
414         double d1 = active_rw_hdg+180.0;
415         if ( d1 > 360.0 ) d1 -=360.0;
416         calc_cd_head_dist(360.0-course*SGD_RADIANS_TO_DEGREES, d/SG_NM_TO_METER, 
417                           d1, active_rw_len/SG_NM_TO_METER/2.0, 
418                           &planes[i].wpts[wpn][0], &planes[i].wpts[wpn][1]);
419         planes[i].wpts[wpn][2] = elev;
420         planes[i].wpts[wpn][4] = 0.0;
421         planes[i].wpts[wpn][5] = 0.0;
422         wpn += 1;
423         
424         // ======================
425         // horizontal navigation
426         // ======================
427         // waypoint 1: point for turning onto final
428         calc_cd_head_dist(planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1], d1, lfl,
429                           &planes[i].wpts[wpn][0], &planes[i].wpts[wpn][1]);
430         calc_hd_course_dist(planes[i].wpts[wpn][0],   planes[i].wpts[wpn][1],
431                             planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
432                             &course, &d);
433         planes[i].wpts[wpn][4] = course;
434         planes[i].wpts[wpn][5] = d;
435         wpn += 1;
436         
437         // calculate course and distance from plane position to waypoint 1
438         calc_hd_course_dist(planes[i].brg, planes[i].dist, planes[i].wpts[1][0], 
439                             planes[i].wpts[1][1], &course, &d);
440         // check if airport is not between plane and waypoint 1 and
441         // DCA to airport on course to waypoint 1 is larger than 10 miles
442         double zero = 0.0;
443         if ( fabs(angle_diff_deg( planes[i].wpts[1][0], planes[i].brg  )) < 90.0 ||
444                 calc_psl_dist( zero, zero, planes[i].brg, planes[i].dist, course ) > 10.0 ) {
445                 // check if turning angle at waypoint 1 would be > max_ta
446                 if ( fabs(angle_diff_deg( planes[i].wpts[1][4], course )) > max_ta ) {
447                         cd = calc_psl_dist(planes[i].brg, planes[i].dist,
448                              planes[i].wpts[1][0], planes[i].wpts[1][1],
449                              planes[i].wpts[1][4]);
450                         a1 = atan2(cd,planes[i].wpts[1][1]);
451                         planes[i].wpts[wpn][0] = planes[i].wpts[1][0] - a1/SGD_DEGREES_TO_RADIANS;
452                         if ( planes[i].wpts[wpn][0] < 0.0)   planes[i].wpts[wpn][0] += 360.0;   
453                         if ( planes[i].wpts[wpn][0] > 360.0) planes[i].wpts[wpn][0] -= 360.0;   
454                         planes[i].wpts[wpn][1] = fabs(cd) / sin(fabs(a1));
455                         calc_hd_course_dist(planes[i].wpts[wpn][0],   planes[i].wpts[wpn][1],
456                                             planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
457                                             &course, &d);
458                         planes[i].wpts[wpn][4] = course;
459                         planes[i].wpts[wpn][5] = d;
460                         wpn += 1;
461                         
462                         calc_hd_course_dist(planes[i].brg, planes[i].dist, planes[i].wpts[wpn-1][0], 
463                                             planes[i].wpts[wpn-1][1], &course, &d);
464                 }
465         } else {
466                 double leg = 10.0;
467                 a1 = atan2(planes[i].wpts[1][1], leg );
468                 
469                 if ( angle_diff_deg(planes[i].brg,planes[i].wpts[1][0]) < 0 ) 
470                         planes[i].wpts[wpn][0] = planes[i].wpts[1][0] + a1/SGD_DEGREES_TO_RADIANS;
471                 else planes[i].wpts[wpn][0] = planes[i].wpts[1][0] - a1/SGD_DEGREES_TO_RADIANS;
472                 
473                 planes[i].wpts[wpn][1] = sqrt( planes[i].wpts[1][1]*planes[i].wpts[1][1] + leg*leg );
474                 calc_hd_course_dist(planes[i].wpts[wpn][0],   planes[i].wpts[wpn][1],
475                                     planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
476                                     &course, &d);
477                 planes[i].wpts[wpn][4] = course;
478                 planes[i].wpts[wpn][5] = d;
479                 wpn += 1;
480                 
481                 calc_hd_course_dist(planes[i].brg, planes[i].dist,
482                                     planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
483                                     &course, &d);
484         }
485         
486         planes[i].wpts[wpn][0] = planes[i].brg;
487         planes[i].wpts[wpn][1] = planes[i].dist;
488         planes[i].wpts[wpn][2] = planes[i].alt;
489         planes[i].wpts[wpn][4] = course;
490         planes[i].wpts[wpn][5] = d;
491         wpn += 1;
492         
493         planes[i].wpn = wpn;
494         
495         // Now check if legs are too short or if legs can be shortend
496         // legs must be at least 2 flight minutes long
497         double mdist = planes[i].spd / 60.0 * 2.0;
498         for ( j=2; j<wpn-1; ++j ) {
499                 if ( planes[i].wpts[j][1] < mdist) {
500                 }
501         }
502         
503         // ====================
504         // vertical navigation
505         // ====================
506         double alt = elev+3000.0;
507         planes[i].wpts[1][2] = round_alt( true, alt );
508         for ( j=2; j<wpn-1; ++j ) {
509                 double dalt = planes[i].alt - planes[i].wpts[j-1][2];
510                 if ( dalt > 0 ) {
511                         alt = planes[i].wpts[j-1][2] + 
512                               (planes[i].wpts[j][5] / planes[i].spd) * 60.0 * planes[i].desc_rate;
513                         planes[i].wpts[j][2] = round_alt( false, alt );
514                         if ( planes[i].wpts[j][2] > planes[i].alt ) 
515                                 planes[i].wpts[j][2] = round_alt( false, planes[i].alt );
516                 }
517                 else {
518                         planes[i].wpts[j][2] = planes[i].wpts[1][2];
519                 }
520         }
521         
522         cout << "Plane position: " << planes[i].brg << " " << planes[i].dist << endl;
523         for ( j=0; j<wpn; ++j ) {
524                 cout << "Waypoint " << j << endl;
525                 cout << "------------------" << endl;
526                 cout << planes[i].wpts[j][0] << "   " << planes[i].wpts[j][1]
527                      << "   " << planes[i].wpts[j][2] << "   " << planes[i].wpts[j][5]; 
528                 cout << endl << endl;
529         }
530         
531 }
532
533
534 // ============================================================================
535 // round altitude value to next highest/lowest 500 feet
536 // ============================================================================
537 double FGApproach::round_alt( const bool hl, double alt ) {
538
539   alt = alt/1000.0;
540   if ( hl ) {
541     if ( alt > (int)(alt)+0.5 ) alt = ((int)(alt)+1)*1000.0;
542     else alt = ((int)(alt)+0.5)*1000.0;
543   }
544   else {
545     if ( alt > (int)(alt)+0.5 ) alt = ((int)(alt)+0.5)*1000.0;
546     else alt = ((int)(alt))*1000.0;
547   }
548   
549   return alt;
550 }
551
552
553 // ============================================================================
554 // get active runway
555 // ============================================================================
556 void FGApproach::get_active_runway() {
557         //cout << "Entering FGApproach::get_active_runway()\n";
558
559 #ifdef FG_WEATHERCM
560   sgVec3 position = { lat, lon, elev };
561   FGPhysicalProperty stationweather = WeatherDatabase->get(position);
562 #else
563   FGEnvironment stationweather =
564       ((FGEnvironmentMgr *)globals->get_subsystem("environment"))
565         ->getEnvironment(lat, lon, elev);
566 #endif
567
568 #ifdef FG_WEATHERCM
569   //Set the heading to into the wind
570   double wind_x = stationweather.Wind[0];
571   double wind_y = stationweather.Wind[1];
572   
573   double speed = sqrt( wind_x*wind_x + wind_y*wind_y ) * SG_METER_TO_NM / (60.0*60.0);
574   double hdg;
575   
576   //If no wind use 270degrees
577   if(speed == 0) {
578     hdg = 270;
579   } else {
580     // //normalize the wind to get the direction
581     //wind_x /= speed; wind_y /= speed;
582     
583     hdg = - atan2 ( wind_x, wind_y ) * SG_RADIANS_TO_DEGREES ;
584     if (hdg < 0.0)
585       hdg += 360.0;
586   }
587 #else
588   double hdg = stationweather.get_wind_from_heading_deg();
589 #endif
590   
591   FGRunway runway;
592   if ( globals->get_runways()->search( ident, int(hdg), &runway) ) {
593     active_runway = runway.rwy_no;
594     active_rw_hdg = runway.heading;
595     active_rw_lon = runway.lon;
596     active_rw_lat = runway.lat;
597     active_rw_len = runway.length;
598     //cout << "Active runway is: " << active_runway << "  heading = " 
599     // << active_rw_hdg 
600     // << " lon = " << active_rw_lon 
601     // << " lat = " << active_rw_lat <<endl;
602   }
603   else cout << "FGRunways search failed\n";
604
605 }
606
607 // ========================================================================
608 // update infos about plane
609 // ========================================================================
610 void FGApproach::update_plane_dat() {
611   
612   //cout << "Update Approach " << ident << "   " << num_planes << " registered" << endl;
613   // update plane positions
614   int i;
615   for (i=0; i<num_planes; i++) {
616     planes[i].lon = lon_node->getDoubleValue();
617     planes[i].lat = lat_node->getDoubleValue();
618     planes[i].alt = elev_node->getDoubleValue();
619     planes[i].hdg = hdg_node->getDoubleValue();
620     planes[i].spd = speed_node->getDoubleValue();
621
622     /*Point3D aircraft = sgGeodToCart( Point3D(planes[i].lon*SGD_DEGREES_TO_RADIANS, 
623                                              planes[i].lat*SGD_DEGREES_TO_RADIANS, 
624                                              planes[i].alt*SG_FEET_TO_METER) );*/
625     double course, distance;
626     calc_gc_course_dist(Point3D(lon*SGD_DEGREES_TO_RADIANS, lat*SGD_DEGREES_TO_RADIANS, 0.0),
627                         Point3D(planes[i].lon*SGD_DEGREES_TO_RADIANS,planes[i].lat*SGD_DEGREES_TO_RADIANS, 0.0 ),
628                         &course, &distance);
629     planes[i].dist = distance/SG_NM_TO_METER;
630     planes[i].brg  = 360.0-course*SGD_RADIANS_TO_DEGREES;
631
632     //cout << "Plane Id: " << planes[i].ident << "  Distance to " << ident 
633     // << " is " << planes[i].dist << " miles   " << "Bearing " << planes[i].brg << endl;
634     
635   }   
636 }
637
638 // =======================================================================
639 // Add plane to Approach list
640 // =======================================================================
641 void FGApproach::AddPlane(string pid) {
642
643   int i;
644   for ( i=0; i<num_planes; i++) {
645     if ( planes[i].ident == pid) {
646       //cout << "Plane already registered: " << planes[i].ident << ' ' << ident << ' ' << num_planes << endl;
647       return;
648     }
649   }
650   planes[num_planes].ident = pid;
651   ++num_planes;
652   //cout << "Plane added to list: " << ident << " " << num_planes << endl;
653   return;
654 }
655
656 // ================================================================================
657 // closest distance between a point (h1,d1) and a straigt line (h2,d2,h3) in 2 dim.
658 // ================================================================================
659 double FGApproach::calc_psl_dist(const double &h1, const double &d1,
660                                  const double &h2, const double &d2,
661                                  const double &h3)
662 {
663   double a1 = h1 * SGD_DEGREES_TO_RADIANS;
664   double a2 = h2 * SGD_DEGREES_TO_RADIANS;
665   double a3 = h3 * SGD_DEGREES_TO_RADIANS;
666   double x1 = cos(a1) * d1;
667   double y1 = sin(a1) * d1;
668   double x2 = cos(a2) * d2;
669   double y2 = sin(a2) * d2;
670   double x3 = cos(a3);
671   double y3 = sin(a3);
672   
673   // formula: dis = sqrt( (v1-v2)**2 - ((v1-v2)*v3)**2 ); vi = (xi,yi)
674   double val1   = (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2);
675   double val2   = ((x1-x2)*x3 + (y1-y2)*y3) * ((x1-x2)*x3 + (y1-y2)*y3);
676   double dis    = val1 - val2;
677   // now get sign for offset 
678   //cout << x1 << " " << x2 << " " << y1 << " " << y2 << " " 
679   //     << x3 << " " << y3 << " " 
680   //     << val1 << " " << val2 << " " << dis << endl;
681   x3 *= sqrt(val2);
682   y3 *= sqrt(val2);
683   double da = fabs(atan2(y3,x3) - atan2(y1-y2,x1-x2));
684   if ( da > SGD_PI ) da -= 2*SGD_PI;
685   if ( fabs(da) > SGD_PI/2.) {
686     //if ( x3*(x1-x2) < 0.0 && y3*(y1-y2) < 0.0) {
687     x3 *= -1.0;
688     y3 *= -1.0;
689   }
690   //cout << x3 << " " << y3 << endl;
691   double dis1   = x1-x2-x3;
692   double dis2   = y1-y2-y3;
693   dis = sqrt(dis);
694   da = atan2(dis2,dis1);
695   if ( da < 0.0 ) da  += 2*SGD_PI;
696   if ( da < a3 )  dis *= -1.0;
697   //cout << dis1 << " " << dis2 << " " << da*SGD_RADIANS_TO_DEGREES << " " << h3
698   //     << " " << sqrt(dis1*dis1 + dis2*dis2) << " " << dis << endl;
699   //cout << atan2(dis2,dis1)*SGD_RADIANS_TO_DEGREES << " " << dis << endl;
700
701   return dis;
702 }
703
704
705 // ========================================================================
706 // Calculate new bear/dist given starting bear/dis, and offset radial,
707 // and distance.
708 // ========================================================================
709 void FGApproach::calc_cd_head_dist(const double &h1, const double &d1, 
710                                    const double &course, const double &dist,
711                                    double *h2, double *d2)
712 {
713   double a1 = h1 * SGD_DEGREES_TO_RADIANS;
714   double a2 = course * SGD_DEGREES_TO_RADIANS;
715   double x1 = cos(a1) * d1;
716   double y1 = sin(a1) * d1;
717   double x2 = cos(a2) * dist;
718   double y2 = sin(a2) * dist;
719     
720   *d2 = sqrt((x1+x2)*(x1+x2) + (y1+y2)*(y1+y2));
721   *h2 = atan2( (y1+y2), (x1+x2) ) * SGD_RADIANS_TO_DEGREES;
722   if ( *h2 < 0 ) *h2 = *h2+360;
723 }
724
725
726
727 // ========================================================================
728 // get heading and distance between two points; point1 ---> point2
729 // ========================================================================
730 void FGApproach::calc_hd_course_dist(const double &h1, const double &d1, 
731                                      const double &h2, const double &d2,
732                                      double *course, double *dist)
733 {
734   double a1 = h1 * SGD_DEGREES_TO_RADIANS;
735   double a2 = h2 * SGD_DEGREES_TO_RADIANS;
736   double x1 = cos(a1) * d1;
737   double y1 = sin(a1) * d1;
738   double x2 = cos(a2) * d2;
739   double y2 = sin(a2) * d2;
740            
741   *dist   = sqrt( (y2-y1)*(y2-y1) + (x2-x1)*(x2-x1) );
742   *course = atan2( (y2-y1), (x2-x1) ) * SGD_RADIANS_TO_DEGREES;
743   if ( *course < 0 ) *course = *course+360;
744   //cout << x1 << " " << y1 << " " << x2 << " " << y2 << " " << *dist << " " << *course << endl;
745 }
746
747
748
749 int FGApproach::RemovePlane() {
750
751   // first check if anything has to be done
752   bool rmplane = false;
753   int i;
754
755   for (i=0; i<num_planes; i++) {
756     if (planes[i].dist > range*SG_NM_TO_METER) {
757       rmplane = true;
758       break;
759     }
760   }
761   if (!rmplane) return num_planes;
762
763   // now make a copy of the plane list
764   PlaneApp tmp[max_planes];
765   for (i=0; i<num_planes; i++) {
766     tmp[i] = planes[i];
767   }
768   
769   int np = 0;
770   // now check which planes are still in range
771   for (i=0; i<num_planes; i++) {
772     if (tmp[i].dist <= range*SG_NM_TO_METER) {
773       planes[np] = tmp[i];
774       np += 1;
775     }
776   }
777   num_planes = np;
778
779   return num_planes;
780 }