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