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 "transmission.hxx"
  23 #include "transmissionlist.hxx"
  24 #include "ATCdisplay.hxx"
  25 #include "ATCDialog.hxx"
  26
  27 #include <Airports/runways.hxx>
  28 #include <simgear/misc/sg_path.hxx>
  29
  30 #ifdef FG_WEATHERCM
  31 # include <WeatherCM/FGLocalWeatherDatabase.h>
  32 #else
  33 # include <Environment/environment_mgr.hxx>
  34 # include <Environment/environment.hxx>
  35 #endif
  36
  37
  38 #include <GUI/gui.h>
  39
  40 //Constructor
  41 FGApproach::FGApproach(){
  42   comm1_node = fgGetNode("/radios/comm[0]/frequencies/selected-mhz", true);
  43   comm2_node = fgGetNode("/radios/comm[1]/frequencies/selected-mhz", true);
  44
  45   num_planes = 0;
  46   lon_node   = fgGetNode("/position/longitude-deg", true);
  47   lat_node   = fgGetNode("/position/latitude-deg", true);
  48   elev_node  = fgGetNode("/position/altitude-ft", true);
  49   hdg_node   = fgGetNode("/orientation/heading-deg", true);
  50   speed_node = fgGetNode("/velocities/airspeed-kt", true);
  51   etime_node = fgGetNode("/sim/time/elapsed-ms", true);
  52
  53   first = true;
  54   active_runway = "";
  55   int i;
  56   for ( i=0; i<max_planes; i++) {
  57     planes[i].contact   = 0;
  58     planes[i].wpn       = 0;
  59     planes[i].dnwp      = -999.;
  60     planes[i].on_crs    = true;
  61     planes[i].turn_rate = 10.0;
  62     planes[i].desc_rate = 1000.0;
  63     planes[i].clmb_rate = 500.0;
  64     planes[i].tlm       = 0.0;
  65     planes[i].lmc.c1    = 0;
  66     planes[i].lmc.c2    = 0;
  67     planes[i].lmc.c3    = -1;
  68     planes[i].wp_change = false;
  69   }
  70 }
  71
  72 //Destructor
  73 FGApproach::~FGApproach(){
  74 }
  75
  76 void FGApproach::Init() {
  77   display    = false;
  78 }
  79
  80
  81
  82 // ============================================================================
  83 // the main update function
  84 // ============================================================================
  85 void FGApproach::Update(double dt) {
  86
  87         const int max_trans = 20;
  88         FGTransmission tmissions[max_trans];
  89         int    wpn;
  90         int    station = 1;
  91         TransCode code;
  92         TransPar TPar;
  93         int    i,j;
  94         //double course, d,
  95         double adif, datp;
  96         //char   buf[10];
  97         string message;
  98         //static string atcmsg1[10];
  99         //static string atcmsg2[10];
 100         string mentry;
 101         string transm;
 102         TransPar tpars;
 103         //static bool TransDisplayed = false;
 104
 105         update_plane_dat();
 106         if ( active_runway == "" ) get_active_runway();
 107
 108         double comm1_freq = comm1_node->getDoubleValue();
 109
 110         //bool DisplayTransmissions = true;
 111
 112         for (i=0; i<num_planes; i++) {
 113                 if ( planes[i].ident == "Player") {
 114                         station = 1;
 115                         tpars.station = name;
 116                         tpars.callsign = "Player";
 117                         tpars.airport = ident;
 118
 119                         int num_trans = 0;
 120                         // is the frequency of the station tuned in?
 121                         if ( freq == (int)(comm1_freq*100.0 + 0.5) ) {
 122                                 current_transmissionlist->query_station( station, tmissions, max_trans, num_trans );
 123                                 // loop over all transmissions for station
 124                                 for ( j=0; j<=num_trans-1; j++ ) {
 125                                         code = tmissions[j].get_code();
 126                                         // select proper transmissions
 127                                         if ( ( code.c2 == -1 && planes[i].lmc.c3 == 0 ) ||
 128                                                 ( code.c1 == 0  && code.c2 == planes[i].lmc.c2 ) ) {
 129                                                 mentry = current_transmissionlist->gen_text(station, code, tpars, false);
 130                                                 transm = current_transmissionlist->gen_text(station, code, tpars, true);
 131                                                 // is the transmission already registered?
 132                                                 if (!current_atcdialog->trans_reg( ident, transm )) {
 133                                                         current_atcdialog->add_entry( ident, transm, mentry );
 134                                                 }
 135                                         }
 136                                 }
 137                         }
 138                 }
 139         }
 140
 141         for ( i=0; i<num_planes; i++ ) {
 142
 143                 if ( planes[i].ident == TPar.callsign && name == TPar.airport && TPar.station == "approach" ) {
 144
 145                         if ( TPar.request && TPar.intention == "landing" && ident == TPar.intid) {
 146                                 planes[i].wpn = 0;
 147                                 // ===========================
 148                                 // === calculate waypoints ===
 149                                 // ===========================
 150                                 calc_wp( i );
 151                                 update_param( i );
 152                                 wpn = planes[i].wpn-1;
 153                                 planes[i].aalt = planes[i].wpts[wpn-1][2];
 154                                 planes[i].ahdg = planes[i].wpts[wpn][4];
 155
 156                                 // generate the message
 157                                 code.c1 = 1;
 158                                 code.c2 = 1;
 159                                 code.c3 = 0;
 160                                 adif = angle_diff_deg( planes[i].hdg, planes[i].ahdg );
 161                                 tpars.station = name;
 162                                 tpars.callsign = "Player";
 163                                 if ( adif < 0 ) tpars.tdir = 1;
 164                                 else            tpars.tdir = 2;
 165                                 tpars.heading = planes[i].ahdg;
 166                                 if      (planes[i].alt-planes[i].aalt > 100.0)  tpars.VDir = 1;
 167                                 else if (planes[i].alt-planes[i].aalt < -100.0) tpars.VDir = 3;
 168                                 else tpars.VDir = 2;
 169                                 tpars.alt = planes[i].aalt;
 170                                 message = current_transmissionlist->gen_text(station, code, tpars, true );
 171                                 globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
 172                                 planes[i].lmc = code;
 173                                 planes[i].tlm = etime_node->getDoubleValue();
 174                                 planes[i].on_crs = true;
 175                                 planes[i].contact = 1;
 176                         }
 177                 }
 178
 179                 if ( planes[i].contact == 1 ) {
 180                         // =========================
 181                         // === update parameters ===
 182                         // =========================
 183                         update_param( i );
 184                         //cout << planes[i].brg << " " << planes[i].dist << " " << planes[i].wpts[wpn+1][0]
 185                         //<< " " << planes[i].wpts[wpn+1][1] << " " << planes[i].wpts[wpn+1][4]
 186                         //cout << wpn << " distance to current course = " << planes[i].dcc << endl;
 187                         //cout << etime_node->getDoubleValue() << endl;
 188
 189                         // =========================
 190                         // === reached waypoint? ===
 191                         // =========================
 192                         wpn = planes[i].wpn-2;
 193                         adif = angle_diff_deg( planes[i].hdg, planes[i].wpts[wpn][4] )
 194                         * SGD_DEGREES_TO_RADIANS;
 195                         datp = 2*sin(fabs(adif)/2.0)*sin(fabs(adif)/2.0) *
 196                         planes[i].spd/3600. * planes[i].turn_rate +
 197                         planes[i].spd/3600. * 3.0;
 198                         //cout << adif/SGD_DEGREES_TO_RADIANS << " "
 199                         //     << datp << " " << planes[i].dnc << " " << planes[i].dcc <<endl;
 200                         if ( fabs(planes[i].dnc) < datp ) {
 201                         //if ( fabs(planes[i].dnc) < 0.3 && planes[i].dnwp < 1.0 ) {
 202                                 planes[i].wpn -= 1;
 203                                 wpn = planes[i].wpn-1;
 204                                 planes[i].ahdg = planes[i].wpts[wpn][4];
 205                                 planes[i].aalt = planes[i].wpts[wpn-1][2];
 206                                 planes[i].wp_change = true;
 207
 208                                 // generate the message
 209                                 adif = angle_diff_deg( planes[i].hdg, planes[i].ahdg );
 210                                 tpars.station = name;
 211                                 tpars.callsign = "Player";
 212                                 if ( adif < 0 ) tpars.tdir = 1;
 213                                 else            tpars.tdir = 2;
 214                                 tpars.heading = planes[i].ahdg;
 215
 216                                 if ( wpn-1 != 0) {
 217                                         code.c1 = 1;
 218                                         code.c2 = 1;
 219                                         code.c3 = 0;
 220                                         if      (planes[i].alt-planes[i].aalt > 100.0)  tpars.VDir = 1;
 221                                         else if (planes[i].alt-planes[i].aalt < -100.0) tpars.VDir = 3;
 222                                         else tpars.VDir = 2;
 223                                         tpars.alt = planes[i].aalt;
 224                                         message = current_transmissionlist->gen_text(station, code, tpars, true );
 225                                         globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
 226
 227                                 }
 228                                 else {
 229                                         code.c1 = 1;
 230                                         code.c2 = 3;
 231                                         code.c3 = 0;
 232                                         tpars.runway = active_runway;
 233                                         message = current_transmissionlist->gen_text(station, code, tpars, true);
 234                                         globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
 235                                 }
 236                                 planes[i].lmc = code;
 237                                 planes[i].tlm = etime_node->getDoubleValue();
 238                                 planes[i].on_crs = true;
 239
 240                                 update_param( i );
 241                         }
 242
 243                         // =========================
 244                         // === come off course ? ===
 245                         // =========================
 246                         if ( fabs(planes[i].dcc) > 1.0 &&
 247                                 ( !planes[i].wp_change ||
 248                         etime_node->getDoubleValue() - planes[i].tlm > tbm ) ) {
 249                                 if ( planes[i].on_crs ) {
 250                                         if ( planes[i].dcc < 0) {
 251                                                 planes[i].ahdg += 30.0;
 252                                         }
 253                                         else {
 254                                                 planes[i].ahdg -= 30.0;
 255                                         }
 256                                         if (planes[i].ahdg > 360.0) planes[i].ahdg -= 360.0;
 257                                         else if (planes[i].ahdg < 0.0) planes[i].ahdg += 360.0;
 258                                 }
 259                                 //cout << planes[i].on_crs << " "
 260                                 //     << angle_diff_deg( planes[i].hdg, planes[i].ahdg) << " "
 261                                 //     << etime_node->getDoubleValue() << " "
 262                                 //     << planes[i].tlm << endl;
 263                                 // generate the message
 264                                 if ( planes[i].on_crs ||
 265                                         ( fabs(angle_diff_deg( planes[i].hdg, planes[i].ahdg )) >  30.0  &&
 266                                 etime_node->getDoubleValue() - planes[i].tlm > tbm) ) {
 267                                         // generate the message
 268                                         code.c1 = 1;
 269                                         code.c2 = 4;
 270                                         code.c3 = 0;
 271                                         adif = angle_diff_deg( planes[i].hdg, planes[i].ahdg );
 272                                         tpars.station = name;
 273                                         tpars.callsign = "Player";
 274                                         tpars.miles   = fabs(planes[i].dcc);
 275                                         if ( adif < 0 ) tpars.tdir = 1;
 276                                         else            tpars.tdir = 2;
 277                                         tpars.heading = planes[i].ahdg;
 278                                         message = current_transmissionlist->gen_text(station, code, tpars, true);
 279                                         globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
 280                                         planes[i].lmc = code;
 281                                         planes[i].tlm = etime_node->getDoubleValue();
 282                                 }
 283
 284                                 planes[i].on_crs = false;
 285                         }
 286                         else if ( !planes[i].on_crs ) {
 287                                 wpn = planes[i].wpn-1;
 288                                 adif = angle_diff_deg( planes[i].hdg, planes[i].wpts[wpn][4] )
 289                                 * SGD_DEGREES_TO_RADIANS;
 290                                 datp = 2*sin(fabs(adif)/2.0)*sin(fabs(adif)/2.0) *
 291                                 planes[i].spd/3600. * planes[i].turn_rate +
 292                                 planes[i].spd/3600. * 3.0;
 293                                 if ( fabs(planes[i].dcc) < datp ) {
 294                                         planes[i].ahdg = fabs(planes[i].wpts[wpn][4]);
 295
 296                                         // generate the message
 297                                         code.c1 = 1;
 298                                         code.c2 = 2;
 299                                         code.c3 = 0;
 300                                         tpars.station = name;
 301                                         tpars.callsign = "Player";
 302                                         if ( adif < 0 ) tpars.tdir = 1;
 303                                         else            tpars.tdir = 2;
 304                                         tpars.heading = planes[i].ahdg;
 305                                         message = current_transmissionlist->gen_text(station, code, tpars, true);
 306                                         globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
 307                                         planes[i].lmc = code;
 308                                         planes[i].tlm = etime_node->getDoubleValue();
 309
 310                                         planes[i].on_crs = true;
 311                                 }
 312                         }
 313                         else if ( planes[i].wp_change  ) {
 314                                 planes[i].wp_change = false;
 315                         }
 316
 317                         // ===================================================================
 318                         // === Less than two minutes away from touchdown? -> Contact Tower ===
 319                         // ===================================================================
 320                         if ( planes[i].wpn == 2 && planes[i].dnwp < planes[i].spd/60.*2.0 ) {
 321
 322                                 double freq = 121.95;
 323                                 // generate message
 324                                 code.c1 = 1;
 325                                 code.c2 = 5;
 326                                 code.c3 = 0;
 327                                 tpars.station = name;
 328                                 tpars.callsign = "Player";
 329                                 tpars.freq    = freq;
 330                                 message = current_transmissionlist->gen_text(station, code, tpars, true);
 331                                 globals->get_ATC_display()->RegisterSingleMessage( message, 0 );
 332                                 planes[i].lmc = code;
 333                                 planes[i].tlm = etime_node->getDoubleValue();
 334
 335                                 planes[i].contact = 2;
 336                         }
 337                 }
 338         }
 339
 340 }
 341
 342
 343 // ============================================================================
 344 // update course parameters
 345 // ============================================================================
 346 void FGApproach::update_param( const int &i ) {
 347
 348   double course, d;
 349
 350   int wpn = planes[i].wpn-1;            // this is the current waypoint
 351
 352   planes[i].dcc  = calc_psl_dist(planes[i].brg, planes[i].dist,
 353                                  planes[i].wpts[wpn][0], planes[i].wpts[wpn][1],
 354                                  planes[i].wpts[wpn][4]);
 355   planes[i].dnc  = calc_psl_dist(planes[i].brg, planes[i].dist,
 356                                  planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
 357                                  planes[i].wpts[wpn-1][4]);
 358   calc_hd_course_dist(planes[i].brg, planes[i].dist,
 359                       planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
 360                       &course, &d);
 361   planes[i].dnwp = d;
 362
 363 }
 364
 365 // ============================================================================
 366 // smallest difference between two angles in degree
 367 // difference is negative if a1 > a2 and positive if a2 > a1
 368 // ===========================================================================
 369 double FGApproach::angle_diff_deg( const double &a1, const double &a2) {
 370
 371   double a3 = a2 - a1;
 372   if (a3 < 180.0) a3 += 360.0;
 373   if (a3 > 180.0) a3 -= 360.0;
 374
 375   return a3;
 376 }
 377
 378 // ============================================================================
 379 // calculate waypoints
 380 // ============================================================================
 381 void FGApproach::calc_wp( const int &i ) {
 382
 383         int j;
 384         double course, d, cd, a1;
 385
 386         int wpn = planes[i].wpn;
 387         // waypoint 0: Threshold of active runway
 388         calc_gc_course_dist(Point3D(lon*SGD_DEGREES_TO_RADIANS, lat*SGD_DEGREES_TO_RADIANS, 0.0),
 389         Point3D(active_rw_lon*SGD_DEGREES_TO_RADIANS,active_rw_lat*SGD_DEGREES_TO_RADIANS, 0.0 ),
 390         &course, &d);
 391         double d1 = active_rw_hdg+180.0;
 392         if ( d1 > 360.0 ) d1 -=360.0;
 393         calc_cd_head_dist(360.0-course*SGD_RADIANS_TO_DEGREES, d/SG_NM_TO_METER,
 394         d1, active_rw_len/SG_NM_TO_METER/2.,
 395         &planes[i].wpts[wpn][0], &planes[i].wpts[wpn][1]);
 396         planes[i].wpts[wpn][2] = elev;
 397         planes[i].wpts[wpn][4] = 0.0;
 398         planes[i].wpts[wpn][5] = 0.0;
 399         wpn += 1;
 400
 401         // ======================
 402         // horizontal navigation
 403         // ======================
 404         // waypoint 1: point for turning onto final
 405         calc_cd_head_dist(planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1] ,
 406         d1, lfl,
 407         &planes[i].wpts[wpn][0], &planes[i].wpts[wpn][1]);
 408         calc_hd_course_dist(planes[i].wpts[wpn][0],   planes[i].wpts[wpn][1],
 409         planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
 410         &course, &d);
 411         planes[i].wpts[wpn][4] = course;
 412         planes[i].wpts[wpn][5] = d;
 413         wpn += 1;
 414
 415         // calculate course and distance from plane position to waypoint 1
 416         calc_hd_course_dist(planes[i].brg, planes[i].dist,
 417         planes[i].wpts[1][0], planes[i].wpts[1][1],
 418         &course, &d);
 419         // check if airport is not between plane and waypoint 1 and
 420         // DCA to airport on course to waypoint 1 is larger than 10 miles
 421         double zero = 0.0;
 422         if ( fabs(angle_diff_deg( planes[i].wpts[1][0], planes[i].brg  )) < 90.0 ||
 423                 calc_psl_dist( zero, zero, planes[i].brg, planes[i].dist, course ) > 10.0 ) {
 424                 // check if turning angle at waypoint 1 would be > max_ta
 425                 if ( fabs(angle_diff_deg( planes[i].wpts[1][4], course )) > max_ta ) {
 426                         cd = calc_psl_dist(planes[i].brg, planes[i].dist,
 427                         planes[i].wpts[1][0], planes[i].wpts[1][1],
 428                         planes[i].wpts[1][4]);
 429                         a1 = atan2(cd,planes[i].wpts[1][1]);
 430                         planes[i].wpts[wpn][0] = planes[i].wpts[1][0] - a1/SGD_DEGREES_TO_RADIANS;
 431                         if ( planes[i].wpts[wpn][0] < 0.0)   planes[i].wpts[wpn][0] += 360.0;
 432                         if ( planes[i].wpts[wpn][0] > 360.0) planes[i].wpts[wpn][0] -= 360.0;
 433                         planes[i].wpts[wpn][1] = fabs(cd) / sin(fabs(a1));
 434                         calc_hd_course_dist(planes[i].wpts[wpn][0],   planes[i].wpts[wpn][1],
 435                         planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
 436                         &course, &d);
 437                         planes[i].wpts[wpn][4] = course;
 438                         planes[i].wpts[wpn][5] = d;
 439                         wpn += 1;
 440
 441                         calc_hd_course_dist(planes[i].brg, planes[i].dist,
 442                         planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
 443                         &course, &d);
 444                 }
 445         } else {
 446                 double leg = 10.0;
 447                 a1 = atan2(planes[i].wpts[1][1], leg );
 448
 449                 if ( angle_diff_deg(planes[i].brg,planes[i].wpts[1][0]) < 0 )
 450                         planes[i].wpts[wpn][0] = planes[i].wpts[1][0] + a1/SGD_DEGREES_TO_RADIANS;
 451                 else planes[i].wpts[wpn][0] = planes[i].wpts[1][0] - a1/SGD_DEGREES_TO_RADIANS;
 452
 453                 planes[i].wpts[wpn][1] = sqrt( planes[i].wpts[1][1]*planes[i].wpts[1][1] + leg*leg );
 454                 calc_hd_course_dist(planes[i].wpts[wpn][0],   planes[i].wpts[wpn][1],
 455                 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
 456                 &course, &d);
 457                 planes[i].wpts[wpn][4] = course;
 458                 planes[i].wpts[wpn][5] = d;
 459                 wpn += 1;
 460
 461                 calc_hd_course_dist(planes[i].brg, planes[i].dist,
 462                 planes[i].wpts[wpn-1][0], planes[i].wpts[wpn-1][1],
 463                 &course, &d);
 464         }
 465
 466         planes[i].wpts[wpn][0] = planes[i].brg;
 467         planes[i].wpts[wpn][1] = planes[i].dist;
 468         planes[i].wpts[wpn][2] = planes[i].alt;
 469         planes[i].wpts[wpn][4] = course;
 470         planes[i].wpts[wpn][5] = d;
 471         wpn += 1;
 472
 473         planes[i].wpn = wpn;
 474
 475         // Now check if legs are too short or if legs can be shortend
 476         // legs must be at least 2 flight minutes long
 477         double mdist = planes[i].spd / 60.0 * 2.0;
 478         for ( j=2; j<wpn-1; ++j ) {
 479                 if ( planes[i].wpts[j][1] < mdist) {
 480                 }
 481         }
 482
 483         // ====================
 484         // vertical navigation
 485         // ====================
 486         double alt = elev+3000.0;
 487         planes[i].wpts[1][2] = round_alt( true, alt );
 488         for ( j=2; j<wpn-1; ++j ) {
 489                 double dalt = planes[i].alt - planes[i].wpts[j-1][2];
 490                 if ( dalt > 0 ) {
 491                         alt = planes[i].wpts[j-1][2] +
 492                         (planes[i].wpts[j][5] / planes[i].spd) * 60.0 * planes[i].desc_rate;
 493                         planes[i].wpts[j][2] = round_alt( false, alt );
 494                         if ( planes[i].wpts[j][2] > planes[i].alt )
 495                                 planes[i].wpts[j][2] = round_alt( false, planes[i].alt );
 496                 }
 497                 else {
 498                         planes[i].wpts[j][2] = planes[i].wpts[1][2];
 499                 }
 500         }
 501
 502         cout << "Plane position: " << planes[i].brg << " " << planes[i].dist << endl;
 503         for ( j=0; j<wpn; ++j ) {
 504                 cout << "Waypoint " << j << endl;
 505                 cout << "------------------" << endl;
 506                 cout << planes[i].wpts[j][0] << "   " << planes[i].wpts[j][1]
 507                 << "   " << planes[i].wpts[j][2] << "   " << planes[i].wpts[j][5];
 508                 cout << endl << endl;
 509         }
 510
 511 }
 512
 513
 514 // ============================================================================
 515 // round altitude value to next highest/lowest 500 feet
 516 // ============================================================================
 517 double FGApproach::round_alt( const bool hl, double alt ) {
 518
 519   alt = alt/1000.0;
 520   if ( hl ) {
 521     if ( alt > (int)(alt)+0.5 ) alt = ((int)(alt)+1)*1000.0;
 522     else alt = ((int)(alt)+0.5)*1000.0;
 523   }
 524   else {
 525     if ( alt > (int)(alt)+0.5 ) alt = ((int)(alt)+0.5)*1000.0;
 526     else alt = ((int)(alt))*1000.0;
 527   }
 528
 529   return alt;
 530 }
 531
 532
 533 // ============================================================================
 534 // get active runway
 535 // ============================================================================
 536 void FGApproach::get_active_runway() {
 537
 538 #ifdef FG_WEATHERCM
 539   sgVec3 position = { lat, lon, elev };
 540   FGPhysicalProperty stationweather = WeatherDatabase->get(position);
 541 #else
 542   FGEnvironment stationweather =
 543     globals->get_environment_mgr()->getEnvironment(lat, lon, elev);
 544 #endif
 545
 546   SGPath path( globals->get_fg_root() );
 547   path.append( "Airports" );
 548   path.append( "runways.mk4" );
 549   FGRunways runways( path.c_str() );
 550
 551 #ifdef FG_WEATHERCM
 552   //Set the heading to into the wind
 553   double wind_x = stationweather.Wind[0];
 554   double wind_y = stationweather.Wind[1];
 555
 556   double speed = sqrt( wind_x*wind_x + wind_y*wind_y ) * SG_METER_TO_NM / (60.0*60.0);
 557   double hdg;
 558
 559   //If no wind use 270degrees
 560   if(speed == 0) {
 561     hdg = 270;
 562   } else {
 563     // //normalize the wind to get the direction
 564     //wind_x /= speed; wind_y /= speed;
 565
 566     hdg = - atan2 ( wind_x, wind_y ) * SG_RADIANS_TO_DEGREES ;
 567     if (hdg < 0.0)
 568       hdg += 360.0;
 569   }
 570 #else
 571   double hdg = stationweather.get_wind_from_heading_deg();
 572 #endif
 573
 574   FGRunway runway;
 575   if ( runways.search( ident, int(hdg), &runway) ) {
 576     active_runway = runway.rwy_no;
 577     active_rw_hdg = runway.heading;
 578     active_rw_lon = runway.lon;
 579     active_rw_lat = runway.lat;
 580     active_rw_len = runway.length;
 581     //cout << "Active runway is: " << active_runway << "  heading = "
 582     // << active_rw_hdg
 583     // << " lon = " << active_rw_lon
 584     // << " lat = " << active_rw_lat <<endl;
 585   }
 586   else cout << "FGRunways search failed\n";
 587
 588 }
 589
 590 // ========================================================================
 591 // update infos about plane
 592 // ========================================================================
 593 void FGApproach::update_plane_dat() {
 594
 595   //cout << "Update Approach " << ident << "   " << num_planes << " registered" << endl;
 596   // update plane positions
 597   int i;
 598   for (i=0; i<num_planes; i++) {
 599     planes[i].lon = lon_node->getDoubleValue();
 600     planes[i].lat = lat_node->getDoubleValue();
 601     planes[i].alt = elev_node->getDoubleValue();
 602     planes[i].hdg = hdg_node->getDoubleValue();
 603     planes[i].spd = speed_node->getDoubleValue();
 604
 605     /*Point3D aircraft = sgGeodToCart( Point3D(planes[i].lon*SGD_DEGREES_TO_RADIANS,
 606                                              planes[i].lat*SGD_DEGREES_TO_RADIANS,
 607                                              planes[i].alt*SG_FEET_TO_METER) );*/
 608     double course, distance;
 609     calc_gc_course_dist(Point3D(lon*SGD_DEGREES_TO_RADIANS, lat*SGD_DEGREES_TO_RADIANS, 0.0),
 610                         Point3D(planes[i].lon*SGD_DEGREES_TO_RADIANS,planes[i].lat*SGD_DEGREES_TO_RADIANS, 0.0 ),
 611                         &course, &distance);
 612     planes[i].dist = distance/SG_NM_TO_METER;
 613     planes[i].brg  = 360.0-course*SGD_RADIANS_TO_DEGREES;
 614
 615     //cout << "Plane Id: " << planes[i].ident << "  Distance to " << ident
 616     // << " is " << planes[i].dist << " miles   " << "Bearing " << planes[i].brg << endl;
 617
 618   }
 619 }
 620
 621 // =======================================================================
 622 // Add plane to Approach list
 623 // =======================================================================
 624 void FGApproach::AddPlane(string pid) {
 625
 626   int i;
 627   for ( i=0; i<num_planes; i++) {
 628     if ( planes[i].ident == pid) {
 629       //cout << "Plane already registered: " << ident << " " << num_planes << endl;
 630       return;
 631     }
 632   }
 633   planes[num_planes].ident = pid;
 634   ++num_planes;
 635   //cout << "Plane added to list: " << ident << " " << num_planes << endl;
 636   return;
 637 }
 638
 639 // ================================================================================
 640 // closest distance between a point (h1,d1) and a straigt line (h2,d2,h3) in 2 dim.
 641 // ================================================================================
 642 double FGApproach::calc_psl_dist(const double &h1, const double &d1,
 643                                  const double &h2, const double &d2,
 644                                  const double &h3)
 645 {
 646   double a1 = h1 * SGD_DEGREES_TO_RADIANS;
 647   double a2 = h2 * SGD_DEGREES_TO_RADIANS;
 648   double a3 = h3 * SGD_DEGREES_TO_RADIANS;
 649   double x1 = cos(a1) * d1;
 650   double y1 = sin(a1) * d1;
 651   double x2 = cos(a2) * d2;
 652   double y2 = sin(a2) * d2;
 653   double x3 = cos(a3);
 654   double y3 = sin(a3);
 655
 656   // formula: dis = sqrt( (v1-v2)**2 - ((v1-v2)*v3)**2 ); vi = (xi,yi)
 657   double val1   = (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2);
 658   double val2   = ((x1-x2)*x3 + (y1-y2)*y3) * ((x1-x2)*x3 + (y1-y2)*y3);
 659   double dis    = val1 - val2;
 660   // now get sign for offset
 661   //cout << x1 << " " << x2 << " " << y1 << " " << y2 << " "
 662   //     << x3 << " " << y3 << " "
 663   //     << val1 << " " << val2 << " " << dis << endl;
 664   x3 *= sqrt(val2);
 665   y3 *= sqrt(val2);
 666   double da = fabs(atan2(y3,x3) - atan2(y1-y2,x1-x2));
 667   if ( da > SGD_PI ) da -= 2*SGD_PI;
 668   if ( fabs(da) > SGD_PI/2.) {
 669     //if ( x3*(x1-x2) < 0.0 && y3*(y1-y2) < 0.0) {
 670     x3 *= -1.0;
 671     y3 *= -1.0;
 672   }
 673   //cout << x3 << " " << y3 << endl;
 674   double dis1   = x1-x2-x3;
 675   double dis2   = y1-y2-y3;
 676   dis = sqrt(dis);
 677   da = atan2(dis2,dis1);
 678   if ( da < 0.0 ) da  += 2*SGD_PI;
 679   if ( da < a3 )  dis *= -1.0;
 680   //cout << dis1 << " " << dis2 << " " << da*SGD_RADIANS_TO_DEGREES << " " << h3
 681   //     << " " << sqrt(dis1*dis1 + dis2*dis2) << " " << dis << endl;
 682   //cout << atan2(dis2,dis1)*SGD_RADIANS_TO_DEGREES << " " << dis << endl;
 683
 684   return dis;
 685 }
 686
 687
 688 // ========================================================================
 689 // Calculate new bear/dist given starting bear/dis, and offset radial,
 690 // and distance.
 691 // ========================================================================
 692 void FGApproach::calc_cd_head_dist(const double &h1, const double &d1,
 693                                    const double &course, const double &dist,
 694                                    double *h2, double *d2)
 695 {
 696   double a1 = h1 * SGD_DEGREES_TO_RADIANS;
 697   double a2 = course * SGD_DEGREES_TO_RADIANS;
 698   double x1 = cos(a1) * d1;
 699   double y1 = sin(a1) * d1;
 700   double x2 = cos(a2) * dist;
 701   double y2 = sin(a2) * dist;
 702
 703   *d2 = sqrt((x1+x2)*(x1+x2) + (y1+y2)*(y1+y2));
 704   *h2 = atan2( (y1+y2), (x1+x2) ) * SGD_RADIANS_TO_DEGREES;
 705   if ( *h2 < 0 ) *h2 = *h2+360;
 706   }
 707
 708
 709
 710 // ========================================================================
 711 // get heading and distance between two points; point1 ---> point2
 712 // ========================================================================
 713 void FGApproach::calc_hd_course_dist(const double &h1, const double &d1,
 714                                      const double &h2, const double &d2,
 715                                      double *course, double *dist)
 716 {
 717   double a1 = h1 * SGD_DEGREES_TO_RADIANS;
 718   double a2 = h2 * SGD_DEGREES_TO_RADIANS;
 719   double x1 = cos(a1) * d1;
 720   double y1 = sin(a1) * d1;
 721   double x2 = cos(a2) * d2;
 722   double y2 = sin(a2) * d2;
 723
 724   *dist   = sqrt( (y2-y1)*(y2-y1) + (x2-x1)*(x2-x1) );
 725   *course = atan2( (y2-y1), (x2-x1) ) * SGD_RADIANS_TO_DEGREES;
 726   if ( *course < 0 ) *course = *course+360;
 727   //cout << x1 << " " << y1 << " " << x2 << " " << y2 << " " << *dist << " " << *course << endl;
 728 }
 729
 730
 731
 732 int FGApproach::RemovePlane() {
 733
 734   // first check if anything has to be done
 735   bool rmplane = false;
 736   int i;
 737
 738   for (i=0; i<num_planes; i++) {
 739     if (planes[i].dist > range*SG_NM_TO_METER) {
 740       rmplane = true;
 741       break;
 742     }
 743   }
 744   if (!rmplane) return num_planes;
 745
 746   // now make a copy of the plane list
 747   PlaneApp tmp[max_planes];
 748   for (i=0; i<num_planes; i++) {
 749     tmp[i] = planes[i];
 750   }
 751
 752   int np = 0;
 753   // now check which planes are still in range
 754   for (i=0; i<num_planes; i++) {
 755     if (tmp[i].dist <= range*SG_NM_TO_METER) {
 756       planes[np] = tmp[i];
 757       np += 1;
 758     }
 759   }
 760   num_planes = np;
 761
 762   return num_planes;
 763 }