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;
426 nav_effective_range = adjustILSRange(nav_elev, elev, offset,
427 nav_loc_dist * SG_METER_TO_NM );
429 nav_effective_range = adjustNavRange(nav_elev, elev, nav_range);
431 // cout << "nav range = " << nav_effective_range
432 // << " (" << nav_range << ")" << endl;
434 if ( nav_loc_dist < nav_effective_range * SG_NM_TO_METER ) {
436 } else if ( nav_loc_dist < 2 * nav_effective_range * SG_NM_TO_METER ) {
437 nav_inrange = sg_random() <
438 ( 2 * nav_effective_range * SG_NM_TO_METER - nav_loc_dist ) /
439 (nav_effective_range * SG_NM_TO_METER);
445 nav_target_radial = nav_sel_radial;
448 // Calculate some values for the nav/ils hold autopilot
450 double cur_radial = get_nav_reciprocal_radial();
452 // ILS localizers radials are already "true" in our
455 cur_radial += nav_twist;
457 if ( get_nav_from_flag() ) {
459 while ( cur_radial >= 360.0 ) { cur_radial -= 360.0; }
464 // determine the target radial in "true" heading
465 nav_target_radial_true = nav_target_radial;
467 // ILS localizers radials are already "true" in our
470 // VOR radials need to have that vor's offset added in
471 nav_target_radial_true += nav_twist;
474 while ( nav_target_radial_true < 0.0 ) {
475 nav_target_radial_true += 360.0;
477 while ( nav_target_radial_true > 360.0 ) {
478 nav_target_radial_true -= 360.0;
481 // determine the heading adjustment needed.
482 // over 8km scale by 3.0
483 // (3 is chosen because max deflection is 10
484 // and 30 is clamped angle to radial)
485 // under 8km scale by 10.0
486 // because the overstated error helps drive it to the radial in a
487 // moderate cross wind.
488 double adjustment = 0.0;
489 if (nav_loc_dist > 8000) {
490 adjustment = get_nav_cdi_deflection() * 3.0;
492 adjustment = get_nav_cdi_deflection() * 10.0;
494 SG_CLAMP_RANGE( adjustment, -30.0, 30.0 );
496 // determine the target heading to fly to intercept the
498 nav_target_auto_hdg = nav_target_radial_true + adjustment;
499 while ( nav_target_auto_hdg < 0.0 ) { nav_target_auto_hdg += 360.0; }
500 while ( nav_target_auto_hdg > 360.0 ) { nav_target_auto_hdg -= 360.0; }
505 // Calculate desired rate of climb for intercepting the GS
506 double x = nav_gs_dist;
507 double y = (alt_node->getDoubleValue() - nav_elev)
509 double current_angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
511 double target_angle = nav_target_gs;
512 double gs_diff = target_angle - current_angle;
514 // convert desired vertical path angle into a climb rate
515 double des_angle = current_angle - 10 * gs_diff;
517 // estimate horizontal speed towards ILS in meters per minute
518 double dist = last_x - x;
522 double new_vel = ( dist / dt );
524 horiz_vel = 0.75 * horiz_vel + 0.25 * new_vel;
525 // double horiz_vel = cur_fdm_state->get_V_ground_speed()
526 // * SG_FEET_TO_METER * 60.0;
527 // double horiz_vel = airspeed_node->getFloatValue()
528 // * SG_FEET_TO_METER * 60.0;
530 nav_gs_rate_of_climb = -sin( des_angle * SGD_DEGREES_TO_RADIANS )
531 * horiz_vel * SG_METER_TO_FEET;
535 // cout << "not picking up vor. :-(" << endl;
538 if ( nav_valid && nav_inrange && nav_serviceable->getBoolValue() ) {
539 // play station ident via audio system if on + ident,
540 // otherwise turn it off
541 if ( power_btn && (bus_power->getDoubleValue() > 1.0)
542 && nav_ident_btn && audio_btn )
544 SGSoundSample *sound;
545 sound = globals->get_soundmgr()->find( nav_fx_name );
546 if ( sound != NULL ) {
547 sound->set_volume( nav_vol_btn );
549 SG_LOG( SG_COCKPIT, SG_ALERT,
550 "Can't find nav-vor-ident sound" );
552 sound = globals->get_soundmgr()->find( dme_fx_name );
553 if ( sound != NULL ) {
554 sound->set_volume( nav_vol_btn );
556 SG_LOG( SG_COCKPIT, SG_ALERT,
557 "Can't find nav-dme-ident sound" );
559 // cout << "nav_last_time = " << nav_last_time << " ";
560 // cout << "cur_time = "
561 // << globals->get_time_params()->get_cur_time();
563 globals->get_time_params()->get_cur_time() - 30 ) {
564 nav_last_time = globals->get_time_params()->get_cur_time();
567 // cout << " nav_play_count = " << nav_play_count << endl;
568 // cout << "playing = "
569 // << globals->get_soundmgr()->is_playing(nav_fx_name)
571 if ( nav_play_count < 4 ) {
573 if ( !globals->get_soundmgr()->is_playing(nav_fx_name) ) {
574 globals->get_soundmgr()->play_once( nav_fx_name );
577 } else if ( nav_play_count < 5 && nav_has_dme ) {
579 if ( !globals->get_soundmgr()->is_playing(nav_fx_name) &&
580 !globals->get_soundmgr()->is_playing(dme_fx_name) ) {
581 globals->get_soundmgr()->play_once( dme_fx_name );
586 globals->get_soundmgr()->stop( nav_fx_name );
587 globals->get_soundmgr()->stop( dme_fx_name );
593 // Update current nav/adf radio stations based on current postition
594 void FGNavCom::search()
596 double lon = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
597 double lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
598 double elev = alt_node->getDoubleValue() * SG_FEET_TO_METER;
603 ////////////////////////////////////////////////////////////////////////
605 ////////////////////////////////////////////////////////////////////////
607 if ( (ils = current_ilslist->findByFreq(nav_freq, lon, lat, elev)) != NULL ) {
608 nav_id = ils->get_locident();
610 if ( last_nav_id != nav_id || last_nav_vor ) {
611 nav_trans_ident = ils->get_trans_ident();
612 last_nav_id = nav_id;
613 last_nav_vor = false;
615 nav_has_dme = ils->get_has_dme();
616 nav_has_gs = ils->get_has_gs();
618 nav_loclon = ils->get_loclon();
619 nav_loclat = ils->get_loclat();
620 nav_gslon = ils->get_gslon();
621 nav_gslat = ils->get_gslat();
622 nav_elev = ils->get_gselev();
624 nav_range = FG_ILS_DEFAULT_RANGE;
625 nav_effective_range = nav_range;
626 nav_target_gs = ils->get_gsangle();
627 nav_target_radial = ils->get_locheading();
628 while ( nav_target_radial < 0.0 ) { nav_target_radial += 360.0; }
629 while ( nav_target_radial > 360.0 ) { nav_target_radial -= 360.0; }
630 nav_x = ils->get_x();
631 nav_y = ils->get_y();
632 nav_z = ils->get_z();
633 nav_gs_x = ils->get_gs_x();
634 nav_gs_y = ils->get_gs_y();
635 nav_gs_z = ils->get_gs_z();
637 // derive GS baseline
638 double tlon, tlat, taz;
639 geo_direct_wgs_84 ( 0.0, nav_gslat, nav_gslon, nav_target_radial + 90,
640 100.0, &tlat, &tlon, &taz );
641 // cout << nav_gslon << "," << nav_gslat << " "
642 // << tlon << "," << tlat << " (" << nav_elev << ")" << endl;
643 Point3D p1 = sgGeodToCart( Point3D(tlon*SGD_DEGREES_TO_RADIANS,
644 tlat*SGD_DEGREES_TO_RADIANS,
645 nav_elev*SG_FEET_TO_METER) );
646 // cout << nav_gs_x << "," << nav_gs_y << "," << nav_gs_z << endl;
647 // cout << p1 << endl;
648 sgdSetVec3( gs_base_vec,
649 p1.x()-nav_gs_x, p1.y()-nav_gs_y, p1.z()-nav_gs_z );
650 // cout << gs_base_vec[0] << "," << gs_base_vec[1] << ","
651 // << gs_base_vec[2] << endl;
653 if ( globals->get_soundmgr()->exists( nav_fx_name ) ) {
654 globals->get_soundmgr()->remove( nav_fx_name );
656 SGSoundSample *sound;
657 sound = morse.make_ident( nav_trans_ident, LO_FREQUENCY );
658 sound->set_volume( 0.3 );
659 globals->get_soundmgr()->add( sound, nav_fx_name );
661 if ( globals->get_soundmgr()->exists( dme_fx_name ) ) {
662 globals->get_soundmgr()->remove( dme_fx_name );
664 sound = morse.make_ident( nav_trans_ident, HI_FREQUENCY );
665 sound->set_volume( 0.3 );
666 globals->get_soundmgr()->add( sound, dme_fx_name );
668 int offset = (int)(sg_random() * 30.0);
669 nav_play_count = offset / 4;
670 nav_last_time = globals->get_time_params()->get_cur_time() -
672 // cout << "offset = " << offset << " play_count = "
674 // << " nav_last_time = " << nav_last_time
675 // << " current time = "
676 // << globals->get_time_params()->get_cur_time() << endl;
678 // cout << "Found an ils station in range" << endl;
679 // cout << " id = " << ils->get_locident() << endl;
681 } else if ( (nav = current_navlist->findByFreq(nav_freq, lon, lat, elev)) != NULL ) {
682 nav_id = nav->get_ident();
683 nav_valid = (nav->get_type() == 'V');
684 if ( last_nav_id != nav_id || !last_nav_vor ) {
685 last_nav_id = nav_id;
687 nav_trans_ident = nav->get_trans_ident();
689 nav_has_dme = nav->get_has_dme();
691 nav_loclon = nav->get_lon();
692 nav_loclat = nav->get_lat();
693 nav_elev = nav->get_elev_ft();
694 nav_twist = nav->get_magvar();
695 nav_range = nav->get_range();
696 nav_effective_range = adjustNavRange(nav_elev, elev, nav_range);
698 nav_target_radial = nav_sel_radial;
699 nav_x = nav->get_x();
700 nav_y = nav->get_y();
701 nav_z = nav->get_z();
703 if ( globals->get_soundmgr()->exists( nav_fx_name ) ) {
704 globals->get_soundmgr()->remove( nav_fx_name );
706 SGSoundSample *sound;
707 sound = morse.make_ident( nav_trans_ident, LO_FREQUENCY );
708 sound->set_volume( 0.3 );
709 if ( globals->get_soundmgr()->add( sound, nav_fx_name ) ) {
710 // cout << "Added nav-vor-ident sound" << endl;
712 SG_LOG(SG_COCKPIT, SG_WARN, "Failed to add v1-vor-ident sound");
715 if ( globals->get_soundmgr()->exists( dme_fx_name ) ) {
716 globals->get_soundmgr()->remove( dme_fx_name );
718 sound = morse.make_ident( nav_trans_ident, HI_FREQUENCY );
719 sound->set_volume( 0.3 );
720 globals->get_soundmgr()->add( sound, dme_fx_name );
722 int offset = (int)(sg_random() * 30.0);
723 nav_play_count = offset / 4;
724 nav_last_time = globals->get_time_params()->get_cur_time() -
726 // cout << "offset = " << offset << " play_count = "
727 // << nav_play_count << " nav_last_time = "
728 // << nav_last_time << " current time = "
729 // << globals->get_time_params()->get_cur_time() << endl;
731 // cout << "Found a vor station in range" << endl;
732 // cout << " id = " << nav->get_ident() << endl;
737 nav_target_radial = 0;
738 nav_trans_ident = "";
740 if ( ! globals->get_soundmgr()->remove( nav_fx_name ) ) {
741 SG_LOG(SG_COCKPIT, SG_WARN, "Failed to remove nav-vor-ident sound");
743 globals->get_soundmgr()->remove( dme_fx_name );
744 // cout << "not picking up vor1. :-(" << endl;
749 // return the amount of heading needle deflection, returns a value
750 // clamped to the range of ( -10 , 10 )
751 double FGNavCom::get_nav_cdi_deflection() const {
755 && nav_serviceable->getBoolValue() && cdi_serviceable->getBoolValue() )
757 r = nav_radial - nav_target_radial;
758 // cout << "Target radial = " << nav_target_radial
759 // << " Actual radial = " << nav_radial << endl;
761 while ( r > 180.0 ) { r -= 360.0;}
762 while ( r < -180.0 ) { r += 360.0;}
763 if ( fabs(r) > 90.0 )
764 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
766 // According to Robin Peel, the ILS is 4x more sensitive than a vor
767 r = -r; // reverse, since radial is outbound
768 if ( nav_loc ) { r *= 4.0; }
769 if ( r < -10.0 ) { r = -10.0; }
770 if ( r > 10.0 ) { r = 10.0; }
778 // return the amount of cross track distance error, returns a meters
779 double FGNavCom::get_nav_cdi_xtrack_error() const {
783 && nav_serviceable->getBoolValue() && cdi_serviceable->getBoolValue() )
785 r = nav_radial - nav_target_radial;
786 // cout << "Target radial = " << nav_target_radial
787 // << " Actual radial = " << nav_radial
788 // << " r = " << r << endl;
790 while ( r > 180.0 ) { r -= 360.0;}
791 while ( r < -180.0 ) { r += 360.0;}
792 if ( fabs(r) > 90.0 )
793 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
795 r = -r; // reverse, since radial is outbound
797 m = nav_loc_dist * sin(r * SGD_DEGREES_TO_RADIANS);
806 // return the amount of glide slope needle deflection (.i.e. the
807 // number of degrees we are off the glide slope * 5.0
808 double FGNavCom::get_nav_gs_deflection() const {
809 if ( nav_inrange && nav_has_gs
810 && nav_serviceable->getBoolValue() && gs_serviceable->getBoolValue() )
812 double x = nav_gs_dist;
813 double y = (fgGetDouble("/position/altitude-ft") - nav_elev)
815 // cout << "dist = " << x << " height = " << y << endl;
816 double angle = asin( y / x ) * SGD_RADIANS_TO_DEGREES;
817 return (nav_target_gs - angle) * 5.0;
825 * Return true if the NAV TO flag should be active.
828 FGNavCom::get_nav_to_flag () const
831 && nav_serviceable->getBoolValue()
832 && tofrom_serviceable->getBoolValue() )
834 double offset = fabs(nav_radial - nav_target_radial);
838 return !(offset <= 90.0 || offset >= 270.0);
847 * Return true if the NAV FROM flag should be active.
850 FGNavCom::get_nav_from_flag () const
853 && nav_serviceable->getBoolValue()
854 && tofrom_serviceable->getBoolValue() ) {
855 double offset = fabs(nav_radial - nav_target_radial);
859 return !(offset > 90.0 && offset < 270.0);
868 * Return the true heading to station
871 FGNavCom::get_nav_heading () const
878 * Return the current radial.
881 FGNavCom::get_nav_radial () const
887 FGNavCom::get_nav_reciprocal_radial () const
889 double recip = nav_radial + 180;
890 if ( recip >= 360 ) {