1 // navradio.cxx -- class to manage a nav radio instance
3 // Written by Curtis Olson, started April 2000.
5 // Copyright (C) 2000 - 2002 Curtis L. Olson - http://www.flightgear.org/~curt
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.
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.
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.
32 #include <simgear/compiler.h>
33 #include <simgear/sg_inlines.h>
34 #include <simgear/math/sg_random.h>
35 #include <simgear/math/vector.hxx>
37 #include <Aircraft/aircraft.hxx>
38 #include <Navaids/navlist.hxx>
40 #include "navradio.hxx"
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)),
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),
74 _time_before_search_sec(-1.0)
76 SGPath path( globals->get_fg_root() );
78 term.append( "Navaids/range.term" );
80 low.append( "Navaids/range.low" );
82 high.append( "Navaids/range.high" );
84 term_tbl = new SGInterpTable( term.str() );
85 low_tbl = new SGInterpTable( low.str() );
86 high_tbl = new SGInterpTable( high.str() );
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" ) {
95 } else if ( cname == "number" ) {
96 num = child->getIntValue();
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 );
110 FGNavRadio::~FGNavRadio()
124 branch = "/instrumentation/" + name;
126 SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
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);
138 std::ostringstream temp;
139 temp << name << "nav-ident" << num;
140 nav_fx_name = temp.str();
141 temp << name << "dme-ident" << num;
142 dme_fx_name = temp.str();
148 std::ostringstream temp;
151 branch = "/instrumentation/" + name + "[" + temp.str() + "]";
154 fgTie( (branch + "power-btn").c_str(), this,
155 &FGNavRadio::get_power_btn, &FGNavRadio::set_power_btn );
156 fgSetArchivable( (branch + "power-btn").c_str() );
158 fgTie( (branch + "/frequencies/selected-mhz").c_str() , this,
159 &FGNavRadio::get_nav_freq, &FGNavRadio::set_nav_freq );
160 fgSetArchivable( (branch + "/frequencies/selected-mhz").c_str() );
162 fgTie( (branch + "/frequencies/standby-mhz").c_str() , this,
163 &FGNavRadio::get_nav_alt_freq, &FGNavRadio::set_nav_alt_freq);
164 fgSetArchivable( (branch + "/frequencies/standby-mhz").c_str() );
166 fgTie( (branch + "/frequencies/selected-mhz-fmt").c_str() , this,
167 &FGNavRadio::get_fmt_freq, &FGNavRadio::set_fmt_freq );
168 fgSetArchivable( (branch + "/frequencies/selected-mhz-fmt").c_str() );
170 fgTie( (branch + "/frequencies/standby-mhz-fmt").c_str() , this,
171 &FGNavRadio::get_fmt_alt_freq, &FGNavRadio::set_fmt_alt_freq);
172 fgSetArchivable( (branch + "/frequencies/standby-mhz-fmt").c_str() );
174 fgTie( (branch + "/radials/selected-deg").c_str() , this,
175 &FGNavRadio::get_nav_sel_radial, &FGNavRadio::set_nav_sel_radial );
176 fgSetArchivable((branch + "/radials/selected-deg").c_str() );
178 fgTie( (branch + "/volume").c_str() , this,
179 &FGNavRadio::get_nav_vol_btn, &FGNavRadio::set_nav_vol_btn );
180 fgSetArchivable( (branch + "/volume").c_str() );
182 fgTie( (branch + "/ident").c_str(), this,
183 &FGNavRadio::get_nav_ident_btn, &FGNavRadio::set_nav_ident_btn );
184 fgSetArchivable( (branch + "/ident").c_str() );
187 fgTie( (branch + "/audio-btn").c_str(), this,
188 &FGNavRadio::get_audio_btn, &FGNavRadio::set_audio_btn );
189 fgSetArchivable( (branch + "/audio-btn").c_str() );
191 fgTie( (branch + "/heading-deg").c_str(),
192 this, &FGNavRadio::get_nav_heading );
194 fgTie( (branch + "/radials/actual-deg").c_str(),
195 this, &FGNavRadio::get_nav_radial );
197 fgTie( (branch + "/radials/target-radial-deg").c_str(),
198 this, &FGNavRadio::get_nav_target_radial_true );
200 fgTie( (branch + "/radials/reciprocal-radial-deg").c_str(),
201 this, &FGNavRadio::get_nav_reciprocal_radial );
203 fgTie( (branch + "/radials/target-auto-hdg-deg").c_str(),
204 this, &FGNavRadio::get_nav_target_auto_hdg );
206 fgTie( (branch + "/to-flag").c_str(),
207 this, &FGNavRadio::get_nav_to_flag );
209 fgTie( (branch + "/from-flag").c_str(),
210 this, &FGNavRadio::get_nav_from_flag );
212 fgTie( (branch + "/in-range").c_str(),
213 this, &FGNavRadio::get_nav_inrange );
215 fgTie( (branch + "/heading-needle-deflection").c_str(),
216 this, &FGNavRadio::get_nav_cdi_deflection );
218 fgTie( (branch + "/crosstrack-error-m").c_str(),
219 this, &FGNavRadio::get_nav_cdi_xtrack_error );
221 fgTie( (branch + "/has-gs").c_str(),
222 this, &FGNavRadio::get_nav_has_gs );
224 fgTie( (branch + "/nav-loc").c_str(),
225 this, &FGNavRadio::get_nav_loc );
227 fgTie( (branch + "/gs-rate-of-climb").c_str(),
228 this, &FGNavRadio::get_nav_gs_rate_of_climb );
230 fgTie( (branch + "/gs-needle-deflection").c_str(),
231 this, &FGNavRadio::get_nav_gs_deflection );
233 fgTie( (branch + "/gs-distance").c_str(),
234 this, &FGNavRadio::get_nav_gs_dist_signed );
236 fgTie( (branch + "/nav-distance").c_str(),
237 this, &FGNavRadio::get_nav_loc_dist );
239 fgTie( (branch + "/nav-id").c_str(),
240 this, &FGNavRadio::get_nav_id );
242 // put nav_id characters into seperate properties for instrument displays
243 fgTie( (branch + "/nav-id_asc1").c_str(),
244 this, &FGNavRadio::get_nav_id_c1 );
246 fgTie( (branch + "/nav-id_asc2").c_str(),
247 this, &FGNavRadio::get_nav_id_c2 );
249 fgTie( (branch + "/nav-id_asc3").c_str(),
250 this, &FGNavRadio::get_nav_id_c3 );
252 fgTie( (branch + "/nav-id_asc4").c_str(),
253 this, &FGNavRadio::get_nav_id_c4 );
260 FGNavRadio::unbind ()
262 std::ostringstream temp;
265 branch = "/instrumentation/" + name + "[" + temp.str() + "]";
267 fgUntie( (branch + "/frequencies/selected-mhz").c_str() );
268 fgUntie( (branch + "/frequencies/standby-mhz").c_str() );
269 fgUntie( (branch + "/radials/actual-deg").c_str() );
270 fgUntie( (branch + "/radials/selected-deg").c_str() );
271 fgUntie( (branch + "/ident").c_str() );
272 fgUntie( (branch + "/to-flag").c_str() );
273 fgUntie( (branch + "/from-flag").c_str() );
274 fgUntie( (branch + "/in-range").c_str() );
275 fgUntie( (branch + "/heading-needle-deflection").c_str() );
276 fgUntie( (branch + "/gs-needle-deflection").c_str() );
280 // model standard VOR/DME/TACAN service volumes as per AIM 1-1-8
281 double FGNavRadio::adjustNavRange( double stationElev, double aircraftElev,
282 double nominalRange )
284 // extend out actual usable range to be 1.3x the published safe range
285 const double usability_factor = 1.3;
287 // assumptions we model the standard service volume, plus
288 // ... rather than specifying a cylinder, we model a cone that
289 // contains the cylinder. Then we put an upside down cone on top
290 // to model diminishing returns at too-high altitudes.
292 // altitude difference
293 double alt = ( aircraftElev * SG_METER_TO_FEET - stationElev );
294 // cout << "aircraft elev = " << aircraftElev * SG_METER_TO_FEET
295 // << " station elev = " << stationElev << endl;
297 if ( nominalRange < 25.0 + SG_EPSILON ) {
298 // Standard Terminal Service Volume
299 return term_tbl->interpolate( alt ) * usability_factor;
300 } else if ( nominalRange < 50.0 + SG_EPSILON ) {
301 // Standard Low Altitude Service Volume
302 // table is based on range of 40, scale to actual range
303 return low_tbl->interpolate( alt ) * nominalRange / 40.0
306 // Standard High Altitude Service Volume
307 // table is based on range of 130, scale to actual range
308 return high_tbl->interpolate( alt ) * nominalRange / 130.0
314 // model standard ILS service volumes as per AIM 1-1-9
315 double FGNavRadio::adjustILSRange( double stationElev, double aircraftElev,
316 double offsetDegrees, double distance )
318 // assumptions we model the standard service volume, plus
320 // altitude difference
321 // double alt = ( aircraftElev * SG_METER_TO_FEET - stationElev );
322 // double offset = fabs( offsetDegrees );
324 // if ( offset < 10 ) {
325 // return FG_ILS_DEFAULT_RANGE;
326 // } else if ( offset < 35 ) {
327 // return 10 + (35 - offset) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
328 // } else if ( offset < 45 ) {
329 // return (45 - offset);
330 // } else if ( offset > 170 ) {
331 // return FG_ILS_DEFAULT_RANGE;
332 // } else if ( offset > 145 ) {
333 // return 10 + (offset - 145) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
334 // } else if ( offset > 135 ) {
335 // return (offset - 135);
339 return FG_LOC_DEFAULT_RANGE;
343 // Update the various nav values based on position and valid tuned in navs
345 FGNavRadio::update(double dt)
347 double lon = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
348 double lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
349 double elev = alt_node->getDoubleValue() * SG_FEET_TO_METER;
353 Point3D aircraft = sgGeodToCart( Point3D( lon, lat, elev ) );
357 // Create "formatted" versions of the nav frequencies for
358 // consistant display output.
360 sprintf( tmp, "%.2f", nav_freq );
362 sprintf( tmp, "%.2f", nav_alt_freq );
365 // On timeout, scan again
366 _time_before_search_sec -= dt;
367 if ( _time_before_search_sec < 0 ) {
371 ////////////////////////////////////////////////////////////////////////
373 ////////////////////////////////////////////////////////////////////////
375 // cout << "nav_valid = " << nav_valid
376 // << " power_btn = " << power_btn
377 // << " bus_power = " << bus_power->getDoubleValue()
378 // << " nav_serviceable = " << nav_serviceable->getBoolValue()
381 if ( nav_valid && power_btn && (bus_power->getDoubleValue() > 1.0)
382 && nav_serviceable->getBoolValue() )
384 station = Point3D( nav_x, nav_y, nav_z );
385 nav_loc_dist = aircraft.distance3D( station );
386 // cout << "station = " << station << " dist = " << nav_loc_dist
390 // find closest distance to the gs base line
392 sgdSetVec3( p, aircraft.x(), aircraft.y(), aircraft.z() );
394 sgdSetVec3( p0, nav_gs_x, nav_gs_y, nav_gs_z );
395 double dist = sgdClosestPointToLineDistSquared( p, p0,
397 nav_gs_dist = sqrt( dist );
398 // cout << "nav_gs_dist = " << nav_gs_dist << endl;
400 Point3D tmp( nav_gs_x, nav_gs_y, nav_gs_z );
401 // cout << " (" << aircraft.distance3D( tmp ) << ")" << endl;
403 // wgs84 heading to glide slope (to determine sign of distance)
404 geo_inverse_wgs_84( elev,
405 lat * SGD_RADIANS_TO_DEGREES,
406 lon * SGD_RADIANS_TO_DEGREES,
407 nav_gslat, nav_gslon,
409 double r = az1 - nav_target_radial;
410 while ( r > 180.0 ) { r -= 360.0;}
411 while ( r < -180.0 ) { r += 360.0;}
412 if ( r >= -90.0 && r <= 90.0 ) {
413 nav_gs_dist_signed = nav_gs_dist;
415 nav_gs_dist_signed = -nav_gs_dist;
417 /* cout << "Target Radial = " << nav_target_radial
418 << " Bearing = " << az1
419 << " dist (signed) = " << nav_gs_dist_signed
426 // wgs84 heading to localizer
427 geo_inverse_wgs_84( elev,
428 lat * SGD_RADIANS_TO_DEGREES,
429 lon * SGD_RADIANS_TO_DEGREES,
430 nav_loclat, nav_loclon,
431 &nav_heading, &az2, &s );
432 // cout << "az1 = " << az1 << " magvar = " << nav_magvar << endl;
433 nav_radial = az2 - nav_twist;
434 // cout << " heading = " << nav_heading
435 // << " dist = " << nav_dist << endl;
438 double offset = nav_radial - nav_target_radial;
439 while ( offset < -180.0 ) { offset += 360.0; }
440 while ( offset > 180.0 ) { offset -= 360.0; }
441 // cout << "ils offset = " << offset << endl;
443 = adjustILSRange( nav_elev, elev, offset,
444 nav_loc_dist * SG_METER_TO_NM );
446 nav_effective_range = adjustNavRange( nav_elev, elev, nav_range );
448 // cout << "nav range = " << nav_effective_range
449 // << " (" << nav_range << ")" << endl;
451 if ( nav_loc_dist < nav_effective_range * SG_NM_TO_METER ) {
453 } else if ( nav_loc_dist < 2 * nav_effective_range * SG_NM_TO_METER ) {
454 nav_inrange = sg_random() <
455 ( 2 * nav_effective_range * SG_NM_TO_METER - nav_loc_dist ) /
456 (nav_effective_range * SG_NM_TO_METER);
462 nav_target_radial = nav_sel_radial;
465 // Calculate some values for the nav/ils hold autopilot
467 double cur_radial = get_nav_reciprocal_radial();
469 // ILS localizers radials are already "true" in our
472 cur_radial += nav_twist;
474 if ( get_nav_from_flag() ) {
476 while ( cur_radial >= 360.0 ) { cur_radial -= 360.0; }
481 // determine the target radial in "true" heading
482 nav_target_radial_true = nav_target_radial;
484 // ILS localizers radials are already "true" in our
487 // VOR radials need to have that vor's offset added in
488 nav_target_radial_true += nav_twist;
491 while ( nav_target_radial_true < 0.0 ) {
492 nav_target_radial_true += 360.0;
494 while ( nav_target_radial_true > 360.0 ) {
495 nav_target_radial_true -= 360.0;
498 // determine the heading adjustment needed.
499 // over 8km scale by 3.0
500 // (3 is chosen because max deflection is 10
501 // and 30 is clamped angle to radial)
502 // under 8km scale by 10.0
503 // because the overstated error helps drive it to the radial in a
504 // moderate cross wind.
505 double adjustment = 0.0;
506 if (nav_loc_dist > 8000) {
507 adjustment = get_nav_cdi_deflection() * 3.0;
509 adjustment = get_nav_cdi_deflection() * 10.0;
511 SG_CLAMP_RANGE( adjustment, -30.0, 30.0 );
513 // determine the target heading to fly to intercept the
515 nav_target_auto_hdg = nav_target_radial_true + adjustment;
516 while ( nav_target_auto_hdg < 0.0 ) { nav_target_auto_hdg += 360.0; }
517 while ( nav_target_auto_hdg > 360.0 ) { nav_target_auto_hdg -= 360.0; }
522 // Calculate desired rate of climb for intercepting the GS
523 double x = nav_gs_dist;
524 double y = (alt_node->getDoubleValue() - nav_elev)
526 double current_angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
528 double target_angle = nav_target_gs;
529 double gs_diff = target_angle - current_angle;
531 // convert desired vertical path angle into a climb rate
532 double des_angle = current_angle - 10 * gs_diff;
534 // estimate horizontal speed towards ILS in meters per minute
535 double dist = last_x - x;
539 double new_vel = ( dist / dt );
541 horiz_vel = 0.75 * horiz_vel + 0.25 * new_vel;
542 // double horiz_vel = cur_fdm_state->get_V_ground_speed()
543 // * SG_FEET_TO_METER * 60.0;
544 // double horiz_vel = airspeed_node->getFloatValue()
545 // * SG_FEET_TO_METER * 60.0;
547 nav_gs_rate_of_climb = -sin( des_angle * SGD_DEGREES_TO_RADIANS )
548 * horiz_vel * SG_METER_TO_FEET;
552 // cout << "not picking up vor. :-(" << endl;
555 if ( nav_valid && nav_inrange && nav_serviceable->getBoolValue() ) {
556 // play station ident via audio system if on + ident,
557 // otherwise turn it off
558 if ( power_btn && (bus_power->getDoubleValue() > 1.0)
559 && nav_ident_btn && audio_btn )
561 SGSoundSample *sound;
562 sound = globals->get_soundmgr()->find( nav_fx_name );
563 if ( sound != NULL ) {
564 sound->set_volume( nav_vol_btn );
566 SG_LOG( SG_COCKPIT, SG_ALERT,
567 "Can't find nav-vor-ident sound" );
569 sound = globals->get_soundmgr()->find( dme_fx_name );
570 if ( sound != NULL ) {
571 sound->set_volume( nav_vol_btn );
573 SG_LOG( SG_COCKPIT, SG_ALERT,
574 "Can't find nav-dme-ident sound" );
576 // cout << "nav_last_time = " << nav_last_time << " ";
577 // cout << "cur_time = "
578 // << globals->get_time_params()->get_cur_time();
580 globals->get_time_params()->get_cur_time() - 30 ) {
581 nav_last_time = globals->get_time_params()->get_cur_time();
584 // cout << " nav_play_count = " << nav_play_count << endl;
585 // cout << "playing = "
586 // << globals->get_soundmgr()->is_playing(nav_fx_name)
588 if ( nav_play_count < 4 ) {
590 if ( !globals->get_soundmgr()->is_playing(nav_fx_name) ) {
591 globals->get_soundmgr()->play_once( nav_fx_name );
594 } else if ( nav_play_count < 5 && nav_has_dme ) {
596 if ( !globals->get_soundmgr()->is_playing(nav_fx_name) &&
597 !globals->get_soundmgr()->is_playing(dme_fx_name) ) {
598 globals->get_soundmgr()->play_once( dme_fx_name );
603 globals->get_soundmgr()->stop( nav_fx_name );
604 globals->get_soundmgr()->stop( dme_fx_name );
610 // Update current nav/adf radio stations based on current postition
611 void FGNavRadio::search()
615 _time_before_search_sec = 1.0;
617 double lon = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
618 double lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
619 double elev = alt_node->getDoubleValue() * SG_FEET_TO_METER;
621 FGNavRecord *nav = NULL;
622 FGNavRecord *loc = NULL;
623 FGNavRecord *dme = NULL;
624 FGNavRecord *gs = NULL;
626 ////////////////////////////////////////////////////////////////////////
628 ////////////////////////////////////////////////////////////////////////
630 nav = globals->get_navlist()->findByFreq(nav_freq, lon, lat, elev);
631 dme = globals->get_dmelist()->findByFreq(nav_freq, lon, lat, elev);
633 loc = globals->get_loclist()->findByFreq(nav_freq, lon, lat, elev);
634 gs = globals->get_gslist()->findByFreq(nav_freq, lon, lat, elev);
638 nav_id = loc->get_ident();
639 // cout << "localizer = " << nav_id << endl;
641 if ( last_nav_id != nav_id || last_nav_vor ) {
642 nav_trans_ident = loc->get_trans_ident();
643 nav_target_radial = loc->get_multiuse();
644 while ( nav_target_radial < 0.0 ) { nav_target_radial += 360.0; }
645 while ( nav_target_radial > 360.0 ) { nav_target_radial -= 360.0; }
646 nav_loclon = loc->get_lon();
647 nav_loclat = loc->get_lat();
648 nav_x = loc->get_x();
649 nav_y = loc->get_y();
650 nav_z = loc->get_z();
651 last_nav_id = nav_id;
652 last_nav_vor = false;
654 nav_has_dme = (dme != NULL);
655 nav_has_gs = (gs != NULL);
657 nav_gslon = gs->get_lon();
658 nav_gslat = gs->get_lat();
659 nav_elev = gs->get_elev_ft();
660 int tmp = (int)(gs->get_multiuse() / 1000.0);
661 nav_target_gs = (double)tmp / 100.0;
662 nav_gs_x = gs->get_x();
663 nav_gs_y = gs->get_y();
664 nav_gs_z = gs->get_z();
666 // derive GS baseline (perpendicular to the runay
668 double tlon, tlat, taz;
669 geo_direct_wgs_84 ( 0.0, nav_gslat, nav_gslon,
670 nav_target_radial + 90,
671 100.0, &tlat, &tlon, &taz );
672 // cout << "nav_target_radial = " << nav_target_radial << endl;
673 // cout << "nav_loc = " << nav_loc << endl;
674 // cout << nav_gslon << "," << nav_gslat << " "
675 // << tlon << "," << tlat << " (" << nav_elev << ")"
677 Point3D p1 = sgGeodToCart( Point3D(tlon*SGD_DEGREES_TO_RADIANS,
678 tlat*SGD_DEGREES_TO_RADIANS,
679 nav_elev*SG_FEET_TO_METER)
681 // cout << nav_gs_x << "," << nav_gs_y << "," << nav_gs_z
683 // cout << p1 << endl;
684 sgdSetVec3( gs_base_vec,
685 p1.x()-nav_gs_x, p1.y()-nav_gs_y, p1.z()-nav_gs_z );
686 // cout << gs_base_vec[0] << "," << gs_base_vec[1] << ","
687 // << gs_base_vec[2] << endl;
689 nav_elev = loc->get_elev_ft();
692 nav_range = FG_LOC_DEFAULT_RANGE;
693 nav_effective_range = nav_range;
695 if ( globals->get_soundmgr()->exists( nav_fx_name ) ) {
696 globals->get_soundmgr()->remove( nav_fx_name );
698 SGSoundSample *sound;
699 sound = morse.make_ident( nav_trans_ident, LO_FREQUENCY );
700 sound->set_volume( 0.3 );
701 globals->get_soundmgr()->add( sound, nav_fx_name );
703 if ( globals->get_soundmgr()->exists( dme_fx_name ) ) {
704 globals->get_soundmgr()->remove( dme_fx_name );
706 sound = morse.make_ident( nav_trans_ident, HI_FREQUENCY );
707 sound->set_volume( 0.3 );
708 globals->get_soundmgr()->add( sound, dme_fx_name );
710 int offset = (int)(sg_random() * 30.0);
711 nav_play_count = offset / 4;
712 nav_last_time = globals->get_time_params()->get_cur_time() -
714 // cout << "offset = " << offset << " play_count = "
716 // << " nav_last_time = " << nav_last_time
717 // << " current time = "
718 // << globals->get_time_params()->get_cur_time() << endl;
720 // cout << "Found an loc station in range" << endl;
721 // cout << " id = " << loc->get_locident() << endl;
723 } else if ( nav != NULL ) {
724 nav_id = nav->get_ident();
725 // cout << "nav = " << nav_id << endl;
727 if ( last_nav_id != nav_id || !last_nav_vor ) {
728 last_nav_id = nav_id;
730 nav_trans_ident = nav->get_trans_ident();
732 nav_has_dme = (dme != NULL);
734 nav_loclon = nav->get_lon();
735 nav_loclat = nav->get_lat();
736 nav_elev = nav->get_elev_ft();
737 nav_twist = nav->get_multiuse();
738 nav_range = nav->get_range();
739 nav_effective_range = adjustNavRange(nav_elev, elev, nav_range);
741 nav_target_radial = nav_sel_radial;
742 nav_x = nav->get_x();
743 nav_y = nav->get_y();
744 nav_z = nav->get_z();
746 if ( globals->get_soundmgr()->exists( nav_fx_name ) ) {
747 globals->get_soundmgr()->remove( nav_fx_name );
749 SGSoundSample *sound;
750 sound = morse.make_ident( nav_trans_ident, LO_FREQUENCY );
751 sound->set_volume( 0.3 );
752 if ( globals->get_soundmgr()->add( sound, nav_fx_name ) ) {
753 // cout << "Added nav-vor-ident sound" << endl;
755 SG_LOG(SG_COCKPIT, SG_WARN, "Failed to add v1-vor-ident sound");
758 if ( globals->get_soundmgr()->exists( dme_fx_name ) ) {
759 globals->get_soundmgr()->remove( dme_fx_name );
761 sound = morse.make_ident( nav_trans_ident, HI_FREQUENCY );
762 sound->set_volume( 0.3 );
763 globals->get_soundmgr()->add( sound, dme_fx_name );
765 int offset = (int)(sg_random() * 30.0);
766 nav_play_count = offset / 4;
767 nav_last_time = globals->get_time_params()->get_cur_time() -
769 // cout << "offset = " << offset << " play_count = "
770 // << nav_play_count << " nav_last_time = "
771 // << nav_last_time << " current time = "
772 // << globals->get_time_params()->get_cur_time() << endl;
774 // cout << "Found a vor station in range" << endl;
775 // cout << " id = " << nav->get_ident() << endl;
780 nav_target_radial = 0;
781 nav_trans_ident = "";
783 if ( ! globals->get_soundmgr()->remove( nav_fx_name ) ) {
784 SG_LOG(SG_COCKPIT, SG_WARN, "Failed to remove nav-vor-ident sound");
786 globals->get_soundmgr()->remove( dme_fx_name );
787 // cout << "not picking up vor1. :-(" << endl;
792 // return the amount of heading needle deflection, returns a value
793 // clamped to the range of ( -10 , 10 )
794 double FGNavRadio::get_nav_cdi_deflection() const {
798 && nav_serviceable->getBoolValue() && cdi_serviceable->getBoolValue() )
800 r = nav_radial - nav_target_radial;
801 // cout << "Target radial = " << nav_target_radial
802 // << " Actual radial = " << nav_radial << endl;
804 while ( r > 180.0 ) { r -= 360.0;}
805 while ( r < -180.0 ) { r += 360.0;}
806 if ( fabs(r) > 90.0 )
807 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
809 // According to Robin Peel, the ILS is 4x more sensitive than a vor
810 r = -r; // reverse, since radial is outbound
811 if ( nav_loc ) { r *= 4.0; }
812 if ( r < -10.0 ) { r = -10.0; }
813 if ( r > 10.0 ) { r = 10.0; }
821 // return the amount of cross track distance error, returns a meters
822 double FGNavRadio::get_nav_cdi_xtrack_error() const {
826 && nav_serviceable->getBoolValue() && cdi_serviceable->getBoolValue() )
828 r = nav_radial - nav_target_radial;
829 // cout << "Target radial = " << nav_target_radial
830 // << " Actual radial = " << nav_radial
831 // << " r = " << r << endl;
833 while ( r > 180.0 ) { r -= 360.0;}
834 while ( r < -180.0 ) { r += 360.0;}
835 if ( fabs(r) > 90.0 )
836 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
838 r = -r; // reverse, since radial is outbound
840 m = nav_loc_dist * sin(r * SGD_DEGREES_TO_RADIANS);
849 // return the amount of glide slope needle deflection (.i.e. the
850 // number of degrees we are off the glide slope * 5.0
851 double FGNavRadio::get_nav_gs_deflection() const {
852 if ( nav_inrange && nav_has_gs
853 && nav_serviceable->getBoolValue() && gs_serviceable->getBoolValue() )
855 double x = nav_gs_dist;
856 double y = (fgGetDouble("/position/altitude-ft") - nav_elev)
858 // cout << "dist = " << x << " height = " << y << endl;
859 double angle = asin( y / x ) * SGD_RADIANS_TO_DEGREES;
860 return (nav_target_gs - angle) * 5.0;
868 * Return true if the NAV TO flag should be active.
871 FGNavRadio::get_nav_to_flag () const
874 && nav_serviceable->getBoolValue()
875 && tofrom_serviceable->getBoolValue() )
877 double offset = fabs(nav_radial - nav_target_radial);
881 return !(offset <= 90.0 || offset >= 270.0);
890 * Return true if the NAV FROM flag should be active.
893 FGNavRadio::get_nav_from_flag () const
896 && nav_serviceable->getBoolValue()
897 && tofrom_serviceable->getBoolValue() ) {
898 double offset = fabs(nav_radial - nav_target_radial);
902 return !(offset > 90.0 && offset < 270.0);
911 * Return the true heading to station
914 FGNavRadio::get_nav_heading () const
921 * Return the current radial.
924 FGNavRadio::get_nav_radial () const
930 FGNavRadio::get_nav_reciprocal_radial () const
932 double recip = nav_radial + 180;
933 if ( recip >= 360 ) {