]> git.mxchange.org Git - flightgear.git/blob - src/Instrumentation/navradio.cxx
James Turner:
[flightgear.git] / src / Instrumentation / navradio.cxx
1 // navradio.cxx -- class to manage a nav radio instance
2 //
3 // Written by Curtis Olson, started April 2000.
4 //
5 // Copyright (C) 2000 - 2002  Curtis L. Olson - http://www.flightgear.org/~curt
6 //
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.
11 //
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.
16 //
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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
20 //
21 // $Id$
22
23
24 #ifdef HAVE_CONFIG_H
25 #  include <config.h>
26 #endif
27
28 #include <iostream>
29 #include <string>
30 #include <sstream>
31
32 #include <simgear/compiler.h>
33 #include <simgear/sg_inlines.h>
34 #include <simgear/math/sg_random.h>
35 #include <simgear/math/vector.hxx>
36 #include <simgear/structure/exception.hxx>
37
38 #include <Aircraft/aircraft.hxx>
39 #include <Navaids/navlist.hxx>
40
41 #include "navradio.hxx"
42
43 #include <string>
44 using std::string;
45
46
47 // Constructor
48 FGNavRadio::FGNavRadio(SGPropertyNode *node) :
49     lon_node(fgGetNode("/position/longitude-deg", true)),
50     lat_node(fgGetNode("/position/latitude-deg", true)),
51     alt_node(fgGetNode("/position/altitude-ft", true)),
52     is_valid_node(NULL),
53     power_btn_node(NULL),
54     freq_node(NULL),
55     alt_freq_node(NULL),
56     sel_radial_node(NULL),
57     vol_btn_node(NULL),
58     ident_btn_node(NULL),
59     audio_btn_node(NULL),
60     backcourse_node(NULL),
61     nav_serviceable_node(NULL),
62     cdi_serviceable_node(NULL),
63     gs_serviceable_node(NULL),
64     tofrom_serviceable_node(NULL),
65     fmt_freq_node(NULL),
66     fmt_alt_freq_node(NULL),
67     heading_node(NULL),
68     radial_node(NULL),
69     recip_radial_node(NULL),
70     target_radial_true_node(NULL),
71     target_auto_hdg_node(NULL),
72     time_to_intercept(NULL),
73     to_flag_node(NULL),
74     from_flag_node(NULL),
75     inrange_node(NULL),
76     cdi_deflection_node(NULL),
77     cdi_xtrack_error_node(NULL),
78     cdi_xtrack_hdg_err_node(NULL),
79     has_gs_node(NULL),
80     loc_node(NULL),
81     loc_dist_node(NULL),
82     gs_deflection_node(NULL),
83     gs_rate_of_climb_node(NULL),
84     gs_dist_node(NULL),
85     nav_id_node(NULL),
86     id_c1_node(NULL),
87     id_c2_node(NULL),
88     id_c3_node(NULL),
89     id_c4_node(NULL),
90     nav_slaved_to_gps_node(NULL),
91     gps_cdi_deflection_node(NULL),
92     gps_to_flag_node(NULL),
93     gps_from_flag_node(NULL),
94     last_nav_id(""),
95     last_nav_vor(false),
96     play_count(0),
97     last_time(0),
98     radial(0.0),
99     target_radial(0.0),
100     horiz_vel(0.0),
101     last_x(0.0),
102     last_loc_dist(0.0),
103     last_xtrack_error(0.0),
104     _name(node->getStringValue("name", "nav")),
105     _num(node->getIntValue("number", 0)),
106     _time_before_search_sec(-1.0)
107 {
108     SGPath path( globals->get_fg_root() );
109     SGPath term = path;
110     term.append( "Navaids/range.term" );
111     SGPath low = path;
112     low.append( "Navaids/range.low" );
113     SGPath high = path;
114     high.append( "Navaids/range.high" );
115
116     term_tbl = new SGInterpTable( term.str() );
117     low_tbl = new SGInterpTable( low.str() );
118     high_tbl = new SGInterpTable( high.str() );
119 }
120
121
122 // Destructor
123 FGNavRadio::~FGNavRadio() 
124 {
125     delete term_tbl;
126     delete low_tbl;
127     delete high_tbl;
128 }
129
130
131 void
132 FGNavRadio::init ()
133 {
134     morse.init();
135
136     string branch;
137     branch = "/instrumentation/" + _name;
138
139     SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
140
141     bus_power_node = 
142         fgGetNode(("/systems/electrical/outputs/" + _name).c_str(), true);
143
144     // inputs
145     is_valid_node = node->getChild("data-is-valid", 0, true);
146     power_btn_node = node->getChild("power-btn", 0, true);
147     power_btn_node->setBoolValue( true );
148     vol_btn_node = node->getChild("volume", 0, true);
149     ident_btn_node = node->getChild("ident", 0, true);
150     ident_btn_node->setBoolValue( true );
151     audio_btn_node = node->getChild("audio-btn", 0, true);
152     audio_btn_node->setBoolValue( true );
153     backcourse_node = node->getChild("back-course-btn", 0, true);
154     backcourse_node->setBoolValue( false );
155     nav_serviceable_node = node->getChild("serviceable", 0, true);
156     cdi_serviceable_node = (node->getChild("cdi", 0, true))
157         ->getChild("serviceable", 0, true);
158     gs_serviceable_node = (node->getChild("gs", 0, true))
159         ->getChild("serviceable");
160     tofrom_serviceable_node = (node->getChild("to-from", 0, true))
161         ->getChild("serviceable", 0, true);
162
163     // frequencies
164     SGPropertyNode *subnode = node->getChild("frequencies", 0, true);
165     freq_node = subnode->getChild("selected-mhz", 0, true);
166     alt_freq_node = subnode->getChild("standby-mhz", 0, true);
167     fmt_freq_node = subnode->getChild("selected-mhz-fmt", 0, true);
168     fmt_alt_freq_node = subnode->getChild("standby-mhz-fmt", 0, true);
169
170     // radials
171     subnode = node->getChild("radials", 0, true);
172     sel_radial_node = subnode->getChild("selected-deg", 0, true);
173     radial_node = subnode->getChild("actual-deg", 0, true);
174     recip_radial_node = subnode->getChild("reciprocal-radial-deg", 0, true);
175     target_radial_true_node = subnode->getChild("target-radial-deg", 0, true);
176     target_auto_hdg_node = subnode->getChild("target-auto-hdg-deg", 0, true);
177
178     // outputs
179     heading_node = node->getChild("heading-deg", 0, true);
180     time_to_intercept = node->getChild("time-to-intercept-sec", 0, true);
181     to_flag_node = node->getChild("to-flag", 0, true);
182     from_flag_node = node->getChild("from-flag", 0, true);
183     inrange_node = node->getChild("in-range", 0, true);
184     cdi_deflection_node = node->getChild("heading-needle-deflection", 0, true);
185     cdi_xtrack_error_node = node->getChild("crosstrack-error-m", 0, true);
186     cdi_xtrack_hdg_err_node
187         = node->getChild("crosstrack-heading-error-deg", 0, true);
188     has_gs_node = node->getChild("has-gs", 0, true);
189     loc_node = node->getChild("nav-loc", 0, true);
190     loc_dist_node = node->getChild("nav-distance", 0, true);
191     gs_deflection_node = node->getChild("gs-needle-deflection", 0, true);
192     gs_rate_of_climb_node = node->getChild("gs-rate-of-climb", 0, true);
193     gs_dist_node = node->getChild("gs-distance", 0, true);
194     nav_id_node = node->getChild("nav-id", 0, true);
195     id_c1_node = node->getChild("nav-id_asc1", 0, true);
196     id_c2_node = node->getChild("nav-id_asc2", 0, true);
197     id_c3_node = node->getChild("nav-id_asc3", 0, true);
198     id_c4_node = node->getChild("nav-id_asc4", 0, true);
199
200     // gps slaving support
201     nav_slaved_to_gps_node = node->getChild("slaved-to-gps", 0, true);
202     gps_cdi_deflection_node = fgGetNode("/instrumentation/gps/cdi-deflection", true);
203     gps_to_flag_node = fgGetNode("/instrumentation/gps/to-flag", true);
204     gps_from_flag_node = fgGetNode("/instrumentation/gps/from-flag", true);
205     
206     std::ostringstream temp;
207     temp << _name << "nav-ident" << _num;
208     nav_fx_name = temp.str();
209     temp << _name << "dme-ident" << _num;
210     dme_fx_name = temp.str();
211 }
212
213 void
214 FGNavRadio::bind ()
215 {
216     std::ostringstream temp;
217     string branch;
218     temp << _num;
219     branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
220 }
221
222
223 void
224 FGNavRadio::unbind ()
225 {
226     std::ostringstream temp;
227     string branch;
228     temp << _num;
229     branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
230 }
231
232
233 // model standard VOR/DME/TACAN service volumes as per AIM 1-1-8
234 double FGNavRadio::adjustNavRange( double stationElev, double aircraftElev,
235                                  double nominalRange )
236 {
237     // extend out actual usable range to be 1.3x the published safe range
238     const double usability_factor = 1.3;
239
240     // assumptions we model the standard service volume, plus
241     // ... rather than specifying a cylinder, we model a cone that
242     // contains the cylinder.  Then we put an upside down cone on top
243     // to model diminishing returns at too-high altitudes.
244
245     // altitude difference
246     double alt = ( aircraftElev * SG_METER_TO_FEET - stationElev );
247     // cout << "aircraft elev = " << aircraftElev * SG_METER_TO_FEET
248     //      << " station elev = " << stationElev << endl;
249
250     if ( nominalRange < 25.0 + SG_EPSILON ) {
251         // Standard Terminal Service Volume
252         return term_tbl->interpolate( alt ) * usability_factor;
253     } else if ( nominalRange < 50.0 + SG_EPSILON ) {
254         // Standard Low Altitude Service Volume
255         // table is based on range of 40, scale to actual range
256         return low_tbl->interpolate( alt ) * nominalRange / 40.0
257             * usability_factor;
258     } else {
259         // Standard High Altitude Service Volume
260         // table is based on range of 130, scale to actual range
261         return high_tbl->interpolate( alt ) * nominalRange / 130.0
262             * usability_factor;
263     }
264 }
265
266
267 // model standard ILS service volumes as per AIM 1-1-9
268 double FGNavRadio::adjustILSRange( double stationElev, double aircraftElev,
269                                  double offsetDegrees, double distance )
270 {
271     // assumptions we model the standard service volume, plus
272
273     // altitude difference
274     // double alt = ( aircraftElev * SG_METER_TO_FEET - stationElev );
275 //     double offset = fabs( offsetDegrees );
276
277 //     if ( offset < 10 ) {
278 //      return FG_ILS_DEFAULT_RANGE;
279 //     } else if ( offset < 35 ) {
280 //      return 10 + (35 - offset) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
281 //     } else if ( offset < 45 ) {
282 //      return (45 - offset);
283 //     } else if ( offset > 170 ) {
284 //         return FG_ILS_DEFAULT_RANGE;
285 //     } else if ( offset > 145 ) {
286 //      return 10 + (offset - 145) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
287 //     } else if ( offset > 135 ) {
288 //         return (offset - 135);
289 //     } else {
290 //      return 0;
291 //     }
292     return FG_LOC_DEFAULT_RANGE;
293 }
294
295
296 //////////////////////////////////////////////////////////////////////////
297 // Update the various nav values based on position and valid tuned in navs
298 //////////////////////////////////////////////////////////////////////////
299 void 
300 FGNavRadio::update(double dt) 
301 {
302     // Do a nav station search only once a second to reduce
303     // unnecessary work. (Also, make sure to do this before caching
304     // any values!)
305     _time_before_search_sec -= dt;
306     if ( _time_before_search_sec < 0 ) {
307         search();
308     }
309
310     // cache a few strategic values locally for speed
311     SGGeod pos = SGGeod::fromDegFt(lon_node->getDoubleValue(),
312                                    lat_node->getDoubleValue(),
313                                    alt_node->getDoubleValue());
314     bool power_btn = power_btn_node->getBoolValue();
315     bool nav_serviceable = nav_serviceable_node->getBoolValue();
316     bool cdi_serviceable = cdi_serviceable_node->getBoolValue();
317     bool tofrom_serviceable = tofrom_serviceable_node->getBoolValue();
318     bool inrange = inrange_node->getBoolValue();
319     bool has_gs = has_gs_node->getBoolValue();
320     bool is_loc = loc_node->getBoolValue();
321     double loc_dist = loc_dist_node->getDoubleValue();
322
323     double az1, az2, s;
324
325     // Create "formatted" versions of the nav frequencies for
326     // instrument displays.
327     char tmp[16];
328     sprintf( tmp, "%.2f", freq_node->getDoubleValue() );
329     fmt_freq_node->setStringValue(tmp);
330     sprintf( tmp, "%.2f", alt_freq_node->getDoubleValue() );
331     fmt_alt_freq_node->setStringValue(tmp);
332
333     // cout << "is_valid = " << is_valid
334     //      << " power_btn = " << power_btn
335     //      << " bus_power = " << bus_power_node->getDoubleValue()
336     //      << " nav_serviceable = " << nav_serviceable
337     //      << endl;
338
339     if ( is_valid && power_btn && (bus_power_node->getDoubleValue() > 1.0)
340          && nav_serviceable )
341     {
342         SGVec3d aircraft = SGVec3d::fromGeod(pos);
343         loc_dist = dist(aircraft, nav_xyz);
344         loc_dist_node->setDoubleValue( loc_dist );
345         // cout << "dt = " << dt << " dist = " << loc_dist << endl;
346
347         if ( has_gs ) {
348             // find closest distance to the gs base line
349             SGVec3d p = aircraft;
350             double dist = sgdClosestPointToLineDistSquared(p.sg(), gs_xyz.sg(),
351                                                            gs_base_vec.sg());
352             gs_dist_node->setDoubleValue( sqrt( dist ) );
353             // cout << "gs_dist = " << gs_dist_node->getDoubleValue()
354             //      << endl;
355
356             // wgs84 heading to glide slope (to determine sign of distance)
357             geo_inverse_wgs_84( pos, SGGeod::fromDeg(gs_lon, gs_lat),
358                                 &az1, &az2, &s );
359             double r = az1 - target_radial;
360             while ( r >  180.0 ) { r -= 360.0;}
361             while ( r < -180.0 ) { r += 360.0;}
362             if ( r >= -90.0 && r <= 90.0 ) {
363                 gs_dist_signed = gs_dist_node->getDoubleValue();
364             } else {
365                 gs_dist_signed = -gs_dist_node->getDoubleValue();
366             }
367             /* cout << "Target Radial = " << target_radial 
368                  << "  Bearing = " << az1
369                  << "  dist (signed) = " << gs_dist_signed
370                  << endl; */
371             
372         } else {
373             gs_dist_node->setDoubleValue( 0.0 );
374         }
375         
376         //////////////////////////////////////////////////////////
377         // compute forward and reverse wgs84 headings to localizer
378         //////////////////////////////////////////////////////////
379         double hdg;
380         geo_inverse_wgs_84( pos, SGGeod::fromDeg(loc_lon, loc_lat),
381                             &hdg, &az2, &s );
382         // cout << "az1 = " << az1 << " magvar = " << nav_magvar << endl;
383         heading_node->setDoubleValue( hdg );
384         radial = az2 - twist;
385         double recip = radial + 180.0;
386         if ( recip >= 360.0 ) { recip -= 360.0; }
387         radial_node->setDoubleValue( radial );
388         recip_radial_node->setDoubleValue( recip );
389         // cout << " heading = " << heading_node->getDoubleValue()
390         //      << " dist = " << nav_dist << endl;
391
392         //////////////////////////////////////////////////////////
393         // compute the target/selected radial in "true" heading
394         //////////////////////////////////////////////////////////
395         double trtrue = 0.0;
396         if ( is_loc ) {
397             // ILS localizers radials are already "true" in our
398             // database
399             trtrue = target_radial;
400         } else {
401             // VOR radials need to have that vor's offset added in
402             trtrue = target_radial + twist;
403         }
404
405         while ( trtrue < 0.0 ) { trtrue += 360.0; }
406         while ( trtrue > 360.0 ) { trtrue -= 360.0; }
407         target_radial_true_node->setDoubleValue( trtrue );
408
409         //////////////////////////////////////////////////////////
410         // adjust reception range for altitude
411         // FIXME: make sure we are using the navdata range now that
412         //        it is valid in the data file
413         //////////////////////////////////////////////////////////
414         if ( is_loc ) {
415             double offset = radial - target_radial;
416             while ( offset < -180.0 ) { offset += 360.0; }
417             while ( offset > 180.0 ) { offset -= 360.0; }
418             // cout << "ils offset = " << offset << endl;
419             effective_range
420                 = adjustILSRange( nav_elev, pos.getElevationM(), offset,
421                                   loc_dist * SG_METER_TO_NM );
422         } else {
423             effective_range
424                 = adjustNavRange( nav_elev, pos.getElevationM(), range );
425         }
426         // cout << "nav range = " << effective_range
427         //      << " (" << range << ")" << endl;
428
429         if ( loc_dist < effective_range * SG_NM_TO_METER ) {
430             inrange = true;
431         } else if ( loc_dist < 2 * effective_range * SG_NM_TO_METER ) {
432             inrange = sg_random() < ( 2 * effective_range * SG_NM_TO_METER
433                                       - loc_dist )
434                                       / (effective_range * SG_NM_TO_METER);
435         } else {
436             inrange = false;
437         }
438         inrange_node->setBoolValue( inrange );
439
440         if ( !is_loc ) {
441             target_radial = sel_radial_node->getDoubleValue();
442         }
443
444         //////////////////////////////////////////////////////////
445         // compute to/from flag status
446         //////////////////////////////////////////////////////////
447         bool value = false;
448         double offset = fabs(radial - target_radial);
449         if ( tofrom_serviceable ) {
450             if ( nav_slaved_to_gps_node->getBoolValue() ) {
451                 value = gps_to_flag_node->getBoolValue();
452             } else if ( inrange ) {
453                 if ( is_loc ) {
454                     value = true;
455                 } else {
456                     value = !(offset <= 90.0 || offset >= 270.0);
457                 }
458             }
459         } else {
460             value = false;
461         }
462         to_flag_node->setBoolValue( value );
463
464         value = false;
465         if ( tofrom_serviceable ) {
466             if ( nav_slaved_to_gps_node->getBoolValue() ) {
467                 value = gps_from_flag_node->getBoolValue();
468             } else if ( inrange ) {
469                 if ( is_loc ) {
470                     value = false;
471                 } else {
472                     value = !(offset > 90.0 && offset < 270.0);
473                 }
474             }
475         } else {
476             value = false;
477         }
478         from_flag_node->setBoolValue( value );
479
480         //////////////////////////////////////////////////////////
481         // compute the deflection of the CDI needle, clamped to the range
482         // of ( -10 , 10 )
483         //////////////////////////////////////////////////////////
484         double r = 0.0;
485         bool loc_backside = false; // an in-code flag indicating that we are
486                                    // on a localizer backcourse.
487         if ( cdi_serviceable ) {
488             if ( nav_slaved_to_gps_node->getBoolValue() ) {
489                 r = gps_cdi_deflection_node->getDoubleValue();
490                 // We want +- 5 dots deflection for the gps, so clamp
491                 // to -12.5/12.5
492                 SG_CLAMP_RANGE( r, -12.5, 12.5 );
493             } else if ( inrange ) {
494                 r = radial - target_radial;
495                 // cout << "Target radial = " << target_radial 
496                 //      << "  Actual radial = " << radial << endl;
497                 
498                 while ( r >  180.0 ) { r -= 360.0;}
499                 while ( r < -180.0 ) { r += 360.0;}
500                 if ( fabs(r) > 90.0 ) {
501                     r = ( r<0.0 ? -r-180.0 : -r+180.0 );
502                 } else {
503                     if ( is_loc ) {
504                         loc_backside = true;
505                     }
506                 }
507
508                 r = -r;         // reverse, since radial is outbound
509                 if ( is_loc ) {
510                     // According to Robin Peel, the ILS is 4x more
511                     // sensitive than a vor
512                     r *= 4.0;
513                 }
514                 SG_CLAMP_RANGE( r, -10.0, 10.0 );
515             }
516         }
517         cdi_deflection_node->setDoubleValue( r );
518
519         //////////////////////////////////////////////////////////
520         // compute the amount of cross track distance error in meters
521         //////////////////////////////////////////////////////////
522         double xtrack_error = 0.0;
523         if ( inrange && nav_serviceable && cdi_serviceable ) {
524             r = radial - target_radial;
525             // cout << "Target radial = " << target_radial 
526             //     << "  Actual radial = " << radial
527             //     << "  r = " << r << endl;
528     
529             while ( r >  180.0 ) { r -= 360.0;}
530             while ( r < -180.0 ) { r += 360.0;}
531             if ( fabs(r) > 90.0 ) {
532                 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
533             }
534
535             r = -r;             // reverse, since radial is outbound
536
537             xtrack_error = loc_dist * sin(r * SGD_DEGREES_TO_RADIANS);
538         } else {
539             xtrack_error = 0.0;
540         }
541         cdi_xtrack_error_node->setDoubleValue( xtrack_error );
542
543         //////////////////////////////////////////////////////////
544         // compute an approximate ground track heading error
545         //////////////////////////////////////////////////////////
546         double hdg_error = 0.0;
547         if ( inrange && cdi_serviceable ) {
548             double vn = fgGetDouble( "/velocities/speed-north-fps" );
549             double ve = fgGetDouble( "/velocities/speed-east-fps" );
550             double gnd_trk_true = atan2( ve, vn ) * SGD_RADIANS_TO_DEGREES;
551             if ( gnd_trk_true < 0.0 ) { gnd_trk_true += 360.0; }
552
553             SGPropertyNode *true_hdg
554                 = fgGetNode("/orientation/heading-deg", true);
555             hdg_error = gnd_trk_true - true_hdg->getDoubleValue();
556
557             // cout << "ground track = " << gnd_trk_true
558             //      << " orientation = " << true_hdg->getDoubleValue() << endl;
559         }
560         cdi_xtrack_hdg_err_node->setDoubleValue( hdg_error );
561
562         //////////////////////////////////////////////////////////
563         // compute the time to intercept selected radial (based on
564         // current and last cross track errors and dt
565         //////////////////////////////////////////////////////////
566         double t = 0.0;
567         if ( inrange && cdi_serviceable ) {
568             double xrate_ms = (last_xtrack_error - xtrack_error) / dt;
569             if ( fabs(xrate_ms) > 0.00001 ) {
570                 t = xtrack_error / xrate_ms;
571             } else {
572                 t = 9999.9;
573             }
574         }
575         time_to_intercept->setDoubleValue( t );
576
577         //////////////////////////////////////////////////////////
578         // compute the amount of glide slope needle deflection
579         // (.i.e. the number of degrees we are off the glide slope * 5.0
580         //
581         // CLO - 13 Mar 2006: The glide slope needle should peg at
582         // +/-0.7 degrees off the ideal glideslope.  I'm not sure why
583         // we compute the factor the way we do (5*gs_error), but we
584         // need to compensate for our 'odd' number in the glideslope
585         // needle animation.  This means that the needle should peg
586         // when this values is +/-3.5.
587         //////////////////////////////////////////////////////////
588         r = 0.0;
589         if ( has_gs && gs_serviceable_node->getBoolValue() ) {
590             if ( nav_slaved_to_gps_node->getBoolValue() ) {
591                 // FIXME/FINISHME, what should be set here?
592             } else if ( inrange ) {
593                 double x = gs_dist_node->getDoubleValue();
594                 double y = (fgGetDouble("/position/altitude-ft") - nav_elev)
595                     * SG_FEET_TO_METER;
596                 // cout << "dist = " << x << " height = " << y << endl;
597                 double angle = asin( y / x ) * SGD_RADIANS_TO_DEGREES;
598                 r = (target_gs - angle) * 5.0;
599             }
600         }
601         gs_deflection_node->setDoubleValue( r );
602
603         //////////////////////////////////////////////////////////
604         // Calculate desired rate of climb for intercepting the GS
605         //////////////////////////////////////////////////////////
606         double x = gs_dist_node->getDoubleValue();
607         double y = (alt_node->getDoubleValue() - nav_elev)
608             * SG_FEET_TO_METER;
609         double current_angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
610
611         double target_angle = target_gs;
612         double gs_diff = target_angle - current_angle;
613
614         // convert desired vertical path angle into a climb rate
615         double des_angle = current_angle - 10 * gs_diff;
616
617         // estimate horizontal speed towards ILS in meters per minute
618         double dist = last_x - x;
619         last_x = x;
620         if ( dt > 0.0 ) {
621             // avoid nan
622             double new_vel = ( dist / dt );
623  
624             horiz_vel = 0.75 * horiz_vel + 0.25 * new_vel;
625             // double horiz_vel = cur_fdm_state->get_V_ground_speed()
626             //    * SG_FEET_TO_METER * 60.0;
627             // double horiz_vel = airspeed_node->getFloatValue()
628             //    * SG_FEET_TO_METER * 60.0;
629
630             gs_rate_of_climb_node
631                 ->setDoubleValue( -sin( des_angle * SGD_DEGREES_TO_RADIANS )
632                                   * horiz_vel * SG_METER_TO_FEET );
633         }
634
635         //////////////////////////////////////////////////////////
636         // Calculate a suggested target heading to smoothly intercept
637         // a nav/ils radial.
638         //////////////////////////////////////////////////////////
639
640         // Now that we have cross track heading adjustment built in,
641         // we shouldn't need to overdrive the heading angle within 8km
642         // of the station.
643         //
644         // The cdi deflection should be +/-10 for a full range of deflection
645         // so multiplying this by 3 gives us +/- 30 degrees heading
646         // compensation.
647         double adjustment = cdi_deflection_node->getDoubleValue() * 3.0;
648         SG_CLAMP_RANGE( adjustment, -30.0, 30.0 );
649
650         // determine the target heading to fly to intercept the
651         // tgt_radial = target radial (true) + cdi offset adjustmest -
652         // xtrack heading error adjustment
653         double nta_hdg;
654         if ( is_loc && backcourse_node->getBoolValue() ) {
655             // tuned to a localizer and backcourse mode activated
656             trtrue += 180.0;   // reverse the target localizer heading
657             while ( trtrue > 360.0 ) { trtrue -= 360.0; }
658             nta_hdg = trtrue - adjustment - hdg_error;
659         } else {
660             nta_hdg = trtrue + adjustment - hdg_error;
661         }
662
663         while ( nta_hdg <   0.0 ) { nta_hdg += 360.0; }
664         while ( nta_hdg >= 360.0 ) { nta_hdg -= 360.0; }
665         target_auto_hdg_node->setDoubleValue( nta_hdg );
666
667         last_xtrack_error = xtrack_error;
668    } else {
669         inrange_node->setBoolValue( false );
670         cdi_deflection_node->setDoubleValue( 0.0 );
671         cdi_xtrack_error_node->setDoubleValue( 0.0 );
672         cdi_xtrack_hdg_err_node->setDoubleValue( 0.0 );
673         time_to_intercept->setDoubleValue( 0.0 );
674         gs_deflection_node->setDoubleValue( 0.0 );
675         to_flag_node->setBoolValue( false );
676         from_flag_node->setBoolValue( false );
677         // cout << "not picking up vor. :-(" << endl;
678     }
679
680     // audio effects
681     if ( is_valid && inrange && nav_serviceable ) {
682         // play station ident via audio system if on + ident,
683         // otherwise turn it off
684         if ( power_btn
685              && (bus_power_node->getDoubleValue() > 1.0)
686              && ident_btn_node->getBoolValue()
687              && audio_btn_node->getBoolValue() )
688         {
689             SGSoundSample *sound;
690             sound = globals->get_soundmgr()->find( nav_fx_name );
691             double vol = vol_btn_node->getDoubleValue();
692             if ( vol < 0.0 ) { vol = 0.0; }
693             if ( vol > 1.0 ) { vol = 1.0; }
694             if ( sound != NULL ) {
695                 sound->set_volume( vol );
696             } else {
697                 SG_LOG( SG_COCKPIT, SG_ALERT,
698                         "Can't find nav-vor-ident sound" );
699             }
700             sound = globals->get_soundmgr()->find( dme_fx_name );
701             if ( sound != NULL ) {
702                 sound->set_volume( vol );
703             } else {
704                 SG_LOG( SG_COCKPIT, SG_ALERT,
705                         "Can't find nav-dme-ident sound" );
706             }
707             // cout << "last_time = " << last_time << " ";
708             // cout << "cur_time = "
709             //      << globals->get_time_params()->get_cur_time();
710             if ( last_time <
711                  globals->get_time_params()->get_cur_time() - 30 ) {
712                 last_time = globals->get_time_params()->get_cur_time();
713                 play_count = 0;
714             }
715             // cout << " play_count = " << play_count << endl;
716             // cout << "playing = "
717             //      << globals->get_soundmgr()->is_playing(nav_fx_name)
718             //      << endl;
719             if ( play_count < 4 ) {
720                 // play VOR ident
721                 if ( !globals->get_soundmgr()->is_playing(nav_fx_name) ) {
722                     globals->get_soundmgr()->play_once( nav_fx_name );
723                     ++play_count;
724                 }
725             } else if ( play_count < 5 && has_dme ) {
726                 // play DME ident
727                 if ( !globals->get_soundmgr()->is_playing(nav_fx_name) &&
728                      !globals->get_soundmgr()->is_playing(dme_fx_name) ) {
729                     globals->get_soundmgr()->play_once( dme_fx_name );
730                     ++play_count;
731                 }
732             }
733         } else {
734             globals->get_soundmgr()->stop( nav_fx_name );
735             globals->get_soundmgr()->stop( dme_fx_name );
736         }
737     }
738
739     last_loc_dist = loc_dist;
740 }
741
742
743 // Update current nav/adf radio stations based on current postition
744 void FGNavRadio::search() 
745 {
746
747     // reset search time
748     _time_before_search_sec = 1.0;
749
750     // cache values locally for speed
751     double lon = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
752     double lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
753     double elev = alt_node->getDoubleValue() * SG_FEET_TO_METER;
754
755     FGNavRecord *nav = NULL;
756     FGNavRecord *loc = NULL;
757     FGNavRecord *dme = NULL;
758     FGNavRecord *gs = NULL;
759
760     ////////////////////////////////////////////////////////////////////////
761     // Nav.
762     ////////////////////////////////////////////////////////////////////////
763
764     double freq = freq_node->getDoubleValue();
765     nav = globals->get_navlist()->findByFreq(freq, lon, lat, elev);
766     dme = globals->get_dmelist()->findByFreq(freq, lon, lat, elev);
767     if ( nav == NULL ) {
768         loc = globals->get_loclist()->findByFreq(freq, lon, lat, elev);
769         gs = globals->get_gslist()->findByFreq(freq, lon, lat, elev);
770     }
771
772     string nav_id = "";
773
774     if ( loc != NULL ) {
775         nav_id = loc->get_ident();
776         nav_id_node->setStringValue( nav_id.c_str() );
777         // cout << "localizer = " << nav_id_node->getStringValue() << endl;
778         is_valid = true;
779         if ( last_nav_id != nav_id || last_nav_vor ) {
780             trans_ident = loc->get_trans_ident();
781             target_radial = loc->get_multiuse();
782             while ( target_radial <   0.0 ) { target_radial += 360.0; }
783             while ( target_radial > 360.0 ) { target_radial -= 360.0; }
784             loc_lon = loc->get_lon();
785             loc_lat = loc->get_lat();
786             nav_xyz = loc->get_cart();
787             last_nav_id = nav_id;
788             last_nav_vor = false;
789             loc_node->setBoolValue( true );
790             has_dme = (dme != NULL);
791             if ( gs != NULL ) {
792                 has_gs_node->setBoolValue( true );
793                 gs_lon = gs->get_lon();
794                 gs_lat = gs->get_lat();
795                 nav_elev = gs->get_elev_ft();
796                 int tmp = (int)(gs->get_multiuse() / 1000.0);
797                 target_gs = (double)tmp / 100.0;
798                 gs_xyz = gs->get_cart();
799
800                 // derive GS baseline (perpendicular to the runay
801                 // along the ground)
802                 double tlon, tlat, taz;
803                 geo_direct_wgs_84 ( 0.0, gs_lat, gs_lon,
804                                     target_radial + 90,
805                                     100.0, &tlat, &tlon, &taz );
806                 // cout << "target_radial = " << target_radial << endl;
807                 // cout << "nav_loc = " << loc_node->getBoolValue() << endl;
808                 // cout << gs_lon << "," << gs_lat << "  "
809                 //      << tlon << "," << tlat << "  (" << nav_elev << ")"
810                 //      << endl;
811                 SGGeod tpos = SGGeod::fromDegFt(tlon, tlat, nav_elev);
812                 SGVec3d p1 = SGVec3d::fromGeod(tpos);
813
814                 // cout << gs_xyz << endl;
815                 // cout << p1 << endl;
816                 gs_base_vec = p1 - gs_xyz;
817                 // cout << gs_base_vec << endl;
818             } else {
819                 has_gs_node->setBoolValue( false );
820                 nav_elev = loc->get_elev_ft();
821             }
822             twist = 0;
823             range = FG_LOC_DEFAULT_RANGE;
824             effective_range = range;
825
826             if ( globals->get_soundmgr()->exists( nav_fx_name ) ) {
827                 globals->get_soundmgr()->remove( nav_fx_name );
828             }
829             SGSoundSample *sound;
830             sound = morse.make_ident( trans_ident, LO_FREQUENCY );
831             sound->set_volume( 0.3 );
832             globals->get_soundmgr()->add( sound, nav_fx_name );
833
834             if ( globals->get_soundmgr()->exists( dme_fx_name ) ) {
835                 globals->get_soundmgr()->remove( dme_fx_name );
836             }
837             sound = morse.make_ident( trans_ident, HI_FREQUENCY );
838             sound->set_volume( 0.3 );
839             globals->get_soundmgr()->add( sound, dme_fx_name );
840
841             int offset = (int)(sg_random() * 30.0);
842             play_count = offset / 4;
843             last_time = globals->get_time_params()->get_cur_time() -
844                 offset;
845             // cout << "offset = " << offset << " play_count = "
846             //      << play_count
847             //      << " last_time = " << last_time
848             //      << " current time = "
849             //      << globals->get_time_params()->get_cur_time() << endl;
850
851             // cout << "Found an loc station in range" << endl;
852             // cout << " id = " << loc->get_locident() << endl;
853         }
854     } else if ( nav != NULL ) {
855         nav_id = nav->get_ident();
856         nav_id_node->setStringValue( nav_id.c_str() );
857         // cout << "nav = " << nav_id << endl;
858         is_valid = true;
859         if ( last_nav_id != nav_id || !last_nav_vor ) {
860             last_nav_id = nav_id;
861             last_nav_vor = true;
862             trans_ident = nav->get_trans_ident();
863             loc_node->setBoolValue( false );
864             has_dme = (dme != NULL);
865             has_gs_node->setBoolValue( false );
866             loc_lon = nav->get_lon();
867             loc_lat = nav->get_lat();
868             nav_elev = nav->get_elev_ft();
869             twist = nav->get_multiuse();
870             range = nav->get_range();
871             effective_range = adjustNavRange(nav_elev, elev, range);
872             target_gs = 0.0;
873             target_radial = sel_radial_node->getDoubleValue();
874             nav_xyz = nav->get_cart();
875
876             if ( globals->get_soundmgr()->exists( nav_fx_name ) ) {
877                 globals->get_soundmgr()->remove( nav_fx_name );
878             }
879             try {
880                 SGSoundSample *sound;
881                 sound = morse.make_ident( trans_ident, LO_FREQUENCY );
882                 sound->set_volume( 0.3 );
883                 if ( globals->get_soundmgr()->add( sound, nav_fx_name ) ) {
884                     // cout << "Added nav-vor-ident sound" << endl;
885                 } else {
886                     SG_LOG(SG_COCKPIT, SG_WARN, "Failed to add v1-vor-ident sound");
887                 }
888
889                 if ( globals->get_soundmgr()->exists( dme_fx_name ) ) {
890                     globals->get_soundmgr()->remove( dme_fx_name );
891                 }
892                 sound = morse.make_ident( trans_ident, HI_FREQUENCY );
893                 sound->set_volume( 0.3 );
894                 globals->get_soundmgr()->add( sound, dme_fx_name );
895
896                 int offset = (int)(sg_random() * 30.0);
897                 play_count = offset / 4;
898                 last_time = globals->get_time_params()->get_cur_time() - offset;
899                 // cout << "offset = " << offset << " play_count = "
900                 //      << play_count << " last_time = "
901                 //      << last_time << " current time = "
902                 //      << globals->get_time_params()->get_cur_time() << endl;
903
904                 // cout << "Found a vor station in range" << endl;
905                 // cout << " id = " << nav->get_ident() << endl;
906             } catch ( sg_io_exception &e ) {
907                 SG_LOG(SG_GENERAL, SG_ALERT, e.getFormattedMessage());
908             }
909         }
910     } else {
911         is_valid = false;
912         nav_id_node->setStringValue( "" );
913         target_radial = 0;
914         trans_ident = "";
915         last_nav_id = "";
916         globals->get_soundmgr()->remove( nav_fx_name );
917         globals->get_soundmgr()->remove( dme_fx_name );
918     }
919
920     is_valid_node->setBoolValue( is_valid );
921
922     char tmpid[5];
923     strncpy( tmpid, nav_id.c_str(), 5 );
924     id_c1_node->setIntValue( (int)tmpid[0] );
925     id_c2_node->setIntValue( (int)tmpid[1] );
926     id_c3_node->setIntValue( (int)tmpid[2] );
927     id_c4_node->setIntValue( (int)tmpid[3] );
928 }