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