1 // FGApproach - a class to provide approach control at larger airports.
\r
3 // Written by Alexander Kappes, started March 2002.
\r
5 // Copyright (C) 2002 Alexander Kappes
\r
7 // This program is free software; you can redistribute it and/or
\r
8 // modify it under the terms of the GNU General Public License as
\r
9 // published by the Free Software Foundation; either version 2 of the
\r
10 // License, or (at your option) any later version.
\r
12 // This program is distributed in the hope that it will be useful, but
\r
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
\r
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
\r
15 // General Public License for more details.
\r
17 // You should have received a copy of the GNU General Public License
\r
18 // along with this program; if not, write to the Free Software
\r
19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
\r
21 #include "approach.hxx"
\r
22 #include "transmission.hxx"
\r
23 #include "transmissionlist.hxx"
\r
24 #include "ATCdisplay.hxx"
\r
25 #include "ATCDialog.hxx"
\r
27 #include <Airports/runways.hxx>
\r
28 #include <simgear/misc/sg_path.hxx>
\r
31 # include <WeatherCM/FGLocalWeatherDatabase.h>
\r
33 # include <Environment/environment_mgr.hxx>
\r
34 # include <Environment/environment.hxx>
\r
38 #include <GUI/gui.h>
\r
41 FGApproach::FGApproach(){
\r
42 comm1_node = fgGetNode("/radios/comm[0]/frequencies/selected-mhz", true);
\r
43 comm2_node = fgGetNode("/radios/comm[1]/frequencies/selected-mhz", true);
\r
46 lon_node = fgGetNode("/position/longitude-deg", true);
\r
47 lat_node = fgGetNode("/position/latitude-deg", true);
\r
48 elev_node = fgGetNode("/position/altitude-ft", true);
\r
49 hdg_node = fgGetNode("/orientation/heading-deg", true);
\r
50 speed_node = fgGetNode("/velocities/airspeed-kt", true);
\r
51 etime_node = fgGetNode("/sim/time/elapsed-ms", true);
\r
56 for ( i=0; i<max_planes; i++) {
\r
57 planes[i].contact = 0;
\r
59 planes[i].dnwp = -999.;
\r
60 planes[i].on_crs = true;
\r
61 planes[i].turn_rate = 10.0;
\r
62 planes[i].desc_rate = 1000.0;
\r
63 planes[i].clmb_rate = 500.0;
\r
64 planes[i].tlm = 0.0;
\r
65 planes[i].lmc.c1 = 0;
\r
66 planes[i].lmc.c2 = 0;
\r
67 planes[i].lmc.c3 = -1;
\r
68 planes[i].wp_change = false;
\r
73 FGApproach::~FGApproach(){
\r
76 void FGApproach::Init() {
\r
82 // ============================================================================
\r
83 // the main update function
\r
84 // ============================================================================
\r
85 void FGApproach::Update() {
\r
87 const int max_trans = 20;
\r
88 FGTransmission tmissions[max_trans];
\r
94 //double course, d,
\r
98 //static string atcmsg1[10];
\r
99 //static string atcmsg2[10];
\r
103 //static bool TransDisplayed = false;
\r
105 update_plane_dat();
\r
106 if ( active_runway == "" ) get_active_runway();
\r
108 double comm1_freq = comm1_node->getDoubleValue();
\r
110 //bool DisplayTransmissions = true;
\r
112 for (i=0; i<num_planes; i++) {
\r
113 if ( planes[i].ident == "Player") {
\r
115 tpars.station = name;
\r
116 tpars.callsign = "Player";
\r
117 tpars.airport = ident;
\r
120 // is the frequency of the station tuned in?
\r
121 if ( freq == (int)(comm1_freq*100.0 + 0.5) ) {
\r
122 current_transmissionlist->query_station( station, tmissions, max_trans, num_trans );
\r
123 // loop over all transmissions for station
\r
124 for ( j=0; j<=num_trans-1; j++ ) {
\r
125 code = tmissions[j].get_code();
\r
126 // select proper transmissions
\r
127 if ( ( code.c2 == -1 && planes[i].lmc.c3 == 0 ) ||
\r
128 ( code.c1 == 0 && code.c2 == planes[i].lmc.c2 ) ) {
\r
129 mentry = current_transmissionlist->gen_text(station, code, tpars, false);
\r
130 transm = current_transmissionlist->gen_text(station, code, tpars, true);
\r
131 // is the transmission already registered?
\r
132 if (!current_atcdialog->trans_reg( ident, transm )) {
\r
133 current_atcdialog->add_entry( ident, transm, mentry );
\r
141 for ( i=0; i<num_planes; i++ ) {
\r
143 if ( planes[i].ident == TPar.callsign && name == TPar.airport && TPar.station == "approach" ) {
\r
145 if ( TPar.request && TPar.intention == "landing" && ident == TPar.intid) {
\r
146 planes[i].wpn = 0;
\r
147 // ===========================
\r
148 // === calculate waypoints ===
\r
149 // ===========================
\r
152 wpn = planes[i].wpn-1;
\r
153 planes[i].aalt = planes[i].wpts[wpn-1][2];
\r
154 planes[i].ahdg = planes[i].wpts[wpn][4];
\r
156 // generate the message
\r
160 adif = angle_diff_deg( planes[i].hdg, planes[i].ahdg );
\r
161 tpars.station = name;
\r
162 tpars.callsign = "Player";
\r
163 if ( adif < 0 ) tpars.tdir = 1;
\r
164 else tpars.tdir = 2;
\r
165 tpars.heading = planes[i].ahdg;
\r
166 if (planes[i].alt-planes[i].aalt > 100.0) tpars.VDir = 1;
\r
167 else if (planes[i].alt-planes[i].aalt < -100.0) tpars.VDir = 3;
\r
168 else tpars.VDir = 2;
\r
169 tpars.alt = planes[i].aalt;
\r
170 message = current_transmissionlist->gen_text(station, code, tpars, true );
\r
171 globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
\r
172 planes[i].lmc = code;
\r
173 planes[i].tlm = etime_node->getDoubleValue();
\r
174 planes[i].on_crs = true;
\r
175 planes[i].contact = 1;
\r
179 if ( planes[i].contact == 1 ) {
\r
180 // =========================
\r
181 // === update parameters ===
\r
182 // =========================
\r
184 //cout << planes[i].brg << " " << planes[i].dist << " " << planes[i].wpts[wpn+1][0]
\r
185 //<< " " << planes[i].wpts[wpn+1][1] << " " << planes[i].wpts[wpn+1][4]
\r
186 //cout << wpn << " distance to current course = " << planes[i].dcc << endl;
\r
187 //cout << etime_node->getDoubleValue() << endl;
\r
189 // =========================
\r
190 // === reached waypoint? ===
\r
191 // =========================
\r
192 wpn = planes[i].wpn-2;
\r
193 adif = angle_diff_deg( planes[i].hdg, planes[i].wpts[wpn][4] )
\r
194 * SGD_DEGREES_TO_RADIANS;
\r
195 datp = 2*sin(fabs(adif)/2.0)*sin(fabs(adif)/2.0) *
\r
196 planes[i].spd/3600. * planes[i].turn_rate +
\r
197 planes[i].spd/3600. * 3.0;
\r
198 //cout << adif/SGD_DEGREES_TO_RADIANS << " "
\r
199 // << datp << " " << planes[i].dnc << " " << planes[i].dcc <<endl;
\r
200 if ( fabs(planes[i].dnc) < datp ) {
\r
201 //if ( fabs(planes[i].dnc) < 0.3 && planes[i].dnwp < 1.0 ) {
\r
202 planes[i].wpn -= 1;
\r
203 wpn = planes[i].wpn-1;
\r
204 planes[i].ahdg = planes[i].wpts[wpn][4];
\r
205 planes[i].aalt = planes[i].wpts[wpn-1][2];
\r
206 planes[i].wp_change = true;
\r
208 // generate the message
\r
209 adif = angle_diff_deg( planes[i].hdg, planes[i].ahdg );
\r
210 tpars.station = name;
\r
211 tpars.callsign = "Player";
\r
212 if ( adif < 0 ) tpars.tdir = 1;
\r
213 else tpars.tdir = 2;
\r
214 tpars.heading = planes[i].ahdg;
\r
216 if ( wpn-1 != 0) {
\r
220 if (planes[i].alt-planes[i].aalt > 100.0) tpars.VDir = 1;
\r
221 else if (planes[i].alt-planes[i].aalt < -100.0) tpars.VDir = 3;
\r
222 else tpars.VDir = 2;
\r
223 tpars.alt = planes[i].aalt;
\r
224 message = current_transmissionlist->gen_text(station, code, tpars, true );
\r
225 globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
\r
232 tpars.runway = active_runway;
\r
233 message = current_transmissionlist->gen_text(station, code, tpars, true);
\r
234 globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
\r
236 planes[i].lmc = code;
\r
237 planes[i].tlm = etime_node->getDoubleValue();
\r
238 planes[i].on_crs = true;
\r
243 // =========================
\r
244 // === come off course ? ===
\r
245 // =========================
\r
246 if ( fabs(planes[i].dcc) > 1.0 &&
\r
247 ( !planes[i].wp_change ||
\r
248 etime_node->getDoubleValue() - planes[i].tlm > tbm ) ) {
\r
249 if ( planes[i].on_crs ) {
\r
250 if ( planes[i].dcc < 0) {
\r
251 planes[i].ahdg += 30.0;
\r
254 planes[i].ahdg -= 30.0;
\r
256 if (planes[i].ahdg > 360.0) planes[i].ahdg -= 360.0;
\r
257 else if (planes[i].ahdg < 0.0) planes[i].ahdg += 360.0;
\r
259 //cout << planes[i].on_crs << " "
\r
260 // << angle_diff_deg( planes[i].hdg, planes[i].ahdg) << " "
\r
261 // << etime_node->getDoubleValue() << " "
\r
262 // << planes[i].tlm << endl;
\r
263 // generate the message
\r
264 if ( planes[i].on_crs ||
\r
265 ( fabs(angle_diff_deg( planes[i].hdg, planes[i].ahdg )) > 30.0 &&
\r
266 etime_node->getDoubleValue() - planes[i].tlm > tbm) ) {
\r
267 // generate the message
\r
271 adif = angle_diff_deg( planes[i].hdg, planes[i].ahdg );
\r
272 tpars.station = name;
\r
273 tpars.callsign = "Player";
\r
274 tpars.miles = fabs(planes[i].dcc);
\r
275 if ( adif < 0 ) tpars.tdir = 1;
\r
276 else tpars.tdir = 2;
\r
277 tpars.heading = planes[i].ahdg;
\r
278 message = current_transmissionlist->gen_text(station, code, tpars, true);
\r
279 globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
\r
280 planes[i].lmc = code;
\r
281 planes[i].tlm = etime_node->getDoubleValue();
\r
284 planes[i].on_crs = false;
\r
286 else if ( !planes[i].on_crs ) {
\r
287 wpn = planes[i].wpn-1;
\r
288 adif = angle_diff_deg( planes[i].hdg, planes[i].wpts[wpn][4] )
\r
289 * SGD_DEGREES_TO_RADIANS;
\r
290 datp = 2*sin(fabs(adif)/2.0)*sin(fabs(adif)/2.0) *
\r
291 planes[i].spd/3600. * planes[i].turn_rate +
\r
292 planes[i].spd/3600. * 3.0;
\r
293 if ( fabs(planes[i].dcc) < datp ) {
\r
294 planes[i].ahdg = fabs(planes[i].wpts[wpn][4]);
\r
296 // generate the message
\r
300 tpars.station = name;
\r
301 tpars.callsign = "Player";
\r
302 if ( adif < 0 ) tpars.tdir = 1;
\r
303 else tpars.tdir = 2;
\r
304 tpars.heading = planes[i].ahdg;
\r
305 message = current_transmissionlist->gen_text(station, code, tpars, true);
\r
306 globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
\r
307 planes[i].lmc = code;
\r
308 planes[i].tlm = etime_node->getDoubleValue();
\r
310 planes[i].on_crs = true;
\r
313 else if ( planes[i].wp_change ) {
\r
314 planes[i].wp_change = false;
\r
317 // ===================================================================
\r
318 // === Less than two minutes away from touchdown? -> Contact Tower ===
\r
319 // ===================================================================
\r
320 if ( planes[i].wpn == 2 && planes[i].dnwp < planes[i].spd/60.*2.0 ) {
\r
322 double freq = 121.95;
\r
323 // generate message
\r
327 tpars.station = name;
\r
328 tpars.callsign = "Player";
\r
330 message = current_transmissionlist->gen_text(station, code, tpars, true);
\r
331 globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
\r
332 planes[i].lmc = code;
\r
333 planes[i].tlm = etime_node->getDoubleValue();
\r
335 planes[i].contact = 2;
\r
343 // ============================================================================
\r
344 // update course parameters
\r
345 // ============================================================================
\r
346 void FGApproach::update_param( const int &i ) {
\r
350 int wpn = planes[i].wpn-1; // this is the current waypoint
\r
352 planes[i].dcc = calc_psl_dist(planes[i].brg, planes[i].dist,
\r
353 planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],
\r
354 planes[i].wpts[wpn][4]);
\r
355 planes[i].dnc = calc_psl_dist(planes[i].brg, planes[i].dist,
\r
356 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
\r
357 planes[i].wpts[wpn-1][4]);
\r
358 calc_hd_course_dist(planes[i].brg, planes[i].dist,
\r
359 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
\r
361 planes[i].dnwp = d;
\r
365 // ============================================================================
\r
366 // smallest difference between two angles in degree
\r
367 // difference is negative if a1 > a2 and positive if a2 > a1
\r
368 // ===========================================================================
\r
369 double FGApproach::angle_diff_deg( const double &a1, const double &a2) {
\r
371 double a3 = a2 - a1;
\r
372 if (a3 < 180.0) a3 += 360.0;
\r
373 if (a3 > 180.0) a3 -= 360.0;
\r
378 // ============================================================================
\r
379 // calculate waypoints
\r
380 // ============================================================================
\r
381 void FGApproach::calc_wp( const int &i ) {
\r
384 double course, d, cd, a1;
\r
386 int wpn = planes[i].wpn;
\r
387 // waypoint 0: Threshold of active runway
\r
388 calc_gc_course_dist(Point3D(lon*SGD_DEGREES_TO_RADIANS, lat*SGD_DEGREES_TO_RADIANS, 0.0),
\r
389 Point3D(active_rw_lon*SGD_DEGREES_TO_RADIANS,active_rw_lat*SGD_DEGREES_TO_RADIANS, 0.0 ),
\r
391 double d1 = active_rw_hdg+180.0;
\r
392 if ( d1 > 360.0 ) d1 -=360.0;
\r
393 calc_cd_head_dist(360.0-course*SGD_RADIANS_TO_DEGREES, d/SG_NM_TO_METER,
\r
394 d1, active_rw_len/SG_NM_TO_METER/2.,
\r
395 &planes[i].wpts[wpn][0], &planes[i].wpts[wpn][1]);
\r
396 planes[i].wpts[wpn][2] = elev;
\r
397 planes[i].wpts[wpn][4] = 0.0;
\r
398 planes[i].wpts[wpn][5] = 0.0;
\r
401 // ======================
\r
402 // horizontal navigation
\r
403 // ======================
\r
404 // waypoint 1: point for turning onto final
\r
405 calc_cd_head_dist(planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1] ,
\r
407 &planes[i].wpts[wpn][0], &planes[i].wpts[wpn][1]);
\r
408 calc_hd_course_dist(planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],
\r
409 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
\r
411 planes[i].wpts[wpn][4] = course;
\r
412 planes[i].wpts[wpn][5] = d;
\r
415 // calculate course and distance from plane position to waypoint 1
\r
416 calc_hd_course_dist(planes[i].brg, planes[i].dist,
\r
417 planes[i].wpts[1][0], planes[i].wpts[1][1],
\r
419 // check if airport is not between plane and waypoint 1 and
\r
420 // DCA to airport on course to waypoint 1 is larger than 10 miles
\r
422 if ( fabs(angle_diff_deg( planes[i].wpts[1][0], planes[i].brg )) < 90.0 ||
\r
423 calc_psl_dist( zero, zero, planes[i].brg, planes[i].dist, course ) > 10.0 ) {
\r
424 // check if turning angle at waypoint 1 would be > max_ta
\r
425 if ( fabs(angle_diff_deg( planes[i].wpts[1][4], course )) > max_ta ) {
\r
426 cd = calc_psl_dist(planes[i].brg, planes[i].dist,
\r
427 planes[i].wpts[1][0], planes[i].wpts[1][1],
\r
428 planes[i].wpts[1][4]);
\r
429 a1 = atan2(cd,planes[i].wpts[1][1]);
\r
430 planes[i].wpts[wpn][0] = planes[i].wpts[1][0] - a1/SGD_DEGREES_TO_RADIANS;
\r
431 if ( planes[i].wpts[wpn][0] < 0.0) planes[i].wpts[wpn][0] += 360.0;
\r
432 if ( planes[i].wpts[wpn][0] > 360.0) planes[i].wpts[wpn][0] -= 360.0;
\r
433 planes[i].wpts[wpn][1] = fabs(cd) / sin(fabs(a1));
\r
434 calc_hd_course_dist(planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],
\r
435 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
\r
437 planes[i].wpts[wpn][4] = course;
\r
438 planes[i].wpts[wpn][5] = d;
\r
441 calc_hd_course_dist(planes[i].brg, planes[i].dist,
\r
442 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
\r
447 a1 = atan2(planes[i].wpts[1][1], leg );
\r
449 if ( angle_diff_deg(planes[i].brg,planes[i].wpts[1][0]) < 0 )
\r
450 planes[i].wpts[wpn][0] = planes[i].wpts[1][0] + a1/SGD_DEGREES_TO_RADIANS;
\r
451 else planes[i].wpts[wpn][0] = planes[i].wpts[1][0] - a1/SGD_DEGREES_TO_RADIANS;
\r
453 planes[i].wpts[wpn][1] = sqrt( planes[i].wpts[1][1]*planes[i].wpts[1][1] + leg*leg );
\r
454 calc_hd_course_dist(planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],
\r
455 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
\r
457 planes[i].wpts[wpn][4] = course;
\r
458 planes[i].wpts[wpn][5] = d;
\r
461 calc_hd_course_dist(planes[i].brg, planes[i].dist,
\r
462 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
\r
466 planes[i].wpts[wpn][0] = planes[i].brg;
\r
467 planes[i].wpts[wpn][1] = planes[i].dist;
\r
468 planes[i].wpts[wpn][2] = planes[i].alt;
\r
469 planes[i].wpts[wpn][4] = course;
\r
470 planes[i].wpts[wpn][5] = d;
\r
473 planes[i].wpn = wpn;
\r
475 // Now check if legs are too short or if legs can be shortend
\r
476 // legs must be at least 2 flight minutes long
\r
477 double mdist = planes[i].spd / 60.0 * 2.0;
\r
478 for ( j=2; j<wpn-1; ++j ) {
\r
479 if ( planes[i].wpts[j][1] < mdist) {
\r
483 // ====================
\r
484 // vertical navigation
\r
485 // ====================
\r
486 double alt = elev+3000.0;
\r
487 planes[i].wpts[1][2] = round_alt( true, alt );
\r
488 for ( j=2; j<wpn-1; ++j ) {
\r
489 double dalt = planes[i].alt - planes[i].wpts[j-1][2];
\r
491 alt = planes[i].wpts[j-1][2] +
\r
492 (planes[i].wpts[j][5] / planes[i].spd) * 60.0 * planes[i].desc_rate;
\r
493 planes[i].wpts[j][2] = round_alt( false, alt );
\r
494 if ( planes[i].wpts[j][2] > planes[i].alt )
\r
495 planes[i].wpts[j][2] = round_alt( false, planes[i].alt );
\r
498 planes[i].wpts[j][2] = planes[i].wpts[1][2];
\r
502 cout << "Plane position: " << planes[i].brg << " " << planes[i].dist << endl;
\r
503 for ( j=0; j<wpn; ++j ) {
\r
504 cout << "Waypoint " << j << endl;
\r
505 cout << "------------------" << endl;
\r
506 cout << planes[i].wpts[j][0] << " " << planes[i].wpts[j][1]
\r
507 << " " << planes[i].wpts[j][2] << " " << planes[i].wpts[j][5];
\r
508 cout << endl << endl;
\r
514 // ============================================================================
\r
515 // round altitude value to next highest/lowest 500 feet
\r
516 // ============================================================================
\r
517 double FGApproach::round_alt( const bool hl, double alt ) {
\r
521 if ( alt > (int)(alt)+0.5 ) alt = ((int)(alt)+1)*1000.0;
\r
522 else alt = ((int)(alt)+0.5)*1000.0;
\r
525 if ( alt > (int)(alt)+0.5 ) alt = ((int)(alt)+0.5)*1000.0;
\r
526 else alt = ((int)(alt))*1000.0;
\r
533 // ============================================================================
\r
534 // get active runway
\r
535 // ============================================================================
\r
536 void FGApproach::get_active_runway() {
\r
538 #ifdef FG_WEATHERCM
\r
539 sgVec3 position = { lat, lon, elev };
\r
540 FGPhysicalProperty stationweather = WeatherDatabase->get(position);
\r
542 FGEnvironment stationweather =
\r
543 globals->get_environment_mgr()->getEnvironment(lat, lon, elev);
\r
546 SGPath path( globals->get_fg_root() );
\r
547 path.append( "Airports" );
\r
548 path.append( "runways.mk4" );
\r
549 FGRunways runways( path.c_str() );
\r
551 #ifdef FG_WEATHERCM
\r
552 //Set the heading to into the wind
\r
553 double wind_x = stationweather.Wind[0];
\r
554 double wind_y = stationweather.Wind[1];
\r
556 double speed = sqrt( wind_x*wind_x + wind_y*wind_y ) * SG_METER_TO_NM / (60.0*60.0);
\r
559 //If no wind use 270degrees
\r
563 // //normalize the wind to get the direction
\r
564 //wind_x /= speed; wind_y /= speed;
\r
566 hdg = - atan2 ( wind_x, wind_y ) * SG_RADIANS_TO_DEGREES ;
\r
571 double hdg = stationweather.get_wind_from_heading_deg();
\r
575 if ( runways.search( ident, int(hdg), &runway) ) {
\r
576 active_runway = runway.rwy_no;
\r
577 active_rw_hdg = runway.heading;
\r
578 active_rw_lon = runway.lon;
\r
579 active_rw_lat = runway.lat;
\r
580 active_rw_len = runway.length;
\r
581 //cout << "Active runway is: " << active_runway << " heading = "
\r
582 // << active_rw_hdg
\r
583 // << " lon = " << active_rw_lon
\r
584 // << " lat = " << active_rw_lat <<endl;
\r
586 else cout << "FGRunways search failed\n";
\r
590 // ========================================================================
\r
591 // update infos about plane
\r
592 // ========================================================================
\r
593 void FGApproach::update_plane_dat() {
\r
595 //cout << "Update Approach " << ident << " " << num_planes << " registered" << endl;
\r
596 // update plane positions
\r
598 for (i=0; i<num_planes; i++) {
\r
599 planes[i].lon = lon_node->getDoubleValue();
\r
600 planes[i].lat = lat_node->getDoubleValue();
\r
601 planes[i].alt = elev_node->getDoubleValue();
\r
602 planes[i].hdg = hdg_node->getDoubleValue();
\r
603 planes[i].spd = speed_node->getDoubleValue();
\r
605 /*Point3D aircraft = sgGeodToCart( Point3D(planes[i].lon*SGD_DEGREES_TO_RADIANS,
\r
606 planes[i].lat*SGD_DEGREES_TO_RADIANS,
\r
607 planes[i].alt*SG_FEET_TO_METER) );*/
\r
608 double course, distance;
\r
609 calc_gc_course_dist(Point3D(lon*SGD_DEGREES_TO_RADIANS, lat*SGD_DEGREES_TO_RADIANS, 0.0),
\r
610 Point3D(planes[i].lon*SGD_DEGREES_TO_RADIANS,planes[i].lat*SGD_DEGREES_TO_RADIANS, 0.0 ),
\r
611 &course, &distance);
\r
612 planes[i].dist = distance/SG_NM_TO_METER;
\r
613 planes[i].brg = 360.0-course*SGD_RADIANS_TO_DEGREES;
\r
615 //cout << "Plane Id: " << planes[i].ident << " Distance to " << ident
\r
616 // << " is " << planes[i].dist << " miles " << "Bearing " << planes[i].brg << endl;
\r
621 // =======================================================================
\r
622 // Add plane to Approach list
\r
623 // =======================================================================
\r
624 void FGApproach::AddPlane(string pid) {
\r
627 for ( i=0; i<num_planes; i++) {
\r
628 if ( planes[i].ident == pid) {
\r
629 //cout << "Plane already registered: " << ident << " " << num_planes << endl;
\r
633 planes[num_planes].ident = pid;
\r
635 //cout << "Plane added to list: " << ident << " " << num_planes << endl;
\r
639 // ================================================================================
\r
640 // closest distance between a point (h1,d1) and a straigt line (h2,d2,h3) in 2 dim.
\r
641 // ================================================================================
\r
642 double FGApproach::calc_psl_dist(const double &h1, const double &d1,
\r
643 const double &h2, const double &d2,
\r
646 double a1 = h1 * SGD_DEGREES_TO_RADIANS;
\r
647 double a2 = h2 * SGD_DEGREES_TO_RADIANS;
\r
648 double a3 = h3 * SGD_DEGREES_TO_RADIANS;
\r
649 double x1 = cos(a1) * d1;
\r
650 double y1 = sin(a1) * d1;
\r
651 double x2 = cos(a2) * d2;
\r
652 double y2 = sin(a2) * d2;
\r
653 double x3 = cos(a3);
\r
654 double y3 = sin(a3);
\r
656 // formula: dis = sqrt( (v1-v2)**2 - ((v1-v2)*v3)**2 ); vi = (xi,yi)
\r
657 double val1 = (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2);
\r
658 double val2 = ((x1-x2)*x3 + (y1-y2)*y3) * ((x1-x2)*x3 + (y1-y2)*y3);
\r
659 double dis = val1 - val2;
\r
660 // now get sign for offset
\r
661 //cout << x1 << " " << x2 << " " << y1 << " " << y2 << " "
\r
662 // << x3 << " " << y3 << " "
\r
663 // << val1 << " " << val2 << " " << dis << endl;
\r
666 double da = fabs(atan2(y3,x3) - atan2(y1-y2,x1-x2));
\r
667 if ( da > SGD_PI ) da -= 2*SGD_PI;
\r
668 if ( fabs(da) > SGD_PI/2.) {
\r
669 //if ( x3*(x1-x2) < 0.0 && y3*(y1-y2) < 0.0) {
\r
673 //cout << x3 << " " << y3 << endl;
\r
674 double dis1 = x1-x2-x3;
\r
675 double dis2 = y1-y2-y3;
\r
677 da = atan2(dis2,dis1);
\r
678 if ( da < 0.0 ) da += 2*SGD_PI;
\r
679 if ( da < a3 ) dis *= -1.0;
\r
680 //cout << dis1 << " " << dis2 << " " << da*SGD_RADIANS_TO_DEGREES << " " << h3
\r
681 // << " " << sqrt(dis1*dis1 + dis2*dis2) << " " << dis << endl;
\r
682 //cout << atan2(dis2,dis1)*SGD_RADIANS_TO_DEGREES << " " << dis << endl;
\r
688 // ========================================================================
\r
689 // Calculate new bear/dist given starting bear/dis, and offset radial,
\r
691 // ========================================================================
\r
692 void FGApproach::calc_cd_head_dist(const double &h1, const double &d1,
\r
693 const double &course, const double &dist,
\r
694 double *h2, double *d2)
\r
696 double a1 = h1 * SGD_DEGREES_TO_RADIANS;
\r
697 double a2 = course * SGD_DEGREES_TO_RADIANS;
\r
698 double x1 = cos(a1) * d1;
\r
699 double y1 = sin(a1) * d1;
\r
700 double x2 = cos(a2) * dist;
\r
701 double y2 = sin(a2) * dist;
\r
703 *d2 = sqrt((x1+x2)*(x1+x2) + (y1+y2)*(y1+y2));
\r
704 *h2 = atan2( (y1+y2), (x1+x2) ) * SGD_RADIANS_TO_DEGREES;
\r
705 if ( *h2 < 0 ) *h2 = *h2+360;
\r
710 // ========================================================================
\r
711 // get heading and distance between two points; point1 ---> point2
\r
712 // ========================================================================
\r
713 void FGApproach::calc_hd_course_dist(const double &h1, const double &d1,
\r
714 const double &h2, const double &d2,
\r
715 double *course, double *dist)
\r
717 double a1 = h1 * SGD_DEGREES_TO_RADIANS;
\r
718 double a2 = h2 * SGD_DEGREES_TO_RADIANS;
\r
719 double x1 = cos(a1) * d1;
\r
720 double y1 = sin(a1) * d1;
\r
721 double x2 = cos(a2) * d2;
\r
722 double y2 = sin(a2) * d2;
\r
724 *dist = sqrt( (y2-y1)*(y2-y1) + (x2-x1)*(x2-x1) );
\r
725 *course = atan2( (y2-y1), (x2-x1) ) * SGD_RADIANS_TO_DEGREES;
\r
726 if ( *course < 0 ) *course = *course+360;
\r
727 //cout << x1 << " " << y1 << " " << x2 << " " << y2 << " " << *dist << " " << *course << endl;
\r
732 int FGApproach::RemovePlane() {
\r
734 // first check if anything has to be done
\r
735 bool rmplane = false;
\r
738 for (i=0; i<num_planes; i++) {
\r
739 if (planes[i].dist > range*SG_NM_TO_METER) {
\r
744 if (!rmplane) return num_planes;
\r
746 // now make a copy of the plane list
\r
747 PlaneApp tmp[max_planes];
\r
748 for (i=0; i<num_planes; i++) {
\r
749 tmp[i] = planes[i];
\r
753 // now check which planes are still in range
\r
754 for (i=0; i<num_planes; i++) {
\r
755 if (tmp[i].dist <= range*SG_NM_TO_METER) {
\r
756 planes[np] = tmp[i];
\r