-// 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., 675 Mass Ave, Cambridge, MA 02139, USA.
-
-#include "approach.hxx"
-#include "ATCdisplay.hxx"
-#include <Airports/runways.hxx>
-
-#include <simgear/misc/sg_path.hxx>
-
-#ifdef FG_WEATHERCM
-# include <WeatherCM/FGLocalWeatherDatabase.h>
-#else
-# include <Environment/environment_mgr.hxx>
-# include <Environment/environment.hxx>
-#endif
-
-
-PlaneApp::PlaneApp()
-:
- ident(""),
- lon(0.0),
- lat(0.0),
- alt(0.0),
- hdg(0.0),
- dist(0.0),
- brg(0.0),
- spd(0.0),
- contact(0),
- wpn(0),
- dnwp(-999.),
- dcc(0.0),
- dnc(0.0),
- aalt(0.0),
- ahdg(0.0),
- on_crs(true),
- tlm(0.0)
-{
-}
-
-//Constructor
-FGApproach::FGApproach() :
- bucket(0),
- active_runway(""),
- active_rw_hdg(0.0),
- display(false),
- displaying(false),
- num_planes(0),
- transmission(""),
- first(true),
- trans_ident(""),
- approach_failed(false)
-{
- comm1_node = fgGetNode("/radios/comm[0]/frequencies/selected-mhz", true);
- comm2_node = fgGetNode("/radios/comm[1]/frequencies/selected-mhz", true);
-
- lon_node = fgGetNode("/position/longitude-deg", true);
- lat_node = fgGetNode("/position/latitude-deg", true);
- elev_node = fgGetNode("/position/altitude-ft", true);
-}
-
-//Destructor
-FGApproach::~FGApproach(){
-}
-
-void FGApproach::Init() {
- display = false;
-}
-
-// ============================================================================
-// the main update function
-// ============================================================================
-void FGApproach::Update() {
-
- int wpn;
- double course, d;
-
- update_plane_dat();
- if ( active_runway == "" ) get_active_runway();
-
- for ( int i=0; i<num_planes; i++ ) {
-
- if ( planes[i].contact == 0) {
- double comm1_freq = comm1_node->getDoubleValue();
- if ( (int)(comm1_freq*100.0 + 0.5) == freq ) planes[i].contact = 1;
- //cout << "comm1 = " << (int)(comm1_freq*100.0 + 0.5) << " freq = " << freq << '\n';
- }
- else if ( planes[i].contact == 1 ) {
- if ( planes[i].wpn == 0 ) { // calculate initial waypoints
- wpn = planes[i].wpn;
- // airport
- planes[i].wpts[wpn][0] = active_rw_hdg;
- planes[i].wpts[wpn][1] = 0.0;
- planes[i].wpts[wpn][2] = elev;
- planes[i].wpts[wpn][4] = 0.0;
- planes[i].wpts[wpn][5] = 0.0;
- wpn += 1;
-
- planes[i].wpts[wpn][0] = active_rw_hdg + 180.0;
- if ( planes[i].wpts[wpn][0] > 360.0 ) planes[i].wpts[wpn][0] -= 360.0;
- planes[i].wpts[wpn][1] = 5;
- planes[i].wpts[wpn][2] = elev + 1000.0;
- 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;
-
- planes[i].wpts[wpn][0] = planes[i].brg;
- planes[i].wpts[wpn][1] = planes[i].dist;
- planes[i].wpts[wpn][2] = planes[i].alt;
- 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;
-
- planes[i].wpn = wpn;
-
- planes[i].ahdg = planes[i].wpts[wpn-1][4];
- cout << endl;
- cout << "Contact " << planes[i].wpn << endl;
- cout << "Turn to heading = " << (int)(planes[i].ahdg) << endl;
- cout << endl;
- planes[i].on_crs = true;
- }
-
- // reached waypoint?
- if ( fabs(planes[i].dnc) < 0.3 && planes[i].dnwp < 1.0 ) {
- planes[i].wpn -= 1;
- wpn = planes[i].wpn-1;
- planes[i].ahdg = planes[i].wpts[wpn][4];
- cout << endl;
- cout << "Next waypoint = " << planes[i].wpn << endl;
- cout << "New heading = " << planes[i].ahdg << endl;
- cout << endl;
- planes[i].on_crs = true;
- }
-
- // update assigned parameters
- 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;
-
- //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 << " distance to current course = " << planes[i].dcc << endl;
-
- // come off course ?
- if ( fabs(planes[i].dcc) > 0.5 && planes[i].on_crs) {
- wpn = wpn-1;
- if ( planes[i].wpts[wpn][4] < 0) {
- planes[i].ahdg += 30.0;
- }
- else {
- planes[i].ahdg -= 30.0;
- }
- planes[i].on_crs = false;
-
- cout << endl;
- cout << "Your are " << planes[i].dcc << " miles off the asigned course: " << endl;
- cout << "New heading = " << (int)(planes[i].ahdg) << endl;
- cout << endl;
- }
- else if ( fabs(planes[i].dcc) < 0.1 && !planes[i].on_crs) {
- planes[i].ahdg = fabs(planes[i].wpts[wpn][4]);
- planes[i].on_crs = true;
-
- cout << endl;
- cout << "New heading = " << (int)(planes[i].ahdg) << endl;
- cout << endl;
- }
-
- // In range of tower?
- if ( planes[i].wpn == 2 && planes[i].dnwp < 3. ) {
- cout << endl;
- cout << "Contact Tower";
- cout << endl;
- planes[i].contact = 2;
- }
- }
- }
-
-}
-
-// ============================================================================
-// get active runway
-// ============================================================================
-void FGApproach::get_active_runway() {
-
-#ifdef FG_WEATHERCM
- sgVec3 position = { lat, lon, elev };
- FGPhysicalProperty stationweather = WeatherDatabase->get(position);
-#else
- FGEnvironment stationweather =
- globals->get_environment_mgr()->getEnvironment(lat, lon, elev);
-#endif
-
- SGPath path( globals->get_fg_root() );
- path.append( "Airports" );
- path.append( "runways.mk4" );
- FGRunways runways( path.c_str() );
-
- //Set the heading to into the wind
-#ifdef FG_WEATHERCM
- double wind_x = stationweather.Wind[0];
- double wind_y = stationweather.Wind[1];
-
- double speed = sqrt( wind_x*wind_x + wind_y*wind_y ) * SG_METER_TO_NM / (60.0*60.0);
- double hdg;
-
- //If no wind use 270degrees
- if(speed == 0) {
- hdg = 270;
- } else {
- // //normalize the wind to get the direction
- //wind_x /= speed; wind_y /= speed;
-
- hdg = - atan2 ( wind_x, wind_y ) * SG_RADIANS_TO_DEGREES ;
- if (hdg < 0.0)
- hdg += 360.0;
- }
-#else
- double hdg = stationweather.get_wind_from_heading_deg();
-#endif
-
- FGRunway runway;
- //if ( runways.search( "EGNX", int(hdg), &runway) ) {
- if ( runways.search( ident, int(hdg), &runway) ) {
- active_runway = runway.rwy_no;
- active_rw_hdg = runway.heading;
- //cout << "Active runway is: " << active_runway << " heading = "
- // << active_rw_hdg << endl;
- }
- else cout << "FGRunways search failed" << endl;
-
-}
-
-// ========================================================================
-// update infos about plane
-// ========================================================================
-void FGApproach::update_plane_dat() {
-
- //cout << "Update Approach " << ident << " " << num_planes << " registered" << endl;
- // update plane positions
- for (int i=0; i<num_planes; i++) {
- planes[i].lon = lon_node->getDoubleValue();
- planes[i].lat = lat_node->getDoubleValue();
- planes[i].alt = elev_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 << " m" << endl;
-
- //if (first) {
- //transmission = ident;
- //globals->get_ATC_display()->RegisterRepeatingMessage(transmission);
- //first = false;
- //}
- }
-}
-
-// =======================================================================
-// Add plane to Approach list
-// =======================================================================
-void FGApproach::AddPlane(string pid) {
- for ( int i=0; i<num_planes; i++) {
- if ( planes[i].ident == pid) {
- //cout << "Plane already registered: " << 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 and a straigt line 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);
- 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);
- if (atan2(dis2,dis1) < a3) dis *= -1.0;
- //cout << dis1 << " " << dis2 << " " << atan2(dis2,dis1)*SGD_RADIANS_TO_DEGREES << " " << h3
- // << " " << sqrt(dis1*dis1 + dis2*dis2) << " " << dis << endl;
- //cout << atan2(dis2,dis1)*SGD_RADIANS_TO_DEGREES << " " << dis << endl;
-
- return dis;
-}
-
-// ========================================================================
-// 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
- int i;
- bool rmplane = false;
- 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;
-}
+// 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
-// approach.hxx -- Approach class
-//
-// 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., 675 Mass Ave, Cambridge, MA 02139, USA.
-
-
-#ifndef _FG_APPROACH_HXX
-#define _FG_APPROACH_HXX
-
-#include <stdio.h>
-
-#include <simgear/compiler.h>
-#include <simgear/math/sg_geodesy.hxx>
-#include <simgear/misc/sgstream.hxx>
-#include <simgear/magvar/magvar.hxx>
-#include <simgear/timing/sg_time.hxx>
-#include <simgear/bucket/newbucket.hxx>
-
-#include <Main/fg_props.hxx>
-
-#ifdef SG_HAVE_STD_INCLUDES
-# include <istream>
-#include <iomanip>
-#elif defined( __BORLANDC__ ) || (__APPLE__)
-# include <iostream>
-#else
-# include <istream.h>
-#include <iomanip.h>
-#endif
-
-SG_USING_STD(istream);
-SG_USING_STD(string);
-
-#include "ATC.hxx"
-
-//DCL - a complete guess for now.
-#define FG_APPROACH_DEFAULT_RANGE 100
-
-// Contains all information about a plane that the approach control needs
-const int max_planes = 20; // max number of planes on the stack
-const int max_wp = 10; // max number of waypoints for approach phase
-struct PlaneApp {
-
- // variables for plane if it's on the radar
- string ident; // indentification of plane
- double lon; // longitude in degrees
- double lat; // latitude in degrees
- double alt; // Altitute above sea level in feet
- double hdg; // heading of plane in degrees
- double dist; // distance to airport in miles
- double brg; // bearing relative to airport in degrees
- double spd; // speed above ground
- int contact; // contact with approach established?
- // 0 = no contact yet
- // 1 = in contact
- // 2 = handed off to tower
-
- // additional variables if contact has been established
- int wpn; // number of waypoints
- double wpts[max_wp][6]; // assigned waypoints for approach phase
- // first wp in list is airport
- // last waypoint point at which contact was established
- // second index: 0 = bearing to airport
- // second index: 1 = distance to aiport
- // second index: 2 = alt
- // second index: 3 = ETA
- // second index: 4 = heading to next waypoint
- // second index: 5 = distance to next waypoint
-
- double dnwp; // distance to next waypoint
- double dcc; // closest distance to current assigned course
- double dnc; // closest distance to course from next to next to next wp
- double aalt; // assigned alt at next waypoint
- double ahdg; // assigned heading
- bool on_crs; // is the plane on course?
- double tlm; // time when last message was sent
-
- PlaneApp(void);
-};
-
-
-class FGApproach : public FGATC {
-
- int bucket;
- string active_runway;
- double active_rw_hdg;
-
- bool display; // Flag to indicate whether we should be outputting to the display.
- bool displaying; // Flag to indicate whether we are outputting to the display.
- int num_planes; // number of planes on the stack
- PlaneApp planes[max_planes]; // Array of planes
- string transmission;
- bool first;
-
- SGPropertyNode *comm1_node;
- SGPropertyNode *comm2_node;
-
- // for failure modeling
- string trans_ident; // transmitted ident
- bool approach_failed; // approach failed?
-
-public:
-
- FGApproach(void);
- ~FGApproach(void);
-
- void Init();
-
- void Update();
-
- // Add new plane to stack if not already registered
- // Input: pid - id of plane (name)
- // Output: "true" if added; "false" if already existend
- void AddPlane(string pid);
-
- // Remove plane from stack if out of range
- int RemovePlane();
-
- //Indicate that this instance should be outputting to the ATC display
- inline void SetDisplay(void) {display = true;}
-
- //Indicate that this instance should not be outputting to the ATC display
- inline void SetNoDisplay(void) {display = false;}
-
- inline double get_bucket() const { return bucket; }
- inline int get_pnum() const { return num_planes; }
- inline string get_trans_ident() { return trans_ident; }
- inline atc_type GetType() { return APPROACH; }
-
-private:
-
- void update_plane_dat();
-
- void get_active_runway();
-
-// ========================================================================
-// get heading and distance between two points; point2 ---> point1
-// input: point1 = heading in degrees, distance
-// input: point2 = heading in degrees, distance
-// ouput: course in degrees, distance
-// ========================================================================
- void calc_hd_course_dist(const double &h1, const double &d1,
- const double &h2, const double &d2,
- double *course, double *dist);
-
-// ========================================================================
-// closest distance between a point and a straigt line in 2 dim.
-// the input variables are given in (heading, distance)
-// relative to a common point
-// input: point = heading in degrees, distance
-// input: straigt line = anker vector (heading in degrees, distance),
-// heading of direction vector
-// output: distance
-// ========================================================================
- double calc_psl_dist(const double &h1, const double &d1,
- const double &h2, const double &d2,
- const double &h3);
-
- // Pointers to current users position
- SGPropertyNode *lon_node;
- SGPropertyNode *lat_node;
- SGPropertyNode *elev_node;
-
- //Update the transmission string
- void UpdateTransmission(void);
-
-};
-
-#endif // _FG_APPROACH_HXX
-
+// approach.hxx -- Approach class\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
+\r
+#ifndef _FG_APPROACH_HXX\r
+#define _FG_APPROACH_HXX\r
+\r
+#include <stdio.h>\r
+\r
+#include <simgear/compiler.h>\r
+#include <simgear/math/sg_geodesy.hxx>\r
+#include <simgear/misc/sgstream.hxx>\r
+#include <simgear/magvar/magvar.hxx>\r
+#include <simgear/timing/sg_time.hxx>\r
+#include <simgear/bucket/newbucket.hxx>\r
+\r
+#include <Main/fg_props.hxx>\r
+\r
+#ifdef SG_HAVE_STD_INCLUDES\r
+# include <istream>\r
+#include <iomanip>\r
+#elif defined( SG_HAVE_NATIVE_SGI_COMPILERS )\r
+# include <iostream.h>\r
+#elif defined( __BORLANDC__ )\r
+# include <iostream>\r
+#else\r
+# include <istream.h>\r
+#include <iomanip.h>\r
+#endif\r
+\r
+#if ! defined( SG_HAVE_NATIVE_SGI_COMPILERS )\r
+SG_USING_STD(istream);\r
+#endif\r
+\r
+SG_USING_STD(string);\r
+\r
+#include "ATC.hxx"\r
+#include "transmission.hxx"\r
+\r
+//DCL - a complete guess for now.\r
+#define FG_APPROACH_DEFAULT_RANGE 100\r
+\r
+// Contains all the information about a plane that the approach control needs\r
+const int max_planes = 20; // max number of planes on the stack\r
+const int max_wp = 10; // max number of waypoints for approach phase\r
+const double max_ta = 130; // max turning angle for plane during approach\r
+const double tbm = 20000.0; // min time (in ms) between two messages\r
+const double lfl = 10.0; // length of final leg\r
+\r
+struct PlaneApp {\r
+\r
+ // variables for plane if it's on the radar\r
+ string ident; // indentification of plane\r
+ double lon; // longitude in degrees\r
+ double lat; // latitude in degrees\r
+ double alt; // Altitute above sea level in feet\r
+ double hdg; // heading of plane in degrees\r
+ double dist; // distance to airport in miles\r
+ double brg; // bearing relative to airport in degrees\r
+ double spd; // speed above ground\r
+ int contact; // contact with approach established?\r
+ // 0 = no contact yet\r
+ // 1 = in contact\r
+ // 2 = handed off to tower\r
+ double turn_rate; // standard turning rate of the plane in seconds per degree\r
+ double desc_rate; // standard descent rate of the plane in feets per minute\r
+ double clmb_rate; // standard climb rate of the plane in feets per minute\r
+\r
+ // additional variables if contact has been established\r
+ int wpn; // number of waypoints\r
+ double wpts[max_wp][6]; // assigned waypoints for approach phase \r
+ // first wp in list is airport\r
+ // last waypoint point at which contact was established\r
+ // second index: 0 = bearing to airport\r
+ // second index: 1 = distance to airport\r
+ // second index: 2 = alt \r
+ // second index: 3 = ETA\r
+ // second index: 4 = heading to next waypoint\r
+ // second index: 5 = distance to next waypoint\r
+\r
+ double dnwp; // distance to next waypoint\r
+ double dcc; // closest distance to current assigned course\r
+ double dnc; // closest distance to course from next to next to next wp\r
+ double aalt; // assigned altitude\r
+ double ahdg; // assigned heading\r
+ bool on_crs; // is the plane on course?\r
+ bool wp_change; // way point has changed\r
+ double tlm; // time when last message was sent\r
+ TransCode lmc; // code of last message\r
+};\r
+\r
+\r
+class FGApproach : public FGATC {\r
+\r
+ int bucket;\r
+\r
+ string active_runway; \r
+ double active_rw_hdg;\r
+ double active_rw_lon;\r
+ double active_rw_lat;\r
+ double active_rw_len;\r
+\r
+ bool display; // Flag to indicate whether we should be outputting to the display.\r
+ bool displaying; // Flag to indicate whether we are outputting to the display.\r
+ int num_planes; // number of planes on the stack\r
+ PlaneApp planes[max_planes]; // Array of planes\r
+ string transmission;\r
+ bool first;\r
+\r
+ SGPropertyNode *comm1_node;\r
+ SGPropertyNode *comm2_node;\r
+\r
+ SGPropertyNode *atcmenu_node;\r
+ SGPropertyNode *atcopt0_node;\r
+ SGPropertyNode *atcopt1_node;\r
+ SGPropertyNode *atcopt2_node;\r
+ SGPropertyNode *atcopt3_node;\r
+ SGPropertyNode *atcopt4_node;\r
+ SGPropertyNode *atcopt5_node;\r
+ SGPropertyNode *atcopt6_node;\r
+ SGPropertyNode *atcopt7_node;\r
+ SGPropertyNode *atcopt8_node;\r
+ SGPropertyNode *atcopt9_node;\r
+\r
+ // for failure modeling\r
+ string trans_ident; // transmitted ident\r
+ bool approach_failed; // approach failed?\r
+\r
+public:\r
+\r
+ FGApproach(void);\r
+ ~FGApproach(void);\r
+\r
+ void Init();\r
+\r
+ void Update();\r
+\r
+ // Add new plane to stack if not already registered \r
+ // Input: pid - id of plane (name) \r
+ // Output: "true" if added; "false" if already existend\r
+ void AddPlane(string pid);\r
+\r
+ // Remove plane from stack if out of range\r
+ int RemovePlane();\r
+ \r
+ //Indicate that this instance should be outputting to the ATC display\r
+ inline void SetDisplay(void) {display = true;}\r
+ \r
+ //Indicate that this instance should not be outputting to the ATC display\r
+ inline void SetNoDisplay(void) {display = false;}\r
+ \r
+ inline double get_bucket() const { return bucket; }\r
+ inline int get_pnum() const { return num_planes; }\r
+ inline string get_trans_ident() { return trans_ident; }\r
+ inline atc_type GetType() { return APPROACH; }\r
+ \r
+private:\r
+\r
+ void calc_wp( const int &i);\r
+\r
+ void update_plane_dat();\r
+\r
+ void get_active_runway();\r
+\r
+ void update_param(const int &i);\r
+\r
+ double round_alt( bool hl, double alt );\r
+\r
+ double angle_diff_deg( const double &a1, const double &a2);\r
+\r
+// ========================================================================\r
+// get point2 given starting point1 and course and distance\r
+// input: point1 = heading in degrees, distance\r
+// input: course in degrees, distance\r
+// output: point2 = heading in degrees, distance\r
+// ========================================================================\r
+ void calc_cd_head_dist(const double &h1, const double &d1,\r
+ const double &course, const double &dist,\r
+ double *h2, double *d2);\r
+\r
+\r
+// ========================================================================\r
+// get heading and distance between two points; point2 ---> point1\r
+// input: point1 = heading in degrees, distance\r
+// input: point2 = heading in degrees, distance\r
+// output: course in degrees, distance\r
+// ========================================================================\r
+ void calc_hd_course_dist(const double &h1, const double &d1,\r
+ const double &h2, const double &d2,\r
+ double *course, double *dist);\r
+\r
+\r
+\r
+// ========================================================================\r
+// closest distance between a point and a straigt line in 2 dim.\r
+// the input variables are given in (heading, distance) \r
+// relative to a common point\r
+// input: point = heading in degrees, distance\r
+// input: straigt line = anker vector (heading in degrees, distance), \r
+// heading of direction vector\r
+// output: distance\r
+// ========================================================================\r
+ double calc_psl_dist(const double &h1, const double &d1,\r
+ const double &h2, const double &d2,\r
+ const double &h3);\r
+\r
+ // Pointers to current users position\r
+ SGPropertyNode *lon_node;\r
+ SGPropertyNode *lat_node;\r
+ SGPropertyNode *elev_node;\r
+ SGPropertyNode *hdg_node;\r
+ SGPropertyNode *speed_node;\r
+ SGPropertyNode *etime_node;\r
+ \r
+ //Update the transmission string\r
+ void UpdateTransmission(void);\r
+ \r
+ friend istream& operator>> ( istream&, FGApproach& );\r
+};\r
+\r
+#endif // _FG_APPROACH_HXX\r
projects / flightgear.git / commitdiff