From: daveluff Date: Thu, 13 Feb 2003 12:09:28 +0000 (+0000) Subject: Alexander's Approach ATC class modified to support his new menu/transmission code X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=f57e4958931cabe834a029f7fa79118a3278c962;p=flightgear.git Alexander's Approach ATC class modified to support his new menu/transmission code --- diff --git a/src/ATC/approach.cxx b/src/ATC/approach.cxx index 273fd82ab..11cbfb24b 100644 --- a/src/ATC/approach.cxx +++ b/src/ATC/approach.cxx @@ -1,408 +1,763 @@ -// 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 - -#include - -#ifdef FG_WEATHERCM -# include -#else -# include -# include -#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; igetDoubleValue(); - 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; igetDoubleValue(); - 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 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 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 +#include + +#ifdef FG_WEATHERCM +# include +#else +# include +# include +#endif + + +#include + +//Constructor +FGApproach::FGApproach(){ + comm1_node = fgGetNode("/radios/comm[0]/frequencies/selected-mhz", true); + comm2_node = fgGetNode("/radios/comm[1]/frequencies/selected-mhz", true); + + 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-ms", true); + + first = true; + active_runway = ""; + int i; + for ( i=0; igetDoubleValue(); + + //bool DisplayTransmissions = true; + + for (i=0; iquery_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(); + // select proper transmissions + 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 )) { + current_atcdialog->add_entry( ident, transm, mentry ); + } + } + } + } + } + } + + for ( i=0; i 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 ); + globals->get_ATC_display()->RegisterSingleMessage( message, 0 ); + planes[i].lmc = code; + planes[i].tlm = etime_node->getDoubleValue(); + planes[i].on_crs = true; + planes[i].contact = 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 < 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 ); + globals->get_ATC_display()->RegisterSingleMessage( message, 0 ); + + } + else { + code.c1 = 1; + code.c2 = 3; + code.c3 = 0; + tpars.runway = active_runway; + message = current_transmissionlist->gen_text(station, code, tpars, true); + globals->get_ATC_display()->RegisterSingleMessage( message, 0 ); + } + 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 ) ) { + 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); + globals->get_ATC_display()->RegisterSingleMessage( message, 0 ); + planes[i].lmc = code; + planes[i].tlm = etime_node->getDoubleValue(); + } + + planes[i].on_crs = false; + } + else if ( !planes[i].on_crs ) { + 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); + globals->get_ATC_display()->RegisterSingleMessage( message, 0 ); + 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; + // 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); + globals->get_ATC_display()->RegisterSingleMessage( message, 0 ); + 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., + &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 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 (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() { + +#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() ); + +#ifdef FG_WEATHERCM + //Set the heading to into the wind + 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( 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 <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(string pid) { + + int i; + for ( i=0; i SGD_PI ) da -= 2*SGD_PI; + 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 += 2*SGD_PI; + 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 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 - -#include -#include -#include -#include -#include -#include - -#include
- -#ifdef SG_HAVE_STD_INCLUDES -# include -#include -#elif defined( __BORLANDC__ ) || (__APPLE__) -# include -#else -# include -#include -#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 +// +// 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 + +#include +#include +#include +#include +#include +#include + +#include
+ +#ifdef SG_HAVE_STD_INCLUDES +# include +#include +#elif defined( SG_HAVE_NATIVE_SGI_COMPILERS ) +# include +#elif defined( __BORLANDC__ ) +# include +#else +# include +#include +#endif + +#if ! defined( SG_HAVE_NATIVE_SGI_COMPILERS ) +SG_USING_STD(istream); +#endif + +SG_USING_STD(string); + +#include "ATC.hxx" +#include "transmission.hxx" + +//DCL - a complete guess for now. +#define FG_APPROACH_DEFAULT_RANGE 100 + +// Contains all the 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 +const double max_ta = 130; // max turning angle for plane during approach +const double tbm = 20000.0; // min time (in ms) between two messages +const double lfl = 10.0; // length of final leg + +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 + double turn_rate; // standard turning rate of the plane in seconds per degree + double desc_rate; // standard descent rate of the plane in feets per minute + double clmb_rate; // standard climb rate of the plane in feets per minute + + // 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 airport + // 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 altitude + double ahdg; // assigned heading + bool on_crs; // is the plane on course? + bool wp_change; // way point has changed + double tlm; // time when last message was sent + TransCode lmc; // code of last message +}; + + +class FGApproach : public FGATC { + + int bucket; + + string active_runway; + double active_rw_hdg; + double active_rw_lon; + double active_rw_lat; + double active_rw_len; + + 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; + + SGPropertyNode *atcmenu_node; + SGPropertyNode *atcopt0_node; + SGPropertyNode *atcopt1_node; + SGPropertyNode *atcopt2_node; + SGPropertyNode *atcopt3_node; + SGPropertyNode *atcopt4_node; + SGPropertyNode *atcopt5_node; + SGPropertyNode *atcopt6_node; + SGPropertyNode *atcopt7_node; + SGPropertyNode *atcopt8_node; + SGPropertyNode *atcopt9_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 calc_wp( const int &i); + + void update_plane_dat(); + + void get_active_runway(); + + void update_param(const int &i); + + double round_alt( bool hl, double alt ); + + double angle_diff_deg( const double &a1, const double &a2); + +// ======================================================================== +// get point2 given starting point1 and course and distance +// input: point1 = heading in degrees, distance +// input: course in degrees, distance +// output: point2 = heading in degrees, distance +// ======================================================================== + void calc_cd_head_dist(const double &h1, const double &d1, + const double &course, const double &dist, + double *h2, double *d2); + + +// ======================================================================== +// get heading and distance between two points; point2 ---> point1 +// input: point1 = heading in degrees, distance +// input: point2 = heading in degrees, distance +// output: 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; + SGPropertyNode *hdg_node; + SGPropertyNode *speed_node; + SGPropertyNode *etime_node; + + //Update the transmission string + void UpdateTransmission(void); + + friend istream& operator>> ( istream&, FGApproach& ); +}; + +#endif // _FG_APPROACH_HXX