src/Cockpit/navcom.cxx

   1 // navcom.cxx -- class to manage a navcom instance
   2 //
   3 // Written by Curtis Olson, started April 2000.
   4 //
   5 // Copyright (C) 2000 - 2002  Curtis L. Olson - curt@flightgear.org
   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 // $Id$
  22
  23
  24 #ifdef HAVE_CONFIG_H
  25 #  include <config.h>
  26 #endif
  27
  28 #include <stdio.h>      // snprintf
  29
  30 #include <simgear/compiler.h>
  31 #include <simgear/sg_inlines.h>
  32 #include <simgear/math/sg_random.h>
  33 #include <simgear/math/vector.hxx>
  34
  35 #include <Aircraft/aircraft.hxx>
  36 #include <Navaids/ilslist.hxx>
  37 #include <Navaids/navlist.hxx>
  38
  39 #include "navcom.hxx"
  40
  41 #include <string>
  42 SG_USING_STD(string);
  43
  44
  45 // Constructor
  46 FGNavCom::FGNavCom() :
  47     lon_node(fgGetNode("/position/longitude-deg", true)),
  48     lat_node(fgGetNode("/position/latitude-deg", true)),
  49     alt_node(fgGetNode("/position/altitude-ft", true)),
  50     last_nav_id(""),
  51     last_nav_vor(false),
  52     nav_play_count(0),
  53     nav_last_time(0),
  54     need_update(true),
  55     power_btn(true),
  56     audio_btn(true),
  57     comm_freq(0.0),
  58     comm_alt_freq(0.0),
  59     comm_vol_btn(0.0),
  60     nav_freq(0.0),
  61     nav_alt_freq(0.0),
  62     nav_heading(0.0),
  63     nav_radial(0.0),
  64     nav_target_radial(0.0),
  65     nav_target_radial_true(0.0),
  66     nav_target_auto_hdg(0.0),
  67     nav_vol_btn(0.0),
  68     nav_ident_btn(true),
  69     horiz_vel(0.0),
  70     last_x(0.0)
  71 {
  72     SGPath path( globals->get_fg_root() );
  73     SGPath term = path;
  74     term.append( "Navaids/range.term" );
  75     SGPath low = path;
  76     low.append( "Navaids/range.low" );
  77     SGPath high = path;
  78     high.append( "Navaids/range.high" );
  79
  80     term_tbl = new SGInterpTable( term.str() );
  81     low_tbl = new SGInterpTable( low.str() );
  82     high_tbl = new SGInterpTable( high.str() );
  83
  84 }
  85
  86
  87 // Destructor
  88 FGNavCom::~FGNavCom()
  89 {
  90     delete term_tbl;
  91     delete low_tbl;
  92     delete high_tbl;
  93 }
  94
  95
  96 void
  97 FGNavCom::init ()
  98 {
  99     morse.init();
 100
 101     // We assume that index is valid now (it must be set before init()
 102     // is called.)
 103     char propname[256];
 104     // FIXME: Get rid of snprintf
 105
 106     snprintf(propname, 256, "/systems/electrical/outputs/navcom[%d]", index);
 107     // default to true in case no electrical system defined.
 108     fgSetDouble( propname, 60.0 );
 109     bus_power = fgGetNode( propname, true );
 110
 111     snprintf(propname, 256, "/instrumentation/comm[%d]/serviceable", index);
 112     com_serviceable = fgGetNode( propname, true );
 113     com_serviceable->setBoolValue( true );
 114
 115     snprintf(propname, 256, "/instrumentation/nav[%d]/serviceable", index);
 116     nav_serviceable = fgGetNode( propname, true );
 117     nav_serviceable->setBoolValue( true );
 118
 119     snprintf(propname, 256, "/instrumentation/vor[%d]/cdi/serviceable", index);
 120     cdi_serviceable = fgGetNode( propname, true );
 121     cdi_serviceable->setBoolValue( true );
 122
 123     snprintf(propname, 256, "/instrumentation/vor[%d]/gs/serviceable", index);
 124     gs_serviceable = fgGetNode( propname, true );
 125     gs_serviceable->setBoolValue( true );
 126
 127     snprintf(propname, 256, "/instrumentation/vor[%d]/to-from/serviceable", index);
 128     tofrom_serviceable = fgGetNode( propname, true );
 129     tofrom_serviceable->setBoolValue( true );
 130 }
 131
 132 void
 133 FGNavCom::bind ()
 134 {
 135     char propname[256];
 136     // FIXME: Get rid of snprintf
 137
 138                                 // User inputs
 139     snprintf(propname, 256, "/radios/comm[%d]/inputs/power-btn", index);
 140     fgTie( propname, this,
 141            &FGNavCom::get_power_btn, &FGNavCom::set_power_btn );
 142     fgSetArchivable( propname );
 143
 144     snprintf(propname, 256, "/radios/comm[%d]/frequencies/selected-mhz", index);
 145     fgTie( propname, this, &FGNavCom::get_comm_freq, &FGNavCom::set_comm_freq );
 146     fgSetArchivable( propname );
 147
 148     snprintf(propname, 256, "/radios/comm[%d]/frequencies/standby-mhz", index);
 149     fgTie( propname, this,
 150            &FGNavCom::get_comm_alt_freq, &FGNavCom::set_comm_alt_freq );
 151     fgSetArchivable( propname );
 152
 153     snprintf(propname, 256, "/radios/comm[%d]/volume", index);
 154     fgTie( propname, this,
 155            &FGNavCom::get_comm_vol_btn, &FGNavCom::set_comm_vol_btn );
 156     fgSetArchivable( propname );
 157
 158     snprintf(propname, 256, "/radios/nav[%d]/frequencies/selected-mhz", index);
 159     fgTie( propname, this,
 160           &FGNavCom::get_nav_freq, &FGNavCom::set_nav_freq );
 161     fgSetArchivable( propname );
 162
 163     snprintf(propname, 256, "/radios/nav[%d]/frequencies/standby-mhz", index);
 164     fgTie( propname , this,
 165            &FGNavCom::get_nav_alt_freq, &FGNavCom::set_nav_alt_freq);
 166     fgSetArchivable( propname );
 167
 168     snprintf(propname, 256, "/radios/nav[%d]/radials/selected-deg", index);
 169     fgTie( propname, this,
 170            &FGNavCom::get_nav_sel_radial, &FGNavCom::set_nav_sel_radial );
 171     fgSetArchivable( propname );
 172
 173     snprintf(propname, 256, "/radios/nav[%d]/volume", index);
 174     fgTie( propname, this,
 175            &FGNavCom::get_nav_vol_btn, &FGNavCom::set_nav_vol_btn );
 176     fgSetArchivable( propname );
 177
 178     snprintf(propname, 256, "/radios/nav[%d]/ident", index);
 179     fgTie( propname, this,
 180            &FGNavCom::get_nav_ident_btn, &FGNavCom::set_nav_ident_btn );
 181     fgSetArchivable( propname );
 182
 183                                 // Radio outputs
 184     snprintf(propname, 256, "/radios/nav[%d]/audio-btn", index);
 185     fgTie( propname, this,
 186            &FGNavCom::get_audio_btn, &FGNavCom::set_audio_btn );
 187     fgSetArchivable( propname );
 188
 189     snprintf(propname, 256, "/radios/nav[%d]/heading-deg", index);
 190     fgTie( propname,  this, &FGNavCom::get_nav_heading );
 191
 192     snprintf(propname, 256, "/radios/nav[%d]/radials/actual-deg", index);
 193     fgTie( propname,  this, &FGNavCom::get_nav_radial );
 194
 195     snprintf(propname, 256, "/radios/nav[%d]/radials/target-radial-deg", index);
 196     fgTie( propname,  this, &FGNavCom::get_nav_target_radial_true );
 197
 198     snprintf(propname, 256, "/radios/nav[%d]/radials/target-auto-hdg-deg",
 199              index);
 200     fgTie( propname,  this, &FGNavCom::get_nav_target_auto_hdg );
 201
 202     snprintf(propname, 256, "/radios/nav[%d]/to-flag", index);
 203     fgTie( propname, this, &FGNavCom::get_nav_to_flag );
 204
 205     snprintf(propname, 256, "/radios/nav[%d]/from-flag", index);
 206     fgTie( propname, this, &FGNavCom::get_nav_from_flag );
 207
 208     snprintf(propname, 256, "/radios/nav[%d]/in-range", index);
 209     fgTie( propname, this, &FGNavCom::get_nav_inrange );
 210
 211     snprintf(propname, 256, "/radios/nav[%d]/heading-needle-deflection", index);
 212     fgTie( propname, this, &FGNavCom::get_nav_cdi_deflection );
 213
 214     snprintf(propname, 256, "/radios/nav[%d]/has-gs", index);
 215     fgTie( propname, this, &FGNavCom::get_nav_has_gs );
 216
 217     snprintf(propname, 256, "/radios/nav[%d]/nav-loc", index);
 218     fgTie( propname, this, &FGNavCom::get_nav_loc );
 219
 220     snprintf(propname, 256, "/radios/nav[%d]/gs-rate-of-climb", index);
 221     fgTie( propname, this, &FGNavCom::get_nav_gs_rate_of_climb );
 222
 223     snprintf(propname, 256, "/radios/nav[%d]/gs-needle-deflection", index);
 224     fgTie( propname, this, &FGNavCom::get_nav_gs_deflection );
 225
 226     snprintf(propname, 256, "/radios/nav[%d]/nav-id", index);
 227     fgTie( propname, this, &FGNavCom::get_nav_id );
 228
 229     // put nav_id characters into seperate properties for instrument displays
 230     snprintf(propname, 256, "/radios/nav[%d]/nav-id_asc1", index);
 231     fgTie( propname, this, &FGNavCom::get_nav_id_c1 );
 232
 233     snprintf(propname, 256, "/radios/nav[%d]/nav-id_asc2", index);
 234     fgTie( propname, this, &FGNavCom::get_nav_id_c2 );
 235
 236     snprintf(propname, 256, "/radios/nav[%d]/nav-id_asc3", index);
 237     fgTie( propname, this, &FGNavCom::get_nav_id_c3 );
 238
 239     snprintf(propname, 256, "/radios/nav[%d]/nav-id_asc4", index);
 240     fgTie( propname, this, &FGNavCom::get_nav_id_c4 );
 241
 242     // end of binding
 243 }
 244
 245
 246 void
 247 FGNavCom::unbind ()
 248 {
 249     char propname[256];
 250     // FIXME: Get rid of snprintf
 251
 252     snprintf(propname, 256, "/radios/comm[%d]/inputs/power-btn", index);
 253     fgUntie( propname );
 254     snprintf(propname, 256, "/radios/comm[%d]/frequencies/selected-mhz", index);
 255     fgUntie( propname );
 256     snprintf(propname, 256, "/radios/comm[%d]/frequencies/standby-mhz", index);
 257     fgUntie( propname );
 258
 259     snprintf(propname, 256, "/radios/nav[%d]/frequencies/selected-mhz", index);
 260     fgUntie( propname );
 261     snprintf(propname, 256, "/radios/nav[%d]/frequencies/standby-mhz", index);
 262     fgUntie( propname );
 263     snprintf(propname, 256, "/radios/nav[%d]/radials/actual-deg", index);
 264     fgUntie( propname );
 265     snprintf(propname, 256, "/radios/nav[%d]/radials/selected-deg", index);
 266     fgUntie( propname );
 267     snprintf(propname, 256, "/radios/nav[%d]/ident", index);
 268     fgUntie( propname );
 269     snprintf(propname, 256, "/radios/nav[%d]/to-flag", index);
 270     fgUntie( propname );
 271     snprintf(propname, 256, "/radios/nav[%d]/from-flag", index);
 272     fgUntie( propname );
 273     snprintf(propname, 256, "/radios/nav[%d]/in-range", index);
 274     fgUntie( propname );
 275     snprintf(propname, 256, "/radios/nav[%d]/heading-needle-deflection", index);
 276     fgUntie( propname );
 277     snprintf(propname, 256, "/radios/nav[%d]/gs-needle-deflection", index);
 278     fgUntie( propname );
 279 }
 280
 281
 282 // model standard VOR/DME/TACAN service volumes as per AIM 1-1-8
 283 double FGNavCom::adjustNavRange( double stationElev, double aircraftElev,
 284                                  double nominalRange )
 285 {
 286     // extend out actual usable range to be 1.3x the published safe range
 287     const double usability_factor = 1.3;
 288
 289     // assumptions we model the standard service volume, plus
 290     // ... rather than specifying a cylinder, we model a cone that
 291     // contains the cylinder.  Then we put an upside down cone on top
 292     // to model diminishing returns at too-high altitudes.
 293
 294     // altitude difference
 295     double alt = ( aircraftElev * SG_METER_TO_FEET - stationElev );
 296     // cout << "aircraft elev = " << aircraftElev * SG_METER_TO_FEET
 297     //      << " station elev = " << stationElev << endl;
 298
 299     if ( nominalRange < 25.0 + SG_EPSILON ) {
 300         // Standard Terminal Service Volume
 301         return term_tbl->interpolate( alt ) * usability_factor;
 302     } else if ( nominalRange < 50.0 + SG_EPSILON ) {
 303         // Standard Low Altitude Service Volume
 304         // table is based on range of 40, scale to actual range
 305         return low_tbl->interpolate( alt ) * nominalRange / 40.0
 306             * usability_factor;
 307     } else {
 308         // Standard High Altitude Service Volume
 309         // table is based on range of 130, scale to actual range
 310         return high_tbl->interpolate( alt ) * nominalRange / 130.0
 311             * usability_factor;
 312     }
 313 }
 314
 315
 316 // model standard ILS service volumes as per AIM 1-1-9
 317 double FGNavCom::adjustILSRange( double stationElev, double aircraftElev,
 318                                      double offsetDegrees, double distance )
 319 {
 320     // assumptions we model the standard service volume, plus
 321
 322     // altitude difference
 323     // double alt = ( aircraftElev * SG_METER_TO_FEET - stationElev );
 324 //     double offset = fabs( offsetDegrees );
 325
 326 //     if ( offset < 10 ) {
 327 //      return FG_ILS_DEFAULT_RANGE;
 328 //     } else if ( offset < 35 ) {
 329 //      return 10 + (35 - offset) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
 330 //     } else if ( offset < 45 ) {
 331 //      return (45 - offset);
 332 //     } else if ( offset > 170 ) {
 333 //         return FG_ILS_DEFAULT_RANGE;
 334 //     } else if ( offset > 145 ) {
 335 //      return 10 + (offset - 145) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
 336 //     } else if ( offset > 135 ) {
 337 //         return (offset - 135);
 338 //     } else {
 339 //      return 0;
 340 //     }
 341     return FG_ILS_DEFAULT_RANGE;
 342 }
 343
 344
 345 // Update the various nav values based on position and valid tuned in navs
 346 void
 347 FGNavCom::update(double dt)
 348 {
 349     double lon = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 350     double lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 351     double elev = alt_node->getDoubleValue() * SG_FEET_TO_METER;
 352
 353     need_update = false;
 354
 355     Point3D aircraft = sgGeodToCart( Point3D( lon, lat, elev ) );
 356     Point3D station;
 357     double az1, az2, s;
 358
 359     ////////////////////////////////////////////////////////////////////////
 360     // Nav.
 361     ////////////////////////////////////////////////////////////////////////
 362
 363     if ( nav_valid && power_btn && (bus_power->getDoubleValue() > 1.0)
 364          && nav_serviceable->getBoolValue() )
 365     {
 366         station = Point3D( nav_x, nav_y, nav_z );
 367         nav_loc_dist = aircraft.distance3D( station );
 368
 369         if ( nav_has_gs ) {
 370             // find closest distance to the gs base line
 371             sgdVec3 p;
 372             sgdSetVec3( p, aircraft.x(), aircraft.y(), aircraft.z() );
 373             sgdVec3 p0;
 374             sgdSetVec3( p0, nav_gs_x, nav_gs_y, nav_gs_z );
 375             double dist = sgdClosestPointToLineDistSquared( p, p0,
 376                                                             gs_base_vec );
 377             nav_gs_dist = sqrt( dist );
 378             // cout << nav_gs_dist;
 379
 380             // Point3D tmp( nav_gs_x, nav_gs_y, nav_gs_z );
 381             // cout << " (" << aircraft.distance3D( tmp ) << ")" << endl;
 382
 383             // wgs84 heading to glide slope (to determine sign of distance)
 384             geo_inverse_wgs_84( elev,
 385                                 lat * SGD_RADIANS_TO_DEGREES,
 386                                 lon * SGD_RADIANS_TO_DEGREES,
 387                                 nav_gslat, nav_gslon,
 388                                 &az1, &az2, &s );
 389             double r = az1 - nav_target_radial;
 390             while ( r >  180.0 ) { r -= 360.0;}
 391             while ( r < -180.0 ) { r += 360.0;}
 392             if ( r >= -90.0 && r <= 90.0 ) {
 393                 nav_gs_dist_signed = nav_gs_dist;
 394             } else {
 395                 nav_gs_dist_signed = -nav_gs_dist;
 396             }
 397             /* cout << "Target Radial = " << nav_target_radial
 398                  << "  Bearing = " << az1
 399                  << "  dist (signed) = " << nav_gs_dist_signed
 400                  << endl; */
 401
 402         } else {
 403             nav_gs_dist = 0.0;
 404         }
 405
 406         // wgs84 heading to localizer
 407         geo_inverse_wgs_84( elev,
 408                             lat * SGD_RADIANS_TO_DEGREES,
 409                             lon * SGD_RADIANS_TO_DEGREES,
 410                             nav_loclat, nav_loclon,
 411                             &nav_heading, &az2, &s );
 412         // cout << "az1 = " << az1 << " magvar = " << nav_magvar << endl;
 413         nav_radial = az2 - nav_twist;
 414         // cout << " heading = " << nav_heading
 415         //      << " dist = " << nav_dist << endl;
 416
 417         if ( nav_loc ) {
 418             double offset = nav_radial - nav_target_radial;
 419             while ( offset < -180.0 ) { offset += 360.0; }
 420             while ( offset > 180.0 ) { offset -= 360.0; }
 421             // cout << "ils offset = " << offset << endl;
 422             nav_effective_range = adjustILSRange(nav_elev, elev, offset,
 423                                                   nav_loc_dist * SG_METER_TO_NM );
 424         } else {
 425             nav_effective_range = adjustNavRange(nav_elev, elev, nav_range);
 426         }
 427         // cout << "nav range = " << nav_effective_range
 428         //      << " (" << nav_range << ")" << endl;
 429
 430         if ( nav_loc_dist < nav_effective_range * SG_NM_TO_METER ) {
 431             nav_inrange = true;
 432         } else if ( nav_loc_dist < 2 * nav_effective_range * SG_NM_TO_METER ) {
 433             nav_inrange = sg_random() <
 434                 ( 2 * nav_effective_range * SG_NM_TO_METER - nav_loc_dist ) /
 435                 (nav_effective_range * SG_NM_TO_METER);
 436         } else {
 437             nav_inrange = false;
 438         }
 439
 440         if ( !nav_loc ) {
 441             nav_target_radial = nav_sel_radial;
 442         }
 443
 444         // Calculate some values for the nav/ils hold autopilot
 445
 446         double cur_radial = get_nav_reciprocal_radial();
 447         if ( nav_loc ) {
 448             // ILS localizers radials are already "true" in our
 449             // database
 450         } else {
 451             cur_radial += nav_twist;
 452         }
 453         if ( get_nav_from_flag() ) {
 454             cur_radial += 180.0;
 455             while ( cur_radial >= 360.0 ) { cur_radial -= 360.0; }
 456         }
 457
 458         // AUTOPILOT HELPERS
 459
 460         // determine the target radial in "true" heading
 461         nav_target_radial_true = nav_target_radial;
 462         if ( nav_loc ) {
 463             // ILS localizers radials are already "true" in our
 464             // database
 465         } else {
 466             // VOR radials need to have that vor's offset added in
 467             nav_target_radial_true += nav_twist;
 468         }
 469
 470         while ( nav_target_radial_true < 0.0 ) {
 471             nav_target_radial_true += 360.0;
 472         }
 473         while ( nav_target_radial_true > 360.0 ) {
 474             nav_target_radial_true -= 360.0;
 475         }
 476
 477         // determine the heading adjustment needed.
 478         double adjustment = get_nav_cdi_deflection()
 479             * (nav_loc_dist * SG_METER_TO_NM);
 480         SG_CLAMP_RANGE( adjustment, -30.0, 30.0 );
 481
 482 #if 0
 483         // CLO - 01/24/2004 - This #ifdef'd out code makes no sense to
 484         // me.  Someone please justify it and explain why it should be
 485         // here if they want this reenabled.
 486
 487         // clamp closer when inside cone when beyond 5km...
 488         if ( nav_loc_dist > 5000 ) {
 489             double clamp_angle = fabs(get_nav_cdi_deflection()) * 3;
 490             if (clamp_angle < 30)
 491                 SG_CLAMP_RANGE( adjustment, -clamp_angle, clamp_angle);
 492         }
 493 #endif
 494
 495         // determine the target heading to fly to intercept the
 496         // tgt_radial
 497         nav_target_auto_hdg = nav_target_radial_true + adjustment;
 498         while ( nav_target_auto_hdg <   0.0 ) { nav_target_auto_hdg += 360.0; }
 499         while ( nav_target_auto_hdg > 360.0 ) { nav_target_auto_hdg -= 360.0; }
 500
 501         // cross track error
 502         // ????
 503
 504         // Calculate desired rate of climb for intercepting the GS
 505         double x = nav_gs_dist;
 506         double y = (alt_node->getDoubleValue() - nav_elev)
 507             * SG_FEET_TO_METER;
 508         double current_angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
 509
 510         double target_angle = nav_target_gs;
 511         double gs_diff = target_angle - current_angle;
 512
 513         // convert desired vertical path angle into a climb rate
 514         double des_angle = current_angle - 10 * gs_diff;
 515
 516         // estimate horizontal speed towards ILS in meters per minute
 517         double dist = last_x - x;
 518         last_x = x;
 519         double new_vel = ( dist / dt );
 520
 521         horiz_vel = 0.75 * horiz_vel + 0.25 * new_vel;
 522         // double horiz_vel = cur_fdm_state->get_V_ground_speed()
 523         //    * SG_FEET_TO_METER * 60.0;
 524         // double horiz_vel = airspeed_node->getFloatValue()
 525         //    * SG_FEET_TO_METER * 60.0;
 526
 527         nav_gs_rate_of_climb = -sin( des_angle * SGD_DEGREES_TO_RADIANS )
 528             * horiz_vel * SG_METER_TO_FEET;
 529     } else {
 530         nav_inrange = false;
 531         // cout << "not picking up vor. :-(" << endl;
 532     }
 533
 534     if ( nav_valid && nav_inrange && nav_serviceable->getBoolValue() ) {
 535         // play station ident via audio system if on + ident,
 536         // otherwise turn it off
 537         if ( power_btn && (bus_power->getDoubleValue() > 1.0)
 538              && nav_ident_btn && audio_btn )
 539         {
 540             SGSimpleSound *sound;
 541             sound = globals->get_soundmgr()->find( nav_fx_name );
 542             if ( sound != NULL ) {
 543                 sound->set_volume( nav_vol_btn );
 544             } else {
 545                 SG_LOG( SG_COCKPIT, SG_ALERT,
 546                         "Can't find nav-vor-ident sound" );
 547             }
 548             sound = globals->get_soundmgr()->find( dme_fx_name );
 549             if ( sound != NULL ) {
 550                 sound->set_volume( nav_vol_btn );
 551             } else {
 552                 SG_LOG( SG_COCKPIT, SG_ALERT,
 553                         "Can't find nav-dme-ident sound" );
 554             }
 555             // cout << "nav_last_time = " << nav_last_time << " ";
 556             // cout << "cur_time = "
 557             //      << globals->get_time_params()->get_cur_time();
 558             if ( nav_last_time <
 559                  globals->get_time_params()->get_cur_time() - 30 ) {
 560                 nav_last_time = globals->get_time_params()->get_cur_time();
 561                 nav_play_count = 0;
 562             }
 563             // cout << " nav_play_count = " << nav_play_count << endl;
 564             // cout << "playing = "
 565             //      << globals->get_soundmgr()->is_playing(nav_fx_name)
 566             //      << endl;
 567             if ( nav_play_count < 4 ) {
 568                 // play VOR ident
 569                 if ( !globals->get_soundmgr()->is_playing(nav_fx_name) ) {
 570                     globals->get_soundmgr()->play_once( nav_fx_name );
 571                     ++nav_play_count;
 572                 }
 573             } else if ( nav_play_count < 5 && nav_has_dme ) {
 574                 // play DME ident
 575                 if ( !globals->get_soundmgr()->is_playing(nav_fx_name) &&
 576                      !globals->get_soundmgr()->is_playing(dme_fx_name) ) {
 577                     globals->get_soundmgr()->play_once( dme_fx_name );
 578                     ++nav_play_count;
 579                 }
 580             }
 581         } else {
 582             globals->get_soundmgr()->stop( nav_fx_name );
 583             globals->get_soundmgr()->stop( dme_fx_name );
 584         }
 585     }
 586 }
 587
 588
 589 // Update current nav/adf radio stations based on current postition
 590 void FGNavCom::search()
 591 {
 592     double lon = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 593     double lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 594     double elev = alt_node->getDoubleValue() * SG_FEET_TO_METER;
 595
 596     FGILS *ils;
 597     FGNav *nav;
 598
 599     ////////////////////////////////////////////////////////////////////////
 600     // Nav.
 601     ////////////////////////////////////////////////////////////////////////
 602
 603     if ( (ils = current_ilslist->findByFreq(nav_freq, lon, lat, elev)) != NULL ) {
 604         nav_id = ils->get_locident();
 605         nav_valid = true;
 606         if ( last_nav_id != nav_id || last_nav_vor ) {
 607             nav_trans_ident = ils->get_trans_ident();
 608             last_nav_id = nav_id;
 609             last_nav_vor = false;
 610             nav_loc = true;
 611             nav_has_dme = ils->get_has_dme();
 612             nav_has_gs = ils->get_has_gs();
 613
 614             nav_loclon = ils->get_loclon();
 615             nav_loclat = ils->get_loclat();
 616             nav_gslon = ils->get_gslon();
 617             nav_gslat = ils->get_gslat();
 618             nav_elev = ils->get_gselev();
 619             nav_twist = 0;
 620             nav_range = FG_ILS_DEFAULT_RANGE;
 621             nav_effective_range = nav_range;
 622             nav_target_gs = ils->get_gsangle();
 623             nav_target_radial = ils->get_locheading();
 624             while ( nav_target_radial <   0.0 ) { nav_target_radial += 360.0; }
 625             while ( nav_target_radial > 360.0 ) { nav_target_radial -= 360.0; }
 626             nav_x = ils->get_x();
 627             nav_y = ils->get_y();
 628             nav_z = ils->get_z();
 629             nav_gs_x = ils->get_gs_x();
 630             nav_gs_y = ils->get_gs_y();
 631             nav_gs_z = ils->get_gs_z();
 632
 633             // derive GS baseline
 634             double tlon, tlat, taz;
 635             geo_direct_wgs_84 ( 0.0, nav_gslat, nav_gslon, nav_target_radial + 90,
 636                                 100.0, &tlat, &tlon, &taz );
 637             // cout << nav_gslon << "," << nav_gslat << "  "
 638             //      << tlon << "," << tlat << "  (" << nav_elev << ")" << endl;
 639             Point3D p1 = sgGeodToCart( Point3D(tlon*SGD_DEGREES_TO_RADIANS,
 640                                                tlat*SGD_DEGREES_TO_RADIANS,
 641                                                nav_elev*SG_FEET_TO_METER) );
 642             // cout << nav_gs_x << "," << nav_gs_y << "," << nav_gs_z << endl;
 643             // cout << p1 << endl;
 644             sgdSetVec3( gs_base_vec,
 645                         p1.x()-nav_gs_x, p1.y()-nav_gs_y, p1.z()-nav_gs_z );
 646             // cout << gs_base_vec[0] << "," << gs_base_vec[1] << ","
 647             //      << gs_base_vec[2] << endl;
 648
 649             if ( globals->get_soundmgr()->exists( nav_fx_name ) ) {
 650                 globals->get_soundmgr()->remove( nav_fx_name );
 651             }
 652             SGSimpleSound *sound;
 653             sound = morse.make_ident( nav_trans_ident, LO_FREQUENCY );
 654             sound->set_volume( 0.3 );
 655             globals->get_soundmgr()->add( sound, nav_fx_name );
 656
 657             if ( globals->get_soundmgr()->exists( dme_fx_name ) ) {
 658                 globals->get_soundmgr()->remove( dme_fx_name );
 659             }
 660             sound = morse.make_ident( nav_trans_ident, HI_FREQUENCY );
 661             sound->set_volume( 0.3 );
 662             globals->get_soundmgr()->add( sound, dme_fx_name );
 663
 664             int offset = (int)(sg_random() * 30.0);
 665             nav_play_count = offset / 4;
 666             nav_last_time = globals->get_time_params()->get_cur_time() -
 667                 offset;
 668             // cout << "offset = " << offset << " play_count = "
 669             //      << nav_play_count
 670             //      << " nav_last_time = " << nav_last_time
 671             //      << " current time = "
 672             //      << globals->get_time_params()->get_cur_time() << endl;
 673
 674             // cout << "Found an ils station in range" << endl;
 675             // cout << " id = " << ils->get_locident() << endl;
 676         }
 677     } else if ( (nav = current_navlist->findByFreq(nav_freq, lon, lat, elev)) != NULL ) {
 678         nav_id = nav->get_ident();
 679         nav_valid = (nav->get_type() == 'V');
 680         if ( last_nav_id != nav_id || !last_nav_vor ) {
 681             last_nav_id = nav_id;
 682             last_nav_vor = true;
 683             nav_trans_ident = nav->get_trans_ident();
 684             nav_loc = false;
 685             nav_has_dme = nav->get_has_dme();
 686             nav_has_gs = false;
 687             nav_loclon = nav->get_lon();
 688             nav_loclat = nav->get_lat();
 689             nav_elev = nav->get_elev();
 690             nav_twist = nav->get_magvar();
 691             nav_range = nav->get_range();
 692             nav_effective_range = adjustNavRange(nav_elev, elev, nav_range);
 693             nav_target_gs = 0.0;
 694             nav_target_radial = nav_sel_radial;
 695             nav_x = nav->get_x();
 696             nav_y = nav->get_y();
 697             nav_z = nav->get_z();
 698
 699             if ( globals->get_soundmgr()->exists( nav_fx_name ) ) {
 700                 globals->get_soundmgr()->remove( nav_fx_name );
 701             }
 702             SGSimpleSound *sound;
 703             sound = morse.make_ident( nav_trans_ident, LO_FREQUENCY );
 704             sound->set_volume( 0.3 );
 705             if ( globals->get_soundmgr()->add( sound, nav_fx_name ) ) {
 706                 // cout << "Added nav-vor-ident sound" << endl;
 707             } else {
 708                 SG_LOG(SG_COCKPIT, SG_WARN, "Failed to add v1-vor-ident sound");
 709             }
 710
 711             if ( globals->get_soundmgr()->exists( dme_fx_name ) ) {
 712                 globals->get_soundmgr()->remove( dme_fx_name );
 713             }
 714             sound = morse.make_ident( nav_trans_ident, HI_FREQUENCY );
 715             sound->set_volume( 0.3 );
 716             globals->get_soundmgr()->add( sound, dme_fx_name );
 717
 718             int offset = (int)(sg_random() * 30.0);
 719             nav_play_count = offset / 4;
 720             nav_last_time = globals->get_time_params()->get_cur_time() -
 721                 offset;
 722             // cout << "offset = " << offset << " play_count = "
 723             //      << nav_play_count << " nav_last_time = "
 724             //      << nav_last_time << " current time = "
 725             //      << globals->get_time_params()->get_cur_time() << endl;
 726
 727             // cout << "Found a vor station in range" << endl;
 728             // cout << " id = " << nav->get_ident() << endl;
 729         }
 730     } else {
 731         nav_valid = false;
 732         nav_id = "";
 733         nav_target_radial = 0;
 734         nav_trans_ident = "";
 735         last_nav_id = "";
 736         if ( ! globals->get_soundmgr()->remove( nav_fx_name ) ) {
 737             SG_LOG(SG_COCKPIT, SG_WARN, "Failed to remove nav-vor-ident sound");
 738         }
 739         globals->get_soundmgr()->remove( dme_fx_name );
 740         // cout << "not picking up vor1. :-(" << endl;
 741     }
 742 }
 743
 744
 745 // return the amount of heading needle deflection, returns a value
 746 // clamped to the range of ( -10 , 10 )
 747 double FGNavCom::get_nav_cdi_deflection() const {
 748     double r;
 749
 750     if ( nav_inrange
 751          && nav_serviceable->getBoolValue() && cdi_serviceable->getBoolValue() )
 752     {
 753         r = nav_radial - nav_target_radial;
 754         // cout << "Target radial = " << nav_target_radial
 755         //      << "  Actual radial = " << nav_radial << endl;
 756
 757         while ( r >  180.0 ) { r -= 360.0;}
 758         while ( r < -180.0 ) { r += 360.0;}
 759         if ( fabs(r) > 90.0 )
 760             r = ( r<0.0 ? -r-180.0 : -r+180.0 );
 761
 762         // According to Robin Peel, the ILS is 4x more sensitive than a vor
 763         r = -r;                 // reverse, since radial is outbound
 764         if ( nav_loc ) { r *= 4.0; }
 765         if ( r < -10.0 ) { r = -10.0; }
 766         if ( r >  10.0 ) { r = 10.0; }
 767     } else {
 768         r = 0.0;
 769     }
 770
 771     return r;
 772 }
 773
 774
 775 // return the amount of glide slope needle deflection (.i.e. the
 776 // number of degrees we are off the glide slope * 5.0
 777 double FGNavCom::get_nav_gs_deflection() const {
 778     if ( nav_inrange && nav_has_gs
 779          && nav_serviceable->getBoolValue() && gs_serviceable->getBoolValue() )
 780     {
 781         double x = nav_gs_dist;
 782         double y = (fgGetDouble("/position/altitude-ft") - nav_elev)
 783             * SG_FEET_TO_METER;
 784         // cout << "dist = " << x << " height = " << y << endl;
 785         double angle = asin( y / x ) * SGD_RADIANS_TO_DEGREES;
 786         return (nav_target_gs - angle) * 5.0;
 787     } else {
 788         return 0.0;
 789     }
 790 }
 791
 792
 793 /**
 794  * Return true if the NAV TO flag should be active.
 795  */
 796 bool
 797 FGNavCom::get_nav_to_flag () const
 798 {
 799     if ( nav_inrange
 800          && nav_serviceable->getBoolValue()
 801          && tofrom_serviceable->getBoolValue() )
 802     {
 803         double offset = fabs(nav_radial - nav_target_radial);
 804         if (nav_loc) {
 805             return true;
 806         } else {
 807             return !(offset <= 90.0 || offset >= 270.0);
 808         }
 809     } else {
 810         return false;
 811     }
 812 }
 813
 814
 815 /**
 816  * Return true if the NAV FROM flag should be active.
 817  */
 818 bool
 819 FGNavCom::get_nav_from_flag () const
 820 {
 821     if ( nav_inrange
 822          && nav_serviceable->getBoolValue()
 823          && tofrom_serviceable->getBoolValue() ) {
 824         double offset = fabs(nav_radial - nav_target_radial);
 825         if (nav_loc) {
 826             return false;
 827         } else {
 828           return !(offset > 90.0 && offset < 270.0);
 829         }
 830     } else {
 831         return false;
 832     }
 833 }
 834
 835
 836 /**
 837  * Return the true heading to station
 838  */
 839 double
 840 FGNavCom::get_nav_heading () const
 841 {
 842     return nav_heading;
 843 }
 844
 845
 846 /**
 847  * Return the current radial.
 848  */
 849 double
 850 FGNavCom::get_nav_radial () const
 851 {
 852     return nav_radial;
 853 }
 854
 855 double
 856 FGNavCom::get_nav_reciprocal_radial () const
 857 {
 858     double recip = nav_radial + 180;
 859     if ( recip >= 360 ) {
 860         recip -= 360;
 861     }
 862     return recip;
 863 }