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>
13 #include <simgear/timing/sg_time.hxx>
15 #include <Main/fg_props.hxx>
16 #include <Main/util.hxx>
17 #include <Navaids/navlist.hxx>
26 // Use a bigger number to be more responsive, or a smaller number
27 // to be more sluggish.
28 #define RESPONSIVENESS 0.5
32 * Fiddle with the reception range a bit.
34 * TODO: better reception at night (??).
37 adjust_range (double transmitter_elevation_ft, double aircraft_altitude_ft,
40 double delta_elevation_ft =
41 aircraft_altitude_ft - transmitter_elevation_ft;
42 double range_nm = max_range_nm;
44 // kludge slightly better reception at
46 if (delta_elevation_ft < 0)
47 delta_elevation_ft = 200;
48 if (delta_elevation_ft <= 1000)
49 range_nm *= sqrt(delta_elevation_ft / 1000);
50 else if (delta_elevation_ft >= 5000)
51 range_nm *= sqrt(delta_elevation_ft / 5000);
52 if (range_nm >= max_range_nm * 3)
53 range_nm = max_range_nm * 3;
55 double rand = sg_random();
56 return range_nm + (range_nm * rand * rand);
60 ADF::ADF (SGPropertyNode *node )
62 _name(node->getStringValue("name", "adf")),
63 _num(node->getIntValue("number", 0)),
64 _time_before_search_sec(0),
65 _last_frequency_khz(-1),
66 _transmitter_valid(false),
67 _transmitter_pos(SGGeod::fromDeg(0, 0)),
68 _transmitter_cart(0, 0, 0),
69 _transmitter_range_nm(0),
84 branch = "/instrumentation/" + _name;
86 SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
87 _longitude_node = fgGetNode("/position/longitude-deg", true);
88 _latitude_node = fgGetNode("/position/latitude-deg", true);
89 _altitude_node = fgGetNode("/position/altitude-ft", true);
90 _heading_node = fgGetNode("/orientation/heading-deg", true);
91 _serviceable_node = node->getChild("serviceable", 0, true);
92 _error_node = node->getChild("error-deg", 0, true);
93 _electrical_node = fgGetNode("/systems/electrical/outputs/adf", true);
94 branch = branch + "/frequencies";
95 SGPropertyNode *fnode = node->getChild("frequencies", 0, true);
96 _frequency_node = fnode->getChild("selected-khz", 0, true);
97 _mode_node = node->getChild("mode", 0, true);
98 _volume_node = node->getChild("volume-norm", 0, true);
99 _in_range_node = node->getChild("in-range", 0, true);
100 _bearing_node = node->getChild("indicated-bearing-deg", 0, true);
101 _ident_node = node->getChild("ident", 0, true);
102 _ident_audible_node = node->getChild("ident-audible", 0, true);
106 std::ostringstream temp;
107 temp << _name << _num;
108 _adf_ident = temp.str();
112 ADF::update (double delta_time_sec)
114 // If it's off, don't waste any time.
115 if (_electrical_node->getDoubleValue() < 1.0
116 || !_serviceable_node->getBoolValue()) {
117 set_bearing(delta_time_sec, 90);
118 _ident_node->setStringValue("");
123 int frequency_khz = _frequency_node->getIntValue();
124 if (frequency_khz != _last_frequency_khz) {
125 _time_before_search_sec = 0;
126 _last_frequency_khz = frequency_khz;
129 // Get the aircraft position
130 double longitude_deg = _longitude_node->getDoubleValue();
131 double latitude_deg = _latitude_node->getDoubleValue();
132 double altitude_m = _altitude_node->getDoubleValue();
134 double longitude_rad = longitude_deg * SGD_DEGREES_TO_RADIANS;
135 double latitude_rad = latitude_deg * SGD_DEGREES_TO_RADIANS;
137 // On timeout, scan again
138 _time_before_search_sec -= delta_time_sec;
139 if (_time_before_search_sec < 0)
140 search(frequency_khz, longitude_rad, latitude_rad, altitude_m);
142 // If it's off, don't bother.
143 string mode = _mode_node->getStringValue();
144 if (!_transmitter_valid || (mode != "bfo" && mode != "adf"))
146 set_bearing(delta_time_sec, 90);
147 _ident_node->setStringValue("");
151 // Calculate the bearing to the transmitter
152 SGGeod geod = SGGeod::fromRadM(longitude_rad, latitude_rad, altitude_m);
153 SGVec3d location = SGVec3d::fromGeod(geod);
155 double distance_nm = dist(_transmitter_cart, location) * SG_METER_TO_NM;
156 double range_nm = adjust_range(_transmitter_pos.getElevationFt(),
157 altitude_m * SG_METER_TO_FEET,
158 _transmitter_range_nm);
159 if (distance_nm <= range_nm) {
161 double bearing, az2, s;
162 double heading = _heading_node->getDoubleValue();
164 geo_inverse_wgs_84(geod, _transmitter_pos,
166 _in_range_node->setBoolValue(true);
171 set_bearing(delta_time_sec, bearing);
175 if ( _ident_audible_node->getBoolValue() )
176 volume = _volume_node->getDoubleValue();
180 if ( volume != _last_volume ) {
181 _last_volume = volume;
183 SGSoundSample *sound;
184 sound = globals->get_soundmgr()->find( _adf_ident );
186 sound->set_volume( volume );
188 SG_LOG( SG_GENERAL, SG_ALERT, "Can't find adf-ident sound" );
191 time_t cur_time = globals->get_time_params()->get_cur_time();
192 if ( _last_ident_time < cur_time - 30 ) {
193 _last_ident_time = cur_time;
197 if ( _ident_count < 4 ) {
198 if ( !globals->get_soundmgr()->is_playing(_adf_ident) ) {
199 globals->get_soundmgr()->play_once( _adf_ident );
204 _in_range_node->setBoolValue(false);
205 set_bearing(delta_time_sec, 90);
206 _ident_node->setStringValue("");
207 globals->get_soundmgr()->stop( _adf_ident );
212 ADF::search (double frequency_khz, double longitude_rad,
213 double latitude_rad, double altitude_m)
217 _time_before_search_sec = 1.0;
219 // try the ILS list first
221 globals->get_navlist()->findByFreq(frequency_khz, longitude_rad,
222 latitude_rad, altitude_m);
224 _transmitter_valid = (nav != NULL);
225 if ( _transmitter_valid ) {
226 ident = nav->get_trans_ident();
227 if ( ident != _last_ident ) {
228 _transmitter_pos = nav->get_pos();
229 _transmitter_cart = nav->get_cart();
230 _transmitter_range_nm = nav->get_range();
234 if ( _last_ident != ident ) {
236 _ident_node->setStringValue(ident.c_str());
238 if ( globals->get_soundmgr()->exists( _adf_ident ) ) {
239 // stop is required! -- remove alone wouldn't stop immediately
240 globals->get_soundmgr()->stop( _adf_ident );
241 globals->get_soundmgr()->remove( _adf_ident );
244 SGSoundSample *sound;
245 sound = morse.make_ident( ident, LO_FREQUENCY );
246 sound->set_volume(_last_volume = 0);
247 globals->get_soundmgr()->add( sound, _adf_ident );
249 int offset = (int)(sg_random() * 30.0);
250 _ident_count = offset / 4;
251 _last_ident_time = globals->get_time_params()->get_cur_time() -
257 ADF::set_bearing (double dt, double bearing_deg)
259 double old_bearing_deg = _bearing_node->getDoubleValue();
261 while ((bearing_deg - old_bearing_deg) >= 180)
262 old_bearing_deg += 360;
263 while ((bearing_deg - old_bearing_deg) <= -180)
264 old_bearing_deg -= 360;
265 bearing_deg += _error_node->getDoubleValue();
267 fgGetLowPass(old_bearing_deg, bearing_deg, dt * RESPONSIVENESS);
269 _bearing_node->setDoubleValue(bearing_deg);