From f57e4958931cabe834a029f7fa79118a3278c962 Mon Sep 17 00:00:00 2001 From: daveluff <daveluff> Date: Thu, 13 Feb 2003 12:09:28 +0000 Subject: [PATCH] Alexander's Approach ATC class modified to support his new menu/transmission code --- src/ATC/approach.cxx | 1171 +++++++++++++++++++++++++++--------------- src/ATC/approach.hxx | 423 ++++++++------- 2 files changed, 1001 insertions(+), 593 deletions(-) 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 <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. +// +// 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 "transmission.hxx" +#include "transmissionlist.hxx" +#include "ATCdisplay.hxx" +#include "ATCDialog.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 + + +#include <GUI/gui.h> + +//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; i<max_planes; i++) { + planes[i].contact = 0; + planes[i].wpn = 0; + planes[i].dnwp = -999.; + planes[i].on_crs = true; + planes[i].turn_rate = 10.0; + planes[i].desc_rate = 1000.0; + planes[i].clmb_rate = 500.0; + planes[i].tlm = 0.0; + planes[i].lmc.c1 = 0; + planes[i].lmc.c2 = 0; + planes[i].lmc.c3 = -1; + planes[i].wp_change = false; + } +} + +//Destructor +FGApproach::~FGApproach(){ +} + +void FGApproach::Init() { + display = false; +} + + + +// ============================================================================ +// the main update function +// ============================================================================ +void FGApproach::Update() { + + const int max_trans = 20; + FGTransmission tmissions[max_trans]; + int wpn; + int station = 1; + TransCode code; + TransPar TPar; + int i,j; + //double course, d, + double adif, datp; + //char buf[10]; + string message; + //static string atcmsg1[10]; + //static string atcmsg2[10]; + string mentry; + string transm; + TransPar tpars; + //static bool TransDisplayed = false; + + update_plane_dat(); + if ( active_runway == "" ) get_active_runway(); + + double comm1_freq = comm1_node->getDoubleValue(); + + //bool DisplayTransmissions = true; + + for (i=0; i<num_planes; i++) { + if ( planes[i].ident == "Player") { + station = 1; + tpars.station = name; + tpars.callsign = "Player"; + tpars.airport = ident; + + int num_trans = 0; + // is the frequency of the station tuned in? + if ( freq == (int)(comm1_freq*100.0 + 0.5) ) { + current_transmissionlist->query_station( station, tmissions, max_trans, num_trans ); + // loop over all transmissions for station + for ( j=0; j<=num_trans-1; j++ ) { + code = tmissions[j].get_code(); + // 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<num_planes; i++ ) { + + if ( planes[i].ident == TPar.callsign && name == TPar.airport && TPar.station == "approach" ) { + + if ( TPar.request && TPar.intention == "landing" && ident == TPar.intid) { + planes[i].wpn = 0; + // =========================== + // === calculate waypoints === + // =========================== + calc_wp( i ); + update_param( i ); + wpn = planes[i].wpn-1; + planes[i].aalt = planes[i].wpts[wpn-1][2]; + planes[i].ahdg = planes[i].wpts[wpn][4]; + + // generate the message + code.c1 = 1; + code.c2 = 1; + code.c3 = 0; + adif = angle_diff_deg( planes[i].hdg, planes[i].ahdg ); + tpars.station = name; + tpars.callsign = "Player"; + if ( adif < 0 ) tpars.tdir = 1; + else tpars.tdir = 2; + tpars.heading = planes[i].ahdg; + if (planes[i].alt-planes[i].aalt > 100.0) tpars.VDir = 1; + else if (planes[i].alt-planes[i].aalt < -100.0) tpars.VDir = 3; + else tpars.VDir = 2; + tpars.alt = planes[i].aalt; + message = current_transmissionlist->gen_text(station, code, tpars, true ); + 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 <<endl; + if ( fabs(planes[i].dnc) < datp ) { + //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]; + planes[i].aalt = planes[i].wpts[wpn-1][2]; + planes[i].wp_change = true; + + // generate the message + adif = angle_diff_deg( planes[i].hdg, planes[i].ahdg ); + tpars.station = name; + tpars.callsign = "Player"; + if ( adif < 0 ) tpars.tdir = 1; + else tpars.tdir = 2; + tpars.heading = planes[i].ahdg; + + if ( wpn-1 != 0) { + code.c1 = 1; + code.c2 = 1; + code.c3 = 0; + if (planes[i].alt-planes[i].aalt > 100.0) tpars.VDir = 1; + else if (planes[i].alt-planes[i].aalt < -100.0) tpars.VDir = 3; + else tpars.VDir = 2; + tpars.alt = planes[i].aalt; + message = current_transmissionlist->gen_text(station, code, tpars, true ); + 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<wpn-1; ++j ) { + if ( planes[i].wpts[j][1] < mdist) { + } + } + + // ==================== + // vertical navigation + // ==================== + double alt = elev+3000.0; + planes[i].wpts[1][2] = round_alt( true, alt ); + for ( j=2; j<wpn-1; ++j ) { + double dalt = planes[i].alt - planes[i].wpts[j-1][2]; + if ( dalt > 0 ) { + alt = planes[i].wpts[j-1][2] + + (planes[i].wpts[j][5] / planes[i].spd) * 60.0 * planes[i].desc_rate; + planes[i].wpts[j][2] = round_alt( false, alt ); + if ( planes[i].wpts[j][2] > planes[i].alt ) + planes[i].wpts[j][2] = round_alt( false, planes[i].alt ); + } + else { + planes[i].wpts[j][2] = planes[i].wpts[1][2]; + } + } + + cout << "Plane position: " << planes[i].brg << " " << planes[i].dist << endl; + for ( j=0; j<wpn; ++j ) { + cout << "Waypoint " << j << endl; + cout << "------------------" << endl; + cout << planes[i].wpts[j][0] << " " << planes[i].wpts[j][1] + << " " << planes[i].wpts[j][2] << " " << planes[i].wpts[j][5]; + cout << endl << endl; + } + +} + + +// ============================================================================ +// round altitude value to next highest/lowest 500 feet +// ============================================================================ +double FGApproach::round_alt( const bool hl, double alt ) { + + alt = alt/1000.0; + if ( hl ) { + if ( alt > (int)(alt)+0.5 ) alt = ((int)(alt)+1)*1000.0; + else alt = ((int)(alt)+0.5)*1000.0; + } + else { + if ( alt > (int)(alt)+0.5 ) alt = ((int)(alt)+0.5)*1000.0; + else alt = ((int)(alt))*1000.0; + } + + return alt; +} + + +// ============================================================================ +// get active runway +// ============================================================================ +void FGApproach::get_active_runway() { + +#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 <<endl; + } + else cout << "FGRunways search failed\n"; + +} + +// ======================================================================== +// update infos about plane +// ======================================================================== +void FGApproach::update_plane_dat() { + + //cout << "Update Approach " << ident << " " << num_planes << " registered" << endl; + // update plane positions + int i; + for (i=0; i<num_planes; i++) { + planes[i].lon = lon_node->getDoubleValue(); + planes[i].lat = lat_node->getDoubleValue(); + planes[i].alt = elev_node->getDoubleValue(); + planes[i].hdg = hdg_node->getDoubleValue(); + planes[i].spd = speed_node->getDoubleValue(); + + /*Point3D aircraft = sgGeodToCart( Point3D(planes[i].lon*SGD_DEGREES_TO_RADIANS, + planes[i].lat*SGD_DEGREES_TO_RADIANS, + planes[i].alt*SG_FEET_TO_METER) );*/ + double course, distance; + calc_gc_course_dist(Point3D(lon*SGD_DEGREES_TO_RADIANS, lat*SGD_DEGREES_TO_RADIANS, 0.0), + Point3D(planes[i].lon*SGD_DEGREES_TO_RADIANS,planes[i].lat*SGD_DEGREES_TO_RADIANS, 0.0 ), + &course, &distance); + planes[i].dist = distance/SG_NM_TO_METER; + planes[i].brg = 360.0-course*SGD_RADIANS_TO_DEGREES; + + //cout << "Plane Id: " << planes[i].ident << " Distance to " << ident + // << " is " << planes[i].dist << " miles " << "Bearing " << planes[i].brg << endl; + + } +} + +// ======================================================================= +// Add plane to Approach list +// ======================================================================= +void FGApproach::AddPlane(string pid) { + + int i; + for ( 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 (h1,d1) and a straigt line (h2,d2,h3) in 2 dim. +// ================================================================================ +double FGApproach::calc_psl_dist(const double &h1, const double &d1, + const double &h2, const double &d2, + const double &h3) +{ + double a1 = h1 * SGD_DEGREES_TO_RADIANS; + double a2 = h2 * SGD_DEGREES_TO_RADIANS; + double a3 = h3 * SGD_DEGREES_TO_RADIANS; + double x1 = cos(a1) * d1; + double y1 = sin(a1) * d1; + double x2 = cos(a2) * d2; + double y2 = sin(a2) * d2; + double x3 = cos(a3); + double y3 = sin(a3); + + // formula: dis = sqrt( (v1-v2)**2 - ((v1-v2)*v3)**2 ); vi = (xi,yi) + double val1 = (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2); + double val2 = ((x1-x2)*x3 + (y1-y2)*y3) * ((x1-x2)*x3 + (y1-y2)*y3); + double dis = val1 - val2; + // now get sign for offset + //cout << x1 << " " << x2 << " " << y1 << " " << y2 << " " + // << x3 << " " << y3 << " " + // << val1 << " " << val2 << " " << dis << endl; + x3 *= sqrt(val2); + y3 *= sqrt(val2); + double da = fabs(atan2(y3,x3) - atan2(y1-y2,x1-x2)); + if ( da > SGD_PI ) da -= 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<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; +} diff --git a/src/ATC/approach.hxx b/src/ATC/approach.hxx index a5217abf5..cdd06a68c 100644 --- a/src/ATC/approach.hxx +++ b/src/ATC/approach.hxx @@ -1,185 +1,238 @@ -// 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 +// +// 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( SG_HAVE_NATIVE_SGI_COMPILERS ) +# include <iostream.h> +#elif defined( __BORLANDC__ ) +# include <iostream> +#else +# include <istream.h> +#include <iomanip.h> +#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