src/ATC/approach.cxx

   1 // FGApproach - a class to provide approach control at larger airports.
   2 //
   3 // Written by Alexander Kappes, started March 2002.
   4 //
   5 // Copyright (C) 2002  Alexander Kappes
   6 //
   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.
  11 //
  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.
  16 //
  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.
  20
  21 #include "approach.hxx"
  22 #include "ATCdisplay.hxx"
  23 #include <Airports/runways.hxx>
  24
  25 #include <simgear/misc/sg_path.hxx>
  26
  27 #ifdef FG_WEATHERCM
  28 # include <WeatherCM/FGLocalWeatherDatabase.h>
  29 #else
  30 # include <Environment/environment_mgr.hxx>
  31 # include <Environment/environment.hxx>
  32 #endif
  33
  34
  35 PlaneApp::PlaneApp()
  36 :
  37   ident(""),
  38   lon(0.0),
  39   lat(0.0),
  40   alt(0.0),
  41   hdg(0.0),
  42   dist(0.0),
  43   brg(0.0),
  44   spd(0.0),
  45   contact(0),
  46   wpn(0),
  47   dnwp(-999.),
  48   dcc(0.0),
  49   dnc(0.0),
  50   aalt(0.0),
  51   ahdg(0.0),
  52   on_crs(true),
  53   tlm(0.0)
  54 {
  55 }
  56
  57 //Constructor
  58 FGApproach::FGApproach()
  59 : type(0),
  60   lon(0.0), lat(0.0), elev(0.0),
  61   x(0.0), y(0.0), z(0.0),
  62   freq(0),
  63   bucket(0),
  64   range(0.0),
  65   active_runway(""),
  66   active_rw_hdg(0.0),
  67   display(false),
  68   displaying(false),
  69   ident(""),
  70   name(""),
  71   num_planes(0),
  72   transmission(""),
  73   first(true),
  74   trans_ident(""),
  75   approach_failed(false)
  76 {
  77   comm1_node = fgGetNode("/radios/comm[0]/frequencies/selected-mhz", true);
  78   comm2_node = fgGetNode("/radios/comm[1]/frequencies/selected-mhz", true);
  79
  80   lon_node = fgGetNode("/position/longitude-deg", true);
  81   lat_node = fgGetNode("/position/latitude-deg", true);
  82   elev_node = fgGetNode("/position/altitude-ft", true);
  83 }
  84
  85 //Destructor
  86 FGApproach::~FGApproach(){
  87 }
  88
  89 void FGApproach::Init() {
  90   display    = false;
  91 }
  92
  93 // ============================================================================
  94 // the main update function
  95 // ============================================================================
  96 void FGApproach::Update() {
  97
  98   int wpn;
  99   double course, d;
 100
 101   update_plane_dat();
 102   if ( active_runway == "" ) get_active_runway();
 103
 104   for ( int i=0; i<num_planes; i++ ) {
 105
 106     if ( planes[i].contact == 0) {
 107       double comm1_freq = comm1_node->getDoubleValue();
 108       if ( (int)(comm1_freq*100.0 + 0.5) == freq ) planes[i].contact = 1;
 109     }
 110     else if ( planes[i].contact == 1 ) {
 111       if ( planes[i].wpn == 0 ) {    // calculate initial waypoints
 112         wpn = planes[i].wpn;
 113         // airport
 114         planes[i].wpts[wpn][0] = active_rw_hdg;
 115         planes[i].wpts[wpn][1] = 0.0;
 116         planes[i].wpts[wpn][2] = elev;
 117         planes[i].wpts[wpn][4] = 0.0;
 118         planes[i].wpts[wpn][5] = 0.0;
 119         wpn += 1;
 120
 121         planes[i].wpts[wpn][0] = active_rw_hdg + 180.0;
 122         if ( planes[i].wpts[wpn][0] > 360.0 ) planes[i].wpts[wpn][0] -= 360.0;
 123         planes[i].wpts[wpn][1] = 5;
 124         planes[i].wpts[wpn][2] = elev + 1000.0;
 125         calc_hd_course_dist(planes[i].wpts[wpn][0],   planes[i].wpts[wpn][1],
 126                             planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
 127                             &course, &d);
 128         planes[i].wpts[wpn][4] = course;
 129         planes[i].wpts[wpn][5] = d;
 130         wpn += 1;
 131
 132         planes[i].wpts[wpn][0] = planes[i].brg;
 133         planes[i].wpts[wpn][1] = planes[i].dist;
 134         planes[i].wpts[wpn][2] = planes[i].alt;
 135         calc_hd_course_dist(planes[i].wpts[wpn][0],   planes[i].wpts[wpn][1],
 136                             planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
 137                             &course, &d);
 138         planes[i].wpts[wpn][4] = course;
 139         planes[i].wpts[wpn][5] = d;
 140         wpn += 1;
 141
 142         planes[i].wpn = wpn;
 143
 144         planes[i].ahdg = planes[i].wpts[wpn-1][4];
 145         cout << endl;
 146         cout << "Contact " << planes[i].wpn << endl;
 147         cout << "Turn to heading   = " << (int)(planes[i].ahdg) << endl;
 148         cout << endl;
 149         planes[i].on_crs = true;
 150       }
 151
 152       // reached waypoint?
 153       if ( fabs(planes[i].dnc) < 0.3 && planes[i].dnwp < 1.0 ) {
 154         planes[i].wpn -= 1;
 155         wpn = planes[i].wpn-1;
 156         planes[i].ahdg = planes[i].wpts[wpn][4];
 157         cout << endl;
 158         cout << "Next waypoint = " << planes[i].wpn << endl;
 159         cout << "New heading   = " << planes[i].ahdg << endl;
 160         cout << endl;
 161         planes[i].on_crs = true;
 162       }
 163
 164       // update assigned parameters
 165       wpn = planes[i].wpn-1;            // this is the current waypoint
 166
 167       planes[i].dcc  = calc_psl_dist(planes[i].brg, planes[i].dist,
 168                                      planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],
 169                                      planes[i].wpts[wpn][4]);
 170       planes[i].dnc  = calc_psl_dist(planes[i].brg, planes[i].dist,
 171                                      planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
 172                                      planes[i].wpts[wpn-1][4]);
 173       calc_hd_course_dist(planes[i].brg, planes[i].dist,
 174                           planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
 175                           &course, &d);
 176       planes[i].dnwp = d;
 177
 178       //cout << planes[i].brg << " " << planes[i].dist << " " << planes[i].wpts[wpn+1][0]
 179       //<< " " << planes[i].wpts[wpn+1][1] << " " << planes[i].wpts[wpn+1][4]
 180       //cout << " distance to current course = " << planes[i].dcc << endl;
 181
 182       // come off course ?
 183       if ( fabs(planes[i].dcc) > 0.5 && planes[i].on_crs) {
 184         wpn = wpn-1;
 185         if ( planes[i].wpts[wpn][4] < 0) {
 186           planes[i].ahdg += 30.0;
 187         }
 188         else {
 189           planes[i].ahdg -= 30.0;
 190         }
 191         planes[i].on_crs = false;
 192
 193         cout << endl;
 194         cout << "Your are " << planes[i].dcc << " miles off the asigned course: " << endl;
 195         cout << "New heading = " << (int)(planes[i].ahdg) << endl;
 196         cout << endl;
 197       }
 198       else if ( fabs(planes[i].dcc) < 0.1 && !planes[i].on_crs) {
 199         planes[i].ahdg = fabs(planes[i].wpts[wpn][4]);
 200         planes[i].on_crs = true;
 201
 202         cout << endl;
 203         cout << "New heading = " << (int)(planes[i].ahdg) << endl;
 204         cout << endl;
 205         }
 206
 207       // In range of tower?
 208       if ( planes[i].wpn == 2 && planes[i].dnwp < 3. ) {
 209         cout << endl;
 210         cout << "Contact Tower";
 211         cout << endl;
 212         planes[i].contact = 2;
 213       }
 214     }
 215   }
 216
 217 }
 218
 219 // ============================================================================
 220 // get active runway
 221 // ============================================================================
 222 void FGApproach::get_active_runway() {
 223
 224 #ifdef FG_WEATHERCM
 225   sgVec3 position = { lat, lon, elev };
 226   FGPhysicalProperty stationweather = WeatherDatabase->get(position);
 227 #else
 228   FGEnvironment stationweather =
 229     globals->get_environment_mgr()->getEnvironment(lat, lon, elev);
 230 #endif
 231
 232   SGPath path( globals->get_fg_root() );
 233   path.append( "Airports" );
 234   path.append( "runways.mk4" );
 235   FGRunways runways( path.c_str() );
 236
 237   //Set the heading to into the wind
 238 #ifdef FG_WEATHERCM
 239   double wind_x = stationweather.Wind[0];
 240   double wind_y = stationweather.Wind[1];
 241
 242   double speed = sqrt( wind_x*wind_x + wind_y*wind_y ) * SG_METER_TO_NM / (60.0*60.0);
 243   double hdg;
 244
 245   //If no wind use 270degrees
 246   if(speed == 0) {
 247     hdg = 270;
 248   } else {
 249     // //normalize the wind to get the direction
 250     //wind_x /= speed; wind_y /= speed;
 251
 252     hdg = - atan2 ( wind_x, wind_y ) * SG_RADIANS_TO_DEGREES ;
 253     if (hdg < 0.0)
 254       hdg += 360.0;
 255   }
 256 #else
 257   double hdg = stationweather.get_wind_from_heading_deg();
 258 #endif
 259
 260   FGRunway runway;
 261   if ( runways.search( ident, int(hdg), &runway) ) {
 262     active_runway = runway.rwy_no;
 263     active_rw_hdg = runway.heading;
 264     //cout << "Active runway is: " << active_runway << "  heading = "
 265     // << active_rw_hdg << endl;
 266   }
 267   else cout << "FGRunways search failed" << endl;
 268
 269 }
 270
 271 // ========================================================================
 272 // update infos about plane
 273 // ========================================================================
 274 void FGApproach::update_plane_dat() {
 275
 276   //cout << "Update Approach " << ident << "   " << num_planes << " registered" << endl;
 277   // update plane positions
 278   for (int i=0; i<num_planes; i++) {
 279     planes[i].lon = lon_node->getDoubleValue();
 280     planes[i].lat = lat_node->getDoubleValue();
 281     planes[i].alt = elev_node->getDoubleValue();
 282 //    Point3D aircraft = sgGeodToCart( Point3D(planes[i].lon*SGD_DEGREES_TO_RADIANS,
 283 //                                           planes[i].lat*SGD_DEGREES_TO_RADIANS,
 284 //                                           planes[i].alt*SG_FEET_TO_METER) );
 285     double course, distance;
 286     calc_gc_course_dist(Point3D(lon*SGD_DEGREES_TO_RADIANS, lat*SGD_DEGREES_TO_RADIANS, 0.0),
 287                         Point3D(planes[i].lon*SGD_DEGREES_TO_RADIANS,planes[i].lat*SGD_DEGREES_TO_RADIANS, 0.0 ),
 288                         &course, &distance);
 289     planes[i].dist = distance/SG_NM_TO_METER;
 290     planes[i].brg  = 360.0-course*SGD_RADIANS_TO_DEGREES;
 291
 292     //cout << "Plane Id: " << planes[i].ident << "  Distance to " << ident
 293     //<< " is " << planes[i].dist << " m" << endl;
 294
 295     //if (first) {
 296     //transmission = ident;
 297     //globals->get_ATC_display()->RegisterRepeatingMessage(transmission);
 298     //first = false;
 299     //}
 300   }
 301 }
 302
 303 // =======================================================================
 304 // Add plane to Approach list
 305 // =======================================================================
 306 void FGApproach::AddPlane(string pid) {
 307   for ( int i=0; i<num_planes; i++) {
 308     if ( planes[i].ident == pid) {
 309       //cout << "Plane already registered: " << ident << " " << num_planes << endl;
 310       return;
 311     }
 312   }
 313   planes[num_planes].ident = pid;
 314   ++num_planes;
 315   //cout << "Plane added to list: " << ident << " " << num_planes << endl;
 316   return;
 317 }
 318
 319 // ========================================================================
 320 // closest distance between a point and a straigt line in 2 dim.
 321 // ========================================================================
 322 double FGApproach::calc_psl_dist(const double &h1, const double &d1,
 323                                  const double &h2, const double &d2,
 324                                  const double &h3)
 325 {
 326   double a1 = h1 * SGD_DEGREES_TO_RADIANS;
 327   double a2 = h2 * SGD_DEGREES_TO_RADIANS;
 328   double a3 = h3 * SGD_DEGREES_TO_RADIANS;
 329   double x1 = cos(a1) * d1;
 330   double y1 = sin(a1) * d1;
 331   double x2 = cos(a2) * d2;
 332   double y2 = sin(a2) * d2;
 333   double x3 = cos(a3);
 334   double y3 = sin(a3);
 335
 336   // formula: dis = sqrt( (v1-v2)**2 - ((v1-v2)*v3)**2 ); vi = (xi,yi)
 337   double val1   = (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2);
 338   double val2   = ((x1-x2)*x3 + (y1-y2)*y3) * ((x1-x2)*x3 + (y1-y2)*y3);
 339   double dis    = val1 - val2;
 340   // now get sign for offset
 341   //cout << x1 << " " << x2 << " " << y1 << " " << y2 << " "
 342   //     << x3 << " " << y3 << " "
 343   //     << val1 << " " << val2 << " " << dis << endl;
 344   x3 *= sqrt(val2);
 345   y3 *= sqrt(val2);
 346   if ( x3*(x1-x2) < 0.0 && y3*(y1-y2) < 0.0) {
 347     x3 *= -1.0;
 348     y3 *= -1.0;
 349   }
 350   //cout << x3 << " " << y3 << endl;
 351   double dis1   = x1-x2-x3;
 352   double dis2   = y1-y2-y3;
 353   dis = sqrt(dis);
 354   if (atan2(dis2,dis1) < a3) dis *= -1.0;
 355   //cout << dis1 << " " << dis2 << " " << atan2(dis2,dis1)*SGD_RADIANS_TO_DEGREES << " " << h3
 356   //     << " " << sqrt(dis1*dis1 + dis2*dis2) << " " << dis << endl;
 357   //cout << atan2(dis2,dis1)*SGD_RADIANS_TO_DEGREES << " " << dis << endl;
 358
 359   return dis;
 360 }
 361
 362 // ========================================================================
 363 // get heading and distance between two points; point1 ---> point2
 364 // ========================================================================
 365 void FGApproach::calc_hd_course_dist(const double &h1, const double &d1,
 366                                      const double &h2, const double &d2,
 367                                      double *course, double *dist)
 368 {
 369   double a1 = h1 * SGD_DEGREES_TO_RADIANS;
 370   double a2 = h2 * SGD_DEGREES_TO_RADIANS;
 371   double x1 = cos(a1) * d1;
 372   double y1 = sin(a1) * d1;
 373   double x2 = cos(a2) * d2;
 374   double y2 = sin(a2) * d2;
 375
 376   *dist   = sqrt( (y2-y1)*(y2-y1) + (x2-x1)*(x2-x1) );
 377   *course = atan2( (y2-y1), (x2-x1) ) * SGD_RADIANS_TO_DEGREES;
 378   if ( *course < 0 ) *course = *course+360;
 379   //cout << x1 << " " << y1 << " " << x2 << " " << y2 << " " << *dist << " " << *course << endl;
 380 }
 381
 382
 383
 384 int FGApproach::RemovePlane() {
 385
 386   // first check if anything has to be done
 387   int i;
 388   bool rmplane = false;
 389   for (i=0; i<num_planes; i++) {
 390     if (planes[i].dist > range*SG_NM_TO_METER) {
 391       rmplane = true;
 392       break;
 393     }
 394   }
 395   if (!rmplane) return num_planes;
 396
 397   // now make a copy of the plane list
 398   PlaneApp tmp[max_planes];
 399   for (i=0; i<num_planes; i++) {
 400     tmp[i] = planes[i];
 401   }
 402
 403   int np = 0;
 404   // now check which planes are still in range
 405   for (i=0; i<num_planes; i++) {
 406     if (tmp[i].dist <= range*SG_NM_TO_METER) {
 407       planes[np] = tmp[i];
 408       np += 1;
 409     }
 410   }
 411   num_planes = np;
 412   return num_planes;
 413 }