src/ATC/approach.cxx

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