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/math/sg_random.h>
33 #include <Aircraft/aircraft.hxx>
34 #include <Navaids/ilslist.hxx>
35 #include <Navaids/navlist.hxx>
36 #include <Time/FGEventMgr.hxx>
45 FGNavCom::FGNavCom() :
46 lon_node(fgGetNode("/position/longitude-deg", true)),
47 lat_node(fgGetNode("/position/latitude-deg", true)),
48 alt_node(fgGetNode("/position/altitude-ft", true)),
64 SGPath path( globals->get_fg_root() );
66 term.append( "Navaids/range.term" );
68 low.append( "Navaids/range.low" );
70 high.append( "Navaids/range.high" );
72 term_tbl = new SGInterpTable( term.str() );
73 low_tbl = new SGInterpTable( low.str() );
74 high_tbl = new SGInterpTable( high.str() );
95 update(0); // FIXME: use dt
103 // we know index is valid now so lets bind to the bus property
105 sprintf( propname, "/systems/electrical/outputs/navcomm[%d]", index );
106 // default to true in case no electrical system defined.
107 fgSetDouble( propname, 60.0 );
108 bus_power = fgGetNode( propname, true );
111 sprintf( propname, "/radios/comm[%d]/inputs/power-btn", index );
112 fgTie( propname, this,
113 &FGNavCom::get_power_btn, &FGNavCom::set_power_btn );
114 fgSetArchivable( propname );
116 sprintf( propname, "/radios/comm[%d]/frequencies/selected-mhz", index );
117 fgTie( propname, this, &FGNavCom::get_comm_freq, &FGNavCom::set_comm_freq );
118 fgSetArchivable( propname );
120 sprintf( propname, "/radios/comm[%d]/frequencies/standby-mhz", index );
121 fgTie( propname, this,
122 &FGNavCom::get_comm_alt_freq, &FGNavCom::set_comm_alt_freq );
123 fgSetArchivable( propname );
125 sprintf( propname, "/radios/comm[%d]/volume", index );
126 fgTie( propname, this,
127 &FGNavCom::get_comm_vol_btn, &FGNavCom::set_comm_vol_btn );
128 fgSetArchivable( propname );
130 sprintf( propname, "/radios/nav[%d]/frequencies/selected-mhz", index );
131 fgTie( propname, this,
132 &FGNavCom::get_nav_freq, &FGNavCom::set_nav_freq );
133 fgSetArchivable( propname );
135 sprintf( propname, "/radios/nav[%d]/frequencies/standby-mhz", index );
136 fgTie( propname , this,
137 &FGNavCom::get_nav_alt_freq, &FGNavCom::set_nav_alt_freq);
138 fgSetArchivable( propname );
140 sprintf( propname, "/radios/nav[%d]/radials/selected-deg", index );
141 fgTie( propname, this,
142 &FGNavCom::get_nav_sel_radial, &FGNavCom::set_nav_sel_radial );
143 fgSetArchivable( propname );
145 sprintf( propname, "/radios/nav[%d]/volume", index );
146 fgTie( propname, this,
147 &FGNavCom::get_nav_vol_btn, &FGNavCom::set_nav_vol_btn );
148 fgSetArchivable( propname );
150 sprintf( propname, "/radios/nav[%d]/ident", index );
151 fgTie( propname, this,
152 &FGNavCom::get_nav_ident_btn, &FGNavCom::set_nav_ident_btn );
153 fgSetArchivable( propname );
156 sprintf( propname, "/radios/nav[%d]/radials/actual-deg", index );
157 fgTie( propname, this, &FGNavCom::get_nav_radial );
159 sprintf( propname, "/radios/nav[%d]/to-flag", index );
160 fgTie( propname, this, &FGNavCom::get_nav_to_flag );
162 sprintf( propname, "/radios/nav[%d]/from-flag", index );
163 fgTie( propname, this, &FGNavCom::get_nav_from_flag );
165 sprintf( propname, "/radios/nav[%d]/in-range", index );
166 fgTie( propname, this, &FGNavCom::get_nav_inrange );
168 sprintf( propname, "/radios/nav[%d]/heading-needle-deflection", index );
169 fgTie( propname, this, &FGNavCom::get_nav_heading_needle_deflection );
171 sprintf( propname, "/radios/nav[%d]/gs-needle-deflection", index );
172 fgTie( propname, this, &FGNavCom::get_nav_gs_needle_deflection );
181 sprintf( propname, "/radios/comm[%d]/inputs/power-btn", index );
183 sprintf( propname, "/radios/comm[%d]/frequencies/selected-mhz", index );
185 sprintf( propname, "/radios/comm[%d]/frequencies/standby-mhz", index );
188 sprintf( propname, "/radios/nav[%d]/frequencies/selected-mhz", index );
190 sprintf( propname, "/radios/nav[%d]/frequencies/standby-mhz", index );
192 sprintf( propname, "/radios/nav[%d]/radials/actual-deg", index );
194 sprintf( propname, "/radios/nav[%d]/radials/selected-deg", index );
196 sprintf( propname, "/radios/nav[%d]/ident", index );
198 sprintf( propname, "/radios/nav[%d]/to-flag", index );
200 sprintf( propname, "/radios/nav[%d]/from-flag", index );
202 sprintf( propname, "/radios/nav[%d]/in-range", index );
204 sprintf( propname, "/radios/nav[%d]/heading-needle-deflection", index );
206 sprintf( propname, "/radios/nav[%d]/gs-needle-deflection", index );
211 // model standard VOR/DME/TACAN service volumes as per AIM 1-1-8
212 double FGNavCom::adjustNavRange( double stationElev, double aircraftElev,
213 double nominalRange )
215 // extend out actual usable range to be 1.3x the published safe range
216 const double usability_factor = 1.3;
218 // assumptions we model the standard service volume, plus
219 // ... rather than specifying a cylinder, we model a cone that
220 // contains the cylinder. Then we put an upside down cone on top
221 // to model diminishing returns at too-high altitudes.
223 // altitude difference
224 double alt = ( aircraftElev * SG_METER_TO_FEET - stationElev );
225 // cout << "aircraft elev = " << aircraftElev * SG_METER_TO_FEET
226 // << " station elev = " << stationElev << endl;
228 if ( nominalRange < 25.0 + SG_EPSILON ) {
229 // Standard Terminal Service Volume
230 return term_tbl->interpolate( alt ) * usability_factor;
231 } else if ( nominalRange < 50.0 + SG_EPSILON ) {
232 // Standard Low Altitude Service Volume
233 // table is based on range of 40, scale to actual range
234 return low_tbl->interpolate( alt ) * nominalRange / 40.0
237 // Standard High Altitude Service Volume
238 // table is based on range of 130, scale to actual range
239 return high_tbl->interpolate( alt ) * nominalRange / 130.0
245 // model standard ILS service volumes as per AIM 1-1-9
246 double FGNavCom::adjustILSRange( double stationElev, double aircraftElev,
247 double offsetDegrees, double distance )
249 // assumptions we model the standard service volume, plus
251 // altitude difference
252 // double alt = ( aircraftElev * SG_METER_TO_FEET - stationElev );
253 double offset = fabs( offsetDegrees );
256 return FG_ILS_DEFAULT_RANGE;
257 } else if ( offset < 35 ) {
258 return 10 + (35 - offset) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
259 } else if ( offset < 45 ) {
260 return (45 - offset);
261 } else if ( offset > 170 ) {
262 return FG_ILS_DEFAULT_RANGE;
263 } else if ( offset > 145 ) {
264 return 10 + (offset - 145) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
265 } else if ( offset > 135 ) {
266 return (offset - 135);
273 // Update the various nav values based on position and valid tuned in navs
275 FGNavCom::update(double dt)
277 double lon = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
278 double lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
279 double elev = alt_node->getDoubleValue() * SG_FEET_TO_METER;
283 Point3D aircraft = sgGeodToCart( Point3D( lon, lat, elev ) );
287 ////////////////////////////////////////////////////////////////////////
289 ////////////////////////////////////////////////////////////////////////
291 if ( nav_valid && power_btn && (bus_power->getDoubleValue() > 1.0) ) {
292 station = Point3D( nav_x, nav_y, nav_z );
293 nav_loc_dist = aircraft.distance3D( station );
296 station = Point3D( nav_gs_x, nav_gs_y, nav_gs_z );
297 nav_gs_dist = aircraft.distance3D( station );
303 geo_inverse_wgs_84( elev, lat * SGD_RADIANS_TO_DEGREES, lon * SGD_RADIANS_TO_DEGREES,
304 nav_loclat, nav_loclon,
306 // cout << "az1 = " << az1 << " magvar = " << nav_magvar << endl;
307 nav_heading = az1 - nav_magvar;
308 // cout << " heading = " << nav_heading
309 // << " dist = " << nav_dist << endl;
312 double offset = nav_heading - nav_radial;
313 while ( offset < -180.0 ) { offset += 360.0; }
314 while ( offset > 180.0 ) { offset -= 360.0; }
315 // cout << "ils offset = " << offset << endl;
316 nav_effective_range = adjustILSRange(nav_elev, elev, offset,
317 nav_loc_dist * SG_METER_TO_NM );
319 nav_effective_range = adjustNavRange(nav_elev, elev, nav_range);
321 // cout << "nav range = " << nav_effective_range
322 // << " (" << nav_range << ")" << endl;
324 if ( nav_loc_dist < nav_effective_range * SG_NM_TO_METER ) {
326 } else if ( nav_loc_dist < 2 * nav_effective_range * SG_NM_TO_METER ) {
327 nav_inrange = sg_random() <
328 ( 2 * nav_effective_range * SG_NM_TO_METER - nav_loc_dist ) /
329 (nav_effective_range * SG_NM_TO_METER);
335 nav_radial = nav_sel_radial;
339 // cout << "not picking up vor. :-(" << endl;
342 #ifdef ENABLE_AUDIO_SUPPORT
343 if ( nav_valid && nav_inrange ) {
344 // play station ident via audio system if on + ident,
345 // otherwise turn it off
346 if ( power_btn && (bus_power->getDoubleValue() > 1.0)
349 FGSimpleSound *sound;
350 sound = globals->get_soundmgr()->find( nav_fx_name );
351 if ( sound != NULL ) {
352 sound->set_volume( nav_vol_btn );
354 SG_LOG( SG_COCKPIT, SG_ALERT,
355 "Can't find nav-vor-ident sound" );
357 sound = globals->get_soundmgr()->find( dme_fx_name );
358 if ( sound != NULL ) {
359 sound->set_volume( nav_vol_btn );
361 SG_LOG( SG_COCKPIT, SG_ALERT,
362 "Can't find nav-dme-ident sound" );
364 // cout << "nav_last_time = " << nav_last_time << " ";
365 // cout << "cur_time = "
366 // << globals->get_time_params()->get_cur_time();
368 globals->get_time_params()->get_cur_time() - 30 ) {
369 nav_last_time = globals->get_time_params()->get_cur_time();
372 // cout << " nav_play_count = " << nav_play_count << endl;
373 // cout << "playing = "
374 // << globals->get_soundmgr()->is_playing(nav_fx_name)
376 if ( nav_play_count < 4 ) {
378 if ( !globals->get_soundmgr()->is_playing(nav_fx_name) ) {
379 globals->get_soundmgr()->play_once( nav_fx_name );
382 } else if ( nav_play_count < 5 && nav_has_dme ) {
384 if ( !globals->get_soundmgr()->is_playing(nav_fx_name) &&
385 !globals->get_soundmgr()->is_playing(dme_fx_name) ) {
386 globals->get_soundmgr()->play_once( dme_fx_name );
391 globals->get_soundmgr()->stop( nav_fx_name );
392 globals->get_soundmgr()->stop( dme_fx_name );
400 // Update current nav/adf radio stations based on current postition
401 void FGNavCom::search()
403 double lon = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
404 double lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
405 double elev = alt_node->getDoubleValue() * SG_FEET_TO_METER;
410 ////////////////////////////////////////////////////////////////////////
412 ////////////////////////////////////////////////////////////////////////
414 if ( current_ilslist->query( lon, lat, elev, nav_freq, &ils ) ) {
415 nav_id = ils.get_locident();
417 if ( last_nav_id != nav_id || last_nav_vor ) {
418 nav_trans_ident = ils.get_trans_ident();
419 last_nav_id = nav_id;
420 last_nav_vor = false;
422 nav_has_dme = ils.get_has_dme();
423 nav_has_gs = ils.get_has_gs();
425 nav_loclon = ils.get_loclon();
426 nav_loclat = ils.get_loclat();
427 nav_gslon = ils.get_gslon();
428 nav_gslat = ils.get_gslat();
429 nav_elev = ils.get_gselev();
431 nav_range = FG_ILS_DEFAULT_RANGE;
432 nav_effective_range = nav_range;
433 nav_target_gs = ils.get_gsangle();
434 nav_radial = ils.get_locheading();
435 while ( nav_radial < 0.0 ) { nav_radial += 360.0; }
436 while ( nav_radial > 360.0 ) { nav_radial -= 360.0; }
440 nav_gs_x = ils.get_gs_x();
441 nav_gs_y = ils.get_gs_y();
442 nav_gs_z = ils.get_gs_z();
444 #ifdef ENABLE_AUDIO_SUPPORT
445 if ( globals->get_soundmgr()->exists( nav_fx_name ) ) {
446 globals->get_soundmgr()->remove( nav_fx_name );
448 FGSimpleSound *sound;
449 sound = morse.make_ident( nav_trans_ident, LO_FREQUENCY );
450 sound->set_volume( 0.3 );
451 globals->get_soundmgr()->add( sound, nav_fx_name );
453 if ( globals->get_soundmgr()->exists( dme_fx_name ) ) {
454 globals->get_soundmgr()->remove( dme_fx_name );
456 sound = morse.make_ident( nav_trans_ident, HI_FREQUENCY );
457 sound->set_volume( 0.3 );
458 globals->get_soundmgr()->add( sound, dme_fx_name );
460 int offset = (int)(sg_random() * 30.0);
461 nav_play_count = offset / 4;
462 nav_last_time = globals->get_time_params()->get_cur_time() -
464 // cout << "offset = " << offset << " play_count = "
466 // << " nav_last_time = " << nav_last_time
467 // << " current time = "
468 // << globals->get_time_params()->get_cur_time() << endl;
471 // cout << "Found an ils station in range" << endl;
472 // cout << " id = " << ils.get_locident() << endl;
474 } else if ( current_navlist->query( lon, lat, elev, nav_freq, &nav ) ) {
475 nav_id = nav.get_ident();
477 if ( last_nav_id != nav_id || !last_nav_vor ) {
478 last_nav_id = nav_id;
480 nav_trans_ident = nav.get_trans_ident();
482 nav_has_dme = nav.get_has_dme();
484 nav_loclon = nav.get_lon();
485 nav_loclat = nav.get_lat();
486 nav_elev = nav.get_elev();
487 nav_magvar = nav.get_magvar();
488 nav_range = nav.get_range();
489 nav_effective_range = adjustNavRange(nav_elev, elev, nav_range);
491 nav_radial = nav_sel_radial;
496 #ifdef ENABLE_AUDIO_SUPPORT
497 if ( globals->get_soundmgr()->exists( nav_fx_name ) ) {
498 globals->get_soundmgr()->remove( nav_fx_name );
500 FGSimpleSound *sound;
501 sound = morse.make_ident( nav_trans_ident, LO_FREQUENCY );
502 sound->set_volume( 0.3 );
503 if ( globals->get_soundmgr()->add( sound, nav_fx_name ) ) {
504 // cout << "Added nav-vor-ident sound" << endl;
506 cout << "Failed to add v1-vor-ident sound" << endl;
509 if ( globals->get_soundmgr()->exists( dme_fx_name ) ) {
510 globals->get_soundmgr()->remove( dme_fx_name );
512 sound = morse.make_ident( nav_trans_ident, HI_FREQUENCY );
513 sound->set_volume( 0.3 );
514 globals->get_soundmgr()->add( sound, dme_fx_name );
516 int offset = (int)(sg_random() * 30.0);
517 nav_play_count = offset / 4;
518 nav_last_time = globals->get_time_params()->get_cur_time() -
520 // cout << "offset = " << offset << " play_count = "
521 // << nav_play_count << " nav_last_time = "
522 // << nav_last_time << " current time = "
523 // << globals->get_time_params()->get_cur_time() << endl;
526 // cout << "Found a vor station in range" << endl;
527 // cout << " id = " << nav.get_ident() << endl;
533 nav_trans_ident = "";
535 #ifdef ENABLE_AUDIO_SUPPORT
536 if ( ! globals->get_soundmgr()->remove( nav_fx_name ) ) {
537 cout << "Failed to remove nav-vor-ident sound" << endl;
539 globals->get_soundmgr()->remove( dme_fx_name );
541 // cout << "not picking up vor1. :-(" << endl;
546 // return the amount of heading needle deflection, returns a value
547 // clamped to the range of ( -10 , 10 )
548 double FGNavCom::get_nav_heading_needle_deflection() const {
552 r = nav_heading - nav_radial;
553 // cout << "Radial = " << nav_radial
554 // << " Bearing = " << nav_heading << endl;
556 while ( r > 180.0 ) { r -= 360.0;}
557 while ( r < -180.0 ) { r += 360.0;}
558 if ( fabs(r) > 90.0 ) {
559 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
565 // According to Robin Peel, the ILS is 4x more sensitive than a vor
566 if ( nav_loc ) { r *= 4.0; }
567 if ( r < -10.0 ) { r = -10.0; }
568 if ( r > 10.0 ) { r = 10.0; }
577 // return the amount of glide slope needle deflection (.i.e. the
578 // number of degrees we are off the glide slope * 5.0
579 double FGNavCom::get_nav_gs_needle_deflection() const {
580 if ( nav_inrange && nav_has_gs ) {
581 double x = nav_gs_dist;
582 double y = (fgGetDouble("/position/altitude-ft") - nav_elev)
584 double angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
585 return (nav_target_gs - angle) * 5.0;
593 * Return true if the NAV TO flag should be active.
596 FGNavCom::get_nav_to_flag () const
599 double offset = fabs(nav_heading - nav_radial);
603 return (offset <= 90.0 || offset >= 270.0);
611 * Return true if the NAV FROM flag should be active.
614 FGNavCom::get_nav_from_flag () const
617 double offset = fabs(nav_heading - nav_radial);
621 return (offset > 90.0 && offset < 270.0);