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