-// FGApproach - a class to provide approach control at larger airports.\r
-//\r
-// Written by Alexander Kappes, started March 2002.\r
-//\r
-// Copyright (C) 2002 Alexander Kappes\r
-//\r
-// This program is free software; you can redistribute it and/or\r
-// modify it under the terms of the GNU General Public License as\r
-// published by the Free Software Foundation; either version 2 of the\r
-// License, or (at your option) any later version.\r
-//\r
-// This program is distributed in the hope that it will be useful, but\r
-// WITHOUT ANY WARRANTY; without even the implied warranty of\r
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU\r
-// General Public License for more details.\r
-//\r
-// You should have received a copy of the GNU General Public License\r
-// along with this program; if not, write to the Free Software\r
-// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.\r
-\r
-#include "approach.hxx"\r
-#include "transmission.hxx"\r
-#include "transmissionlist.hxx"\r
-#include "ATCdisplay.hxx"\r
-#include "ATCDialog.hxx"\r
-\r
-#include <Airports/runways.hxx>\r
-#include <simgear/misc/sg_path.hxx>\r
-\r
-#ifdef FG_WEATHERCM\r
-# include <WeatherCM/FGLocalWeatherDatabase.h>\r
-#else\r
-# include <Environment/environment_mgr.hxx>\r
-# include <Environment/environment.hxx>\r
-#endif\r
-\r
-\r
-#include <GUI/gui.h>\r
-\r
-//Constructor\r
-FGApproach::FGApproach(){\r
- comm1_node = fgGetNode("/radios/comm[0]/frequencies/selected-mhz", true);\r
- comm2_node = fgGetNode("/radios/comm[1]/frequencies/selected-mhz", true);\r
-\r
- num_planes = 0;\r
- lon_node = fgGetNode("/position/longitude-deg", true);\r
- lat_node = fgGetNode("/position/latitude-deg", true);\r
- elev_node = fgGetNode("/position/altitude-ft", true);\r
- hdg_node = fgGetNode("/orientation/heading-deg", true);\r
- speed_node = fgGetNode("/velocities/airspeed-kt", true);\r
- etime_node = fgGetNode("/sim/time/elapsed-ms", true);\r
-\r
- first = true;\r
- active_runway = "";\r
- int i;\r
- for ( i=0; i<max_planes; i++) {\r
- planes[i].contact = 0;\r
- planes[i].wpn = 0;\r
- planes[i].dnwp = -999.;\r
- planes[i].on_crs = true;\r
- planes[i].turn_rate = 10.0;\r
- planes[i].desc_rate = 1000.0;\r
- planes[i].clmb_rate = 500.0;\r
- planes[i].tlm = 0.0;\r
- planes[i].lmc.c1 = 0;\r
- planes[i].lmc.c2 = 0;\r
- planes[i].lmc.c3 = -1;\r
- planes[i].wp_change = false;\r
- }\r
-}\r
-\r
-//Destructor\r
-FGApproach::~FGApproach(){\r
-}\r
-\r
-void FGApproach::Init() {\r
- display = false;\r
-}\r
-\r
-\r
-\r
-// ============================================================================\r
-// the main update function\r
-// ============================================================================\r
-void FGApproach::Update() {\r
- \r
- const int max_trans = 20;\r
- FGTransmission tmissions[max_trans];\r
- int wpn;\r
- int station = 1;\r
- TransCode code;\r
- TransPar TPar;\r
- int i,j;\r
- //double course, d, \r
- double adif, datp;\r
- //char buf[10];\r
- string message;\r
- //static string atcmsg1[10];\r
- //static string atcmsg2[10];\r
- string mentry;\r
- string transm;\r
- TransPar tpars;\r
- //static bool TransDisplayed = false;\r
- \r
- update_plane_dat();\r
- if ( active_runway == "" ) get_active_runway();\r
- \r
- double comm1_freq = comm1_node->getDoubleValue();\r
- \r
- //bool DisplayTransmissions = true;\r
- \r
- for (i=0; i<num_planes; i++) {\r
- if ( planes[i].ident == "Player") { \r
- station = 1;\r
- tpars.station = name;\r
- tpars.callsign = "Player";\r
- tpars.airport = ident;\r
- \r
- int num_trans = 0;\r
- // is the frequency of the station tuned in?\r
- if ( freq == (int)(comm1_freq*100.0 + 0.5) ) {\r
- current_transmissionlist->query_station( station, tmissions, max_trans, num_trans );\r
- // loop over all transmissions for station\r
- for ( j=0; j<=num_trans-1; j++ ) {\r
- code = tmissions[j].get_code();\r
- // select proper transmissions\r
- if ( ( code.c2 == -1 && planes[i].lmc.c3 == 0 ) || \r
- ( code.c1 == 0 && code.c2 == planes[i].lmc.c2 ) ) {\r
- mentry = current_transmissionlist->gen_text(station, code, tpars, false);\r
- transm = current_transmissionlist->gen_text(station, code, tpars, true);\r
- // is the transmission already registered?\r
- if (!current_atcdialog->trans_reg( ident, transm )) {\r
- current_atcdialog->add_entry( ident, transm, mentry );\r
- }\r
- }\r
- }\r
- }\r
- }\r
- }\r
- \r
- for ( i=0; i<num_planes; i++ ) {\r
- \r
- if ( planes[i].ident == TPar.callsign && name == TPar.airport && TPar.station == "approach" ) {\r
- \r
- if ( TPar.request && TPar.intention == "landing" && ident == TPar.intid) {\r
- planes[i].wpn = 0; \r
- // ===========================\r
- // === calculate waypoints ===\r
- // ===========================\r
- calc_wp( i ); \r
- update_param( i );\r
- wpn = planes[i].wpn-1;\r
- planes[i].aalt = planes[i].wpts[wpn-1][2];\r
- planes[i].ahdg = planes[i].wpts[wpn][4];\r
- \r
- // generate the message\r
- code.c1 = 1;\r
- code.c2 = 1;\r
- code.c3 = 0;\r
- adif = angle_diff_deg( planes[i].hdg, planes[i].ahdg );\r
- tpars.station = name;\r
- tpars.callsign = "Player";\r
- if ( adif < 0 ) tpars.tdir = 1;\r
- else tpars.tdir = 2;\r
- tpars.heading = planes[i].ahdg;\r
- if (planes[i].alt-planes[i].aalt > 100.0) tpars.VDir = 1;\r
- else if (planes[i].alt-planes[i].aalt < -100.0) tpars.VDir = 3;\r
- else tpars.VDir = 2;\r
- tpars.alt = planes[i].aalt;\r
- message = current_transmissionlist->gen_text(station, code, tpars, true );\r
- globals->get_ATC_display()->RegisterSingleMessage( message, 0 );\r
- planes[i].lmc = code;\r
- planes[i].tlm = etime_node->getDoubleValue();\r
- planes[i].on_crs = true;\r
- planes[i].contact = 1;\r
- }\r
- }\r
- \r
- if ( planes[i].contact == 1 ) {\r
- // =========================\r
- // === update parameters ===\r
- // =========================\r
- update_param( i );\r
- //cout << planes[i].brg << " " << planes[i].dist << " " << planes[i].wpts[wpn+1][0] \r
- //<< " " << planes[i].wpts[wpn+1][1] << " " << planes[i].wpts[wpn+1][4] \r
- //cout << wpn << " distance to current course = " << planes[i].dcc << endl;\r
- //cout << etime_node->getDoubleValue() << endl;\r
- \r
- // =========================\r
- // === reached waypoint? ===\r
- // =========================\r
- wpn = planes[i].wpn-2;\r
- adif = angle_diff_deg( planes[i].hdg, planes[i].wpts[wpn][4] ) \r
- * SGD_DEGREES_TO_RADIANS;\r
- datp = 2*sin(fabs(adif)/2.0)*sin(fabs(adif)/2.0) *\r
- planes[i].spd/3600. * planes[i].turn_rate + \r
- planes[i].spd/3600. * 3.0;\r
- //cout << adif/SGD_DEGREES_TO_RADIANS << " " \r
- // << datp << " " << planes[i].dnc << " " << planes[i].dcc <<endl;\r
- if ( fabs(planes[i].dnc) < datp ) {\r
- //if ( fabs(planes[i].dnc) < 0.3 && planes[i].dnwp < 1.0 ) {\r
- planes[i].wpn -= 1;\r
- wpn = planes[i].wpn-1;\r
- planes[i].ahdg = planes[i].wpts[wpn][4];\r
- planes[i].aalt = planes[i].wpts[wpn-1][2];\r
- planes[i].wp_change = true;\r
- \r
- // generate the message\r
- adif = angle_diff_deg( planes[i].hdg, planes[i].ahdg );\r
- tpars.station = name;\r
- tpars.callsign = "Player";\r
- if ( adif < 0 ) tpars.tdir = 1;\r
- else tpars.tdir = 2;\r
- tpars.heading = planes[i].ahdg;\r
- \r
- if ( wpn-1 != 0) { \r
- code.c1 = 1;\r
- code.c2 = 1;\r
- code.c3 = 0;\r
- if (planes[i].alt-planes[i].aalt > 100.0) tpars.VDir = 1;\r
- else if (planes[i].alt-planes[i].aalt < -100.0) tpars.VDir = 3;\r
- else tpars.VDir = 2;\r
- tpars.alt = planes[i].aalt;\r
- message = current_transmissionlist->gen_text(station, code, tpars, true );\r
- globals->get_ATC_display()->RegisterSingleMessage( message, 0 );\r
- \r
- }\r
- else {\r
- code.c1 = 1;\r
- code.c2 = 3;\r
- code.c3 = 0;\r
- tpars.runway = active_runway;\r
- message = current_transmissionlist->gen_text(station, code, tpars, true);\r
- globals->get_ATC_display()->RegisterSingleMessage( message, 0 );\r
- }\r
- planes[i].lmc = code;\r
- planes[i].tlm = etime_node->getDoubleValue();\r
- planes[i].on_crs = true;\r
- \r
- update_param( i );\r
- }\r
- \r
- // =========================\r
- // === come off course ? ===\r
- // =========================\r
- if ( fabs(planes[i].dcc) > 1.0 && \r
- ( !planes[i].wp_change || \r
- etime_node->getDoubleValue() - planes[i].tlm > tbm ) ) {\r
- if ( planes[i].on_crs ) {\r
- if ( planes[i].dcc < 0) {\r
- planes[i].ahdg += 30.0;\r
- }\r
- else {\r
- planes[i].ahdg -= 30.0;\r
- }\r
- if (planes[i].ahdg > 360.0) planes[i].ahdg -= 360.0;\r
- else if (planes[i].ahdg < 0.0) planes[i].ahdg += 360.0;\r
- }\r
- //cout << planes[i].on_crs << " " \r
- // << angle_diff_deg( planes[i].hdg, planes[i].ahdg) << " "\r
- // << etime_node->getDoubleValue() << " "\r
- // << planes[i].tlm << endl;\r
- // generate the message\r
- if ( planes[i].on_crs || \r
- ( fabs(angle_diff_deg( planes[i].hdg, planes[i].ahdg )) > 30.0 && \r
- etime_node->getDoubleValue() - planes[i].tlm > tbm) ) {\r
- // generate the message\r
- code.c1 = 1;\r
- code.c2 = 4;\r
- code.c3 = 0;\r
- adif = angle_diff_deg( planes[i].hdg, planes[i].ahdg );\r
- tpars.station = name;\r
- tpars.callsign = "Player";\r
- tpars.miles = fabs(planes[i].dcc);\r
- if ( adif < 0 ) tpars.tdir = 1;\r
- else tpars.tdir = 2;\r
- tpars.heading = planes[i].ahdg;\r
- message = current_transmissionlist->gen_text(station, code, tpars, true);\r
- globals->get_ATC_display()->RegisterSingleMessage( message, 0 );\r
- planes[i].lmc = code;\r
- planes[i].tlm = etime_node->getDoubleValue();\r
- }\r
- \r
- planes[i].on_crs = false;\r
- }\r
- else if ( !planes[i].on_crs ) {\r
- wpn = planes[i].wpn-1;\r
- adif = angle_diff_deg( planes[i].hdg, planes[i].wpts[wpn][4] ) \r
- * SGD_DEGREES_TO_RADIANS;\r
- datp = 2*sin(fabs(adif)/2.0)*sin(fabs(adif)/2.0) *\r
- planes[i].spd/3600. * planes[i].turn_rate + \r
- planes[i].spd/3600. * 3.0;\r
- if ( fabs(planes[i].dcc) < datp ) { \r
- planes[i].ahdg = fabs(planes[i].wpts[wpn][4]);\r
- \r
- // generate the message\r
- code.c1 = 1;\r
- code.c2 = 2;\r
- code.c3 = 0;\r
- tpars.station = name;\r
- tpars.callsign = "Player";\r
- if ( adif < 0 ) tpars.tdir = 1;\r
- else tpars.tdir = 2;\r
- tpars.heading = planes[i].ahdg;\r
- message = current_transmissionlist->gen_text(station, code, tpars, true);\r
- globals->get_ATC_display()->RegisterSingleMessage( message, 0 );\r
- planes[i].lmc = code;\r
- planes[i].tlm = etime_node->getDoubleValue();\r
- \r
- planes[i].on_crs = true; \r
- } \r
- }\r
- else if ( planes[i].wp_change ) {\r
- planes[i].wp_change = false;\r
- }\r
- \r
- // ===================================================================\r
- // === Less than two minutes away from touchdown? -> Contact Tower ===\r
- // ===================================================================\r
- if ( planes[i].wpn == 2 && planes[i].dnwp < planes[i].spd/60.*2.0 ) {\r
- \r
- double freq = 121.95;\r
- // generate message\r
- code.c1 = 1;\r
- code.c2 = 5;\r
- code.c3 = 0;\r
- tpars.station = name;\r
- tpars.callsign = "Player";\r
- tpars.freq = freq;\r
- message = current_transmissionlist->gen_text(station, code, tpars, true);\r
- globals->get_ATC_display()->RegisterSingleMessage( message, 0 );\r
- planes[i].lmc = code;\r
- planes[i].tlm = etime_node->getDoubleValue();\r
- \r
- planes[i].contact = 2;\r
- }\r
- }\r
- }\r
- \r
-}\r
-\r
-\r
-// ============================================================================\r
-// update course parameters\r
-// ============================================================================\r
-void FGApproach::update_param( const int &i ) {\r
- \r
- double course, d;\r
-\r
- int wpn = planes[i].wpn-1; // this is the current waypoint\r
-\r
- planes[i].dcc = calc_psl_dist(planes[i].brg, planes[i].dist,\r
- planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],\r
- planes[i].wpts[wpn][4]);\r
- planes[i].dnc = calc_psl_dist(planes[i].brg, planes[i].dist,\r
- planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],\r
- planes[i].wpts[wpn-1][4]);\r
- calc_hd_course_dist(planes[i].brg, planes[i].dist, \r
- planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],\r
- &course, &d);\r
- planes[i].dnwp = d;\r
-\r
-}\r
-\r
-// ============================================================================\r
-// smallest difference between two angles in degree\r
-// difference is negative if a1 > a2 and positive if a2 > a1\r
-// ===========================================================================\r
-double FGApproach::angle_diff_deg( const double &a1, const double &a2) {\r
- \r
- double a3 = a2 - a1;\r
- if (a3 < 180.0) a3 += 360.0;\r
- if (a3 > 180.0) a3 -= 360.0;\r
-\r
- return a3;\r
-}\r
-\r
-// ============================================================================\r
-// calculate waypoints\r
-// ============================================================================\r
-void FGApproach::calc_wp( const int &i ) {\r
- \r
- int j;\r
- double course, d, cd, a1;\r
- \r
- int wpn = planes[i].wpn;\r
- // waypoint 0: Threshold of active runway\r
- calc_gc_course_dist(Point3D(lon*SGD_DEGREES_TO_RADIANS, lat*SGD_DEGREES_TO_RADIANS, 0.0),\r
- Point3D(active_rw_lon*SGD_DEGREES_TO_RADIANS,active_rw_lat*SGD_DEGREES_TO_RADIANS, 0.0 ),\r
- &course, &d);\r
- double d1 = active_rw_hdg+180.0;\r
- if ( d1 > 360.0 ) d1 -=360.0;\r
- calc_cd_head_dist(360.0-course*SGD_RADIANS_TO_DEGREES, d/SG_NM_TO_METER, \r
- d1, active_rw_len/SG_NM_TO_METER/2., \r
- &planes[i].wpts[wpn][0], &planes[i].wpts[wpn][1]);\r
- planes[i].wpts[wpn][2] = elev;\r
- planes[i].wpts[wpn][4] = 0.0;\r
- planes[i].wpts[wpn][5] = 0.0;\r
- wpn += 1;\r
- \r
- // ======================\r
- // horizontal navigation\r
- // ======================\r
- // waypoint 1: point for turning onto final\r
- calc_cd_head_dist(planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1] , \r
- d1, lfl,\r
- &planes[i].wpts[wpn][0], &planes[i].wpts[wpn][1]);\r
- calc_hd_course_dist(planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],\r
- planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],\r
- &course, &d);\r
- planes[i].wpts[wpn][4] = course;\r
- planes[i].wpts[wpn][5] = d;\r
- wpn += 1;\r
- \r
- // calculate course and distance from plane position to waypoint 1\r
- calc_hd_course_dist(planes[i].brg, planes[i].dist,\r
- planes[i].wpts[1][0], planes[i].wpts[1][1],\r
- &course, &d);\r
- // check if airport is not between plane and waypoint 1 and\r
- // DCA to airport on course to waypoint 1 is larger than 10 miles\r
- double zero = 0.0;\r
- if ( fabs(angle_diff_deg( planes[i].wpts[1][0], planes[i].brg )) < 90.0 ||\r
- calc_psl_dist( zero, zero, planes[i].brg, planes[i].dist, course ) > 10.0 ) {\r
- // check if turning angle at waypoint 1 would be > max_ta\r
- if ( fabs(angle_diff_deg( planes[i].wpts[1][4], course )) > max_ta ) {\r
- cd = calc_psl_dist(planes[i].brg, planes[i].dist,\r
- planes[i].wpts[1][0], planes[i].wpts[1][1],\r
- planes[i].wpts[1][4]);\r
- a1 = atan2(cd,planes[i].wpts[1][1]);\r
- planes[i].wpts[wpn][0] = planes[i].wpts[1][0] - a1/SGD_DEGREES_TO_RADIANS;\r
- if ( planes[i].wpts[wpn][0] < 0.0) planes[i].wpts[wpn][0] += 360.0; \r
- if ( planes[i].wpts[wpn][0] > 360.0) planes[i].wpts[wpn][0] -= 360.0; \r
- planes[i].wpts[wpn][1] = fabs(cd) / sin(fabs(a1));\r
- calc_hd_course_dist(planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],\r
- planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],\r
- &course, &d);\r
- planes[i].wpts[wpn][4] = course;\r
- planes[i].wpts[wpn][5] = d;\r
- wpn += 1;\r
- \r
- calc_hd_course_dist(planes[i].brg, planes[i].dist,\r
- planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],\r
- &course, &d);\r
- }\r
- } else {\r
- double leg = 10.0;\r
- a1 = atan2(planes[i].wpts[1][1], leg );\r
- \r
- if ( angle_diff_deg(planes[i].brg,planes[i].wpts[1][0]) < 0 ) \r
- planes[i].wpts[wpn][0] = planes[i].wpts[1][0] + a1/SGD_DEGREES_TO_RADIANS;\r
- else planes[i].wpts[wpn][0] = planes[i].wpts[1][0] - a1/SGD_DEGREES_TO_RADIANS;\r
- \r
- planes[i].wpts[wpn][1] = sqrt( planes[i].wpts[1][1]*planes[i].wpts[1][1] + leg*leg );\r
- calc_hd_course_dist(planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],\r
- planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],\r
- &course, &d);\r
- planes[i].wpts[wpn][4] = course;\r
- planes[i].wpts[wpn][5] = d;\r
- wpn += 1;\r
- \r
- calc_hd_course_dist(planes[i].brg, planes[i].dist,\r
- planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],\r
- &course, &d);\r
- }\r
- \r
- planes[i].wpts[wpn][0] = planes[i].brg;\r
- planes[i].wpts[wpn][1] = planes[i].dist;\r
- planes[i].wpts[wpn][2] = planes[i].alt;\r
- planes[i].wpts[wpn][4] = course;\r
- planes[i].wpts[wpn][5] = d;\r
- wpn += 1;\r
- \r
- planes[i].wpn = wpn;\r
- \r
- // Now check if legs are too short or if legs can be shortend\r
- // legs must be at least 2 flight minutes long\r
- double mdist = planes[i].spd / 60.0 * 2.0;\r
- for ( j=2; j<wpn-1; ++j ) {\r
- if ( planes[i].wpts[j][1] < mdist) {\r
- }\r
- }\r
- \r
- // ====================\r
- // vertical navigation\r
- // ====================\r
- double alt = elev+3000.0;\r
- planes[i].wpts[1][2] = round_alt( true, alt );\r
- for ( j=2; j<wpn-1; ++j ) {\r
- double dalt = planes[i].alt - planes[i].wpts[j-1][2];\r
- if ( dalt > 0 ) {\r
- alt = planes[i].wpts[j-1][2] + \r
- (planes[i].wpts[j][5] / planes[i].spd) * 60.0 * planes[i].desc_rate;\r
- planes[i].wpts[j][2] = round_alt( false, alt );\r
- if ( planes[i].wpts[j][2] > planes[i].alt ) \r
- planes[i].wpts[j][2] = round_alt( false, planes[i].alt );\r
- }\r
- else {\r
- planes[i].wpts[j][2] = planes[i].wpts[1][2];\r
- }\r
- }\r
- \r
- cout << "Plane position: " << planes[i].brg << " " << planes[i].dist << endl;\r
- for ( j=0; j<wpn; ++j ) {\r
- cout << "Waypoint " << j << endl;\r
- cout << "------------------" << endl;\r
- cout << planes[i].wpts[j][0] << " " << planes[i].wpts[j][1]\r
- << " " << planes[i].wpts[j][2] << " " << planes[i].wpts[j][5]; \r
- cout << endl << endl;\r
- }\r
- \r
-}\r
-\r
-\r
-// ============================================================================\r
-// round altitude value to next highest/lowest 500 feet\r
-// ============================================================================\r
-double FGApproach::round_alt( const bool hl, double alt ) {\r
-\r
- alt = alt/1000.0;\r
- if ( hl ) {\r
- if ( alt > (int)(alt)+0.5 ) alt = ((int)(alt)+1)*1000.0;\r
- else alt = ((int)(alt)+0.5)*1000.0;\r
- }\r
- else {\r
- if ( alt > (int)(alt)+0.5 ) alt = ((int)(alt)+0.5)*1000.0;\r
- else alt = ((int)(alt))*1000.0;\r
- }\r
- \r
- return alt;\r
-}\r
-\r
-\r
-// ============================================================================\r
-// get active runway\r
-// ============================================================================\r
-void FGApproach::get_active_runway() {\r
-\r
-#ifdef FG_WEATHERCM\r
- sgVec3 position = { lat, lon, elev };\r
- FGPhysicalProperty stationweather = WeatherDatabase->get(position);\r
-#else\r
- FGEnvironment stationweather =\r
- globals->get_environment_mgr()->getEnvironment(lat, lon, elev);\r
-#endif\r
-\r
- SGPath path( globals->get_fg_root() );\r
- path.append( "Airports" );\r
- path.append( "runways.mk4" );\r
- FGRunways runways( path.c_str() );\r
- \r
-#ifdef FG_WEATHERCM\r
- //Set the heading to into the wind\r
- double wind_x = stationweather.Wind[0];\r
- double wind_y = stationweather.Wind[1];\r
- \r
- double speed = sqrt( wind_x*wind_x + wind_y*wind_y ) * SG_METER_TO_NM / (60.0*60.0);\r
- double hdg;\r
- \r
- //If no wind use 270degrees\r
- if(speed == 0) {\r
- hdg = 270;\r
- } else {\r
- // //normalize the wind to get the direction\r
- //wind_x /= speed; wind_y /= speed;\r
- \r
- hdg = - atan2 ( wind_x, wind_y ) * SG_RADIANS_TO_DEGREES ;\r
- if (hdg < 0.0)\r
- hdg += 360.0;\r
- }\r
-#else\r
- double hdg = stationweather.get_wind_from_heading_deg();\r
-#endif\r
- \r
- FGRunway runway;\r
- if ( runways.search( ident, int(hdg), &runway) ) {\r
- active_runway = runway.rwy_no;\r
- active_rw_hdg = runway.heading;\r
- active_rw_lon = runway.lon;\r
- active_rw_lat = runway.lat;\r
- active_rw_len = runway.length;\r
- //cout << "Active runway is: " << active_runway << " heading = " \r
- // << active_rw_hdg \r
- // << " lon = " << active_rw_lon \r
- // << " lat = " << active_rw_lat <<endl;\r
- }\r
- else cout << "FGRunways search failed\n";\r
-\r
-}\r
-\r
-// ========================================================================\r
-// update infos about plane\r
-// ========================================================================\r
-void FGApproach::update_plane_dat() {\r
- \r
- //cout << "Update Approach " << ident << " " << num_planes << " registered" << endl;\r
- // update plane positions\r
- int i;\r
- for (i=0; i<num_planes; i++) {\r
- planes[i].lon = lon_node->getDoubleValue();\r
- planes[i].lat = lat_node->getDoubleValue();\r
- planes[i].alt = elev_node->getDoubleValue();\r
- planes[i].hdg = hdg_node->getDoubleValue();\r
- planes[i].spd = speed_node->getDoubleValue();\r
-\r
- /*Point3D aircraft = sgGeodToCart( Point3D(planes[i].lon*SGD_DEGREES_TO_RADIANS, \r
- planes[i].lat*SGD_DEGREES_TO_RADIANS, \r
- planes[i].alt*SG_FEET_TO_METER) );*/\r
- double course, distance;\r
- calc_gc_course_dist(Point3D(lon*SGD_DEGREES_TO_RADIANS, lat*SGD_DEGREES_TO_RADIANS, 0.0),\r
- Point3D(planes[i].lon*SGD_DEGREES_TO_RADIANS,planes[i].lat*SGD_DEGREES_TO_RADIANS, 0.0 ),\r
- &course, &distance);\r
- planes[i].dist = distance/SG_NM_TO_METER;\r
- planes[i].brg = 360.0-course*SGD_RADIANS_TO_DEGREES;\r
-\r
- //cout << "Plane Id: " << planes[i].ident << " Distance to " << ident \r
- // << " is " << planes[i].dist << " miles " << "Bearing " << planes[i].brg << endl;\r
- \r
- } \r
-}\r
-\r
-// =======================================================================\r
-// Add plane to Approach list\r
-// =======================================================================\r
-void FGApproach::AddPlane(string pid) {\r
-\r
- int i;\r
- for ( i=0; i<num_planes; i++) {\r
- if ( planes[i].ident == pid) {\r
- //cout << "Plane already registered: " << ident << " " << num_planes << endl;\r
- return;\r
- }\r
- }\r
- planes[num_planes].ident = pid;\r
- ++num_planes;\r
- //cout << "Plane added to list: " << ident << " " << num_planes << endl;\r
- return;\r
-}\r
-\r
-// ================================================================================\r
-// closest distance between a point (h1,d1) and a straigt line (h2,d2,h3) in 2 dim.\r
-// ================================================================================\r
-double FGApproach::calc_psl_dist(const double &h1, const double &d1,\r
- const double &h2, const double &d2,\r
- const double &h3)\r
-{\r
- double a1 = h1 * SGD_DEGREES_TO_RADIANS;\r
- double a2 = h2 * SGD_DEGREES_TO_RADIANS;\r
- double a3 = h3 * SGD_DEGREES_TO_RADIANS;\r
- double x1 = cos(a1) * d1;\r
- double y1 = sin(a1) * d1;\r
- double x2 = cos(a2) * d2;\r
- double y2 = sin(a2) * d2;\r
- double x3 = cos(a3);\r
- double y3 = sin(a3);\r
- \r
- // formula: dis = sqrt( (v1-v2)**2 - ((v1-v2)*v3)**2 ); vi = (xi,yi)\r
- double val1 = (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2);\r
- double val2 = ((x1-x2)*x3 + (y1-y2)*y3) * ((x1-x2)*x3 + (y1-y2)*y3);\r
- double dis = val1 - val2;\r
- // now get sign for offset \r
- //cout << x1 << " " << x2 << " " << y1 << " " << y2 << " " \r
- // << x3 << " " << y3 << " " \r
- // << val1 << " " << val2 << " " << dis << endl;\r
- x3 *= sqrt(val2);\r
- y3 *= sqrt(val2);\r
- double da = fabs(atan2(y3,x3) - atan2(y1-y2,x1-x2));\r
- if ( da > SGD_PI ) da -= 2*SGD_PI;\r
- if ( fabs(da) > SGD_PI/2.) {\r
- //if ( x3*(x1-x2) < 0.0 && y3*(y1-y2) < 0.0) {\r
- x3 *= -1.0;\r
- y3 *= -1.0;\r
- }\r
- //cout << x3 << " " << y3 << endl;\r
- double dis1 = x1-x2-x3;\r
- double dis2 = y1-y2-y3;\r
- dis = sqrt(dis);\r
- da = atan2(dis2,dis1);\r
- if ( da < 0.0 ) da += 2*SGD_PI;\r
- if ( da < a3 ) dis *= -1.0;\r
- //cout << dis1 << " " << dis2 << " " << da*SGD_RADIANS_TO_DEGREES << " " << h3\r
- // << " " << sqrt(dis1*dis1 + dis2*dis2) << " " << dis << endl;\r
- //cout << atan2(dis2,dis1)*SGD_RADIANS_TO_DEGREES << " " << dis << endl;\r
-\r
- return dis;\r
-}\r
-\r
-\r
-// ========================================================================\r
-// Calculate new bear/dist given starting bear/dis, and offset radial,\r
-// and distance.\r
-// ========================================================================\r
-void FGApproach::calc_cd_head_dist(const double &h1, const double &d1, \r
- const double &course, const double &dist,\r
- double *h2, double *d2)\r
-{\r
- double a1 = h1 * SGD_DEGREES_TO_RADIANS;\r
- double a2 = course * SGD_DEGREES_TO_RADIANS;\r
- double x1 = cos(a1) * d1;\r
- double y1 = sin(a1) * d1;\r
- double x2 = cos(a2) * dist;\r
- double y2 = sin(a2) * dist;\r
- \r
- *d2 = sqrt((x1+x2)*(x1+x2) + (y1+y2)*(y1+y2));\r
- *h2 = atan2( (y1+y2), (x1+x2) ) * SGD_RADIANS_TO_DEGREES;\r
- if ( *h2 < 0 ) *h2 = *h2+360;\r
- }\r
-\r
-\r
-\r
-// ========================================================================\r
-// get heading and distance between two points; point1 ---> point2\r
-// ========================================================================\r
-void FGApproach::calc_hd_course_dist(const double &h1, const double &d1, \r
- const double &h2, const double &d2,\r
- double *course, double *dist)\r
-{\r
- double a1 = h1 * SGD_DEGREES_TO_RADIANS;\r
- double a2 = h2 * SGD_DEGREES_TO_RADIANS;\r
- double x1 = cos(a1) * d1;\r
- double y1 = sin(a1) * d1;\r
- double x2 = cos(a2) * d2;\r
- double y2 = sin(a2) * d2;\r
- \r
- *dist = sqrt( (y2-y1)*(y2-y1) + (x2-x1)*(x2-x1) );\r
- *course = atan2( (y2-y1), (x2-x1) ) * SGD_RADIANS_TO_DEGREES;\r
- if ( *course < 0 ) *course = *course+360;\r
- //cout << x1 << " " << y1 << " " << x2 << " " << y2 << " " << *dist << " " << *course << endl;\r
-}\r
-\r
-\r
-\r
-int FGApproach::RemovePlane() {\r
-\r
- // first check if anything has to be done\r
- bool rmplane = false;\r
- int i;\r
-\r
- for (i=0; i<num_planes; i++) {\r
- if (planes[i].dist > range*SG_NM_TO_METER) {\r
- rmplane = true;\r
- break;\r
- }\r
- }\r
- if (!rmplane) return num_planes;\r
-\r
- // now make a copy of the plane list\r
- PlaneApp tmp[max_planes];\r
- for (i=0; i<num_planes; i++) {\r
- tmp[i] = planes[i];\r
- }\r
- \r
- int np = 0;\r
- // now check which planes are still in range\r
- for (i=0; i<num_planes; i++) {\r
- if (tmp[i].dist <= range*SG_NM_TO_METER) {\r
- planes[np] = tmp[i];\r
- np += 1;\r
- }\r
- }\r
- num_planes = np;\r
-\r
- return num_planes;\r
-}\r
+// FGApproach - a class to provide approach control at larger airports.
+//
+// Written by Alexander Kappes, started March 2002.
+//
+// Copyright (C) 2002 Alexander Kappes
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+#ifdef HAVE_CONFIG_H
+# include <config.h>
+#endif
+
+#include "approach.hxx"
+#include "transmission.hxx"
+#include "transmissionlist.hxx"
+#include "ATCdisplay.hxx"
+#include "ATCDialog.hxx"
+
+#include <Airports/runways.hxx>
+#include <simgear/constants.h>
+#include <simgear/math/polar3d.hxx>
+#include <simgear/misc/sg_path.hxx>
+
+#include <Environment/environment_mgr.hxx>
+#include <Environment/environment.hxx>
+
+
+#include <GUI/gui.h>
+
+//Constructor
+FGApproach::FGApproach(){
+ comm1_node = fgGetNode("/instrumentation/comm[0]/frequencies/selected-mhz", true);
+ comm2_node = fgGetNode("/instrumentation/comm[1]/frequencies/selected-mhz", true);
+
+ _type = APPROACH;
+
+ num_planes = 0;
+ lon_node = fgGetNode("/position/longitude-deg", true);
+ lat_node = fgGetNode("/position/latitude-deg", true);
+ elev_node = fgGetNode("/position/altitude-ft", true);
+ hdg_node = fgGetNode("/orientation/heading-deg", true);
+ speed_node = fgGetNode("/velocities/airspeed-kt", true);
+ etime_node = fgGetNode("/sim/time/elapsed-sec", true);
+
+ first = true;
+ active_runway = "";
+ int i;
+ for ( i=0; i<max_planes; i++) {
+ planes[i].contact = 0;
+ planes[i].wpn = 0;
+ planes[i].dnwp = -999.;
+ planes[i].on_crs = true;
+ planes[i].turn_rate = 10.0;
+ planes[i].desc_rate = 1000.0;
+ planes[i].clmb_rate = 500.0;
+ planes[i].tlm = 0.0;
+ planes[i].lmc.c1 = 0;
+ planes[i].lmc.c2 = 0;
+ planes[i].lmc.c3 = -1;
+ planes[i].wp_change = false;
+ }
+}
+
+//Destructor
+FGApproach::~FGApproach(){
+}
+
+void FGApproach::Init() {
+}
+
+
+
+// ============================================================================
+// the main update function
+// ============================================================================
+void FGApproach::Update(double dt) {
+
+ const int max_trans = 20;
+ FGTransmission tmissions[max_trans];
+ int wpn;
+ atc_type station = APPROACH;
+ TransCode code;
+ TransPar TPar;
+ int i,j;
+ //double course, d,
+ double adif, datp;
+ //char buf[10];
+ string message;
+ //static string atcmsg1[10];
+ //static string atcmsg2[10];
+ string mentry;
+ string transm;
+ TransPar tpars;
+ //static bool TransDisplayed = false;
+
+ update_plane_dat();
+ if ( active_runway == "" ) get_active_runway();
+
+ double comm1_freq = comm1_node->getDoubleValue();
+
+ //bool DisplayTransmissions = true;
+
+ for (i=0; i<num_planes; i++) {
+ if ( planes[i].ident == "Player") {
+ station = APPROACH;
+ tpars.station = name;
+ tpars.callsign = "Player";
+ tpars.airport = ident;
+
+ //cout << "ident = " << ident << " name = " << name << '\n';
+
+ int num_trans = 0;
+ // is the frequency of the station tuned in?
+ if ( freq == (int)(comm1_freq*100.0 + 0.5) ) {
+ current_transmissionlist->query_station( station, tmissions, max_trans, num_trans );
+ // loop over all transmissions for station
+ for ( j=0; j<=num_trans-1; j++ ) {
+ code = tmissions[j].get_code();
+ //cout << "code is " << code.c1 << " " << code.c2 << " " << code.c3 << '\n';
+ // select proper transmissions
+ if(code.c3 != 2) { // DCL - hack to prevent request crossing airspace being displayed since this isn't implemented yet.
+ if ( ( code.c2 == -1 && planes[i].lmc.c3 == 0 ) ||
+ ( code.c1 == 0 && code.c2 == planes[i].lmc.c2 ) ) {
+ mentry = current_transmissionlist->gen_text(station, code, tpars, false);
+ transm = current_transmissionlist->gen_text(station, code, tpars, true);
+ // is the transmission already registered?
+ if (!current_atcdialog->trans_reg( ident, transm, APPROACH )) {
+ current_atcdialog->add_entry( ident, transm, mentry, APPROACH, 0 );
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+
+ for ( i=0; i<num_planes; i++ ) {
+ //cout << "TPar.airport = " << TPar.airport << " TPar.station = " << TPar.station << " TPar.callsign = " << TPar.callsign << '\n';
+ //if ( planes[i].ident == TPar.callsign && name == TPar.airport && TPar.station == "approach" ) {
+ //if ( TPar.request && TPar.intention == "landing" && ident == TPar.intid) {
+ if(planes[i].ident == "Player" && fgGetBool("/sim/atc/opt0")) {
+ //cout << "Landing requested\n";
+ fgSetBool("/sim/atc/opt0", false);
+ planes[i].wpn = 0;
+ // ===========================
+ // === calculate waypoints ===
+ // ===========================
+ calc_wp( i );
+ update_param( i );
+ wpn = planes[i].wpn-1;
+ planes[i].aalt = planes[i].wpts[wpn-1][2];
+ planes[i].ahdg = planes[i].wpts[wpn][4];
+
+ // generate the message
+ code.c1 = 1;
+ code.c2 = 1;
+ code.c3 = 0;
+ adif = angle_diff_deg( planes[i].hdg, planes[i].ahdg );
+ tpars.station = name;
+ tpars.callsign = "Player";
+ if ( adif < 0 ) tpars.tdir = 1;
+ else tpars.tdir = 2;
+ tpars.heading = planes[i].ahdg;
+ if (planes[i].alt-planes[i].aalt > 100.0) tpars.VDir = 1;
+ else if (planes[i].alt-planes[i].aalt < -100.0) tpars.VDir = 3;
+ else tpars.VDir = 2;
+ tpars.alt = planes[i].aalt;
+ message = current_transmissionlist->gen_text(station, code, tpars, true );
+ //cout << message << '\n';
+ set_message(message);
+ planes[i].lmc = code;
+ planes[i].tlm = etime_node->getDoubleValue();
+ planes[i].on_crs = true;
+ planes[i].contact = 1;
+ }
+ //}
+
+ //if(1) {
+ if ( planes[i].contact == 1 ) {
+ // =========================
+ // === update parameters ===
+ // =========================
+ update_param( i );
+ //cout << planes[i].brg << " " << planes[i].dist << " " << planes[i].wpts[wpn+1][0]
+ //<< " " << planes[i].wpts[wpn+1][1] << " " << planes[i].wpts[wpn+1][4]
+ //cout << wpn << " distance to current course = " << planes[i].dcc << endl;
+ //cout << etime_node->getDoubleValue() << endl;
+
+ // =========================
+ // === reached waypoint? ===
+ // =========================
+ wpn = planes[i].wpn-2;
+ adif = angle_diff_deg( planes[i].hdg, planes[i].wpts[wpn][4] )
+ * SGD_DEGREES_TO_RADIANS;
+ datp = 2*sin(fabs(adif)/2.0)*sin(fabs(adif)/2.0) *
+ planes[i].spd/3600. * planes[i].turn_rate +
+ planes[i].spd/3600. * 3.0;
+ //cout << adif/SGD_DEGREES_TO_RADIANS << " "
+ // << datp << " " << planes[i].dnc << " " << planes[i].dcc <<endl;
+ if ( fabs(planes[i].dnc) < datp ) {
+ //if ( fabs(planes[i].dnc) < 0.3 && planes[i].dnwp < 1.0 ) {
+ //cout << "Reached next waypoint!\n";
+ planes[i].wpn -= 1;
+ wpn = planes[i].wpn-1;
+ planes[i].ahdg = planes[i].wpts[wpn][4];
+ planes[i].aalt = planes[i].wpts[wpn-1][2];
+ planes[i].wp_change = true;
+
+ // generate the message
+ adif = angle_diff_deg( planes[i].hdg, planes[i].ahdg );
+ tpars.station = name;
+ tpars.callsign = "Player";
+ if ( adif < 0 ) tpars.tdir = 1;
+ else tpars.tdir = 2;
+ tpars.heading = planes[i].ahdg;
+
+ if ( wpn-1 != 0) {
+ code.c1 = 1;
+ code.c2 = 1;
+ code.c3 = 0;
+ if (planes[i].alt-planes[i].aalt > 100.0) tpars.VDir = 1;
+ else if (planes[i].alt-planes[i].aalt < -100.0) tpars.VDir = 3;
+ else tpars.VDir = 2;
+ tpars.alt = planes[i].aalt;
+ message = current_transmissionlist->gen_text(station, code, tpars, true );
+ //cout << "Approach transmitting...\n";
+ //cout << message << endl;
+ set_message(message);
+ }
+ else {
+ code.c1 = 1;
+ code.c2 = 3;
+ code.c3 = 0;
+ tpars.runway = active_runway;
+ message = current_transmissionlist->gen_text(station, code, tpars, true);
+ //cout << "Approach transmitting 2 ...\n";
+ //cout << message << endl;
+ set_message(message);
+ }
+ planes[i].lmc = code;
+ planes[i].tlm = etime_node->getDoubleValue();
+ planes[i].on_crs = true;
+
+ update_param( i );
+ }
+
+ // =========================
+ // === come off course ? ===
+ // =========================
+ if ( fabs(planes[i].dcc) > 1.0 &&
+ ( !planes[i].wp_change || etime_node->getDoubleValue() - planes[i].tlm > tbm ) ) {
+ //cout << "Off course!\n";
+ if ( planes[i].on_crs ) {
+ if ( planes[i].dcc < 0) {
+ planes[i].ahdg += 30.0;
+ }
+ else {
+ planes[i].ahdg -= 30.0;
+ }
+ if (planes[i].ahdg > 360.0) planes[i].ahdg -= 360.0;
+ else if (planes[i].ahdg < 0.0) planes[i].ahdg += 360.0;
+ }
+ //cout << planes[i].on_crs << " "
+ // << angle_diff_deg( planes[i].hdg, planes[i].ahdg) << " "
+ // << etime_node->getDoubleValue() << " "
+ // << planes[i].tlm << endl;
+ // generate the message
+ if ( planes[i].on_crs ||
+ ( fabs(angle_diff_deg( planes[i].hdg, planes[i].ahdg )) > 30.0 &&
+ etime_node->getDoubleValue() - planes[i].tlm > tbm) ) {
+ // generate the message
+ code.c1 = 1;
+ code.c2 = 4;
+ code.c3 = 0;
+ adif = angle_diff_deg( planes[i].hdg, planes[i].ahdg );
+ tpars.station = name;
+ tpars.callsign = "Player";
+ tpars.miles = fabs(planes[i].dcc);
+ if ( adif < 0 ) tpars.tdir = 1;
+ else tpars.tdir = 2;
+ tpars.heading = planes[i].ahdg;
+ message = current_transmissionlist->gen_text(station, code, tpars, true);
+ //cout << "Approach transmitting 3 ...\n";
+ //cout << message << '\n';
+ set_message(message);
+ planes[i].lmc = code;
+ planes[i].tlm = etime_node->getDoubleValue();
+ }
+
+ planes[i].on_crs = false;
+ }
+ else if ( !planes[i].on_crs ) {
+ //cout << "Off course 2!\n";
+ wpn = planes[i].wpn-1;
+ adif = angle_diff_deg( planes[i].hdg, planes[i].wpts[wpn][4] )
+ * SGD_DEGREES_TO_RADIANS;
+ datp = 2*sin(fabs(adif)/2.0)*sin(fabs(adif)/2.0) *
+ planes[i].spd/3600. * planes[i].turn_rate +
+ planes[i].spd/3600. * 3.0;
+ if ( fabs(planes[i].dcc) < datp ) {
+ planes[i].ahdg = fabs(planes[i].wpts[wpn][4]);
+
+ // generate the message
+ code.c1 = 1;
+ code.c2 = 2;
+ code.c3 = 0;
+ tpars.station = name;
+ tpars.callsign = "Player";
+ if ( adif < 0 ) tpars.tdir = 1;
+ else tpars.tdir = 2;
+ tpars.heading = planes[i].ahdg;
+ message = current_transmissionlist->gen_text(station, code, tpars, true);
+ //cout << "Approach transmitting 4 ...\n";
+ //cout << message << '\n';
+ set_message(message);
+ planes[i].lmc = code;
+ planes[i].tlm = etime_node->getDoubleValue();
+
+ planes[i].on_crs = true;
+ }
+ }
+ else if ( planes[i].wp_change ) {
+ planes[i].wp_change = false;
+ }
+
+ // ===================================================================
+ // === Less than two minutes away from touchdown? -> Contact Tower ===
+ // ===================================================================
+ if ( planes[i].wpn == 2 && planes[i].dnwp < planes[i].spd/60.*2.0 ) {
+
+ double freq = 121.95; // Hardwired - FIXME
+ // generate message
+ code.c1 = 1;
+ code.c2 = 5;
+ code.c3 = 0;
+ tpars.station = name;
+ tpars.callsign = "Player";
+ tpars.freq = freq;
+ message = current_transmissionlist->gen_text(station, code, tpars, true);
+ //cout << "Approach transmitting 5 ...\n";
+ //cout << message << '\n';
+ set_message(message);
+ planes[i].lmc = code;
+ planes[i].tlm = etime_node->getDoubleValue();
+
+ planes[i].contact = 2;
+ }
+ }
+ }
+}
+
+
+// ============================================================================
+// update course parameters
+// ============================================================================
+void FGApproach::update_param( const int &i ) {
+
+ double course, d;
+
+ int wpn = planes[i].wpn-1; // this is the current waypoint
+
+ planes[i].dcc = calc_psl_dist(planes[i].brg, planes[i].dist,
+ planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],
+ planes[i].wpts[wpn][4]);
+ planes[i].dnc = calc_psl_dist(planes[i].brg, planes[i].dist,
+ planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
+ planes[i].wpts[wpn-1][4]);
+ calc_hd_course_dist(planes[i].brg, planes[i].dist,
+ planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
+ &course, &d);
+ planes[i].dnwp = d;
+
+}
+
+// ============================================================================
+// smallest difference between two angles in degree
+// difference is negative if a1 > a2 and positive if a2 > a1
+// ===========================================================================
+double FGApproach::angle_diff_deg( const double &a1, const double &a2) {
+
+ double a3 = a2 - a1;
+ if (a3 < 180.0) a3 += 360.0;
+ if (a3 > 180.0) a3 -= 360.0;
+
+ return a3;
+}
+
+// ============================================================================
+// calculate waypoints
+// ============================================================================
+void FGApproach::calc_wp( const int &i ) {
+
+ int j;
+ double course, d, cd, a1;
+
+ int wpn = planes[i].wpn;
+ // waypoint 0: Threshold of active runway
+ calc_gc_course_dist(Point3D(lon*SGD_DEGREES_TO_RADIANS, lat*SGD_DEGREES_TO_RADIANS, 0.0),
+ Point3D(active_rw_lon*SGD_DEGREES_TO_RADIANS,active_rw_lat*SGD_DEGREES_TO_RADIANS, 0.0 ),
+ &course, &d);
+ double d1 = active_rw_hdg+180.0;
+ if ( d1 > 360.0 ) d1 -=360.0;
+ calc_cd_head_dist(360.0-course*SGD_RADIANS_TO_DEGREES, d/SG_NM_TO_METER,
+ d1, active_rw_len/SG_NM_TO_METER/2.0,
+ &planes[i].wpts[wpn][0], &planes[i].wpts[wpn][1]);
+ planes[i].wpts[wpn][2] = elev;
+ planes[i].wpts[wpn][4] = 0.0;
+ planes[i].wpts[wpn][5] = 0.0;
+ wpn += 1;
+
+ // ======================
+ // horizontal navigation
+ // ======================
+ // waypoint 1: point for turning onto final
+ calc_cd_head_dist(planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1], d1, lfl,
+ &planes[i].wpts[wpn][0], &planes[i].wpts[wpn][1]);
+ calc_hd_course_dist(planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],
+ planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
+ &course, &d);
+ planes[i].wpts[wpn][4] = course;
+ planes[i].wpts[wpn][5] = d;
+ wpn += 1;
+
+ // calculate course and distance from plane position to waypoint 1
+ calc_hd_course_dist(planes[i].brg, planes[i].dist, planes[i].wpts[1][0],
+ planes[i].wpts[1][1], &course, &d);
+ // check if airport is not between plane and waypoint 1 and
+ // DCA to airport on course to waypoint 1 is larger than 10 miles
+ double zero = 0.0;
+ if ( fabs(angle_diff_deg( planes[i].wpts[1][0], planes[i].brg )) < 90.0 ||
+ calc_psl_dist( zero, zero, planes[i].brg, planes[i].dist, course ) > 10.0 ) {
+ // check if turning angle at waypoint 1 would be > max_ta
+ if ( fabs(angle_diff_deg( planes[i].wpts[1][4], course )) > max_ta ) {
+ cd = calc_psl_dist(planes[i].brg, planes[i].dist,
+ planes[i].wpts[1][0], planes[i].wpts[1][1],
+ planes[i].wpts[1][4]);
+ a1 = atan2(cd,planes[i].wpts[1][1]);
+ planes[i].wpts[wpn][0] = planes[i].wpts[1][0] - a1/SGD_DEGREES_TO_RADIANS;
+ if ( planes[i].wpts[wpn][0] < 0.0) planes[i].wpts[wpn][0] += 360.0;
+ if ( planes[i].wpts[wpn][0] > 360.0) planes[i].wpts[wpn][0] -= 360.0;
+ planes[i].wpts[wpn][1] = fabs(cd) / sin(fabs(a1));
+ calc_hd_course_dist(planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],
+ planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
+ &course, &d);
+ planes[i].wpts[wpn][4] = course;
+ planes[i].wpts[wpn][5] = d;
+ wpn += 1;
+
+ calc_hd_course_dist(planes[i].brg, planes[i].dist, planes[i].wpts[wpn-1][0],
+ planes[i].wpts[wpn-1][1], &course, &d);
+ }
+ } else {
+ double leg = 10.0;
+ a1 = atan2(planes[i].wpts[1][1], leg );
+
+ if ( angle_diff_deg(planes[i].brg,planes[i].wpts[1][0]) < 0 )
+ planes[i].wpts[wpn][0] = planes[i].wpts[1][0] + a1/SGD_DEGREES_TO_RADIANS;
+ else planes[i].wpts[wpn][0] = planes[i].wpts[1][0] - a1/SGD_DEGREES_TO_RADIANS;
+
+ planes[i].wpts[wpn][1] = sqrt( planes[i].wpts[1][1]*planes[i].wpts[1][1] + leg*leg );
+ calc_hd_course_dist(planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],
+ planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
+ &course, &d);
+ planes[i].wpts[wpn][4] = course;
+ planes[i].wpts[wpn][5] = d;
+ wpn += 1;
+
+ calc_hd_course_dist(planes[i].brg, planes[i].dist,
+ planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
+ &course, &d);
+ }
+
+ planes[i].wpts[wpn][0] = planes[i].brg;
+ planes[i].wpts[wpn][1] = planes[i].dist;
+ planes[i].wpts[wpn][2] = planes[i].alt;
+ planes[i].wpts[wpn][4] = course;
+ planes[i].wpts[wpn][5] = d;
+ wpn += 1;
+
+ planes[i].wpn = wpn;
+
+ // Now check if legs are too short or if legs can be shortend
+ // legs must be at least 2 flight minutes long
+ double mdist = planes[i].spd / 60.0 * 2.0;
+ for ( j=2; j<wpn-1; ++j ) {
+ if ( planes[i].wpts[j][1] < mdist) {
+ }
+ }
+
+ // ====================
+ // vertical navigation
+ // ====================
+ double alt = elev+3000.0;
+ planes[i].wpts[1][2] = round_alt( true, alt );
+ for ( j=2; j<wpn-1; ++j ) {
+ double dalt = planes[i].alt - planes[i].wpts[j-1][2];
+ if ( dalt > 0 ) {
+ alt = planes[i].wpts[j-1][2] +
+ (planes[i].wpts[j][5] / planes[i].spd) * 60.0 * planes[i].desc_rate;
+ planes[i].wpts[j][2] = round_alt( false, alt );
+ if ( planes[i].wpts[j][2] > planes[i].alt )
+ planes[i].wpts[j][2] = round_alt( false, planes[i].alt );
+ }
+ else {
+ planes[i].wpts[j][2] = planes[i].wpts[1][2];
+ }
+ }
+
+ cout << "Plane position: " << planes[i].brg << " " << planes[i].dist << endl;
+ for ( j=0; j<wpn; ++j ) {
+ cout << "Waypoint " << j << endl;
+ cout << "------------------" << endl;
+ cout << planes[i].wpts[j][0] << " " << planes[i].wpts[j][1]
+ << " " << planes[i].wpts[j][2] << " " << planes[i].wpts[j][5];
+ cout << endl << endl;
+ }
+
+}
+
+
+// ============================================================================
+// round altitude value to next highest/lowest 500 feet
+// ============================================================================
+double FGApproach::round_alt( const bool hl, double alt ) {
+
+ alt = alt/1000.0;
+ if ( hl ) {
+ if ( alt > (int)(alt)+0.5 ) alt = ((int)(alt)+1)*1000.0;
+ else alt = ((int)(alt)+0.5)*1000.0;
+ }
+ else {
+ if ( alt > (int)(alt)+0.5 ) alt = ((int)(alt)+0.5)*1000.0;
+ else alt = ((int)(alt))*1000.0;
+ }
+
+ return alt;
+}
+
+
+// ============================================================================
+// get active runway
+// ============================================================================
+void FGApproach::get_active_runway() {
+ //cout << "Entering FGApproach::get_active_runway()\n";
+
+ FGEnvironment stationweather =
+ ((FGEnvironmentMgr *)globals->get_subsystem("environment"))
+ ->getEnvironment(lat, lon, elev);
+
+ double hdg = stationweather.get_wind_from_heading_deg();
+
+ FGRunway runway;
+ if ( globals->get_runways()->search( ident, int(hdg), &runway) ) {
+ active_runway = runway._rwy_no;
+ active_rw_hdg = runway._heading;
+ active_rw_lon = runway._lon;
+ active_rw_lat = runway._lat;
+ active_rw_len = runway._length;
+ //cout << "Active runway is: " << active_runway << " heading = "
+ // << active_rw_hdg
+ // << " lon = " << active_rw_lon
+ // << " lat = " << active_rw_lat <<endl;
+ }
+ else cout << "FGRunways search failed\n";
+
+}
+
+// ========================================================================
+// update infos about plane
+// ========================================================================
+void FGApproach::update_plane_dat() {
+
+ //cout << "Update Approach " << ident << " " << num_planes << " registered" << endl;
+ // update plane positions
+ int i;
+ for (i=0; i<num_planes; i++) {
+ planes[i].lon = lon_node->getDoubleValue();
+ planes[i].lat = lat_node->getDoubleValue();
+ planes[i].alt = elev_node->getDoubleValue();
+ planes[i].hdg = hdg_node->getDoubleValue();
+ planes[i].spd = speed_node->getDoubleValue();
+
+ /*Point3D aircraft = sgGeodToCart( Point3D(planes[i].lon*SGD_DEGREES_TO_RADIANS,
+ planes[i].lat*SGD_DEGREES_TO_RADIANS,
+ planes[i].alt*SG_FEET_TO_METER) );*/
+ double course, distance;
+ calc_gc_course_dist(Point3D(lon*SGD_DEGREES_TO_RADIANS, lat*SGD_DEGREES_TO_RADIANS, 0.0),
+ Point3D(planes[i].lon*SGD_DEGREES_TO_RADIANS,planes[i].lat*SGD_DEGREES_TO_RADIANS, 0.0 ),
+ &course, &distance);
+ planes[i].dist = distance/SG_NM_TO_METER;
+ planes[i].brg = 360.0-course*SGD_RADIANS_TO_DEGREES;
+
+ //cout << "Plane Id: " << planes[i].ident << " Distance to " << ident
+ // << " is " << planes[i].dist << " miles " << "Bearing " << planes[i].brg << endl;
+
+ }
+}
+
+// =======================================================================
+// Add plane to Approach list
+// =======================================================================
+void FGApproach::AddPlane(const string& pid) {
+
+ int i;
+ for ( i=0; i<num_planes; i++) {
+ if ( planes[i].ident == pid) {
+ //cout << "Plane already registered: " << planes[i].ident << ' ' << ident << ' ' << num_planes << endl;
+ return;
+ }
+ }
+ planes[num_planes].ident = pid;
+ ++num_planes;
+ //cout << "Plane added to list: " << ident << " " << num_planes << endl;
+ return;
+}
+
+// ================================================================================
+// closest distance between a point (h1,d1) and a straigt line (h2,d2,h3) in 2 dim.
+// ================================================================================
+double FGApproach::calc_psl_dist(const double &h1, const double &d1,
+ const double &h2, const double &d2,
+ const double &h3)
+{
+ double a1 = h1 * SGD_DEGREES_TO_RADIANS;
+ double a2 = h2 * SGD_DEGREES_TO_RADIANS;
+ double a3 = h3 * SGD_DEGREES_TO_RADIANS;
+ double x1 = cos(a1) * d1;
+ double y1 = sin(a1) * d1;
+ double x2 = cos(a2) * d2;
+ double y2 = sin(a2) * d2;
+ double x3 = cos(a3);
+ double y3 = sin(a3);
+
+ // formula: dis = sqrt( (v1-v2)**2 - ((v1-v2)*v3)**2 ); vi = (xi,yi)
+ double val1 = (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2);
+ double val2 = ((x1-x2)*x3 + (y1-y2)*y3) * ((x1-x2)*x3 + (y1-y2)*y3);
+ double dis = val1 - val2;
+ // now get sign for offset
+ //cout << x1 << " " << x2 << " " << y1 << " " << y2 << " "
+ // << x3 << " " << y3 << " "
+ // << val1 << " " << val2 << " " << dis << endl;
+ x3 *= sqrt(val2);
+ y3 *= sqrt(val2);
+ double da = fabs(atan2(y3,x3) - atan2(y1-y2,x1-x2));
+ if ( da > SGD_PI ) da -= SGD_2PI;
+ if ( fabs(da) > SGD_PI_2) {
+ //if ( x3*(x1-x2) < 0.0 && y3*(y1-y2) < 0.0) {
+ x3 *= -1.0;
+ y3 *= -1.0;
+ }
+ //cout << x3 << " " << y3 << endl;
+ double dis1 = x1-x2-x3;
+ double dis2 = y1-y2-y3;
+ dis = sqrt(dis);
+ da = atan2(dis2,dis1);
+ if ( da < 0.0 ) da += SGD_2PI;
+ if ( da < a3 ) dis *= -1.0;
+ //cout << dis1 << " " << dis2 << " " << da*SGD_RADIANS_TO_DEGREES << " " << h3
+ // << " " << sqrt(dis1*dis1 + dis2*dis2) << " " << dis << endl;
+ //cout << atan2(dis2,dis1)*SGD_RADIANS_TO_DEGREES << " " << dis << endl;
+
+ return dis;
+}
+
+
+// ========================================================================
+// Calculate new bear/dist given starting bear/dis, and offset radial,
+// and distance.
+// ========================================================================
+void FGApproach::calc_cd_head_dist(const double &h1, const double &d1,
+ const double &course, const double &dist,
+ double *h2, double *d2)
+{
+ double a1 = h1 * SGD_DEGREES_TO_RADIANS;
+ double a2 = course * SGD_DEGREES_TO_RADIANS;
+ double x1 = cos(a1) * d1;
+ double y1 = sin(a1) * d1;
+ double x2 = cos(a2) * dist;
+ double y2 = sin(a2) * dist;
+
+ *d2 = sqrt((x1+x2)*(x1+x2) + (y1+y2)*(y1+y2));
+ *h2 = atan2( (y1+y2), (x1+x2) ) * SGD_RADIANS_TO_DEGREES;
+ if ( *h2 < 0 ) *h2 = *h2+360;
+}
+
+
+
+// ========================================================================
+// get heading and distance between two points; point1 ---> point2
+// ========================================================================
+void FGApproach::calc_hd_course_dist(const double &h1, const double &d1,
+ const double &h2, const double &d2,
+ double *course, double *dist)
+{
+ double a1 = h1 * SGD_DEGREES_TO_RADIANS;
+ double a2 = h2 * SGD_DEGREES_TO_RADIANS;
+ double x1 = cos(a1) * d1;
+ double y1 = sin(a1) * d1;
+ double x2 = cos(a2) * d2;
+ double y2 = sin(a2) * d2;
+
+ *dist = sqrt( (y2-y1)*(y2-y1) + (x2-x1)*(x2-x1) );
+ *course = atan2( (y2-y1), (x2-x1) ) * SGD_RADIANS_TO_DEGREES;
+ if ( *course < 0 ) *course = *course+360;
+ //cout << x1 << " " << y1 << " " << x2 << " " << y2 << " " << *dist << " " << *course << endl;
+}
+
+
+
+int FGApproach::RemovePlane() {
+
+ // first check if anything has to be done
+ bool rmplane = false;
+ int i;
+
+ for (i=0; i<num_planes; i++) {
+ if (planes[i].dist > range*SG_NM_TO_METER) {
+ rmplane = true;
+ break;
+ }
+ }
+ if (!rmplane) return num_planes;
+
+ // now make a copy of the plane list
+ PlaneApp tmp[max_planes];
+ for (i=0; i<num_planes; i++) {
+ tmp[i] = planes[i];
+ }
+
+ int np = 0;
+ // now check which planes are still in range
+ for (i=0; i<num_planes; i++) {
+ if (tmp[i].dist <= range*SG_NM_TO_METER) {
+ planes[np] = tmp[i];
+ np += 1;
+ }
+ }
+ num_planes = np;
+
+ return num_planes;
+}
+
+
+void FGApproach::set_message(const string &msg)
+{
+ fgSetString("/sim/messages/approach", msg.c_str());
+ globals->get_ATC_display()->RegisterSingleMessage( msg, 0 );
+}
+
projects / flightgear.git / blobdiff