src/Instrumentation/navradio.cxx

   1 // navradio.cxx -- class to manage a nav radio instance
   2 //
   3 // Written by Curtis Olson, started April 2000.
   4 //
   5 // Copyright (C) 2000 - 2002  Curtis L. Olson - http://www.flightgear.org/~curt
   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 // $Id$
  22
  23
  24 #ifdef HAVE_CONFIG_H
  25 #  include <config.h>
  26 #endif
  27
  28 #include <sstream>
  29
  30 #include <simgear/sg_inlines.h>
  31 #include <simgear/timing/sg_time.hxx>
  32 #include <simgear/math/vector.hxx>
  33 #include <simgear/math/sg_random.h>
  34 #include <simgear/misc/sg_path.hxx>
  35 #include <simgear/math/sg_geodesy.hxx>
  36 #include <simgear/structure/exception.hxx>
  37 #include <simgear/math/interpolater.hxx>
  38
  39 #include "Navaids/navrecord.hxx"
  40 #include <Navaids/navlist.hxx>
  41 #include <Main/util.hxx>
  42 #include "navradio.hxx"
  43
  44 using std::string;
  45
  46 // Constructor
  47 FGNavRadio::FGNavRadio(SGPropertyNode *node) :
  48     lon_node(fgGetNode("/position/longitude-deg", true)),
  49     lat_node(fgGetNode("/position/latitude-deg", true)),
  50     alt_node(fgGetNode("/position/altitude-ft", true)),
  51     is_valid_node(NULL),
  52     power_btn_node(NULL),
  53     freq_node(NULL),
  54     alt_freq_node(NULL),
  55     sel_radial_node(NULL),
  56     vol_btn_node(NULL),
  57     ident_btn_node(NULL),
  58     audio_btn_node(NULL),
  59     backcourse_node(NULL),
  60     nav_serviceable_node(NULL),
  61     cdi_serviceable_node(NULL),
  62     gs_serviceable_node(NULL),
  63     tofrom_serviceable_node(NULL),
  64     fmt_freq_node(NULL),
  65     fmt_alt_freq_node(NULL),
  66     heading_node(NULL),
  67     radial_node(NULL),
  68     recip_radial_node(NULL),
  69     target_radial_true_node(NULL),
  70     target_auto_hdg_node(NULL),
  71     time_to_intercept(NULL),
  72     to_flag_node(NULL),
  73     from_flag_node(NULL),
  74     inrange_node(NULL),
  75     signal_quality_norm_node(NULL),
  76     cdi_deflection_node(NULL),
  77     cdi_xtrack_error_node(NULL),
  78     cdi_xtrack_hdg_err_node(NULL),
  79     has_gs_node(NULL),
  80     loc_node(NULL),
  81     loc_dist_node(NULL),
  82     gs_deflection_node(NULL),
  83     gs_deflection_norm_node(NULL),
  84     gs_rate_of_climb_node(NULL),
  85     gs_dist_node(NULL),
  86     nav_id_node(NULL),
  87     id_c1_node(NULL),
  88     id_c2_node(NULL),
  89     id_c3_node(NULL),
  90     id_c4_node(NULL),
  91     nav_slaved_to_gps_node(NULL),
  92     gps_cdi_deflection_node(NULL),
  93     gps_to_flag_node(NULL),
  94     gps_from_flag_node(NULL),
  95     gps_has_gs_node(NULL),
  96     play_count(0),
  97     last_time(0),
  98     target_radial(0.0),
  99     horiz_vel(0.0),
 100     last_x(0.0),
 101     last_loc_dist(0.0),
 102     last_xtrack_error(0.0),
 103     _name(node->getStringValue("name", "nav")),
 104     _num(node->getIntValue("number", 0)),
 105     _time_before_search_sec(-1.0)
 106 {
 107     SGPath path( globals->get_fg_root() );
 108     SGPath term = path;
 109     term.append( "Navaids/range.term" );
 110     SGPath low = path;
 111     low.append( "Navaids/range.low" );
 112     SGPath high = path;
 113     high.append( "Navaids/range.high" );
 114
 115     term_tbl = new SGInterpTable( term.str() );
 116     low_tbl = new SGInterpTable( low.str() );
 117     high_tbl = new SGInterpTable( high.str() );
 118 }
 119
 120
 121 // Destructor
 122 FGNavRadio::~FGNavRadio()
 123 {
 124     delete term_tbl;
 125     delete low_tbl;
 126     delete high_tbl;
 127 }
 128
 129
 130 void
 131 FGNavRadio::init ()
 132 {
 133     morse.init();
 134
 135     string branch;
 136     branch = "/instrumentation/" + _name;
 137
 138     SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
 139
 140     bus_power_node =
 141         fgGetNode(("/systems/electrical/outputs/" + _name).c_str(), true);
 142
 143     // inputs
 144     is_valid_node = node->getChild("data-is-valid", 0, true);
 145     power_btn_node = node->getChild("power-btn", 0, true);
 146     power_btn_node->setBoolValue( true );
 147     vol_btn_node = node->getChild("volume", 0, true);
 148     ident_btn_node = node->getChild("ident", 0, true);
 149     ident_btn_node->setBoolValue( true );
 150     audio_btn_node = node->getChild("audio-btn", 0, true);
 151     audio_btn_node->setBoolValue( true );
 152     backcourse_node = node->getChild("back-course-btn", 0, true);
 153     backcourse_node->setBoolValue( false );
 154     nav_serviceable_node = node->getChild("serviceable", 0, true);
 155     cdi_serviceable_node = (node->getChild("cdi", 0, true))
 156         ->getChild("serviceable", 0, true);
 157     gs_serviceable_node = (node->getChild("gs", 0, true))
 158         ->getChild("serviceable");
 159     tofrom_serviceable_node = (node->getChild("to-from", 0, true))
 160         ->getChild("serviceable", 0, true);
 161
 162     // frequencies
 163     SGPropertyNode *subnode = node->getChild("frequencies", 0, true);
 164     freq_node = subnode->getChild("selected-mhz", 0, true);
 165     alt_freq_node = subnode->getChild("standby-mhz", 0, true);
 166     fmt_freq_node = subnode->getChild("selected-mhz-fmt", 0, true);
 167     fmt_alt_freq_node = subnode->getChild("standby-mhz-fmt", 0, true);
 168
 169     // radials
 170     subnode = node->getChild("radials", 0, true);
 171     sel_radial_node = subnode->getChild("selected-deg", 0, true);
 172     radial_node = subnode->getChild("actual-deg", 0, true);
 173     recip_radial_node = subnode->getChild("reciprocal-radial-deg", 0, true);
 174     target_radial_true_node = subnode->getChild("target-radial-deg", 0, true);
 175     target_auto_hdg_node = subnode->getChild("target-auto-hdg-deg", 0, true);
 176
 177     // outputs
 178     heading_node = node->getChild("heading-deg", 0, true);
 179     time_to_intercept = node->getChild("time-to-intercept-sec", 0, true);
 180     to_flag_node = node->getChild("to-flag", 0, true);
 181     from_flag_node = node->getChild("from-flag", 0, true);
 182     inrange_node = node->getChild("in-range", 0, true);
 183     signal_quality_norm_node = node->getChild("signal-quality-norm", 0, true);
 184     cdi_deflection_node = node->getChild("heading-needle-deflection", 0, true);
 185     cdi_xtrack_error_node = node->getChild("crosstrack-error-m", 0, true);
 186     cdi_xtrack_hdg_err_node
 187         = node->getChild("crosstrack-heading-error-deg", 0, true);
 188     has_gs_node = node->getChild("has-gs", 0, true);
 189     loc_node = node->getChild("nav-loc", 0, true);
 190     loc_dist_node = node->getChild("nav-distance", 0, true);
 191     gs_deflection_node = node->getChild("gs-needle-deflection", 0, true);
 192     gs_deflection_norm_node = node->getChild("gs-needle-deflection-norm", 0, true);
 193     gs_rate_of_climb_node = node->getChild("gs-rate-of-climb", 0, true);
 194     gs_dist_node = node->getChild("gs-distance", 0, true);
 195     nav_id_node = node->getChild("nav-id", 0, true);
 196     id_c1_node = node->getChild("nav-id_asc1", 0, true);
 197     id_c2_node = node->getChild("nav-id_asc2", 0, true);
 198     id_c3_node = node->getChild("nav-id_asc3", 0, true);
 199     id_c4_node = node->getChild("nav-id_asc4", 0, true);
 200
 201     // gps slaving support
 202     nav_slaved_to_gps_node = node->getChild("slaved-to-gps", 0, true);
 203     gps_cdi_deflection_node = fgGetNode("/instrumentation/gps/cdi-deflection", true);
 204     gps_to_flag_node = fgGetNode("/instrumentation/gps/to-flag", true);
 205     gps_from_flag_node = fgGetNode("/instrumentation/gps/from-flag", true);
 206     gps_has_gs_node = fgGetNode("/instrumentation/gps/has-gs", true);
 207
 208     std::ostringstream temp;
 209     temp << _name << "nav-ident" << _num;
 210     nav_fx_name = temp.str();
 211     temp << _name << "dme-ident" << _num;
 212     dme_fx_name = temp.str();
 213 }
 214
 215 void
 216 FGNavRadio::bind ()
 217 {
 218     std::ostringstream temp;
 219     string branch;
 220     temp << _num;
 221     branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
 222 }
 223
 224
 225 void
 226 FGNavRadio::unbind ()
 227 {
 228     std::ostringstream temp;
 229     string branch;
 230     temp << _num;
 231     branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
 232 }
 233
 234
 235 // model standard VOR/DME/TACAN service volumes as per AIM 1-1-8
 236 double FGNavRadio::adjustNavRange( double stationElev, double aircraftElev,
 237                                  double nominalRange )
 238 {
 239     // extend out actual usable range to be 1.3x the published safe range
 240     const double usability_factor = 1.3;
 241
 242     // assumptions we model the standard service volume, plus
 243     // ... rather than specifying a cylinder, we model a cone that
 244     // contains the cylinder.  Then we put an upside down cone on top
 245     // to model diminishing returns at too-high altitudes.
 246
 247     // altitude difference
 248     double alt = ( aircraftElev * SG_METER_TO_FEET - stationElev );
 249     // cout << "aircraft elev = " << aircraftElev * SG_METER_TO_FEET
 250     //      << " station elev = " << stationElev << endl;
 251
 252     if ( nominalRange < 25.0 + SG_EPSILON ) {
 253         // Standard Terminal Service Volume
 254         return term_tbl->interpolate( alt ) * usability_factor;
 255     } else if ( nominalRange < 50.0 + SG_EPSILON ) {
 256         // Standard Low Altitude Service Volume
 257         // table is based on range of 40, scale to actual range
 258         return low_tbl->interpolate( alt ) * nominalRange / 40.0
 259             * usability_factor;
 260     } else {
 261         // Standard High Altitude Service Volume
 262         // table is based on range of 130, scale to actual range
 263         return high_tbl->interpolate( alt ) * nominalRange / 130.0
 264             * usability_factor;
 265     }
 266 }
 267
 268
 269 // model standard ILS service volumes as per AIM 1-1-9
 270 double FGNavRadio::adjustILSRange( double stationElev, double aircraftElev,
 271                                  double offsetDegrees, double distance )
 272 {
 273     // assumptions we model the standard service volume, plus
 274
 275     // altitude difference
 276     // double alt = ( aircraftElev * SG_METER_TO_FEET - stationElev );
 277 //     double offset = fabs( offsetDegrees );
 278
 279 //     if ( offset < 10 ) {
 280 //      return FG_ILS_DEFAULT_RANGE;
 281 //     } else if ( offset < 35 ) {
 282 //      return 10 + (35 - offset) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
 283 //     } else if ( offset < 45 ) {
 284 //      return (45 - offset);
 285 //     } else if ( offset > 170 ) {
 286 //         return FG_ILS_DEFAULT_RANGE;
 287 //     } else if ( offset > 145 ) {
 288 //      return 10 + (offset - 145) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
 289 //     } else if ( offset > 135 ) {
 290 //         return (offset - 135);
 291 //     } else {
 292 //      return 0;
 293 //     }
 294     return FG_LOC_DEFAULT_RANGE;
 295 }
 296
 297
 298 //////////////////////////////////////////////////////////////////////////
 299 // Update the various nav values based on position and valid tuned in navs
 300 //////////////////////////////////////////////////////////////////////////
 301 void
 302 FGNavRadio::update(double dt)
 303 {
 304   if (dt <= 0.0) {
 305     return; // paused
 306   }
 307
 308   // Create "formatted" versions of the nav frequencies for
 309   // instrument displays.
 310   char tmp[16];
 311   sprintf( tmp, "%.2f", freq_node->getDoubleValue() );
 312   fmt_freq_node->setStringValue(tmp);
 313   sprintf( tmp, "%.2f", alt_freq_node->getDoubleValue() );
 314   fmt_alt_freq_node->setStringValue(tmp);
 315
 316   if (power_btn_node->getBoolValue()
 317       && (bus_power_node->getDoubleValue() > 1.0)
 318       && nav_serviceable_node->getBoolValue() )
 319   {
 320     if (nav_slaved_to_gps_node->getBoolValue()) {
 321       updateGPSSlaved();
 322     } else {
 323       updateReceiver(dt);
 324     }
 325
 326     updateCDI(dt);
 327   } else {
 328     clearOutputs();
 329   }
 330
 331   updateAudio();
 332 }
 333
 334 void FGNavRadio::clearOutputs()
 335 {
 336   inrange_node->setBoolValue( false );
 337   cdi_deflection_node->setDoubleValue( 0.0 );
 338   cdi_xtrack_error_node->setDoubleValue( 0.0 );
 339   cdi_xtrack_hdg_err_node->setDoubleValue( 0.0 );
 340   time_to_intercept->setDoubleValue( 0.0 );
 341   gs_deflection_node->setDoubleValue( 0.0 );
 342   gs_deflection_norm_node->setDoubleValue(0.0);
 343
 344   to_flag_node->setBoolValue( false );
 345   from_flag_node->setBoolValue( false );
 346 }
 347
 348 void FGNavRadio::updateReceiver(double dt)
 349 {
 350   // Do a nav station search only once a second to reduce
 351   // unnecessary work. (Also, make sure to do this before caching
 352   // any values!)
 353   _time_before_search_sec -= dt;
 354   if ( _time_before_search_sec < 0 ) {
 355    search();
 356   }
 357
 358   if (!_navaid) {
 359     _cdiDeflection = 0.0;
 360     _cdiCrossTrackErrorM = 0.0;
 361     _toFlag = _fromFlag = false;
 362     _gsNeedleDeflection = 0.0;
 363     _gsNeedleDeflectionNorm = 0.0;
 364     inrange_node->setBoolValue(false);
 365     return;
 366   }
 367
 368   SGGeod pos = SGGeod::fromDegFt(lon_node->getDoubleValue(),
 369                                lat_node->getDoubleValue(),
 370                                alt_node->getDoubleValue());
 371
 372   double nav_elev = _navaid->get_elev_ft();
 373   SGVec3d aircraft = SGVec3d::fromGeod(pos);
 374   double loc_dist = dist(aircraft, _navaid->cart());
 375   loc_dist_node->setDoubleValue( loc_dist );
 376   bool is_loc = loc_node->getBoolValue();
 377   double signal_quality_norm = signal_quality_norm_node->getDoubleValue();
 378
 379   double az2, s;
 380   //////////////////////////////////////////////////////////
 381         // compute forward and reverse wgs84 headings to localizer
 382   //////////////////////////////////////////////////////////
 383   double hdg;
 384   SGGeodesy::inverse(pos, _navaid->geod(), hdg, az2, s);
 385   heading_node->setDoubleValue(hdg);
 386   double radial = az2 - twist;
 387   double recip = radial + 180.0;
 388   SG_NORMALIZE_RANGE(recip, 0.0, 360.0);
 389   radial_node->setDoubleValue( radial );
 390   recip_radial_node->setDoubleValue( recip );
 391
 392   //////////////////////////////////////////////////////////
 393   // compute the target/selected radial in "true" heading
 394   //////////////////////////////////////////////////////////
 395   if (!is_loc) {
 396     target_radial = sel_radial_node->getDoubleValue();
 397   }
 398
 399   // VORs need twist (mag-var) added; ILS/LOCs don't but we set twist to 0.0
 400   double trtrue = target_radial + twist;
 401   SG_NORMALIZE_RANGE(trtrue, 0.0, 360.0);
 402   target_radial_true_node->setDoubleValue( trtrue );
 403
 404   //////////////////////////////////////////////////////////
 405   // adjust reception range for altitude
 406   // FIXME: make sure we are using the navdata range now that
 407   //        it is valid in the data file
 408   //////////////////////////////////////////////////////////
 409         if ( is_loc ) {
 410             double offset = radial - target_radial;
 411       SG_NORMALIZE_RANGE(offset, -180.0, 180.0);
 412             effective_range
 413                 = adjustILSRange( nav_elev, pos.getElevationM(), offset,
 414                                   loc_dist * SG_METER_TO_NM );
 415         } else {
 416             effective_range
 417                 = adjustNavRange( nav_elev, pos.getElevationM(), _navaid->get_range() );
 418         }
 419
 420   double effective_range_m = effective_range * SG_NM_TO_METER;
 421
 422   //////////////////////////////////////////////////////////
 423   // compute signal quality
 424   // 100% within effective_range
 425   // decreases 1/x^2 further out
 426   //////////////////////////////////////////////////////////
 427   double last_signal_quality_norm = signal_quality_norm;
 428
 429   if ( loc_dist < effective_range_m ) {
 430     signal_quality_norm = 1.0;
 431   } else {
 432     double range_exceed_norm = loc_dist/effective_range_m;
 433     signal_quality_norm = 1/(range_exceed_norm*range_exceed_norm);
 434   }
 435
 436   signal_quality_norm = fgGetLowPass( last_signal_quality_norm,
 437            signal_quality_norm, dt );
 438
 439   signal_quality_norm_node->setDoubleValue( signal_quality_norm );
 440   bool inrange = signal_quality_norm > 0.2;
 441   inrange_node->setBoolValue( inrange );
 442
 443   //////////////////////////////////////////////////////////
 444   // compute to/from flag status
 445   //////////////////////////////////////////////////////////
 446   if (inrange) {
 447     if (is_loc) {
 448       _toFlag = true;
 449     } else {
 450       double offset = fabs(radial - target_radial);
 451       _toFlag = (offset > 90.0 && offset < 270.0);
 452     }
 453     _fromFlag = !_toFlag;
 454   } else {
 455     _toFlag = _fromFlag = false;
 456   }
 457
 458   // CDI deflection
 459   double r = radial - target_radial;
 460   SG_NORMALIZE_RANGE(r, -180.0, 180.0);
 461   if ( fabs(r) > 90.0 ) {
 462     r = ( r<0.0 ? -r-180.0 : -r+180.0 );
 463   }
 464
 465   r = -r; // reverse, since radial is outbound
 466   _cdiDeflection = r;
 467   if ( is_loc ) {
 468     // According to Robin Peel, the ILS is 4x more
 469     // sensitive than a vor
 470     // http://www.allstar.fiu.edu/aero/ILS.htm confirms both the 4x sensitvity
 471     // increase, and also the 'full-deflection is 10-degrees for a VOR' clamp
 472     _cdiDeflection *= 4.0;
 473   }
 474   SG_CLAMP_RANGE(_cdiDeflection, -10.0, 10.0 );
 475   _cdiDeflection *= signal_quality_norm;
 476
 477   // cross-track error (in metres)
 478   _cdiCrossTrackErrorM = loc_dist * sin(r * SGD_DEGREES_TO_RADIANS);
 479
 480   updateGlideSlope(dt, aircraft, signal_quality_norm);
 481
 482   last_loc_dist = loc_dist;
 483 }
 484
 485 void FGNavRadio::updateGlideSlope(double dt, const SGVec3d& aircraft, double signal_quality_norm)
 486 {
 487   if (!_gs || !inrange_node->getBoolValue()) {
 488     gs_dist_node->setDoubleValue( 0.0 );
 489     return;
 490   }
 491
 492   // find closest distance to the gs base line
 493   double dist = sgdClosestPointToLineDistSquared(aircraft.data(), _gs->cart().data(),
 494                                                  gs_base_vec.data());
 495   dist = sqrt(dist);
 496   gs_dist_node->setDoubleValue(dist);
 497   double heightAboveStationM =
 498     (alt_node->getDoubleValue() - _gs->elevation()) * SG_FEET_TO_METER;
 499
 500   //////////////////////////////////////////////////////////
 501   // compute the amount of glide slope needle deflection
 502   // (.i.e. the number of degrees we are off the glide slope * 5.0
 503   //
 504   // CLO - 13 Mar 2006: The glide slope needle should peg at
 505   // +/-0.7 degrees off the ideal glideslope.  I'm not sure why
 506   // we compute the factor the way we do (5*gs_error), but we
 507   // need to compensate for our 'odd' number in the glideslope
 508   // needle animation.  This means that the needle should peg
 509   // when this values is +/-3.5.
 510   //////////////////////////////////////////////////////////
 511   double angle = atan2(heightAboveStationM, dist) * SGD_RADIANS_TO_DEGREES;
 512   double deflectionAngle = target_gs - angle;
 513   //SG_CLAMP_RANGE(deflectionAngle, -0.7, 0.7);
 514   _gsNeedleDeflection = deflectionAngle * 5.0;
 515   _gsNeedleDeflection *= signal_quality_norm;
 516   _gsNeedleDeflectionNorm = (deflectionAngle / 0.7) * signal_quality_norm;
 517   SG_CLAMP_RANGE(_gsNeedleDeflectionNorm, -1.0, 1.0);
 518
 519   //////////////////////////////////////////////////////////
 520   // Calculate desired rate of climb for intercepting the GS
 521   //////////////////////////////////////////////////////////
 522   double gs_diff = target_gs - angle;
 523   // convert desired vertical path angle into a climb rate
 524   double des_angle = angle - 10 * gs_diff;
 525
 526   // estimate horizontal speed towards ILS in meters per minute
 527   double elapsedDistance = last_x - dist;
 528   last_x = dist;
 529
 530   double new_vel = ( elapsedDistance / dt );
 531   horiz_vel = 0.75 * horiz_vel + 0.25 * new_vel;
 532
 533   gs_rate_of_climb_node
 534       ->setDoubleValue( -sin( des_angle * SGD_DEGREES_TO_RADIANS )
 535                         * horiz_vel * SG_METER_TO_FEET );
 536 }
 537
 538 void FGNavRadio::updateGPSSlaved()
 539 {
 540   has_gs_node->setBoolValue(gps_has_gs_node->getBoolValue());
 541
 542   _toFlag = gps_to_flag_node->getBoolValue();
 543   _fromFlag = gps_from_flag_node->getBoolValue();
 544
 545   inrange_node->setBoolValue(_toFlag | _fromFlag);
 546
 547   _cdiDeflection =  gps_cdi_deflection_node->getDoubleValue();
 548   // clmap to some range (+/- 10 degrees) as the regular deflection
 549   SG_CLAMP_RANGE(_cdiDeflection, -10.0, 10.0 );
 550
 551   _cdiCrossTrackErrorM = 0.0; // FIXME, supply this
 552   _gsNeedleDeflection = 0.0; // FIXME, supply this
 553 }
 554
 555 void FGNavRadio::updateCDI(double dt)
 556 {
 557   bool cdi_serviceable = cdi_serviceable_node->getBoolValue();
 558   bool inrange = inrange_node->getBoolValue();
 559
 560   if (tofrom_serviceable_node->getBoolValue()) {
 561     to_flag_node->setBoolValue(_toFlag);
 562     from_flag_node->setBoolValue(_fromFlag);
 563   } else {
 564     to_flag_node->setBoolValue(false);
 565     from_flag_node->setBoolValue(false);
 566   }
 567
 568   if (!cdi_serviceable) {
 569     _cdiDeflection = 0.0;
 570     _cdiCrossTrackErrorM = 0.0;
 571   }
 572
 573   cdi_deflection_node->setDoubleValue(_cdiDeflection);
 574   cdi_xtrack_error_node->setDoubleValue(_cdiCrossTrackErrorM);
 575
 576   //////////////////////////////////////////////////////////
 577   // compute an approximate ground track heading error
 578   //////////////////////////////////////////////////////////
 579   double hdg_error = 0.0;
 580   if ( inrange && cdi_serviceable ) {
 581     double vn = fgGetDouble( "/velocities/speed-north-fps" );
 582     double ve = fgGetDouble( "/velocities/speed-east-fps" );
 583     double gnd_trk_true = atan2( ve, vn ) * SGD_RADIANS_TO_DEGREES;
 584     if ( gnd_trk_true < 0.0 ) { gnd_trk_true += 360.0; }
 585
 586     SGPropertyNode *true_hdg
 587         = fgGetNode("/orientation/heading-deg", true);
 588     hdg_error = gnd_trk_true - true_hdg->getDoubleValue();
 589
 590     // cout << "ground track = " << gnd_trk_true
 591     //      << " orientation = " << true_hdg->getDoubleValue() << endl;
 592   }
 593   cdi_xtrack_hdg_err_node->setDoubleValue( hdg_error );
 594
 595   //////////////////////////////////////////////////////////
 596   // Calculate a suggested target heading to smoothly intercept
 597   // a nav/ils radial.
 598   //////////////////////////////////////////////////////////
 599
 600   // Now that we have cross track heading adjustment built in,
 601   // we shouldn't need to overdrive the heading angle within 8km
 602   // of the station.
 603   //
 604   // The cdi deflection should be +/-10 for a full range of deflection
 605   // so multiplying this by 3 gives us +/- 30 degrees heading
 606   // compensation.
 607   double adjustment = _cdiDeflection * 3.0;
 608   SG_CLAMP_RANGE( adjustment, -30.0, 30.0 );
 609
 610   // determine the target heading to fly to intercept the
 611   // tgt_radial = target radial (true) + cdi offset adjustmest -
 612   // xtrack heading error adjustment
 613   double nta_hdg;
 614   double trtrue = target_radial_true_node->getDoubleValue();
 615   if ( loc_node->getBoolValue() && backcourse_node->getBoolValue() ) {
 616       // tuned to a localizer and backcourse mode activated
 617       trtrue += 180.0;   // reverse the target localizer heading
 618       SG_NORMALIZE_RANGE(trtrue, 0.0, 360.0);
 619       nta_hdg = trtrue - adjustment - hdg_error;
 620   } else {
 621       nta_hdg = trtrue + adjustment - hdg_error;
 622   }
 623
 624   SG_NORMALIZE_RANGE(nta_hdg, 0.0, 360.0);
 625   target_auto_hdg_node->setDoubleValue( nta_hdg );
 626
 627   //////////////////////////////////////////////////////////
 628   // compute the time to intercept selected radial (based on
 629   // current and last cross track errors and dt
 630   //////////////////////////////////////////////////////////
 631   double t = 0.0;
 632   if ( inrange && cdi_serviceable ) {
 633     double xrate_ms = (last_xtrack_error - _cdiCrossTrackErrorM) / dt;
 634     if ( fabs(xrate_ms) > 0.00001 ) {
 635         t = _cdiCrossTrackErrorM / xrate_ms;
 636     } else {
 637         t = 9999.9;
 638     }
 639   }
 640   time_to_intercept->setDoubleValue( t );
 641
 642   if (!gs_serviceable_node->getBoolValue() ) {
 643     _gsNeedleDeflection = 0.0;
 644     _gsNeedleDeflectionNorm = 0.0;
 645   }
 646   gs_deflection_node->setDoubleValue(_gsNeedleDeflection);
 647   gs_deflection_norm_node->setDoubleValue(_gsNeedleDeflectionNorm);
 648
 649   last_xtrack_error = _cdiCrossTrackErrorM;
 650 }
 651
 652 void FGNavRadio::updateAudio()
 653 {
 654   if (!_navaid || !inrange_node->getBoolValue() || !nav_serviceable_node->getBoolValue()) {
 655     return;
 656   }
 657
 658         // play station ident via audio system if on + ident,
 659         // otherwise turn it off
 660         if (!power_btn_node->getBoolValue()
 661       || !(bus_power_node->getDoubleValue() > 1.0)
 662       || !ident_btn_node->getBoolValue()
 663       || !audio_btn_node->getBoolValue() ) {
 664     globals->get_soundmgr()->stop( nav_fx_name );
 665     globals->get_soundmgr()->stop( dme_fx_name );
 666     return;
 667   }
 668
 669   SGSoundSample *sound = globals->get_soundmgr()->find( nav_fx_name );
 670   double vol = vol_btn_node->getDoubleValue();
 671   SG_CLAMP_RANGE(vol, 0.0, 1.0);
 672
 673   if ( sound != NULL ) {
 674     sound->set_volume( vol );
 675   } else {
 676     SG_LOG( SG_COCKPIT, SG_ALERT, "Can't find nav-vor-ident sound" );
 677   }
 678
 679   sound = globals->get_soundmgr()->find( dme_fx_name );
 680   if ( sound != NULL ) {
 681     sound->set_volume( vol );
 682   } else {
 683     SG_LOG( SG_COCKPIT, SG_ALERT, "Can't find nav-dme-ident sound" );
 684   }
 685
 686   if ( last_time < globals->get_time_params()->get_cur_time() - 30 ) {
 687                 last_time = globals->get_time_params()->get_cur_time();
 688                 play_count = 0;
 689   }
 690
 691   if ( play_count < 4 ) {
 692                 // play VOR ident
 693                 if ( !globals->get_soundmgr()->is_playing(nav_fx_name) ) {
 694                     globals->get_soundmgr()->play_once( nav_fx_name );
 695                     ++play_count;
 696     }
 697   } else if ( play_count < 5 &&  has_dme) {
 698                 // play DME ident
 699                 if ( !globals->get_soundmgr()->is_playing(nav_fx_name) &&
 700                      !globals->get_soundmgr()->is_playing(dme_fx_name) ) {
 701                     globals->get_soundmgr()->play_once( dme_fx_name );
 702                     ++play_count;
 703                 }
 704   }
 705 }
 706
 707 FGNavRecord* FGNavRadio::findPrimaryNavaid(const SGGeod& aPos, double aFreqMHz)
 708 {
 709   FGNavRecord* nav = globals->get_navlist()->findByFreq(aFreqMHz, aPos);
 710   if (nav) {
 711     return nav;
 712   }
 713
 714   return globals->get_loclist()->findByFreq(aFreqMHz, aPos);
 715 }
 716
 717 // Update current nav/adf radio stations based on current postition
 718 void FGNavRadio::search()
 719 {
 720   _time_before_search_sec = 1.0;
 721   SGGeod pos = SGGeod::fromDegFt(lon_node->getDoubleValue(),
 722     lat_node->getDoubleValue(), alt_node->getDoubleValue());
 723   double freq = freq_node->getDoubleValue();
 724
 725   FGNavRecord* nav = findPrimaryNavaid(pos, freq);
 726   if (nav == _navaid) {
 727     return; // found the same as last search, we're done
 728   }
 729
 730   _navaid = nav;
 731   char identBuffer[5] = "    ";
 732   if (nav) {
 733     FGNavRecord* dme = globals->get_dmelist()->findByFreq(freq, pos);
 734     has_dme = (dme != NULL);
 735
 736     nav_id_node->setStringValue(nav->get_ident());
 737     strncpy(identBuffer, nav->ident().c_str(), 5);
 738
 739     effective_range = adjustNavRange(nav->get_elev_ft(), pos.getElevationM(), nav->get_range());
 740     loc_node->setBoolValue(nav->type() != FGPositioned::VOR);
 741     twist = nav->get_multiuse();
 742
 743     if (nav->type() == FGPositioned::VOR) {
 744       target_radial = sel_radial_node->getDoubleValue();
 745       _gs = NULL;
 746     } else { // ILS or LOC
 747       _gs = globals->get_gslist()->findByFreq(freq, pos);
 748       has_gs_node->setBoolValue(_gs != NULL);
 749       twist = 0.0;
 750             effective_range = FG_LOC_DEFAULT_RANGE;
 751
 752       target_radial = nav->get_multiuse();
 753       SG_NORMALIZE_RANGE(target_radial, 0.0, 360.0);
 754
 755       if (_gs) {
 756         int tmp = (int)(_gs->get_multiuse() / 1000.0);
 757         target_gs = (double)tmp / 100.0;
 758
 759         SGGeod baseLine;
 760         double dummy;
 761         SGGeodesy::direct(_gs->geod(), target_radial + 90.0, 100.0, baseLine, dummy);
 762         gs_base_vec = SGVec3d::fromGeod(baseLine) - _gs->cart();
 763       } // of have glideslope
 764     } // of found LOC or ILS
 765
 766     audioNavidChanged();
 767   } else { // found nothing
 768     _gs = NULL;
 769     nav_id_node->setStringValue("");
 770     has_dme = false;
 771     globals->get_soundmgr()->remove( nav_fx_name );
 772     globals->get_soundmgr()->remove( dme_fx_name );
 773   }
 774
 775   is_valid_node->setBoolValue(nav != NULL);
 776   id_c1_node->setIntValue( (int)identBuffer[0] );
 777   id_c2_node->setIntValue( (int)identBuffer[1] );
 778   id_c3_node->setIntValue( (int)identBuffer[2] );
 779   id_c4_node->setIntValue( (int)identBuffer[3] );
 780 }
 781
 782 void FGNavRadio::audioNavidChanged()
 783 {
 784   if ( globals->get_soundmgr()->exists(nav_fx_name)) {
 785                 globals->get_soundmgr()->remove(nav_fx_name);
 786   }
 787
 788   try {
 789     string trans_ident(_navaid->get_trans_ident());
 790     SGSoundSample* sound = morse.make_ident(trans_ident, LO_FREQUENCY);
 791     sound->set_volume( 0.3 );
 792     if (!globals->get_soundmgr()->add( sound, nav_fx_name )) {
 793       SG_LOG(SG_COCKPIT, SG_WARN, "Failed to add v1-vor-ident sound");
 794     }
 795
 796           if ( globals->get_soundmgr()->exists( dme_fx_name ) ) {
 797       globals->get_soundmgr()->remove( dme_fx_name );
 798     }
 799
 800     sound = morse.make_ident( trans_ident, HI_FREQUENCY );
 801     sound->set_volume( 0.3 );
 802     globals->get_soundmgr()->add( sound, dme_fx_name );
 803
 804           int offset = (int)(sg_random() * 30.0);
 805           play_count = offset / 4;
 806     last_time = globals->get_time_params()->get_cur_time() - offset;
 807   } catch (sg_io_exception& e) {
 808     SG_LOG(SG_GENERAL, SG_ALERT, e.getFormattedMessage());
 809   }
 810 }