1 // adf.cxx - distance-measuring equipment.
2 // Written by David Megginson, started 2003.
4 // This file is in the Public Domain and comes with no warranty.
6 #include <simgear/compiler.h>
7 #include <simgear/math/sg_geodesy.hxx>
8 #include <simgear/math/sg_random.h>
10 #include <Main/fg_props.hxx>
11 #include <Main/util.hxx>
12 #include <Navaids/navlist.hxx>
17 // Use a bigger number to be more responsive, or a smaller number
18 // to be more sluggish.
19 #define RESPONSIVENESS 0.5
23 * Fiddle with the reception range a bit.
25 * TODO: better reception at night (??).
28 adjust_range (double transmitter_elevation_ft, double aircraft_altitude_ft,
31 double delta_elevation_ft =
32 aircraft_altitude_ft - transmitter_elevation_ft;
33 double range_nm = max_range_nm;
35 // kludge slightly better reception at
37 if (delta_elevation_ft < 0)
38 delta_elevation_ft = 200;
39 if (delta_elevation_ft <= 1000)
40 range_nm *= sqrt(delta_elevation_ft / 1000);
41 else if (delta_elevation_ft >= 5000)
42 range_nm *= sqrt(delta_elevation_ft / 5000);
43 if (range_nm >= max_range_nm * 3)
44 range_nm = max_range_nm * 3;
46 double rand = sg_random();
47 return range_nm + (range_nm * rand * rand);
52 : _time_before_search_sec(0),
53 _last_frequency_khz(-1),
54 _transmitter_valid(false),
55 _transmitter_elevation_ft(0),
56 _transmitter_range_nm(0),
70 _longitude_node = fgGetNode("/position/longitude-deg", true);
71 _latitude_node = fgGetNode("/position/latitude-deg", true);
72 _altitude_node = fgGetNode("/position/altitude-ft", true);
73 _heading_node = fgGetNode("/orientation/heading-deg", true);
74 _serviceable_node = fgGetNode("/instrumentation/adf/serviceable", true);
75 _error_node = fgGetNode("/instrumentation/adf/error-deg", true);
76 _electrical_node = fgGetNode("/systems/electrical/outputs/adf", true);
78 fgGetNode("/instrumentation/adf/frequencies/selected-khz", true);
79 _mode_node = fgGetNode("/instrumentation/adf/mode", true);
81 _in_range_node = fgGetNode("/instrumentation/adf/in-range", true);
83 fgGetNode("/instrumentation/adf/indicated-bearing-deg", true);
84 _ident_node = fgGetNode("/instrumentation/adf/ident", true);
85 _volume_node = fgGetNode("/instrumentation/adf/volume-norm", true);
86 _ident_audible_node = fgGetNode("/instrumentation/adf/ident-audible", true);
91 ADF::update (double delta_time_sec)
93 // If it's off, don't waste any time.
94 if (!_electrical_node->getBoolValue() ||
95 !_serviceable_node->getBoolValue()) {
96 set_bearing(delta_time_sec, 90);
97 _ident_node->setStringValue("");
102 int frequency_khz = _frequency_node->getIntValue();
103 if (frequency_khz != _last_frequency_khz) {
104 _time_before_search_sec = 0;
105 _last_frequency_khz = frequency_khz;
108 // Get the aircraft position
109 double longitude_deg = _longitude_node->getDoubleValue();
110 double latitude_deg = _latitude_node->getDoubleValue();
111 double altitude_m = _altitude_node->getDoubleValue();
113 double longitude_rad = longitude_deg * SGD_DEGREES_TO_RADIANS;
114 double latitude_rad = latitude_deg * SGD_DEGREES_TO_RADIANS;
116 // On timeout, scan again
117 _time_before_search_sec -= delta_time_sec;
118 if (_time_before_search_sec < 0)
119 search(frequency_khz, longitude_rad, latitude_rad, altitude_m);
121 // If it's off, don't bother.
122 string mode = _mode_node->getStringValue();
123 if (!_transmitter_valid || (mode != "bfo" && mode != "adf"))
125 set_bearing(delta_time_sec, 90);
126 _ident_node->setStringValue("");
130 // Calculate the bearing to the transmitter
132 sgGeodToCart(Point3D(longitude_rad, latitude_rad, altitude_m));
134 double distance_nm = _transmitter.distance3D(location) * SG_METER_TO_NM;
135 double range_nm = adjust_range(_transmitter_elevation_ft,
136 altitude_m * SG_METER_TO_FEET,
137 _transmitter_range_nm);
138 if (distance_nm <= range_nm) {
140 double bearing, az2, s;
141 double heading = _heading_node->getDoubleValue();
143 geo_inverse_wgs_84(altitude_m,
146 _transmitter_lat_deg,
147 _transmitter_lon_deg,
149 _in_range_node->setBoolValue(true);
154 set_bearing(delta_time_sec, bearing);
158 if ( _ident_audible_node->getBoolValue() )
159 volume = _volume_node->getDoubleValue();
163 if ( volume != _last_volume ) {
164 _last_volume = volume;
166 SGSoundSample *sound;
167 sound = globals->get_soundmgr()->find( "adf-ident" );
169 sound->set_volume( volume );
171 SG_LOG( SG_GENERAL, SG_ALERT, "Can't find adf-ident sound" );
174 time_t cur_time = globals->get_time_params()->get_cur_time();
175 if ( _last_ident_time < cur_time - 30 ) {
176 _last_ident_time = cur_time;
180 if ( _ident_count < 4 ) {
181 if ( !globals->get_soundmgr()->is_playing("adf-ident") ) {
182 globals->get_soundmgr()->play_once( "adf-ident" );
187 _in_range_node->setBoolValue(false);
188 set_bearing(delta_time_sec, 90);
189 _ident_node->setStringValue("");
190 globals->get_soundmgr()->stop( "adf-ident" );
195 ADF::search (double frequency_khz, double longitude_rad,
196 double latitude_rad, double altitude_m)
200 _time_before_search_sec = 1.0;
202 // try the ILS list first
204 globals->get_navlist()->findByFreq(frequency_khz, longitude_rad,
205 latitude_rad, altitude_m);
207 _transmitter_valid = (nav != NULL);
208 if ( _transmitter_valid ) {
209 ident = nav->get_trans_ident();
210 if ( ident != _last_ident ) {
211 _transmitter_lon_deg = nav->get_lon();
212 _transmitter_lat_deg = nav->get_lat();
213 _transmitter = Point3D(nav->get_x(), nav->get_y(), nav->get_z());
214 _transmitter_elevation_ft = nav->get_elev_ft();
215 _transmitter_range_nm = nav->get_range();
219 if ( _last_ident != ident ) {
221 _ident_node->setStringValue(ident.c_str());
223 if ( globals->get_soundmgr()->exists( "adf-ident" ) ) {
224 // stop is required! -- remove alone wouldn't stop immediately
225 globals->get_soundmgr()->stop( "adf-ident" );
226 globals->get_soundmgr()->remove( "adf-ident" );
229 SGSoundSample *sound;
230 sound = morse.make_ident( ident, LO_FREQUENCY );
231 sound->set_volume(_last_volume = 0);
232 globals->get_soundmgr()->add( sound, "adf-ident" );
234 int offset = (int)(sg_random() * 30.0);
235 _ident_count = offset / 4;
236 _last_ident_time = globals->get_time_params()->get_cur_time() -
242 ADF::set_bearing (double dt, double bearing_deg)
244 double old_bearing_deg = _bearing_node->getDoubleValue();
246 bearing_deg += _error_node->getDoubleValue();
248 while ((bearing_deg - old_bearing_deg) >= 180)
249 old_bearing_deg += 360;
250 while ((bearing_deg - old_bearing_deg) <= -180)
251 old_bearing_deg -= 360;
254 fgGetLowPass(old_bearing_deg, bearing_deg, dt * RESPONSIVENESS);
256 _bearing_node->setDoubleValue(bearing_deg);