1 // FGApproach - a class to provide approach control at larger airports.
3 // Written by Alexander Kappes, started March 2002.
5 // Copyright (C) 2002 Alexander Kappes
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Public License for more details.
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
21 #include "approach.hxx"
22 #include "ATCdisplay.hxx"
23 #include <Airports/runways.hxx>
25 #include <simgear/misc/sg_path.hxx>
27 #ifdef FG_NEW_ENVIRONMENT
28 #include <Environment/environment_mgr.hxx>
29 #include <Environment/environment.hxx>
31 #include <WeatherCM/FGLocalWeatherDatabase.h>
35 FGApproach::FGApproach(){
36 comm1_node = fgGetNode("/radios/comm[0]/frequencies/selected-mhz", true);
37 comm2_node = fgGetNode("/radios/comm[1]/frequencies/selected-mhz", true);
40 lon_node = fgGetNode("/position/longitude-deg", true);
41 lat_node = fgGetNode("/position/latitude-deg", true);
42 elev_node = fgGetNode("/position/altitude-ft", true);
45 for ( int i=0; i<max_planes; i++) {
46 planes[i].contact = 0;
48 planes[i].dnwp = -999.;
49 planes[i].on_crs = true;
54 FGApproach::~FGApproach(){
57 void FGApproach::Init() {
61 // ============================================================================
62 // the main update function
63 // ============================================================================
64 void FGApproach::Update() {
70 if ( active_runway == "" ) get_active_runway();
72 for ( int i=0; i<num_planes; i++ ) {
74 if ( planes[i].contact == 0) {
75 double comm1_freq = comm1_node->getDoubleValue();
76 if ( (int)(comm1_freq*100.0 + 0.5) == freq ) planes[i].contact = 1;
78 else if ( planes[i].contact == 1 ) {
79 if ( planes[i].wpn == 0 ) { // calculate initial waypoints
82 planes[i].wpts[wpn][0] = active_rw_hdg;
83 planes[i].wpts[wpn][1] = 0.0;
84 planes[i].wpts[wpn][2] = elev;
85 planes[i].wpts[wpn][4] = 0.0;
86 planes[i].wpts[wpn][5] = 0.0;
89 planes[i].wpts[wpn][0] = active_rw_hdg + 180.0;
90 if ( planes[i].wpts[wpn][0] > 360.0 ) planes[i].wpts[wpn][0] -= 360.0;
91 planes[i].wpts[wpn][1] = 5;
92 planes[i].wpts[wpn][2] = elev + 1000.0;
93 calc_hd_course_dist(planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],
94 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
96 planes[i].wpts[wpn][4] = course;
97 planes[i].wpts[wpn][5] = d;
100 planes[i].wpts[wpn][0] = planes[i].brg;
101 planes[i].wpts[wpn][1] = planes[i].dist;
102 planes[i].wpts[wpn][2] = planes[i].alt;
103 calc_hd_course_dist(planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],
104 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
106 planes[i].wpts[wpn][4] = course;
107 planes[i].wpts[wpn][5] = d;
112 planes[i].ahdg = planes[i].wpts[wpn-1][4];
114 cout << "Contact " << planes[i].wpn << endl;
115 cout << "Turn to heading = " << (int)(planes[i].ahdg) << endl;
117 planes[i].on_crs = true;
121 if ( fabs(planes[i].dnc) < 0.3 && planes[i].dnwp < 1.0 ) {
123 wpn = planes[i].wpn-1;
124 planes[i].ahdg = planes[i].wpts[wpn][4];
126 cout << "Next waypoint = " << planes[i].wpn << endl;
127 cout << "New heading = " << planes[i].ahdg << endl;
129 planes[i].on_crs = true;
132 // update assigned parameters
133 wpn = planes[i].wpn-1; // this is the current waypoint
135 planes[i].dcc = calc_psl_dist(planes[i].brg, planes[i].dist,
136 planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],
137 planes[i].wpts[wpn][4]);
138 planes[i].dnc = calc_psl_dist(planes[i].brg, planes[i].dist,
139 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
140 planes[i].wpts[wpn-1][4]);
141 calc_hd_course_dist(planes[i].brg, planes[i].dist,
142 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
146 //cout << planes[i].brg << " " << planes[i].dist << " " << planes[i].wpts[wpn+1][0]
147 //<< " " << planes[i].wpts[wpn+1][1] << " " << planes[i].wpts[wpn+1][4]
148 //cout << " distance to current course = " << planes[i].dcc << endl;
151 if ( fabs(planes[i].dcc) > 0.5 && planes[i].on_crs) {
153 if ( planes[i].wpts[wpn][4] < 0) {
154 planes[i].ahdg += 30.0;
157 planes[i].ahdg -= 30.0;
159 planes[i].on_crs = false;
162 cout << "Your are " << planes[i].dcc << " miles off the asigned course: " << endl;
163 cout << "New heading = " << (int)(planes[i].ahdg) << endl;
166 else if ( fabs(planes[i].dcc) < 0.1 && !planes[i].on_crs) {
167 planes[i].ahdg = fabs(planes[i].wpts[wpn][4]);
168 planes[i].on_crs = true;
171 cout << "New heading = " << (int)(planes[i].ahdg) << endl;
175 // In range of tower?
176 if ( planes[i].wpn == 2 && planes[i].dnwp < 3. ) {
178 cout << "Contact Tower";
180 planes[i].contact = 2;
187 // ============================================================================
189 // ============================================================================
190 void FGApproach::get_active_runway() {
192 #ifdef FG_NEW_ENVIRONMENT
193 FGEnvironment stationweather =
194 globals->get_environment_mgr()->getEnvironment(lat, lon, elev);
196 sgVec3 position = { lat, lon, elev };
197 FGPhysicalProperty stationweather = WeatherDatabase->get(position);
200 SGPath path( globals->get_fg_root() );
201 path.append( "Airports" );
202 path.append( "runways.mk4" );
203 FGRunways runways( path.c_str() );
205 //Set the heading to into the wind
206 #ifdef FG_NEW_ENVIRONMENT
207 double hdg = stationweather.get_wind_from_heading_deg();
209 double wind_x = stationweather.Wind[0];
210 double wind_y = stationweather.Wind[1];
212 double speed = sqrt( wind_x*wind_x + wind_y*wind_y ) * SG_METER_TO_NM / (60.0*60.0);
215 //If no wind use 270degrees
219 // //normalize the wind to get the direction
220 //wind_x /= speed; wind_y /= speed;
222 hdg = - atan2 ( wind_x, wind_y ) * SG_RADIANS_TO_DEGREES ;
229 if ( runways.search( ident, int(hdg), &runway) ) {
230 active_runway = runway.rwy_no;
231 active_rw_hdg = runway.heading;
232 //cout << "Active runway is: " << active_runway << " heading = "
233 // << active_rw_hdg << endl;
235 else cout << "FGRunways search failed" << endl;
239 // ========================================================================
240 // update infos about plane
241 // ========================================================================
242 void FGApproach::update_plane_dat() {
244 //cout << "Update Approach " << ident << " " << num_planes << " registered" << endl;
245 // update plane positions
246 for (int i=0; i<num_planes; i++) {
247 planes[i].lon = lon_node->getDoubleValue();
248 planes[i].lat = lat_node->getDoubleValue();
249 planes[i].alt = elev_node->getDoubleValue();
250 // Point3D aircraft = sgGeodToCart( Point3D(planes[i].lon*SGD_DEGREES_TO_RADIANS,
251 // planes[i].lat*SGD_DEGREES_TO_RADIANS,
252 // planes[i].alt*SG_FEET_TO_METER) );
253 double course, distance;
254 calc_gc_course_dist(Point3D(lon*SGD_DEGREES_TO_RADIANS, lat*SGD_DEGREES_TO_RADIANS, 0.0),
255 Point3D(planes[i].lon*SGD_DEGREES_TO_RADIANS,planes[i].lat*SGD_DEGREES_TO_RADIANS, 0.0 ),
257 planes[i].dist = distance/SG_NM_TO_METER;
258 planes[i].brg = 360.0-course*SGD_RADIANS_TO_DEGREES;
260 //cout << "Plane Id: " << planes[i].ident << " Distance to " << ident
261 //<< " is " << planes[i].dist << " m" << endl;
264 //transmission = ident;
265 //globals->get_ATC_display()->RegisterRepeatingMessage(transmission);
271 // =======================================================================
272 // Add plane to Approach list
273 // =======================================================================
274 void FGApproach::AddPlane(string pid) {
275 for ( int i=0; i<num_planes; i++) {
276 if ( planes[i].ident == pid) {
277 //cout << "Plane already registered: " << ident << " " << num_planes << endl;
281 planes[num_planes].ident = pid;
283 //cout << "Plane added to list: " << ident << " " << num_planes << endl;
287 // ========================================================================
288 // closest distance between a point and a straigt line in 2 dim.
289 // ========================================================================
290 double FGApproach::calc_psl_dist(const double &h1, const double &d1,
291 const double &h2, const double &d2,
294 double a1 = h1 * SGD_DEGREES_TO_RADIANS;
295 double a2 = h2 * SGD_DEGREES_TO_RADIANS;
296 double a3 = h3 * SGD_DEGREES_TO_RADIANS;
297 double x1 = cos(a1) * d1;
298 double y1 = sin(a1) * d1;
299 double x2 = cos(a2) * d2;
300 double y2 = sin(a2) * d2;
304 // formula: dis = sqrt( (v1-v2)**2 - ((v1-v2)*v3)**2 ); vi = (xi,yi)
305 double val1 = (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2);
306 double val2 = ((x1-x2)*x3 + (y1-y2)*y3) * ((x1-x2)*x3 + (y1-y2)*y3);
307 double dis = val1 - val2;
308 // now get sign for offset
309 //cout << x1 << " " << x2 << " " << y1 << " " << y2 << " "
310 // << x3 << " " << y3 << " "
311 // << val1 << " " << val2 << " " << dis << endl;
314 if ( x3*(x1-x2) < 0.0 && y3*(y1-y2) < 0.0) {
318 //cout << x3 << " " << y3 << endl;
319 double dis1 = x1-x2-x3;
320 double dis2 = y1-y2-y3;
322 if (atan2(dis2,dis1) < a3) dis *= -1.0;
323 //cout << dis1 << " " << dis2 << " " << atan2(dis2,dis1)*SGD_RADIANS_TO_DEGREES << " " << h3
324 // << " " << sqrt(dis1*dis1 + dis2*dis2) << " " << dis << endl;
325 //cout << atan2(dis2,dis1)*SGD_RADIANS_TO_DEGREES << " " << dis << endl;
330 // ========================================================================
331 // get heading and distance between two points; point1 ---> point2
332 // ========================================================================
333 void FGApproach::calc_hd_course_dist(const double &h1, const double &d1,
334 const double &h2, const double &d2,
335 double *course, double *dist)
337 double a1 = h1 * SGD_DEGREES_TO_RADIANS;
338 double a2 = h2 * SGD_DEGREES_TO_RADIANS;
339 double x1 = cos(a1) * d1;
340 double y1 = sin(a1) * d1;
341 double x2 = cos(a2) * d2;
342 double y2 = sin(a2) * d2;
344 *dist = sqrt( (y2-y1)*(y2-y1) + (x2-x1)*(x2-x1) );
345 *course = atan2( (y2-y1), (x2-x1) ) * SGD_RADIANS_TO_DEGREES;
346 if ( *course < 0 ) *course = *course+360;
347 //cout << x1 << " " << y1 << " " << x2 << " " << y2 << " " << *dist << " " << *course << endl;
352 int FGApproach::RemovePlane() {
354 // first check if anything has to be done
356 bool rmplane = false;
357 for (i=0; i<num_planes; i++) {
358 if (planes[i].dist > range*SG_NM_TO_METER) {
363 if (!rmplane) return num_planes;
365 // now make a copy of the plane list
366 PlaneApp tmp[max_planes];
367 for (i=0; i<num_planes; i++) {
372 // now check which planes are still in range
373 for (i=0; i<num_planes; i++) {
374 if (tmp[i].dist <= range*SG_NM_TO_METER) {