1 // FGApproach - a class to provide approach control at larger airports.
3 // Written by Alexander Kappes, started March 2002.
5 // Copyright (C) 2002 Alexander Kappes
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.
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.
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.
27 #include "approach.hxx"
28 #include "transmission.hxx"
29 #include "transmissionlist.hxx"
30 #include "ATCDialog.hxx"
32 #include <Airports/runways.hxx>
33 #include <Airports/simple.hxx>
35 #include <simgear/constants.h>
36 #include <simgear/misc/sg_path.hxx>
38 #include <Environment/environment_mgr.hxx>
39 #include <Environment/environment.hxx>
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);
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);
65 for ( i=0; i<max_planes; i++) {
66 planes[i].contact = 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;
76 planes[i].lmc.c3 = -1;
77 planes[i].wp_change = false;
82 FGApproach::~FGApproach(){
85 void FGApproach::Init() {
90 // ============================================================================
91 // the main update function
92 // ============================================================================
93 void FGApproach::Update(double dt) {
95 const int max_trans = 20;
96 FGTransmission tmissions[max_trans];
98 atc_type station = APPROACH;
106 //static string atcmsg1[10];
107 //static string atcmsg2[10];
111 //static bool TransDisplayed = false;
114 if ( active_runway == "" ) get_active_runway();
116 double comm1_freq = comm1_node->getDoubleValue();
118 //bool DisplayTransmissions = true;
120 for (i=0; i<num_planes; i++) {
121 if ( planes[i].ident == "Player") {
123 tpars.station = name;
124 tpars.callsign = "Player";
125 tpars.airport = ident;
127 //cout << "ident = " << ident << " name = " << name << '\n';
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 );
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);
162 // ===========================
163 // === calculate waypoints ===
164 // ===========================
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];
171 // generate the message
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;
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;
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;
196 if ( planes[i].contact == 1 ) {
197 // =========================
198 // === update parameters ===
199 // =========================
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;
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";
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;
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;
232 tpars.heading = planes[i].ahdg;
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;
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);
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);
257 planes[i].lmc = code;
258 planes[i].tlm = etime_node->getDoubleValue();
259 planes[i].on_crs = true;
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;
275 planes[i].ahdg -= 30.0;
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;
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
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;
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();
307 planes[i].on_crs = false;
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]);
320 // generate the message
324 tpars.station = name;
325 tpars.callsign = "Player";
326 if ( adif < 0 ) tpars.tdir = 1;
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();
336 planes[i].on_crs = true;
339 else if ( planes[i].wp_change ) {
340 planes[i].wp_change = false;
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 ) {
348 double freq = 121.95; // Hardwired - FIXME
353 tpars.station = name;
354 tpars.callsign = "Player";
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();
363 planes[i].contact = 2;
370 // ============================================================================
371 // update course parameters
372 // ============================================================================
373 void FGApproach::update_param( const int &i ) {
377 int wpn = planes[i].wpn-1; // this is the current waypoint
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],
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) {
399 if (a3 < 180.0) a3 += 360.0;
400 if (a3 > 180.0) a3 -= 360.0;
405 // ============================================================================
406 // calculate waypoints
407 // ============================================================================
408 void FGApproach::calc_wp( const int &i ) {
411 double course, d, cd, a1, az2;
413 int wpn = planes[i].wpn;
414 // waypoint 0: Threshold of active runway
415 SGGeod activeRunway(SGGeod::fromDeg(active_rw_lon, active_rw_lat));
416 SGGeodesy::inverse(_geod, activeRunway, course, az2, d);
418 double d1 = active_rw_hdg+180.0;
419 if ( d1 > 360.0 ) d1 -=360.0;
420 calc_cd_head_dist(360.0-course, d/SG_NM_TO_METER,
421 d1, active_rw_len/SG_NM_TO_METER/2.0,
422 &planes[i].wpts[wpn][0], &planes[i].wpts[wpn][1]);
423 planes[i].wpts[wpn][2] = _geod.getElevationM();
424 planes[i].wpts[wpn][4] = 0.0;
425 planes[i].wpts[wpn][5] = 0.0;
428 // ======================
429 // horizontal navigation
430 // ======================
431 // waypoint 1: point for turning onto final
432 calc_cd_head_dist(planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1], d1, lfl,
433 &planes[i].wpts[wpn][0], &planes[i].wpts[wpn][1]);
434 calc_hd_course_dist(planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],
435 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
437 planes[i].wpts[wpn][4] = course;
438 planes[i].wpts[wpn][5] = d;
441 // calculate course and distance from plane position to waypoint 1
442 calc_hd_course_dist(planes[i].brg, planes[i].dist, planes[i].wpts[1][0],
443 planes[i].wpts[1][1], &course, &d);
444 // check if airport is not between plane and waypoint 1 and
445 // DCA to airport on course to waypoint 1 is larger than 10 miles
447 if ( fabs(angle_diff_deg( planes[i].wpts[1][0], planes[i].brg )) < 90.0 ||
448 calc_psl_dist( zero, zero, planes[i].brg, planes[i].dist, course ) > 10.0 ) {
449 // check if turning angle at waypoint 1 would be > max_ta
450 if ( fabs(angle_diff_deg( planes[i].wpts[1][4], course )) > max_ta ) {
451 cd = calc_psl_dist(planes[i].brg, planes[i].dist,
452 planes[i].wpts[1][0], planes[i].wpts[1][1],
453 planes[i].wpts[1][4]);
454 a1 = atan2(cd,planes[i].wpts[1][1]);
455 planes[i].wpts[wpn][0] = planes[i].wpts[1][0] - a1/SGD_DEGREES_TO_RADIANS;
456 if ( planes[i].wpts[wpn][0] < 0.0) planes[i].wpts[wpn][0] += 360.0;
457 if ( planes[i].wpts[wpn][0] > 360.0) planes[i].wpts[wpn][0] -= 360.0;
458 planes[i].wpts[wpn][1] = fabs(cd) / sin(fabs(a1));
459 calc_hd_course_dist(planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],
460 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
462 planes[i].wpts[wpn][4] = course;
463 planes[i].wpts[wpn][5] = d;
466 calc_hd_course_dist(planes[i].brg, planes[i].dist, planes[i].wpts[wpn-1][0],
467 planes[i].wpts[wpn-1][1], &course, &d);
471 a1 = atan2(planes[i].wpts[1][1], leg );
473 if ( angle_diff_deg(planes[i].brg,planes[i].wpts[1][0]) < 0 )
474 planes[i].wpts[wpn][0] = planes[i].wpts[1][0] + a1/SGD_DEGREES_TO_RADIANS;
475 else planes[i].wpts[wpn][0] = planes[i].wpts[1][0] - a1/SGD_DEGREES_TO_RADIANS;
477 planes[i].wpts[wpn][1] = sqrt( planes[i].wpts[1][1]*planes[i].wpts[1][1] + leg*leg );
478 calc_hd_course_dist(planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],
479 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
481 planes[i].wpts[wpn][4] = course;
482 planes[i].wpts[wpn][5] = d;
485 calc_hd_course_dist(planes[i].brg, planes[i].dist,
486 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
490 planes[i].wpts[wpn][0] = planes[i].brg;
491 planes[i].wpts[wpn][1] = planes[i].dist;
492 planes[i].wpts[wpn][2] = planes[i].alt;
493 planes[i].wpts[wpn][4] = course;
494 planes[i].wpts[wpn][5] = d;
499 // Now check if legs are too short or if legs can be shortend
500 // legs must be at least 2 flight minutes long
501 double mdist = planes[i].spd / 60.0 * 2.0;
502 for ( j=2; j<wpn-1; ++j ) {
503 if ( planes[i].wpts[j][1] < mdist) {
507 // ====================
508 // vertical navigation
509 // ====================
510 double alt = _geod.getElevationM()+3000.0;
511 planes[i].wpts[1][2] = round_alt( true, alt );
512 for ( j=2; j<wpn-1; ++j ) {
513 double dalt = planes[i].alt - planes[i].wpts[j-1][2];
515 alt = planes[i].wpts[j-1][2] +
516 (planes[i].wpts[j][5] / planes[i].spd) * 60.0 * planes[i].desc_rate;
517 planes[i].wpts[j][2] = round_alt( false, alt );
518 if ( planes[i].wpts[j][2] > planes[i].alt )
519 planes[i].wpts[j][2] = round_alt( false, planes[i].alt );
522 planes[i].wpts[j][2] = planes[i].wpts[1][2];
526 cout << "Plane position: " << planes[i].brg << " " << planes[i].dist << endl;
527 for ( j=0; j<wpn; ++j ) {
528 cout << "Waypoint " << j << endl;
529 cout << "------------------" << endl;
530 cout << planes[i].wpts[j][0] << " " << planes[i].wpts[j][1]
531 << " " << planes[i].wpts[j][2] << " " << planes[i].wpts[j][5];
532 cout << endl << endl;
538 // ============================================================================
539 // round altitude value to next highest/lowest 500 feet
540 // ============================================================================
541 double FGApproach::round_alt( const bool hl, double alt ) {
545 if ( alt > (int)(alt)+0.5 ) alt = ((int)(alt)+1)*1000.0;
546 else alt = ((int)(alt)+0.5)*1000.0;
549 if ( alt > (int)(alt)+0.5 ) alt = ((int)(alt)+0.5)*1000.0;
550 else alt = ((int)(alt))*1000.0;
557 // ============================================================================
559 // ============================================================================
560 void FGApproach::get_active_runway() {
561 //cout << "Entering FGApproach::get_active_runway()\n";
563 const FGAirport* apt = fgFindAirportID(ident);
565 FGRunway* runway = apt->getActiveRunwayForUsage();
567 active_runway = runway->ident();
568 active_rw_hdg = runway->headingDeg();
569 active_rw_lon = runway->longitude();
570 active_rw_lat = runway->latitude();
571 active_rw_len = runway->lengthFt();
574 // ========================================================================
575 // update infos about plane
576 // ========================================================================
577 void FGApproach::update_plane_dat() {
579 //cout << "Update Approach " << ident << " " << num_planes << " registered" << endl;
580 // update plane positions
582 for (i=0; i<num_planes; i++) {
583 planes[i].lon = lon_node->getDoubleValue();
584 planes[i].lat = lat_node->getDoubleValue();
585 planes[i].alt = elev_node->getDoubleValue();
586 planes[i].hdg = hdg_node->getDoubleValue();
587 planes[i].spd = speed_node->getDoubleValue();
589 double course, distance, az2;
590 SGGeod plane(SGGeod::fromDeg(planes[1].lon, active_rw_lat));
591 SGGeodesy::inverse(_geod, plane, course, az2, distance);
592 planes[i].dist = distance * SG_METER_TO_NM;
593 planes[i].brg = 360.0-course;
595 //cout << "Plane Id: " << planes[i].ident << " Distance to " << ident
596 // << " is " << planes[i].dist << " miles " << "Bearing " << planes[i].brg << endl;
601 // =======================================================================
602 // Add plane to Approach list
603 // =======================================================================
604 void FGApproach::AddPlane(const string& pid) {
607 for ( i=0; i<num_planes; i++) {
608 if ( planes[i].ident == pid) {
609 //cout << "Plane already registered: " << planes[i].ident << ' ' << ident << ' ' << num_planes << endl;
613 planes[num_planes].ident = pid;
615 //cout << "Plane added to list: " << ident << " " << num_planes << endl;
619 // ================================================================================
620 // closest distance between a point (h1,d1) and a straigt line (h2,d2,h3) in 2 dim.
621 // ================================================================================
622 double FGApproach::calc_psl_dist(const double &h1, const double &d1,
623 const double &h2, const double &d2,
626 double a1 = h1 * SGD_DEGREES_TO_RADIANS;
627 double a2 = h2 * SGD_DEGREES_TO_RADIANS;
628 double a3 = h3 * SGD_DEGREES_TO_RADIANS;
629 double x1 = cos(a1) * d1;
630 double y1 = sin(a1) * d1;
631 double x2 = cos(a2) * d2;
632 double y2 = sin(a2) * d2;
636 // formula: dis = sqrt( (v1-v2)**2 - ((v1-v2)*v3)**2 ); vi = (xi,yi)
637 double val1 = (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2);
638 double val2 = ((x1-x2)*x3 + (y1-y2)*y3) * ((x1-x2)*x3 + (y1-y2)*y3);
639 double dis = val1 - val2;
640 // now get sign for offset
641 //cout << x1 << " " << x2 << " " << y1 << " " << y2 << " "
642 // << x3 << " " << y3 << " "
643 // << val1 << " " << val2 << " " << dis << endl;
646 double da = fabs(atan2(y3,x3) - atan2(y1-y2,x1-x2));
647 if ( da > SGD_PI ) da -= SGD_2PI;
648 if ( fabs(da) > SGD_PI_2) {
649 //if ( x3*(x1-x2) < 0.0 && y3*(y1-y2) < 0.0) {
653 //cout << x3 << " " << y3 << endl;
654 double dis1 = x1-x2-x3;
655 double dis2 = y1-y2-y3;
657 da = atan2(dis2,dis1);
658 if ( da < 0.0 ) da += SGD_2PI;
659 if ( da < a3 ) dis *= -1.0;
660 //cout << dis1 << " " << dis2 << " " << da*SGD_RADIANS_TO_DEGREES << " " << h3
661 // << " " << sqrt(dis1*dis1 + dis2*dis2) << " " << dis << endl;
662 //cout << atan2(dis2,dis1)*SGD_RADIANS_TO_DEGREES << " " << dis << endl;
668 // ========================================================================
669 // Calculate new bear/dist given starting bear/dis, and offset radial,
671 // ========================================================================
672 void FGApproach::calc_cd_head_dist(const double &h1, const double &d1,
673 const double &course, const double &dist,
674 double *h2, double *d2)
676 double a1 = h1 * SGD_DEGREES_TO_RADIANS;
677 double a2 = course * SGD_DEGREES_TO_RADIANS;
678 double x1 = cos(a1) * d1;
679 double y1 = sin(a1) * d1;
680 double x2 = cos(a2) * dist;
681 double y2 = sin(a2) * dist;
683 *d2 = sqrt((x1+x2)*(x1+x2) + (y1+y2)*(y1+y2));
684 *h2 = atan2( (y1+y2), (x1+x2) ) * SGD_RADIANS_TO_DEGREES;
685 if ( *h2 < 0 ) *h2 = *h2+360;
690 // ========================================================================
691 // get heading and distance between two points; point1 ---> point2
692 // ========================================================================
693 void FGApproach::calc_hd_course_dist(const double &h1, const double &d1,
694 const double &h2, const double &d2,
695 double *course, double *dist)
697 double a1 = h1 * SGD_DEGREES_TO_RADIANS;
698 double a2 = h2 * SGD_DEGREES_TO_RADIANS;
699 double x1 = cos(a1) * d1;
700 double y1 = sin(a1) * d1;
701 double x2 = cos(a2) * d2;
702 double y2 = sin(a2) * d2;
704 *dist = sqrt( (y2-y1)*(y2-y1) + (x2-x1)*(x2-x1) );
705 *course = atan2( (y2-y1), (x2-x1) ) * SGD_RADIANS_TO_DEGREES;
706 if ( *course < 0 ) *course = *course+360;
707 //cout << x1 << " " << y1 << " " << x2 << " " << y2 << " " << *dist << " " << *course << endl;
712 int FGApproach::RemovePlane() {
714 // first check if anything has to be done
715 bool rmplane = false;
718 for (i=0; i<num_planes; i++) {
719 if (planes[i].dist > range*SG_NM_TO_METER) {
724 if (!rmplane) return num_planes;
726 // now make a copy of the plane list
727 PlaneApp tmp[max_planes];
728 for (i=0; i<num_planes; i++) {
733 // now check which planes are still in range
734 for (i=0; i<num_planes; i++) {
735 if (tmp[i].dist <= range*SG_NM_TO_METER) {
746 void FGApproach::set_message(const string &msg)
748 fgSetString("/sim/messages/approach", msg.c_str());