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