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