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)),
62 nav_target_radial(0.0),
63 nav_target_radial_true(0.0),
64 nav_target_auto_hdg(0.0),
65 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() );
86 for ( i = 0; i < node->nChildren(); ++i ) {
87 SGPropertyNode *child = node->getChild(i);
88 string cname = child->getName();
89 string cval = child->getStringValue();
90 if ( cname == "name" ) {
92 } else if ( cname == "number" ) {
93 num = child->getIntValue();
95 SG_LOG( SG_INSTR, SG_WARN,
96 "Error in nav radio config logic" );
97 if ( name.length() ) {
98 SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
107 FGNavRadio::~FGNavRadio()
121 branch = "/instrumentation/" + name;
123 SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
126 fgGetNode(("/systems/electrical/outputs/" + name).c_str(), true);
127 nav_serviceable = node->getChild("serviceable", 0, true);
128 cdi_serviceable = (node->getChild("cdi", 0, true))
129 ->getChild("serviceable", 0, true);
130 gs_serviceable = (node->getChild("gs", 0, true))
131 ->getChild("serviceable");
132 tofrom_serviceable = (node->getChild("to-from", 0, true))
133 ->getChild("serviceable", 0, true);
135 std::ostringstream temp;
136 temp << name << "nav-ident" << num;
137 nav_fx_name = temp.str();
138 temp << name << "dme-ident" << num;
139 dme_fx_name = temp.str();
145 std::ostringstream temp;
148 branch = "/instrumentation/" + name + "[" + temp.str() + "]";
151 fgTie( (branch + "/frequencies/selected-mhz").c_str() , this,
152 &FGNavRadio::get_nav_freq, &FGNavRadio::set_nav_freq );
153 fgSetArchivable( (branch + "/frequencies/selected-mhz").c_str() );
155 fgTie( (branch + "/frequencies/standby-mhz").c_str() , this,
156 &FGNavRadio::get_nav_alt_freq, &FGNavRadio::set_nav_alt_freq);
157 fgSetArchivable( (branch + "/frequencies/standby-mhz").c_str() );
159 fgTie( (branch + "/radials/selected-deg").c_str() , this,
160 &FGNavRadio::get_nav_sel_radial, &FGNavRadio::set_nav_sel_radial );
161 fgSetArchivable((branch + "/radials/selected-deg").c_str() );
163 fgTie( (branch + "/volume").c_str() , this,
164 &FGNavRadio::get_nav_vol_btn, &FGNavRadio::set_nav_vol_btn );
165 fgSetArchivable( (branch + "/volume").c_str() );
167 fgTie( (branch + "/ident").c_str(), this,
168 &FGNavRadio::get_nav_ident_btn, &FGNavRadio::set_nav_ident_btn );
169 fgSetArchivable( (branch + "/ident").c_str() );
172 fgTie( (branch + "/audio-btn").c_str(), this,
173 &FGNavRadio::get_audio_btn, &FGNavRadio::set_audio_btn );
174 fgSetArchivable( (branch + "/audio-btn").c_str() );
176 fgTie( (branch + "/heading-deg").c_str(),
177 this, &FGNavRadio::get_nav_heading );
179 fgTie( (branch + "/radials/actual-deg").c_str(),
180 this, &FGNavRadio::get_nav_radial );
182 fgTie( (branch + "/radials/target-radial-deg").c_str(),
183 this, &FGNavRadio::get_nav_target_radial_true );
185 fgTie( (branch + "/radials/reciprocal-radial-deg").c_str(),
186 this, &FGNavRadio::get_nav_reciprocal_radial );
188 fgTie( (branch + "/radials/target-radial2-deg").c_str(),
189 this, &FGNavRadio::get_nav_target_radial );
191 fgTie( (branch + "/radials/target-auto-hdg-deg").c_str(),
192 this, &FGNavRadio::get_nav_target_auto_hdg );
194 fgTie( (branch + "/to-flag").c_str(),
195 this, &FGNavRadio::get_nav_to_flag );
197 fgTie( (branch + "/from-flag").c_str(),
198 this, &FGNavRadio::get_nav_from_flag );
200 fgTie( (branch + "/in-range").c_str(),
201 this, &FGNavRadio::get_nav_inrange );
203 fgTie( (branch + "/heading-needle-deflection").c_str(),
204 this, &FGNavRadio::get_nav_cdi_deflection );
206 fgTie( (branch + "/crosstrack-error-m").c_str(),
207 this, &FGNavRadio::get_nav_cdi_xtrack_error );
209 fgTie( (branch + "/has-gs").c_str(),
210 this, &FGNavRadio::get_nav_has_gs );
212 fgTie( (branch + "/nav-loc").c_str(),
213 this, &FGNavRadio::get_nav_loc );
215 fgTie( (branch + "/gs-rate-of-climb").c_str(),
216 this, &FGNavRadio::get_nav_gs_rate_of_climb );
218 fgTie( (branch + "/gs-needle-deflection").c_str(),
219 this, &FGNavRadio::get_nav_gs_deflection );
221 fgTie( (branch + "/gs-distance").c_str(),
222 this, &FGNavRadio::get_nav_gs_dist_signed );
224 fgTie( (branch + "/nav-distance").c_str(),
225 this, &FGNavRadio::get_nav_loc_dist );
227 fgTie( (branch + "/nav-id").c_str(),
228 this, &FGNavRadio::get_nav_id );
230 // put nav_id characters into seperate properties for instrument displays
231 fgTie( (branch + "/nav-id_asc1").c_str(),
232 this, &FGNavRadio::get_nav_id_c1 );
234 fgTie( (branch + "/nav-id_asc2").c_str(),
235 this, &FGNavRadio::get_nav_id_c2 );
237 fgTie( (branch + "/nav-id_asc3").c_str(),
238 this, &FGNavRadio::get_nav_id_c3 );
240 fgTie( (branch + "/nav-id_asc4").c_str(),
241 this, &FGNavRadio::get_nav_id_c4 );
248 FGNavRadio::unbind ()
250 std::ostringstream temp;
253 branch = "/instrumentation/" + name + "[" + temp.str() + "]";
255 fgUntie( (branch + "/frequencies/selected-mhz").c_str() );
256 fgUntie( (branch + "/frequencies/standby-mhz").c_str() );
257 fgUntie( (branch + "/radials/actual-deg").c_str() );
258 fgUntie( (branch + "/radials/selected-deg").c_str() );
259 fgUntie( (branch + "/ident").c_str() );
260 fgUntie( (branch + "/to-flag").c_str() );
261 fgUntie( (branch + "/from-flag").c_str() );
262 fgUntie( (branch + "/in-range").c_str() );
263 fgUntie( (branch + "/heading-needle-deflection").c_str() );
264 fgUntie( (branch + "/gs-needle-deflection").c_str() );
268 // model standard VOR/DME/TACAN service volumes as per AIM 1-1-8
269 double FGNavRadio::adjustNavRange( double stationElev, double aircraftElev,
270 double nominalRange )
272 // extend out actual usable range to be 1.3x the published safe range
273 const double usability_factor = 1.3;
275 // assumptions we model the standard service volume, plus
276 // ... rather than specifying a cylinder, we model a cone that
277 // contains the cylinder. Then we put an upside down cone on top
278 // to model diminishing returns at too-high altitudes.
280 // altitude difference
281 double alt = ( aircraftElev * SG_METER_TO_FEET - stationElev );
282 // cout << "aircraft elev = " << aircraftElev * SG_METER_TO_FEET
283 // << " station elev = " << stationElev << endl;
285 if ( nominalRange < 25.0 + SG_EPSILON ) {
286 // Standard Terminal Service Volume
287 return term_tbl->interpolate( alt ) * usability_factor;
288 } else if ( nominalRange < 50.0 + SG_EPSILON ) {
289 // Standard Low Altitude Service Volume
290 // table is based on range of 40, scale to actual range
291 return low_tbl->interpolate( alt ) * nominalRange / 40.0
294 // Standard High Altitude Service Volume
295 // table is based on range of 130, scale to actual range
296 return high_tbl->interpolate( alt ) * nominalRange / 130.0
302 // model standard ILS service volumes as per AIM 1-1-9
303 double FGNavRadio::adjustILSRange( double stationElev, double aircraftElev,
304 double offsetDegrees, double distance )
306 // assumptions we model the standard service volume, plus
308 // altitude difference
309 // double alt = ( aircraftElev * SG_METER_TO_FEET - stationElev );
310 // double offset = fabs( offsetDegrees );
312 // if ( offset < 10 ) {
313 // return FG_ILS_DEFAULT_RANGE;
314 // } else if ( offset < 35 ) {
315 // return 10 + (35 - offset) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
316 // } else if ( offset < 45 ) {
317 // return (45 - offset);
318 // } else if ( offset > 170 ) {
319 // return FG_ILS_DEFAULT_RANGE;
320 // } else if ( offset > 145 ) {
321 // return 10 + (offset - 145) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
322 // } else if ( offset > 135 ) {
323 // return (offset - 135);
327 return FG_LOC_DEFAULT_RANGE;
331 // Update the various nav values based on position and valid tuned in navs
333 FGNavRadio::update(double dt)
335 double lon = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
336 double lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
337 double elev = alt_node->getDoubleValue() * SG_FEET_TO_METER;
341 Point3D aircraft = sgGeodToCart( Point3D( lon, lat, elev ) );
345 // On timeout, scan again
346 _time_before_search_sec -= dt;
347 if ( _time_before_search_sec < 0 ) {
351 ////////////////////////////////////////////////////////////////////////
353 ////////////////////////////////////////////////////////////////////////
355 if ( nav_valid && power_btn && (bus_power->getDoubleValue() > 1.0)
356 && nav_serviceable->getBoolValue() )
358 station = Point3D( nav_x, nav_y, nav_z );
359 nav_loc_dist = aircraft.distance3D( station );
362 // find closest distance to the gs base line
364 sgdSetVec3( p, aircraft.x(), aircraft.y(), aircraft.z() );
366 sgdSetVec3( p0, nav_gs_x, nav_gs_y, nav_gs_z );
367 double dist = sgdClosestPointToLineDistSquared( p, p0,
369 nav_gs_dist = sqrt( dist );
370 // cout << "nav_gs_dist = " << nav_gs_dist << endl;
372 Point3D tmp( nav_gs_x, nav_gs_y, nav_gs_z );
373 // cout << " (" << aircraft.distance3D( tmp ) << ")" << endl;
375 // wgs84 heading to glide slope (to determine sign of distance)
376 geo_inverse_wgs_84( elev,
377 lat * SGD_RADIANS_TO_DEGREES,
378 lon * SGD_RADIANS_TO_DEGREES,
379 nav_gslat, nav_gslon,
381 double r = az1 - nav_target_radial;
382 while ( r > 180.0 ) { r -= 360.0;}
383 while ( r < -180.0 ) { r += 360.0;}
384 if ( r >= -90.0 && r <= 90.0 ) {
385 nav_gs_dist_signed = nav_gs_dist;
387 nav_gs_dist_signed = -nav_gs_dist;
389 /* cout << "Target Radial = " << nav_target_radial
390 << " Bearing = " << az1
391 << " dist (signed) = " << nav_gs_dist_signed
398 // wgs84 heading to localizer
399 geo_inverse_wgs_84( elev,
400 lat * SGD_RADIANS_TO_DEGREES,
401 lon * SGD_RADIANS_TO_DEGREES,
402 nav_loclat, nav_loclon,
403 &nav_heading, &az2, &s );
404 // cout << "az1 = " << az1 << " magvar = " << nav_magvar << endl;
405 nav_radial = az2 - nav_twist;
406 // cout << " heading = " << nav_heading
407 // << " dist = " << nav_dist << endl;
410 double offset = nav_radial - nav_target_radial;
411 while ( offset < -180.0 ) { offset += 360.0; }
412 while ( offset > 180.0 ) { offset -= 360.0; }
413 // cout << "ils offset = " << offset << endl;
415 = adjustILSRange( nav_elev, elev, offset,
416 nav_loc_dist * SG_METER_TO_NM );
418 nav_effective_range = adjustNavRange( nav_elev, elev, nav_range );
420 // cout << "nav range = " << nav_effective_range
421 // << " (" << nav_range << ")" << endl;
423 if ( nav_loc_dist < nav_effective_range * SG_NM_TO_METER ) {
425 } else if ( nav_loc_dist < 2 * nav_effective_range * SG_NM_TO_METER ) {
426 nav_inrange = sg_random() <
427 ( 2 * nav_effective_range * SG_NM_TO_METER - nav_loc_dist ) /
428 (nav_effective_range * SG_NM_TO_METER);
434 nav_target_radial = nav_sel_radial;
437 // Calculate some values for the nav/ils hold autopilot
439 double cur_radial = get_nav_reciprocal_radial();
441 // ILS localizers radials are already "true" in our
444 cur_radial += nav_twist;
446 if ( get_nav_from_flag() ) {
448 while ( cur_radial >= 360.0 ) { cur_radial -= 360.0; }
453 // determine the target radial in "true" heading
454 nav_target_radial_true = nav_target_radial;
456 // ILS localizers radials are already "true" in our
459 // VOR radials need to have that vor's offset added in
460 nav_target_radial_true += nav_twist;
463 while ( nav_target_radial_true < 0.0 ) {
464 nav_target_radial_true += 360.0;
466 while ( nav_target_radial_true > 360.0 ) {
467 nav_target_radial_true -= 360.0;
470 // determine the heading adjustment needed.
471 // over 8km scale by 3.0
472 // (3 is chosen because max deflection is 10
473 // and 30 is clamped angle to radial)
474 // under 8km scale by 10.0
475 // because the overstated error helps drive it to the radial in a
476 // moderate cross wind.
477 double adjustment = 0.0;
478 if (nav_loc_dist > 8000) {
479 adjustment = get_nav_cdi_deflection() * 3.0;
481 adjustment = get_nav_cdi_deflection() * 10.0;
483 SG_CLAMP_RANGE( adjustment, -30.0, 30.0 );
485 // determine the target heading to fly to intercept the
487 nav_target_auto_hdg = nav_target_radial_true + adjustment;
488 while ( nav_target_auto_hdg < 0.0 ) { nav_target_auto_hdg += 360.0; }
489 while ( nav_target_auto_hdg > 360.0 ) { nav_target_auto_hdg -= 360.0; }
494 // Calculate desired rate of climb for intercepting the GS
495 double x = nav_gs_dist;
496 double y = (alt_node->getDoubleValue() - nav_elev)
498 double current_angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
500 double target_angle = nav_target_gs;
501 double gs_diff = target_angle - current_angle;
503 // convert desired vertical path angle into a climb rate
504 double des_angle = current_angle - 10 * gs_diff;
506 // estimate horizontal speed towards ILS in meters per minute
507 double dist = last_x - x;
511 double new_vel = ( dist / dt );
513 horiz_vel = 0.75 * horiz_vel + 0.25 * new_vel;
514 // double horiz_vel = cur_fdm_state->get_V_ground_speed()
515 // * SG_FEET_TO_METER * 60.0;
516 // double horiz_vel = airspeed_node->getFloatValue()
517 // * SG_FEET_TO_METER * 60.0;
519 nav_gs_rate_of_climb = -sin( des_angle * SGD_DEGREES_TO_RADIANS )
520 * horiz_vel * SG_METER_TO_FEET;
524 // cout << "not picking up vor. :-(" << endl;
527 if ( nav_valid && nav_inrange && nav_serviceable->getBoolValue() ) {
528 // play station ident via audio system if on + ident,
529 // otherwise turn it off
530 if ( power_btn && (bus_power->getDoubleValue() > 1.0)
531 && nav_ident_btn && audio_btn )
533 SGSoundSample *sound;
534 sound = globals->get_soundmgr()->find( nav_fx_name );
535 if ( sound != NULL ) {
536 sound->set_volume( nav_vol_btn );
538 SG_LOG( SG_COCKPIT, SG_ALERT,
539 "Can't find nav-vor-ident sound" );
541 sound = globals->get_soundmgr()->find( dme_fx_name );
542 if ( sound != NULL ) {
543 sound->set_volume( nav_vol_btn );
545 SG_LOG( SG_COCKPIT, SG_ALERT,
546 "Can't find nav-dme-ident sound" );
548 // cout << "nav_last_time = " << nav_last_time << " ";
549 // cout << "cur_time = "
550 // << globals->get_time_params()->get_cur_time();
552 globals->get_time_params()->get_cur_time() - 30 ) {
553 nav_last_time = globals->get_time_params()->get_cur_time();
556 // cout << " nav_play_count = " << nav_play_count << endl;
557 // cout << "playing = "
558 // << globals->get_soundmgr()->is_playing(nav_fx_name)
560 if ( nav_play_count < 4 ) {
562 if ( !globals->get_soundmgr()->is_playing(nav_fx_name) ) {
563 globals->get_soundmgr()->play_once( nav_fx_name );
566 } else if ( nav_play_count < 5 && nav_has_dme ) {
568 if ( !globals->get_soundmgr()->is_playing(nav_fx_name) &&
569 !globals->get_soundmgr()->is_playing(dme_fx_name) ) {
570 globals->get_soundmgr()->play_once( dme_fx_name );
575 globals->get_soundmgr()->stop( nav_fx_name );
576 globals->get_soundmgr()->stop( dme_fx_name );
582 // Update current nav/adf radio stations based on current postition
583 void FGNavRadio::search()
587 _time_before_search_sec = 1.0;
589 double lon = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
590 double lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
591 double elev = alt_node->getDoubleValue() * SG_FEET_TO_METER;
593 FGNavRecord *nav = NULL;
594 FGNavRecord *loc = NULL;
595 FGNavRecord *dme = NULL;
596 FGNavRecord *gs = NULL;
598 ////////////////////////////////////////////////////////////////////////
600 ////////////////////////////////////////////////////////////////////////
602 nav = globals->get_navlist()->findByFreq(nav_freq, lon, lat, elev);
603 dme = globals->get_dmelist()->findByFreq(nav_freq, lon, lat, elev);
605 loc = globals->get_loclist()->findByFreq(nav_freq, lon, lat, elev);
606 gs = globals->get_gslist()->findByFreq(nav_freq, lon, lat, elev);
610 nav_id = loc->get_ident();
611 // cout << "localizer = " << nav_id << endl;
613 if ( last_nav_id != nav_id || last_nav_vor ) {
614 nav_trans_ident = loc->get_trans_ident();
615 nav_target_radial = loc->get_multiuse();
616 while ( nav_target_radial < 0.0 ) { nav_target_radial += 360.0; }
617 while ( nav_target_radial > 360.0 ) { nav_target_radial -= 360.0; }
618 nav_loclon = loc->get_lon();
619 nav_loclat = loc->get_lat();
620 nav_x = loc->get_x();
621 nav_y = loc->get_y();
622 nav_z = loc->get_z();
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 nav_elev = gs->get_elev_ft();
632 int tmp = (int)(gs->get_multiuse() / 1000.0);
633 nav_target_gs = (double)tmp / 100.0;
634 nav_gs_x = gs->get_x();
635 nav_gs_y = gs->get_y();
636 nav_gs_z = gs->get_z();
638 // derive GS baseline (perpendicular to the runay
640 double tlon, tlat, taz;
641 geo_direct_wgs_84 ( 0.0, nav_gslat, nav_gslon,
642 nav_target_radial + 90,
643 100.0, &tlat, &tlon, &taz );
644 // cout << "nav_target_radial = " << nav_target_radial << endl;
645 // cout << "nav_loc = " << nav_loc << endl;
646 // cout << nav_gslon << "," << nav_gslat << " "
647 // << tlon << "," << tlat << " (" << nav_elev << ")"
649 Point3D p1 = sgGeodToCart( Point3D(tlon*SGD_DEGREES_TO_RADIANS,
650 tlat*SGD_DEGREES_TO_RADIANS,
651 nav_elev*SG_FEET_TO_METER)
653 // cout << nav_gs_x << "," << nav_gs_y << "," << nav_gs_z
655 // cout << p1 << endl;
656 sgdSetVec3( gs_base_vec,
657 p1.x()-nav_gs_x, p1.y()-nav_gs_y, p1.z()-nav_gs_z );
658 // cout << gs_base_vec[0] << "," << gs_base_vec[1] << ","
659 // << gs_base_vec[2] << endl;
661 nav_elev = loc->get_elev_ft();
664 nav_range = FG_LOC_DEFAULT_RANGE;
665 nav_effective_range = nav_range;
667 if ( globals->get_soundmgr()->exists( nav_fx_name ) ) {
668 globals->get_soundmgr()->remove( nav_fx_name );
670 SGSoundSample *sound;
671 sound = morse.make_ident( nav_trans_ident, LO_FREQUENCY );
672 sound->set_volume( 0.3 );
673 globals->get_soundmgr()->add( sound, nav_fx_name );
675 if ( globals->get_soundmgr()->exists( dme_fx_name ) ) {
676 globals->get_soundmgr()->remove( dme_fx_name );
678 sound = morse.make_ident( nav_trans_ident, HI_FREQUENCY );
679 sound->set_volume( 0.3 );
680 globals->get_soundmgr()->add( sound, dme_fx_name );
682 int offset = (int)(sg_random() * 30.0);
683 nav_play_count = offset / 4;
684 nav_last_time = globals->get_time_params()->get_cur_time() -
686 // cout << "offset = " << offset << " play_count = "
688 // << " nav_last_time = " << nav_last_time
689 // << " current time = "
690 // << globals->get_time_params()->get_cur_time() << endl;
692 // cout << "Found an loc station in range" << endl;
693 // cout << " id = " << loc->get_locident() << endl;
695 } else if ( nav != NULL ) {
696 nav_id = nav->get_ident();
697 // cout << "nav = " << nav_id << endl;
699 if ( last_nav_id != nav_id || !last_nav_vor ) {
700 last_nav_id = nav_id;
702 nav_trans_ident = nav->get_trans_ident();
704 nav_has_dme = (dme != NULL);
706 nav_loclon = nav->get_lon();
707 nav_loclat = nav->get_lat();
708 nav_elev = nav->get_elev_ft();
709 nav_twist = nav->get_multiuse();
710 nav_range = nav->get_range();
711 nav_effective_range = adjustNavRange(nav_elev, elev, nav_range);
713 nav_target_radial = nav_sel_radial;
714 nav_x = nav->get_x();
715 nav_y = nav->get_y();
716 nav_z = nav->get_z();
718 if ( globals->get_soundmgr()->exists( nav_fx_name ) ) {
719 globals->get_soundmgr()->remove( nav_fx_name );
721 SGSoundSample *sound;
722 sound = morse.make_ident( nav_trans_ident, LO_FREQUENCY );
723 sound->set_volume( 0.3 );
724 if ( globals->get_soundmgr()->add( sound, nav_fx_name ) ) {
725 // cout << "Added nav-vor-ident sound" << endl;
727 SG_LOG(SG_COCKPIT, SG_WARN, "Failed to add v1-vor-ident sound");
730 if ( globals->get_soundmgr()->exists( dme_fx_name ) ) {
731 globals->get_soundmgr()->remove( dme_fx_name );
733 sound = morse.make_ident( nav_trans_ident, HI_FREQUENCY );
734 sound->set_volume( 0.3 );
735 globals->get_soundmgr()->add( sound, dme_fx_name );
737 int offset = (int)(sg_random() * 30.0);
738 nav_play_count = offset / 4;
739 nav_last_time = globals->get_time_params()->get_cur_time() -
741 // cout << "offset = " << offset << " play_count = "
742 // << nav_play_count << " nav_last_time = "
743 // << nav_last_time << " current time = "
744 // << globals->get_time_params()->get_cur_time() << endl;
746 // cout << "Found a vor station in range" << endl;
747 // cout << " id = " << nav->get_ident() << endl;
752 nav_target_radial = 0;
753 nav_trans_ident = "";
755 if ( ! globals->get_soundmgr()->remove( nav_fx_name ) ) {
756 SG_LOG(SG_COCKPIT, SG_WARN, "Failed to remove nav-vor-ident sound");
758 globals->get_soundmgr()->remove( dme_fx_name );
759 // cout << "not picking up vor1. :-(" << endl;
764 // return the amount of heading needle deflection, returns a value
765 // clamped to the range of ( -10 , 10 )
766 double FGNavRadio::get_nav_cdi_deflection() const {
770 && nav_serviceable->getBoolValue() && cdi_serviceable->getBoolValue() )
772 r = nav_radial - nav_target_radial;
773 // cout << "Target radial = " << nav_target_radial
774 // << " Actual radial = " << nav_radial << endl;
776 while ( r > 180.0 ) { r -= 360.0;}
777 while ( r < -180.0 ) { r += 360.0;}
778 if ( fabs(r) > 90.0 )
779 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
781 // According to Robin Peel, the ILS is 4x more sensitive than a vor
782 r = -r; // reverse, since radial is outbound
783 if ( nav_loc ) { r *= 4.0; }
784 if ( r < -10.0 ) { r = -10.0; }
785 if ( r > 10.0 ) { r = 10.0; }
793 // return the amount of cross track distance error, returns a meters
794 double FGNavRadio::get_nav_cdi_xtrack_error() 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
803 // << " r = " << r << endl;
805 while ( r > 180.0 ) { r -= 360.0;}
806 while ( r < -180.0 ) { r += 360.0;}
807 if ( fabs(r) > 90.0 )
808 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
810 r = -r; // reverse, since radial is outbound
812 m = nav_loc_dist * sin(r * SGD_DEGREES_TO_RADIANS);
821 // return the amount of glide slope needle deflection (.i.e. the
822 // number of degrees we are off the glide slope * 5.0
823 double FGNavRadio::get_nav_gs_deflection() const {
824 if ( nav_inrange && nav_has_gs
825 && nav_serviceable->getBoolValue() && gs_serviceable->getBoolValue() )
827 double x = nav_gs_dist;
828 double y = (fgGetDouble("/position/altitude-ft") - nav_elev)
830 // cout << "dist = " << x << " height = " << y << endl;
831 double angle = asin( y / x ) * SGD_RADIANS_TO_DEGREES;
832 return (nav_target_gs - angle) * 5.0;
840 * Return true if the NAV TO flag should be active.
843 FGNavRadio::get_nav_to_flag () const
846 && nav_serviceable->getBoolValue()
847 && tofrom_serviceable->getBoolValue() )
849 double offset = fabs(nav_radial - nav_target_radial);
853 return !(offset <= 90.0 || offset >= 270.0);
862 * Return true if the NAV FROM flag should be active.
865 FGNavRadio::get_nav_from_flag () const
868 && nav_serviceable->getBoolValue()
869 && tofrom_serviceable->getBoolValue() ) {
870 double offset = fabs(nav_radial - nav_target_radial);
874 return !(offset > 90.0 && offset < 270.0);
883 * Return the true heading to station
886 FGNavRadio::get_nav_heading () const
893 * Return the current radial.
896 FGNavRadio::get_nav_radial () const
902 FGNavRadio::get_nav_reciprocal_radial () const
904 double recip = nav_radial + 180;
905 if ( recip >= 360 ) {