1 // navcom.cxx -- class to manage a navcom instance
3 // Written by Curtis Olson, started April 2000.
5 // Copyright (C) 2000 - 2002 Curtis L. Olson - curt@flightgear.org
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.
28 #include <stdio.h> // snprintf
30 #include <simgear/compiler.h>
31 #include <simgear/sg_inlines.h>
32 #include <simgear/math/sg_random.h>
33 #include <simgear/math/vector.hxx>
35 #include <Aircraft/aircraft.hxx>
36 #include <Navaids/ilslist.hxx>
37 #include <Navaids/navlist.hxx>
46 FGNavCom::FGNavCom() :
47 lon_node(fgGetNode("/position/longitude-deg", true)),
48 lat_node(fgGetNode("/position/latitude-deg", true)),
49 alt_node(fgGetNode("/position/altitude-ft", true)),
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),
73 SGPath path( globals->get_fg_root() );
75 term.append( "Navaids/range.term" );
77 low.append( "Navaids/range.low" );
79 high.append( "Navaids/range.high" );
81 term_tbl = new SGInterpTable( term.str() );
82 low_tbl = new SGInterpTable( low.str() );
83 high_tbl = new SGInterpTable( high.str() );
102 // We assume that index is valid now (it must be set before init()
105 // FIXME: Get rid of snprintf
107 snprintf(propname, 256, "/systems/electrical/outputs/navcom[%d]", index);
108 // default to true in case no electrical system defined.
109 fgSetDouble( propname, 60.0 );
110 bus_power = fgGetNode( propname, true );
112 snprintf(propname, 256, "/instrumentation/comm[%d]/serviceable", index);
113 com_serviceable = fgGetNode( propname, true );
114 com_serviceable->setBoolValue( true );
116 snprintf(propname, 256, "/instrumentation/nav[%d]/serviceable", index);
117 nav_serviceable = fgGetNode( propname, true );
118 nav_serviceable->setBoolValue( true );
120 snprintf(propname, 256, "/instrumentation/vor[%d]/cdi/serviceable", index);
121 cdi_serviceable = fgGetNode( propname, true );
122 cdi_serviceable->setBoolValue( true );
124 snprintf(propname, 256, "/instrumentation/vor[%d]/gs/serviceable", index);
125 gs_serviceable = fgGetNode( propname, true );
126 gs_serviceable->setBoolValue( true );
128 snprintf(propname, 256, "/instrumentation/vor[%d]/to-from/serviceable", index);
129 tofrom_serviceable = fgGetNode( propname, true );
130 tofrom_serviceable->setBoolValue( true );
137 // FIXME: Get rid of snprintf
140 snprintf(propname, 256, "/radios/comm[%d]/inputs/power-btn", index);
141 fgTie( propname, this,
142 &FGNavCom::get_power_btn, &FGNavCom::set_power_btn );
143 fgSetArchivable( propname );
145 snprintf(propname, 256, "/radios/comm[%d]/frequencies/selected-mhz", index);
146 fgTie( propname, this, &FGNavCom::get_comm_freq, &FGNavCom::set_comm_freq );
147 fgSetArchivable( propname );
149 snprintf(propname, 256, "/radios/comm[%d]/frequencies/standby-mhz", index);
150 fgTie( propname, this,
151 &FGNavCom::get_comm_alt_freq, &FGNavCom::set_comm_alt_freq );
152 fgSetArchivable( propname );
154 snprintf(propname, 256, "/radios/comm[%d]/volume", index);
155 fgTie( propname, this,
156 &FGNavCom::get_comm_vol_btn, &FGNavCom::set_comm_vol_btn );
157 fgSetArchivable( propname );
159 snprintf(propname, 256, "/radios/nav[%d]/frequencies/selected-mhz", index);
160 fgTie( propname, this,
161 &FGNavCom::get_nav_freq, &FGNavCom::set_nav_freq );
162 fgSetArchivable( propname );
164 snprintf(propname, 256, "/radios/nav[%d]/frequencies/standby-mhz", index);
165 fgTie( propname , this,
166 &FGNavCom::get_nav_alt_freq, &FGNavCom::set_nav_alt_freq);
167 fgSetArchivable( propname );
169 snprintf(propname, 256, "/radios/nav[%d]/radials/selected-deg", index);
170 fgTie( propname, this,
171 &FGNavCom::get_nav_sel_radial, &FGNavCom::set_nav_sel_radial );
172 fgSetArchivable( propname );
174 snprintf(propname, 256, "/radios/nav[%d]/volume", index);
175 fgTie( propname, this,
176 &FGNavCom::get_nav_vol_btn, &FGNavCom::set_nav_vol_btn );
177 fgSetArchivable( propname );
179 snprintf(propname, 256, "/radios/nav[%d]/ident", index);
180 fgTie( propname, this,
181 &FGNavCom::get_nav_ident_btn, &FGNavCom::set_nav_ident_btn );
182 fgSetArchivable( propname );
185 snprintf(propname, 256, "/radios/nav[%d]/audio-btn", index);
186 fgTie( propname, this,
187 &FGNavCom::get_audio_btn, &FGNavCom::set_audio_btn );
188 fgSetArchivable( propname );
190 snprintf(propname, 256, "/radios/nav[%d]/heading-deg", index);
191 fgTie( propname, this, &FGNavCom::get_nav_heading );
193 snprintf(propname, 256, "/radios/nav[%d]/radials/actual-deg", index);
194 fgTie( propname, this, &FGNavCom::get_nav_radial );
196 snprintf(propname, 256, "/radios/nav[%d]/radials/target-radial-deg", index);
197 fgTie( propname, this, &FGNavCom::get_nav_target_radial_true );
199 snprintf(propname, 256, "/radios/nav[%d]/radials/target-auto-hdg-deg",
201 fgTie( propname, this, &FGNavCom::get_nav_target_auto_hdg );
203 snprintf(propname, 256, "/radios/nav[%d]/to-flag", index);
204 fgTie( propname, this, &FGNavCom::get_nav_to_flag );
206 snprintf(propname, 256, "/radios/nav[%d]/from-flag", index);
207 fgTie( propname, this, &FGNavCom::get_nav_from_flag );
209 snprintf(propname, 256, "/radios/nav[%d]/in-range", index);
210 fgTie( propname, this, &FGNavCom::get_nav_inrange );
212 snprintf(propname, 256, "/radios/nav[%d]/heading-needle-deflection", index);
213 fgTie( propname, this, &FGNavCom::get_nav_cdi_deflection );
215 snprintf(propname, 256, "/radios/nav[%d]/crosstrack-error-m", index);
216 fgTie( propname, this, &FGNavCom::get_nav_cdi_xtrack_error );
218 snprintf(propname, 256, "/radios/nav[%d]/has-gs", index);
219 fgTie( propname, this, &FGNavCom::get_nav_has_gs );
221 snprintf(propname, 256, "/radios/nav[%d]/nav-loc", index);
222 fgTie( propname, this, &FGNavCom::get_nav_loc );
224 snprintf(propname, 256, "/radios/nav[%d]/gs-rate-of-climb", index);
225 fgTie( propname, this, &FGNavCom::get_nav_gs_rate_of_climb );
227 snprintf(propname, 256, "/radios/nav[%d]/gs-needle-deflection", index);
228 fgTie( propname, this, &FGNavCom::get_nav_gs_deflection );
230 snprintf(propname, 256, "/radios/nav[%d]/nav-id", index);
231 fgTie( propname, this, &FGNavCom::get_nav_id );
233 // put nav_id characters into seperate properties for instrument displays
234 snprintf(propname, 256, "/radios/nav[%d]/nav-id_asc1", index);
235 fgTie( propname, this, &FGNavCom::get_nav_id_c1 );
237 snprintf(propname, 256, "/radios/nav[%d]/nav-id_asc2", index);
238 fgTie( propname, this, &FGNavCom::get_nav_id_c2 );
240 snprintf(propname, 256, "/radios/nav[%d]/nav-id_asc3", index);
241 fgTie( propname, this, &FGNavCom::get_nav_id_c3 );
243 snprintf(propname, 256, "/radios/nav[%d]/nav-id_asc4", index);
244 fgTie( propname, this, &FGNavCom::get_nav_id_c4 );
254 // FIXME: Get rid of snprintf
256 snprintf(propname, 256, "/radios/comm[%d]/inputs/power-btn", index);
258 snprintf(propname, 256, "/radios/comm[%d]/frequencies/selected-mhz", index);
260 snprintf(propname, 256, "/radios/comm[%d]/frequencies/standby-mhz", index);
263 snprintf(propname, 256, "/radios/nav[%d]/frequencies/selected-mhz", index);
265 snprintf(propname, 256, "/radios/nav[%d]/frequencies/standby-mhz", index);
267 snprintf(propname, 256, "/radios/nav[%d]/radials/actual-deg", index);
269 snprintf(propname, 256, "/radios/nav[%d]/radials/selected-deg", index);
271 snprintf(propname, 256, "/radios/nav[%d]/ident", index);
273 snprintf(propname, 256, "/radios/nav[%d]/to-flag", index);
275 snprintf(propname, 256, "/radios/nav[%d]/from-flag", index);
277 snprintf(propname, 256, "/radios/nav[%d]/in-range", index);
279 snprintf(propname, 256, "/radios/nav[%d]/heading-needle-deflection", index);
281 snprintf(propname, 256, "/radios/nav[%d]/gs-needle-deflection", index);
286 // model standard VOR/DME/TACAN service volumes as per AIM 1-1-8
287 double FGNavCom::adjustNavRange( double stationElev, double aircraftElev,
288 double nominalRange )
290 // extend out actual usable range to be 1.3x the published safe range
291 const double usability_factor = 1.3;
293 // assumptions we model the standard service volume, plus
294 // ... rather than specifying a cylinder, we model a cone that
295 // contains the cylinder. Then we put an upside down cone on top
296 // to model diminishing returns at too-high altitudes.
298 // altitude difference
299 double alt = ( aircraftElev * SG_METER_TO_FEET - stationElev );
300 // cout << "aircraft elev = " << aircraftElev * SG_METER_TO_FEET
301 // << " station elev = " << stationElev << endl;
303 if ( nominalRange < 25.0 + SG_EPSILON ) {
304 // Standard Terminal Service Volume
305 return term_tbl->interpolate( alt ) * usability_factor;
306 } else if ( nominalRange < 50.0 + SG_EPSILON ) {
307 // Standard Low Altitude Service Volume
308 // table is based on range of 40, scale to actual range
309 return low_tbl->interpolate( alt ) * nominalRange / 40.0
312 // Standard High Altitude Service Volume
313 // table is based on range of 130, scale to actual range
314 return high_tbl->interpolate( alt ) * nominalRange / 130.0
320 // model standard ILS service volumes as per AIM 1-1-9
321 double FGNavCom::adjustILSRange( double stationElev, double aircraftElev,
322 double offsetDegrees, double distance )
324 // assumptions we model the standard service volume, plus
326 // altitude difference
327 // double alt = ( aircraftElev * SG_METER_TO_FEET - stationElev );
328 // double offset = fabs( offsetDegrees );
330 // if ( offset < 10 ) {
331 // return FG_ILS_DEFAULT_RANGE;
332 // } else if ( offset < 35 ) {
333 // return 10 + (35 - offset) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
334 // } else if ( offset < 45 ) {
335 // return (45 - offset);
336 // } else if ( offset > 170 ) {
337 // return FG_ILS_DEFAULT_RANGE;
338 // } else if ( offset > 145 ) {
339 // return 10 + (offset - 145) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
340 // } else if ( offset > 135 ) {
341 // return (offset - 135);
345 return FG_ILS_DEFAULT_RANGE;
349 // Update the various nav values based on position and valid tuned in navs
351 FGNavCom::update(double dt)
353 double lon = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
354 double lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
355 double elev = alt_node->getDoubleValue() * SG_FEET_TO_METER;
359 Point3D aircraft = sgGeodToCart( Point3D( lon, lat, elev ) );
363 ////////////////////////////////////////////////////////////////////////
365 ////////////////////////////////////////////////////////////////////////
367 if ( nav_valid && power_btn && (bus_power->getDoubleValue() > 1.0)
368 && nav_serviceable->getBoolValue() )
370 station = Point3D( nav_x, nav_y, nav_z );
371 nav_loc_dist = aircraft.distance3D( station );
374 // find closest distance to the gs base line
376 sgdSetVec3( p, aircraft.x(), aircraft.y(), aircraft.z() );
378 sgdSetVec3( p0, nav_gs_x, nav_gs_y, nav_gs_z );
379 double dist = sgdClosestPointToLineDistSquared( p, p0,
381 nav_gs_dist = sqrt( dist );
382 // cout << nav_gs_dist;
384 // Point3D tmp( nav_gs_x, nav_gs_y, nav_gs_z );
385 // cout << " (" << aircraft.distance3D( tmp ) << ")" << endl;
387 // wgs84 heading to glide slope (to determine sign of distance)
388 geo_inverse_wgs_84( elev,
389 lat * SGD_RADIANS_TO_DEGREES,
390 lon * SGD_RADIANS_TO_DEGREES,
391 nav_gslat, nav_gslon,
393 double r = az1 - nav_target_radial;
394 while ( r > 180.0 ) { r -= 360.0;}
395 while ( r < -180.0 ) { r += 360.0;}
396 if ( r >= -90.0 && r <= 90.0 ) {
397 nav_gs_dist_signed = nav_gs_dist;
399 nav_gs_dist_signed = -nav_gs_dist;
401 /* cout << "Target Radial = " << nav_target_radial
402 << " Bearing = " << az1
403 << " dist (signed) = " << nav_gs_dist_signed
410 // wgs84 heading to localizer
411 geo_inverse_wgs_84( elev,
412 lat * SGD_RADIANS_TO_DEGREES,
413 lon * SGD_RADIANS_TO_DEGREES,
414 nav_loclat, nav_loclon,
415 &nav_heading, &az2, &s );
416 // cout << "az1 = " << az1 << " magvar = " << nav_magvar << endl;
417 nav_radial = az2 - nav_twist;
418 // cout << " heading = " << nav_heading
419 // << " dist = " << nav_dist << endl;
422 double offset = nav_radial - nav_target_radial;
423 while ( offset < -180.0 ) { offset += 360.0; }
424 while ( offset > 180.0 ) { offset -= 360.0; }
425 // cout << "ils offset = " << offset << endl;
427 = adjustILSRange( nav_elev, elev, offset,
428 nav_loc_dist * SG_METER_TO_NM );
430 nav_effective_range = adjustNavRange( nav_elev, elev, nav_range );
432 // cout << "nav range = " << nav_effective_range
433 // << " (" << nav_range << ")" << endl;
435 if ( nav_loc_dist < nav_effective_range * SG_NM_TO_METER ) {
437 } else if ( nav_loc_dist < 2 * nav_effective_range * SG_NM_TO_METER ) {
438 nav_inrange = sg_random() <
439 ( 2 * nav_effective_range * SG_NM_TO_METER - nav_loc_dist ) /
440 (nav_effective_range * SG_NM_TO_METER);
446 nav_target_radial = nav_sel_radial;
449 // Calculate some values for the nav/ils hold autopilot
451 double cur_radial = get_nav_reciprocal_radial();
453 // ILS localizers radials are already "true" in our
456 cur_radial += nav_twist;
458 if ( get_nav_from_flag() ) {
460 while ( cur_radial >= 360.0 ) { cur_radial -= 360.0; }
465 // determine the target radial in "true" heading
466 nav_target_radial_true = nav_target_radial;
468 // ILS localizers radials are already "true" in our
471 // VOR radials need to have that vor's offset added in
472 nav_target_radial_true += nav_twist;
475 while ( nav_target_radial_true < 0.0 ) {
476 nav_target_radial_true += 360.0;
478 while ( nav_target_radial_true > 360.0 ) {
479 nav_target_radial_true -= 360.0;
482 // determine the heading adjustment needed.
483 // over 8km scale by 3.0
484 // (3 is chosen because max deflection is 10
485 // and 30 is clamped angle to radial)
486 // under 8km scale by 10.0
487 // because the overstated error helps drive it to the radial in a
488 // moderate cross wind.
489 double adjustment = 0.0;
490 if (nav_loc_dist > 8000) {
491 adjustment = get_nav_cdi_deflection() * 3.0;
493 adjustment = get_nav_cdi_deflection() * 10.0;
495 SG_CLAMP_RANGE( adjustment, -30.0, 30.0 );
497 // determine the target heading to fly to intercept the
499 nav_target_auto_hdg = nav_target_radial_true + adjustment;
500 while ( nav_target_auto_hdg < 0.0 ) { nav_target_auto_hdg += 360.0; }
501 while ( nav_target_auto_hdg > 360.0 ) { nav_target_auto_hdg -= 360.0; }
506 // Calculate desired rate of climb for intercepting the GS
507 double x = nav_gs_dist;
508 double y = (alt_node->getDoubleValue() - nav_elev)
510 double current_angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
512 double target_angle = nav_target_gs;
513 double gs_diff = target_angle - current_angle;
515 // convert desired vertical path angle into a climb rate
516 double des_angle = current_angle - 10 * gs_diff;
518 // estimate horizontal speed towards ILS in meters per minute
519 double dist = last_x - x;
523 double new_vel = ( dist / dt );
525 horiz_vel = 0.75 * horiz_vel + 0.25 * new_vel;
526 // double horiz_vel = cur_fdm_state->get_V_ground_speed()
527 // * SG_FEET_TO_METER * 60.0;
528 // double horiz_vel = airspeed_node->getFloatValue()
529 // * SG_FEET_TO_METER * 60.0;
531 nav_gs_rate_of_climb = -sin( des_angle * SGD_DEGREES_TO_RADIANS )
532 * horiz_vel * SG_METER_TO_FEET;
536 // cout << "not picking up vor. :-(" << endl;
539 if ( nav_valid && nav_inrange && nav_serviceable->getBoolValue() ) {
540 // play station ident via audio system if on + ident,
541 // otherwise turn it off
542 if ( power_btn && (bus_power->getDoubleValue() > 1.0)
543 && nav_ident_btn && audio_btn )
545 SGSoundSample *sound;
546 sound = globals->get_soundmgr()->find( nav_fx_name );
547 if ( sound != NULL ) {
548 sound->set_volume( nav_vol_btn );
550 SG_LOG( SG_COCKPIT, SG_ALERT,
551 "Can't find nav-vor-ident sound" );
553 sound = globals->get_soundmgr()->find( dme_fx_name );
554 if ( sound != NULL ) {
555 sound->set_volume( nav_vol_btn );
557 SG_LOG( SG_COCKPIT, SG_ALERT,
558 "Can't find nav-dme-ident sound" );
560 // cout << "nav_last_time = " << nav_last_time << " ";
561 // cout << "cur_time = "
562 // << globals->get_time_params()->get_cur_time();
564 globals->get_time_params()->get_cur_time() - 30 ) {
565 nav_last_time = globals->get_time_params()->get_cur_time();
568 // cout << " nav_play_count = " << nav_play_count << endl;
569 // cout << "playing = "
570 // << globals->get_soundmgr()->is_playing(nav_fx_name)
572 if ( nav_play_count < 4 ) {
574 if ( !globals->get_soundmgr()->is_playing(nav_fx_name) ) {
575 globals->get_soundmgr()->play_once( nav_fx_name );
578 } else if ( nav_play_count < 5 && nav_has_dme ) {
580 if ( !globals->get_soundmgr()->is_playing(nav_fx_name) &&
581 !globals->get_soundmgr()->is_playing(dme_fx_name) ) {
582 globals->get_soundmgr()->play_once( dme_fx_name );
587 globals->get_soundmgr()->stop( nav_fx_name );
588 globals->get_soundmgr()->stop( dme_fx_name );
594 // Update current nav/adf radio stations based on current postition
595 void FGNavCom::search()
597 double lon = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
598 double lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
599 double elev = alt_node->getDoubleValue() * SG_FEET_TO_METER;
601 FGNavRecord *nav = NULL;
602 FGNavRecord *loc = NULL;
603 FGNavRecord *dme = NULL;
604 FGNavRecord *gs = NULL;
606 ////////////////////////////////////////////////////////////////////////
608 ////////////////////////////////////////////////////////////////////////
610 nav = globals->get_navlist()->findByFreq(nav_freq, lon, lat, elev);
611 dme = globals->get_dmelist()->findByFreq(nav_freq, lon, lat, elev);
613 loc = globals->get_loclist()->findByFreq(nav_freq, lon, lat, elev);
614 gs = globals->get_gslist()->findByFreq(nav_freq, lon, lat, elev);
619 nav_id = loc->get_ident();
621 if ( last_nav_id != nav_id || last_nav_vor ) {
622 nav_trans_ident = loc->get_trans_ident();
623 last_nav_id = nav_id;
624 last_nav_vor = false;
626 nav_has_dme = (dme != NULL);
627 nav_has_gs = (gs != NULL);
629 nav_gslon = gs->get_lon();
630 nav_gslat = gs->get_lat();
631 int tmp = (int)(gs->get_multiuse() / 1000.0);
632 nav_target_gs = (double)tmp / 100.0;
633 nav_gs_x = gs->get_x();
634 nav_gs_y = gs->get_y();
635 nav_gs_z = gs->get_z();
637 // derive GS baseline
638 double tlon, tlat, taz;
639 geo_direct_wgs_84 ( 0.0, nav_gslat, nav_gslon,
640 nav_target_radial + 90,
641 100.0, &tlat, &tlon, &taz );
642 // cout << nav_gslon << "," << nav_gslat << " "
643 // << tlon << "," << tlat << " (" << nav_elev << ")"
645 Point3D p1 = sgGeodToCart( Point3D(tlon*SGD_DEGREES_TO_RADIANS,
646 tlat*SGD_DEGREES_TO_RADIANS,
647 nav_elev*SG_FEET_TO_METER) );
648 // cout << nav_gs_x << "," << nav_gs_y << "," << nav_gs_z
650 // cout << p1 << endl;
651 sgdSetVec3( gs_base_vec,
652 p1.x()-nav_gs_x, p1.y()-nav_gs_y, p1.z()-nav_gs_z );
653 // cout << gs_base_vec[0] << "," << gs_base_vec[1] << ","
654 // << gs_base_vec[2] << endl;
656 nav_loclon = loc->get_lon();
657 nav_loclat = loc->get_lat();
658 nav_elev = loc->get_elev_ft();
660 nav_range = FG_LOC_DEFAULT_RANGE;
661 nav_effective_range = nav_range;
662 nav_target_radial = loc->get_multiuse();
663 while ( nav_target_radial < 0.0 ) { nav_target_radial += 360.0; }
664 while ( nav_target_radial > 360.0 ) { nav_target_radial -= 360.0; }
665 nav_x = loc->get_x();
666 nav_y = loc->get_y();
667 nav_z = loc->get_z();
669 if ( globals->get_soundmgr()->exists( nav_fx_name ) ) {
670 globals->get_soundmgr()->remove( nav_fx_name );
672 SGSoundSample *sound;
673 sound = morse.make_ident( nav_trans_ident, LO_FREQUENCY );
674 sound->set_volume( 0.3 );
675 globals->get_soundmgr()->add( sound, nav_fx_name );
677 if ( globals->get_soundmgr()->exists( dme_fx_name ) ) {
678 globals->get_soundmgr()->remove( dme_fx_name );
680 sound = morse.make_ident( nav_trans_ident, HI_FREQUENCY );
681 sound->set_volume( 0.3 );
682 globals->get_soundmgr()->add( sound, dme_fx_name );
684 int offset = (int)(sg_random() * 30.0);
685 nav_play_count = offset / 4;
686 nav_last_time = globals->get_time_params()->get_cur_time() -
688 // cout << "offset = " << offset << " play_count = "
690 // << " nav_last_time = " << nav_last_time
691 // << " current time = "
692 // << globals->get_time_params()->get_cur_time() << endl;
694 // cout << "Found an loc station in range" << endl;
695 // cout << " id = " << loc->get_locident() << endl;
697 } else if ( nav != NULL ) {
698 nav_id = nav->get_ident();
700 if ( last_nav_id != nav_id || !last_nav_vor ) {
701 last_nav_id = nav_id;
703 nav_trans_ident = nav->get_trans_ident();
705 nav_has_dme = (dme != NULL);
707 nav_loclon = nav->get_lon();
708 nav_loclat = nav->get_lat();
709 nav_elev = nav->get_elev_ft();
710 nav_twist = nav->get_multiuse();
711 nav_range = nav->get_range();
712 nav_effective_range = adjustNavRange(nav_elev, elev, nav_range);
714 nav_target_radial = nav_sel_radial;
715 nav_x = nav->get_x();
716 nav_y = nav->get_y();
717 nav_z = nav->get_z();
719 if ( globals->get_soundmgr()->exists( nav_fx_name ) ) {
720 globals->get_soundmgr()->remove( nav_fx_name );
722 SGSoundSample *sound;
723 sound = morse.make_ident( nav_trans_ident, LO_FREQUENCY );
724 sound->set_volume( 0.3 );
725 if ( globals->get_soundmgr()->add( sound, nav_fx_name ) ) {
726 // cout << "Added nav-vor-ident sound" << endl;
728 SG_LOG(SG_COCKPIT, SG_WARN, "Failed to add v1-vor-ident sound");
731 if ( globals->get_soundmgr()->exists( dme_fx_name ) ) {
732 globals->get_soundmgr()->remove( dme_fx_name );
734 sound = morse.make_ident( nav_trans_ident, HI_FREQUENCY );
735 sound->set_volume( 0.3 );
736 globals->get_soundmgr()->add( sound, dme_fx_name );
738 int offset = (int)(sg_random() * 30.0);
739 nav_play_count = offset / 4;
740 nav_last_time = globals->get_time_params()->get_cur_time() -
742 // cout << "offset = " << offset << " play_count = "
743 // << nav_play_count << " nav_last_time = "
744 // << nav_last_time << " current time = "
745 // << globals->get_time_params()->get_cur_time() << endl;
747 // cout << "Found a vor station in range" << endl;
748 // cout << " id = " << nav->get_ident() << endl;
753 nav_target_radial = 0;
754 nav_trans_ident = "";
756 if ( ! globals->get_soundmgr()->remove( nav_fx_name ) ) {
757 SG_LOG(SG_COCKPIT, SG_WARN, "Failed to remove nav-vor-ident sound");
759 globals->get_soundmgr()->remove( dme_fx_name );
760 // cout << "not picking up vor1. :-(" << endl;
765 // return the amount of heading needle deflection, returns a value
766 // clamped to the range of ( -10 , 10 )
767 double FGNavCom::get_nav_cdi_deflection() const {
771 && nav_serviceable->getBoolValue() && cdi_serviceable->getBoolValue() )
773 r = nav_radial - nav_target_radial;
774 // cout << "Target radial = " << nav_target_radial
775 // << " Actual radial = " << nav_radial << endl;
777 while ( r > 180.0 ) { r -= 360.0;}
778 while ( r < -180.0 ) { r += 360.0;}
779 if ( fabs(r) > 90.0 )
780 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
782 // According to Robin Peel, the ILS is 4x more sensitive than a vor
783 r = -r; // reverse, since radial is outbound
784 if ( nav_loc ) { r *= 4.0; }
785 if ( r < -10.0 ) { r = -10.0; }
786 if ( r > 10.0 ) { r = 10.0; }
794 // return the amount of cross track distance error, returns a meters
795 double FGNavCom::get_nav_cdi_xtrack_error() const {
799 && nav_serviceable->getBoolValue() && cdi_serviceable->getBoolValue() )
801 r = nav_radial - nav_target_radial;
802 // cout << "Target radial = " << nav_target_radial
803 // << " Actual radial = " << nav_radial
804 // << " r = " << r << endl;
806 while ( r > 180.0 ) { r -= 360.0;}
807 while ( r < -180.0 ) { r += 360.0;}
808 if ( fabs(r) > 90.0 )
809 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
811 r = -r; // reverse, since radial is outbound
813 m = nav_loc_dist * sin(r * SGD_DEGREES_TO_RADIANS);
822 // return the amount of glide slope needle deflection (.i.e. the
823 // number of degrees we are off the glide slope * 5.0
824 double FGNavCom::get_nav_gs_deflection() const {
825 if ( nav_inrange && nav_has_gs
826 && nav_serviceable->getBoolValue() && gs_serviceable->getBoolValue() )
828 double x = nav_gs_dist;
829 double y = (fgGetDouble("/position/altitude-ft") - nav_elev)
831 // cout << "dist = " << x << " height = " << y << endl;
832 double angle = asin( y / x ) * SGD_RADIANS_TO_DEGREES;
833 return (nav_target_gs - angle) * 5.0;
841 * Return true if the NAV TO flag should be active.
844 FGNavCom::get_nav_to_flag () const
847 && nav_serviceable->getBoolValue()
848 && tofrom_serviceable->getBoolValue() )
850 double offset = fabs(nav_radial - nav_target_radial);
854 return !(offset <= 90.0 || offset >= 270.0);
863 * Return true if the NAV FROM flag should be active.
866 FGNavCom::get_nav_from_flag () const
869 && nav_serviceable->getBoolValue()
870 && tofrom_serviceable->getBoolValue() ) {
871 double offset = fabs(nav_radial - nav_target_radial);
875 return !(offset > 90.0 && offset < 270.0);
884 * Return the true heading to station
887 FGNavCom::get_nav_heading () const
894 * Return the current radial.
897 FGNavCom::get_nav_radial () const
903 FGNavCom::get_nav_reciprocal_radial () const
905 double recip = nav_radial + 180;
906 if ( recip >= 360 ) {