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