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.
10 #include <simgear/compiler.h>
11 #include <simgear/math/sg_geodesy.hxx>
12 #include <simgear/math/sg_random.h>
14 #include <Main/fg_props.hxx>
15 #include <Main/util.hxx>
16 #include <Navaids/navlist.hxx>
25 // Use a bigger number to be more responsive, or a smaller number
26 // to be more sluggish.
27 #define RESPONSIVENESS 0.5
31 * Fiddle with the reception range a bit.
33 * TODO: better reception at night (??).
36 adjust_range (double transmitter_elevation_ft, double aircraft_altitude_ft,
39 double delta_elevation_ft =
40 aircraft_altitude_ft - transmitter_elevation_ft;
41 double range_nm = max_range_nm;
43 // kludge slightly better reception at
45 if (delta_elevation_ft < 0)
46 delta_elevation_ft = 200;
47 if (delta_elevation_ft <= 1000)
48 range_nm *= sqrt(delta_elevation_ft / 1000);
49 else if (delta_elevation_ft >= 5000)
50 range_nm *= sqrt(delta_elevation_ft / 5000);
51 if (range_nm >= max_range_nm * 3)
52 range_nm = max_range_nm * 3;
54 double rand = sg_random();
55 return range_nm + (range_nm * rand * rand);
59 ADF::ADF (SGPropertyNode *node )
61 _time_before_search_sec(0),
62 _last_frequency_khz(-1),
63 _transmitter_valid(false),
64 _transmitter_elevation_ft(0),
65 _transmitter_range_nm(0),
73 for ( i = 0; i < node->nChildren(); ++i ) {
74 SGPropertyNode *child = node->getChild(i);
75 string cname = child->getName();
76 string cval = child->getStringValue();
77 if ( cname == "name" ) {
79 } else if ( cname == "number" ) {
80 num = child->getIntValue();
82 SG_LOG( SG_INSTR, SG_WARN, "Error in adf config logic" );
83 if ( name.length() ) {
84 SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
92 : _time_before_search_sec(0),
93 _last_frequency_khz(-1),
94 _transmitter_valid(false),
95 _transmitter_elevation_ft(0),
96 _transmitter_range_nm(0),
113 branch = "/instrumentation/" + name;
115 SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
116 _longitude_node = fgGetNode("/position/longitude-deg", true);
117 _latitude_node = fgGetNode("/position/latitude-deg", true);
118 _altitude_node = fgGetNode("/position/altitude-ft", true);
119 _heading_node = fgGetNode("/orientation/heading-deg", true);
120 _serviceable_node = node->getChild("serviceable", 0, true);
121 _error_node = node->getChild("error-deg", 0, true);
122 _electrical_node = fgGetNode("/systems/electrical/outputs/adf", true);
123 branch = branch + "/frequencies";
124 SGPropertyNode *fnode = node->getChild("frequencies", 0, true);
125 _frequency_node = fnode->getChild("selected-khz", 0, true);
126 _mode_node = node->getChild("mode", 0, true);
127 _volume_node = node->getChild("volume-norm", 0, true);
128 _in_range_node = node->getChild("in-range", 0, true);
129 _bearing_node = node->getChild("indicated-bearing-deg", 0, true);
130 _ident_node = node->getChild("ident", 0, true);
131 _ident_audible_node = node->getChild("ident-audible", 0, true);
135 std::ostringstream temp;
137 adf_ident = temp.str();
141 ADF::update (double delta_time_sec)
143 // If it's off, don't waste any time.
144 if (!_electrical_node->getBoolValue() ||
145 !_serviceable_node->getBoolValue()) {
146 set_bearing(delta_time_sec, 90);
147 _ident_node->setStringValue("");
152 int frequency_khz = _frequency_node->getIntValue();
153 if (frequency_khz != _last_frequency_khz) {
154 _time_before_search_sec = 0;
155 _last_frequency_khz = frequency_khz;
158 // Get the aircraft position
159 double longitude_deg = _longitude_node->getDoubleValue();
160 double latitude_deg = _latitude_node->getDoubleValue();
161 double altitude_m = _altitude_node->getDoubleValue();
163 double longitude_rad = longitude_deg * SGD_DEGREES_TO_RADIANS;
164 double latitude_rad = latitude_deg * SGD_DEGREES_TO_RADIANS;
166 // On timeout, scan again
167 _time_before_search_sec -= delta_time_sec;
168 if (_time_before_search_sec < 0)
169 search(frequency_khz, longitude_rad, latitude_rad, altitude_m);
171 // If it's off, don't bother.
172 string mode = _mode_node->getStringValue();
173 if (!_transmitter_valid || (mode != "bfo" && mode != "adf"))
175 set_bearing(delta_time_sec, 90);
176 _ident_node->setStringValue("");
180 // Calculate the bearing to the transmitter
182 sgGeodToCart(Point3D(longitude_rad, latitude_rad, altitude_m));
184 double distance_nm = _transmitter.distance3D(location) * SG_METER_TO_NM;
185 double range_nm = adjust_range(_transmitter_elevation_ft,
186 altitude_m * SG_METER_TO_FEET,
187 _transmitter_range_nm);
188 if (distance_nm <= range_nm) {
190 double bearing, az2, s;
191 double heading = _heading_node->getDoubleValue();
193 geo_inverse_wgs_84(altitude_m,
196 _transmitter_lat_deg,
197 _transmitter_lon_deg,
199 _in_range_node->setBoolValue(true);
204 set_bearing(delta_time_sec, bearing);
208 if ( _ident_audible_node->getBoolValue() )
209 volume = _volume_node->getDoubleValue();
213 if ( volume != _last_volume ) {
214 _last_volume = volume;
216 SGSoundSample *sound;
217 sound = globals->get_soundmgr()->find( adf_ident );
219 sound->set_volume( volume );
221 SG_LOG( SG_GENERAL, SG_ALERT, "Can't find adf-ident sound" );
224 time_t cur_time = globals->get_time_params()->get_cur_time();
225 if ( _last_ident_time < cur_time - 30 ) {
226 _last_ident_time = cur_time;
230 if ( _ident_count < 4 ) {
231 if ( !globals->get_soundmgr()->is_playing(adf_ident) ) {
232 globals->get_soundmgr()->play_once( adf_ident );
237 _in_range_node->setBoolValue(false);
238 set_bearing(delta_time_sec, 90);
239 _ident_node->setStringValue("");
240 globals->get_soundmgr()->stop( adf_ident );
245 ADF::search (double frequency_khz, double longitude_rad,
246 double latitude_rad, double altitude_m)
250 _time_before_search_sec = 1.0;
252 // try the ILS list first
254 globals->get_navlist()->findByFreq(frequency_khz, longitude_rad,
255 latitude_rad, altitude_m);
257 _transmitter_valid = (nav != NULL);
258 if ( _transmitter_valid ) {
259 ident = nav->get_trans_ident();
260 if ( ident != _last_ident ) {
261 _transmitter_lon_deg = nav->get_lon();
262 _transmitter_lat_deg = nav->get_lat();
263 _transmitter = Point3D(nav->get_x(), nav->get_y(), nav->get_z());
264 _transmitter_elevation_ft = nav->get_elev_ft();
265 _transmitter_range_nm = nav->get_range();
269 if ( _last_ident != ident ) {
271 _ident_node->setStringValue(ident.c_str());
273 if ( globals->get_soundmgr()->exists( adf_ident ) ) {
274 // stop is required! -- remove alone wouldn't stop immediately
275 globals->get_soundmgr()->stop( adf_ident );
276 globals->get_soundmgr()->remove( adf_ident );
279 SGSoundSample *sound;
280 sound = morse.make_ident( ident, LO_FREQUENCY );
281 sound->set_volume(_last_volume = 0);
282 globals->get_soundmgr()->add( sound, adf_ident );
284 int offset = (int)(sg_random() * 30.0);
285 _ident_count = offset / 4;
286 _last_ident_time = globals->get_time_params()->get_cur_time() -
292 ADF::set_bearing (double dt, double bearing_deg)
294 double old_bearing_deg = _bearing_node->getDoubleValue();
296 while ((bearing_deg - old_bearing_deg) >= 180)
297 old_bearing_deg += 360;
298 while ((bearing_deg - old_bearing_deg) <= -180)
299 old_bearing_deg -= 360;
300 bearing_deg += _error_node->getDoubleValue();
302 fgGetLowPass(old_bearing_deg, bearing_deg, dt * RESPONSIVENESS);
304 _bearing_node->setDoubleValue(bearing_deg);