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., 675 Mass Ave, Cambridge, MA 02139, USA.
21 #include "approach.hxx"
22 #include "transmission.hxx"
23 #include "transmissionlist.hxx"
24 #include "ATCdisplay.hxx"
25 #include "ATCDialog.hxx"
27 #include <Airports/runways.hxx>
28 #include <simgear/misc/sg_path.hxx>
31 # include <WeatherCM/FGLocalWeatherDatabase.h>
33 # include <Environment/environment_mgr.hxx>
34 # include <Environment/environment.hxx>
41 FGApproach::FGApproach(){
42 comm1_node = fgGetNode("/radios/comm[0]/frequencies/selected-mhz", true);
43 comm2_node = fgGetNode("/radios/comm[1]/frequencies/selected-mhz", true);
46 lon_node = fgGetNode("/position/longitude-deg", true);
47 lat_node = fgGetNode("/position/latitude-deg", true);
48 elev_node = fgGetNode("/position/altitude-ft", true);
49 hdg_node = fgGetNode("/orientation/heading-deg", true);
50 speed_node = fgGetNode("/velocities/airspeed-kt", true);
51 etime_node = fgGetNode("/sim/time/elapsed-ms", true);
56 for ( i=0; i<max_planes; i++) {
57 planes[i].contact = 0;
59 planes[i].dnwp = -999.;
60 planes[i].on_crs = true;
61 planes[i].turn_rate = 10.0;
62 planes[i].desc_rate = 1000.0;
63 planes[i].clmb_rate = 500.0;
67 planes[i].lmc.c3 = -1;
68 planes[i].wp_change = false;
73 FGApproach::~FGApproach(){
76 void FGApproach::Init() {
82 // ============================================================================
83 // the main update function
84 // ============================================================================
85 void FGApproach::Update(double dt) {
87 const int max_trans = 20;
88 FGTransmission tmissions[max_trans];
98 //static string atcmsg1[10];
99 //static string atcmsg2[10];
103 //static bool TransDisplayed = false;
106 if ( active_runway == "" ) get_active_runway();
108 double comm1_freq = comm1_node->getDoubleValue();
110 //bool DisplayTransmissions = true;
112 for (i=0; i<num_planes; i++) {
113 if ( planes[i].ident == "Player") {
115 tpars.station = name;
116 tpars.callsign = "Player";
117 tpars.airport = ident;
119 //cout << "ident = " << ident << " name = " << name << '\n';
122 // is the frequency of the station tuned in?
123 if ( freq == (int)(comm1_freq*100.0 + 0.5) ) {
124 current_transmissionlist->query_station( station, tmissions, max_trans, num_trans );
125 // loop over all transmissions for station
126 for ( j=0; j<=num_trans-1; j++ ) {
127 code = tmissions[j].get_code();
128 // select proper transmissions
129 if ( ( code.c2 == -1 && planes[i].lmc.c3 == 0 ) ||
130 ( code.c1 == 0 && code.c2 == planes[i].lmc.c2 ) ) {
131 mentry = current_transmissionlist->gen_text(station, code, tpars, false);
132 transm = current_transmissionlist->gen_text(station, code, tpars, true);
133 // is the transmission already registered?
134 if (!current_atcdialog->trans_reg( ident, transm )) {
135 current_atcdialog->add_entry( ident, transm, mentry );
143 for ( i=0; i<num_planes; i++ ) {
144 //cout << "TPar.airport = " << TPar.airport << " TPar.station = " << TPar.station << " TPar.callsign = " << TPar.callsign << '\n';
145 //if ( planes[i].ident == TPar.callsign && name == TPar.airport && TPar.station == "approach" ) {
146 //if ( TPar.request && TPar.intention == "landing" && ident == TPar.intid) {
147 if(planes[i].ident == "Player" && fgGetBool("/sim/atc/opt0")) {
148 //cout << "Landing requested\n";
149 fgSetBool("/sim/atc/opt0", false);
151 // ===========================
152 // === calculate waypoints ===
153 // ===========================
156 wpn = planes[i].wpn-1;
157 planes[i].aalt = planes[i].wpts[wpn-1][2];
158 planes[i].ahdg = planes[i].wpts[wpn][4];
160 // generate the message
164 adif = angle_diff_deg( planes[i].hdg, planes[i].ahdg );
165 tpars.station = name;
166 tpars.callsign = "Player";
167 if ( adif < 0 ) tpars.tdir = 1;
169 tpars.heading = planes[i].ahdg;
170 if (planes[i].alt-planes[i].aalt > 100.0) tpars.VDir = 1;
171 else if (planes[i].alt-planes[i].aalt < -100.0) tpars.VDir = 3;
173 tpars.alt = planes[i].aalt;
174 message = current_transmissionlist->gen_text(station, code, tpars, true );
175 cout << message << '\n';
176 globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
177 planes[i].lmc = code;
178 planes[i].tlm = etime_node->getDoubleValue();
179 planes[i].on_crs = true;
180 planes[i].contact = 1;
185 if ( planes[i].contact == 1 ) {
186 // =========================
187 // === update parameters ===
188 // =========================
190 //cout << planes[i].brg << " " << planes[i].dist << " " << planes[i].wpts[wpn+1][0]
191 //<< " " << planes[i].wpts[wpn+1][1] << " " << planes[i].wpts[wpn+1][4]
192 //cout << wpn << " distance to current course = " << planes[i].dcc << endl;
193 //cout << etime_node->getDoubleValue() << endl;
195 // =========================
196 // === reached waypoint? ===
197 // =========================
198 wpn = planes[i].wpn-2;
199 adif = angle_diff_deg( planes[i].hdg, planes[i].wpts[wpn][4] )
200 * SGD_DEGREES_TO_RADIANS;
201 datp = 2*sin(fabs(adif)/2.0)*sin(fabs(adif)/2.0) *
202 planes[i].spd/3600. * planes[i].turn_rate +
203 planes[i].spd/3600. * 3.0;
204 //cout << adif/SGD_DEGREES_TO_RADIANS << " "
205 // << datp << " " << planes[i].dnc << " " << planes[i].dcc <<endl;
206 if ( fabs(planes[i].dnc) < datp ) {
207 //if ( fabs(planes[i].dnc) < 0.3 && planes[i].dnwp < 1.0 ) {
208 //cout << "Reached next waypoint!\n";
210 wpn = planes[i].wpn-1;
211 planes[i].ahdg = planes[i].wpts[wpn][4];
212 planes[i].aalt = planes[i].wpts[wpn-1][2];
213 planes[i].wp_change = true;
215 // generate the message
216 adif = angle_diff_deg( planes[i].hdg, planes[i].ahdg );
217 tpars.station = name;
218 tpars.callsign = "Player";
219 if ( adif < 0 ) tpars.tdir = 1;
221 tpars.heading = planes[i].ahdg;
227 if (planes[i].alt-planes[i].aalt > 100.0) tpars.VDir = 1;
228 else if (planes[i].alt-planes[i].aalt < -100.0) tpars.VDir = 3;
230 tpars.alt = planes[i].aalt;
231 message = current_transmissionlist->gen_text(station, code, tpars, true );
232 //cout << "Approach transmitting...\n";
233 //cout << message << '\n';
234 globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
241 tpars.runway = active_runway;
242 message = current_transmissionlist->gen_text(station, code, tpars, true);
243 //cout << "Approach transmitting 2 ...\n";
244 //cout << message << '\n';
245 globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
247 planes[i].lmc = code;
248 planes[i].tlm = etime_node->getDoubleValue();
249 planes[i].on_crs = true;
254 // =========================
255 // === come off course ? ===
256 // =========================
257 if ( fabs(planes[i].dcc) > 1.0 &&
258 ( !planes[i].wp_change || etime_node->getDoubleValue() - planes[i].tlm > tbm ) ) {
259 //cout << "Off course!\n";
260 if ( planes[i].on_crs ) {
261 if ( planes[i].dcc < 0) {
262 planes[i].ahdg += 30.0;
265 planes[i].ahdg -= 30.0;
267 if (planes[i].ahdg > 360.0) planes[i].ahdg -= 360.0;
268 else if (planes[i].ahdg < 0.0) planes[i].ahdg += 360.0;
270 //cout << planes[i].on_crs << " "
271 // << angle_diff_deg( planes[i].hdg, planes[i].ahdg) << " "
272 // << etime_node->getDoubleValue() << " "
273 // << planes[i].tlm << endl;
274 // generate the message
275 if ( planes[i].on_crs ||
276 ( fabs(angle_diff_deg( planes[i].hdg, planes[i].ahdg )) > 30.0 &&
277 etime_node->getDoubleValue() - planes[i].tlm > tbm) ) {
278 // generate the message
282 adif = angle_diff_deg( planes[i].hdg, planes[i].ahdg );
283 tpars.station = name;
284 tpars.callsign = "Player";
285 tpars.miles = fabs(planes[i].dcc);
286 if ( adif < 0 ) tpars.tdir = 1;
288 tpars.heading = planes[i].ahdg;
289 message = current_transmissionlist->gen_text(station, code, tpars, true);
290 //cout << "Approach transmitting 3 ...\n";
291 //cout << message << '\n';
292 globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
293 planes[i].lmc = code;
294 planes[i].tlm = etime_node->getDoubleValue();
297 planes[i].on_crs = false;
299 else if ( !planes[i].on_crs ) {
300 //cout << "Off course 2!\n";
301 wpn = planes[i].wpn-1;
302 adif = angle_diff_deg( planes[i].hdg, planes[i].wpts[wpn][4] )
303 * SGD_DEGREES_TO_RADIANS;
304 datp = 2*sin(fabs(adif)/2.0)*sin(fabs(adif)/2.0) *
305 planes[i].spd/3600. * planes[i].turn_rate +
306 planes[i].spd/3600. * 3.0;
307 if ( fabs(planes[i].dcc) < datp ) {
308 planes[i].ahdg = fabs(planes[i].wpts[wpn][4]);
310 // generate the message
314 tpars.station = name;
315 tpars.callsign = "Player";
316 if ( adif < 0 ) tpars.tdir = 1;
318 tpars.heading = planes[i].ahdg;
319 message = current_transmissionlist->gen_text(station, code, tpars, true);
320 //cout << "Approach transmitting 4 ...\n";
321 //cout << message << '\n';
322 globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
323 planes[i].lmc = code;
324 planes[i].tlm = etime_node->getDoubleValue();
326 planes[i].on_crs = true;
329 else if ( planes[i].wp_change ) {
330 planes[i].wp_change = false;
333 // ===================================================================
334 // === Less than two minutes away from touchdown? -> Contact Tower ===
335 // ===================================================================
336 if ( planes[i].wpn == 2 && planes[i].dnwp < planes[i].spd/60.*2.0 ) {
338 double freq = 121.95; // Hardwired - FIXME
343 tpars.station = name;
344 tpars.callsign = "Player";
346 message = current_transmissionlist->gen_text(station, code, tpars, true);
347 //cout << "Approach transmitting 5 ...\n";
348 //cout << message << '\n';
349 globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
350 planes[i].lmc = code;
351 planes[i].tlm = etime_node->getDoubleValue();
353 planes[i].contact = 2;
360 // ============================================================================
361 // update course parameters
362 // ============================================================================
363 void FGApproach::update_param( const int &i ) {
367 int wpn = planes[i].wpn-1; // this is the current waypoint
369 planes[i].dcc = calc_psl_dist(planes[i].brg, planes[i].dist,
370 planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],
371 planes[i].wpts[wpn][4]);
372 planes[i].dnc = calc_psl_dist(planes[i].brg, planes[i].dist,
373 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
374 planes[i].wpts[wpn-1][4]);
375 calc_hd_course_dist(planes[i].brg, planes[i].dist,
376 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
382 // ============================================================================
383 // smallest difference between two angles in degree
384 // difference is negative if a1 > a2 and positive if a2 > a1
385 // ===========================================================================
386 double FGApproach::angle_diff_deg( const double &a1, const double &a2) {
389 if (a3 < 180.0) a3 += 360.0;
390 if (a3 > 180.0) a3 -= 360.0;
395 // ============================================================================
396 // calculate waypoints
397 // ============================================================================
398 void FGApproach::calc_wp( const int &i ) {
401 double course, d, cd, a1;
403 int wpn = planes[i].wpn;
404 // waypoint 0: Threshold of active runway
405 calc_gc_course_dist(Point3D(lon*SGD_DEGREES_TO_RADIANS, lat*SGD_DEGREES_TO_RADIANS, 0.0),
406 Point3D(active_rw_lon*SGD_DEGREES_TO_RADIANS,active_rw_lat*SGD_DEGREES_TO_RADIANS, 0.0 ),
408 double d1 = active_rw_hdg+180.0;
409 if ( d1 > 360.0 ) d1 -=360.0;
410 calc_cd_head_dist(360.0-course*SGD_RADIANS_TO_DEGREES, d/SG_NM_TO_METER,
411 d1, active_rw_len/SG_NM_TO_METER/2.0,
412 &planes[i].wpts[wpn][0], &planes[i].wpts[wpn][1]);
413 planes[i].wpts[wpn][2] = elev;
414 planes[i].wpts[wpn][4] = 0.0;
415 planes[i].wpts[wpn][5] = 0.0;
418 // ======================
419 // horizontal navigation
420 // ======================
421 // waypoint 1: point for turning onto final
422 calc_cd_head_dist(planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1], d1, lfl,
423 &planes[i].wpts[wpn][0], &planes[i].wpts[wpn][1]);
424 calc_hd_course_dist(planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],
425 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
427 planes[i].wpts[wpn][4] = course;
428 planes[i].wpts[wpn][5] = d;
431 // calculate course and distance from plane position to waypoint 1
432 calc_hd_course_dist(planes[i].brg, planes[i].dist, planes[i].wpts[1][0],
433 planes[i].wpts[1][1], &course, &d);
434 // check if airport is not between plane and waypoint 1 and
435 // DCA to airport on course to waypoint 1 is larger than 10 miles
437 if ( fabs(angle_diff_deg( planes[i].wpts[1][0], planes[i].brg )) < 90.0 ||
438 calc_psl_dist( zero, zero, planes[i].brg, planes[i].dist, course ) > 10.0 ) {
439 // check if turning angle at waypoint 1 would be > max_ta
440 if ( fabs(angle_diff_deg( planes[i].wpts[1][4], course )) > max_ta ) {
441 cd = calc_psl_dist(planes[i].brg, planes[i].dist,
442 planes[i].wpts[1][0], planes[i].wpts[1][1],
443 planes[i].wpts[1][4]);
444 a1 = atan2(cd,planes[i].wpts[1][1]);
445 planes[i].wpts[wpn][0] = planes[i].wpts[1][0] - a1/SGD_DEGREES_TO_RADIANS;
446 if ( planes[i].wpts[wpn][0] < 0.0) planes[i].wpts[wpn][0] += 360.0;
447 if ( planes[i].wpts[wpn][0] > 360.0) planes[i].wpts[wpn][0] -= 360.0;
448 planes[i].wpts[wpn][1] = fabs(cd) / sin(fabs(a1));
449 calc_hd_course_dist(planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],
450 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
452 planes[i].wpts[wpn][4] = course;
453 planes[i].wpts[wpn][5] = d;
456 calc_hd_course_dist(planes[i].brg, planes[i].dist, planes[i].wpts[wpn-1][0],
457 planes[i].wpts[wpn-1][1], &course, &d);
461 a1 = atan2(planes[i].wpts[1][1], leg );
463 if ( angle_diff_deg(planes[i].brg,planes[i].wpts[1][0]) < 0 )
464 planes[i].wpts[wpn][0] = planes[i].wpts[1][0] + a1/SGD_DEGREES_TO_RADIANS;
465 else planes[i].wpts[wpn][0] = planes[i].wpts[1][0] - a1/SGD_DEGREES_TO_RADIANS;
467 planes[i].wpts[wpn][1] = sqrt( planes[i].wpts[1][1]*planes[i].wpts[1][1] + leg*leg );
468 calc_hd_course_dist(planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],
469 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
471 planes[i].wpts[wpn][4] = course;
472 planes[i].wpts[wpn][5] = d;
475 calc_hd_course_dist(planes[i].brg, planes[i].dist,
476 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
480 planes[i].wpts[wpn][0] = planes[i].brg;
481 planes[i].wpts[wpn][1] = planes[i].dist;
482 planes[i].wpts[wpn][2] = planes[i].alt;
483 planes[i].wpts[wpn][4] = course;
484 planes[i].wpts[wpn][5] = d;
489 // Now check if legs are too short or if legs can be shortend
490 // legs must be at least 2 flight minutes long
491 double mdist = planes[i].spd / 60.0 * 2.0;
492 for ( j=2; j<wpn-1; ++j ) {
493 if ( planes[i].wpts[j][1] < mdist) {
497 // ====================
498 // vertical navigation
499 // ====================
500 double alt = elev+3000.0;
501 planes[i].wpts[1][2] = round_alt( true, alt );
502 for ( j=2; j<wpn-1; ++j ) {
503 double dalt = planes[i].alt - planes[i].wpts[j-1][2];
505 alt = planes[i].wpts[j-1][2] +
506 (planes[i].wpts[j][5] / planes[i].spd) * 60.0 * planes[i].desc_rate;
507 planes[i].wpts[j][2] = round_alt( false, alt );
508 if ( planes[i].wpts[j][2] > planes[i].alt )
509 planes[i].wpts[j][2] = round_alt( false, planes[i].alt );
512 planes[i].wpts[j][2] = planes[i].wpts[1][2];
516 cout << "Plane position: " << planes[i].brg << " " << planes[i].dist << endl;
517 for ( j=0; j<wpn; ++j ) {
518 cout << "Waypoint " << j << endl;
519 cout << "------------------" << endl;
520 cout << planes[i].wpts[j][0] << " " << planes[i].wpts[j][1]
521 << " " << planes[i].wpts[j][2] << " " << planes[i].wpts[j][5];
522 cout << endl << endl;
528 // ============================================================================
529 // round altitude value to next highest/lowest 500 feet
530 // ============================================================================
531 double FGApproach::round_alt( const bool hl, double alt ) {
535 if ( alt > (int)(alt)+0.5 ) alt = ((int)(alt)+1)*1000.0;
536 else alt = ((int)(alt)+0.5)*1000.0;
539 if ( alt > (int)(alt)+0.5 ) alt = ((int)(alt)+0.5)*1000.0;
540 else alt = ((int)(alt))*1000.0;
547 // ============================================================================
549 // ============================================================================
550 void FGApproach::get_active_runway() {
551 //cout << "Entering FGApproach::get_active_runway()\n";
554 sgVec3 position = { lat, lon, elev };
555 FGPhysicalProperty stationweather = WeatherDatabase->get(position);
557 FGEnvironment stationweather =
558 ((FGEnvironmentMgr *)globals->get_subsystem("environment"))
559 ->getEnvironment(lat, lon, elev);
563 //Set the heading to into the wind
564 double wind_x = stationweather.Wind[0];
565 double wind_y = stationweather.Wind[1];
567 double speed = sqrt( wind_x*wind_x + wind_y*wind_y ) * SG_METER_TO_NM / (60.0*60.0);
570 //If no wind use 270degrees
574 // //normalize the wind to get the direction
575 //wind_x /= speed; wind_y /= speed;
577 hdg = - atan2 ( wind_x, wind_y ) * SG_RADIANS_TO_DEGREES ;
582 double hdg = stationweather.get_wind_from_heading_deg();
586 if ( globals->get_runways()->search( ident, int(hdg), &runway) ) {
587 active_runway = runway.rwy_no;
588 active_rw_hdg = runway.heading;
589 active_rw_lon = runway.lon;
590 active_rw_lat = runway.lat;
591 active_rw_len = runway.length;
592 //cout << "Active runway is: " << active_runway << " heading = "
594 // << " lon = " << active_rw_lon
595 // << " lat = " << active_rw_lat <<endl;
597 else cout << "FGRunways search failed\n";
601 // ========================================================================
602 // update infos about plane
603 // ========================================================================
604 void FGApproach::update_plane_dat() {
606 //cout << "Update Approach " << ident << " " << num_planes << " registered" << endl;
607 // update plane positions
609 for (i=0; i<num_planes; i++) {
610 planes[i].lon = lon_node->getDoubleValue();
611 planes[i].lat = lat_node->getDoubleValue();
612 planes[i].alt = elev_node->getDoubleValue();
613 planes[i].hdg = hdg_node->getDoubleValue();
614 planes[i].spd = speed_node->getDoubleValue();
616 /*Point3D aircraft = sgGeodToCart( Point3D(planes[i].lon*SGD_DEGREES_TO_RADIANS,
617 planes[i].lat*SGD_DEGREES_TO_RADIANS,
618 planes[i].alt*SG_FEET_TO_METER) );*/
619 double course, distance;
620 calc_gc_course_dist(Point3D(lon*SGD_DEGREES_TO_RADIANS, lat*SGD_DEGREES_TO_RADIANS, 0.0),
621 Point3D(planes[i].lon*SGD_DEGREES_TO_RADIANS,planes[i].lat*SGD_DEGREES_TO_RADIANS, 0.0 ),
623 planes[i].dist = distance/SG_NM_TO_METER;
624 planes[i].brg = 360.0-course*SGD_RADIANS_TO_DEGREES;
626 //cout << "Plane Id: " << planes[i].ident << " Distance to " << ident
627 // << " is " << planes[i].dist << " miles " << "Bearing " << planes[i].brg << endl;
632 // =======================================================================
633 // Add plane to Approach list
634 // =======================================================================
635 void FGApproach::AddPlane(string pid) {
638 for ( i=0; i<num_planes; i++) {
639 if ( planes[i].ident == pid) {
640 //cout << "Plane already registered: " << planes[i].ident << ' ' << ident << ' ' << num_planes << endl;
644 planes[num_planes].ident = pid;
646 //cout << "Plane added to list: " << ident << " " << num_planes << endl;
650 // ================================================================================
651 // closest distance between a point (h1,d1) and a straigt line (h2,d2,h3) in 2 dim.
652 // ================================================================================
653 double FGApproach::calc_psl_dist(const double &h1, const double &d1,
654 const double &h2, const double &d2,
657 double a1 = h1 * SGD_DEGREES_TO_RADIANS;
658 double a2 = h2 * SGD_DEGREES_TO_RADIANS;
659 double a3 = h3 * SGD_DEGREES_TO_RADIANS;
660 double x1 = cos(a1) * d1;
661 double y1 = sin(a1) * d1;
662 double x2 = cos(a2) * d2;
663 double y2 = sin(a2) * d2;
667 // formula: dis = sqrt( (v1-v2)**2 - ((v1-v2)*v3)**2 ); vi = (xi,yi)
668 double val1 = (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2);
669 double val2 = ((x1-x2)*x3 + (y1-y2)*y3) * ((x1-x2)*x3 + (y1-y2)*y3);
670 double dis = val1 - val2;
671 // now get sign for offset
672 //cout << x1 << " " << x2 << " " << y1 << " " << y2 << " "
673 // << x3 << " " << y3 << " "
674 // << val1 << " " << val2 << " " << dis << endl;
677 double da = fabs(atan2(y3,x3) - atan2(y1-y2,x1-x2));
678 if ( da > SGD_PI ) da -= 2*SGD_PI;
679 if ( fabs(da) > SGD_PI/2.) {
680 //if ( x3*(x1-x2) < 0.0 && y3*(y1-y2) < 0.0) {
684 //cout << x3 << " " << y3 << endl;
685 double dis1 = x1-x2-x3;
686 double dis2 = y1-y2-y3;
688 da = atan2(dis2,dis1);
689 if ( da < 0.0 ) da += 2*SGD_PI;
690 if ( da < a3 ) dis *= -1.0;
691 //cout << dis1 << " " << dis2 << " " << da*SGD_RADIANS_TO_DEGREES << " " << h3
692 // << " " << sqrt(dis1*dis1 + dis2*dis2) << " " << dis << endl;
693 //cout << atan2(dis2,dis1)*SGD_RADIANS_TO_DEGREES << " " << dis << endl;
699 // ========================================================================
700 // Calculate new bear/dist given starting bear/dis, and offset radial,
702 // ========================================================================
703 void FGApproach::calc_cd_head_dist(const double &h1, const double &d1,
704 const double &course, const double &dist,
705 double *h2, double *d2)
707 double a1 = h1 * SGD_DEGREES_TO_RADIANS;
708 double a2 = course * SGD_DEGREES_TO_RADIANS;
709 double x1 = cos(a1) * d1;
710 double y1 = sin(a1) * d1;
711 double x2 = cos(a2) * dist;
712 double y2 = sin(a2) * dist;
714 *d2 = sqrt((x1+x2)*(x1+x2) + (y1+y2)*(y1+y2));
715 *h2 = atan2( (y1+y2), (x1+x2) ) * SGD_RADIANS_TO_DEGREES;
716 if ( *h2 < 0 ) *h2 = *h2+360;
721 // ========================================================================
722 // get heading and distance between two points; point1 ---> point2
723 // ========================================================================
724 void FGApproach::calc_hd_course_dist(const double &h1, const double &d1,
725 const double &h2, const double &d2,
726 double *course, double *dist)
728 double a1 = h1 * SGD_DEGREES_TO_RADIANS;
729 double a2 = h2 * SGD_DEGREES_TO_RADIANS;
730 double x1 = cos(a1) * d1;
731 double y1 = sin(a1) * d1;
732 double x2 = cos(a2) * d2;
733 double y2 = sin(a2) * d2;
735 *dist = sqrt( (y2-y1)*(y2-y1) + (x2-x1)*(x2-x1) );
736 *course = atan2( (y2-y1), (x2-x1) ) * SGD_RADIANS_TO_DEGREES;
737 if ( *course < 0 ) *course = *course+360;
738 //cout << x1 << " " << y1 << " " << x2 << " " << y2 << " " << *dist << " " << *course << endl;
743 int FGApproach::RemovePlane() {
745 // first check if anything has to be done
746 bool rmplane = false;
749 for (i=0; i<num_planes; i++) {
750 if (planes[i].dist > range*SG_NM_TO_METER) {
755 if (!rmplane) return num_planes;
757 // now make a copy of the plane list
758 PlaneApp tmp[max_planes];
759 for (i=0; i<num_planes; i++) {
764 // now check which planes are still in range
765 for (i=0; i<num_planes; i++) {
766 if (tmp[i].dist <= range*SG_NM_TO_METER) {