1 // adf.cxx - distance-measuring equipment.
2 // Written by David Megginson, started 2003.
3 //
4 // This file is in the Public Domain and comes with no warranty.
5
6 #ifdef HAVE_CONFIG_H
7 # include <config.h>
8 #endif
9
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>
14
15 #include <Main/fg_props.hxx>
16 #include <Main/util.hxx>
17 #include <Navaids/navlist.hxx>
18
19 #include "adf.hxx"
20 #include <Sound/morse.hxx>
21 #include <simgear/sound/sample_group.hxx>
22
23 #include <iostream>
24 #include <string>
25 #include <sstream>
26
27 using std::string;
28
29 // Use a bigger number to be more responsive, or a smaller number
30 // to be more sluggish.
31 #define RESPONSIVENESS 0.5
32
33
34 /**
35 * Fiddle with the reception range a bit.
36 *
37 * TODO: better reception at night (??).
38 */
39 static double
adjust_range(double transmitter_elevation_ft,double aircraft_altitude_ft,double max_range_nm)40 adjust_range (double transmitter_elevation_ft, double aircraft_altitude_ft,
41 double max_range_nm)
42 {
43 double delta_elevation_ft =
44 aircraft_altitude_ft - transmitter_elevation_ft;
45 double range_nm = max_range_nm;
46
47 // kludge slightly better reception at
48 // altitude
49 if (delta_elevation_ft < 0)
50 delta_elevation_ft = 200;
51 if (delta_elevation_ft <= 1000)
52 range_nm *= sqrt(delta_elevation_ft / 1000);
53 else if (delta_elevation_ft >= 5000)
54 range_nm *= sqrt(delta_elevation_ft / 5000);
55 if (range_nm >= max_range_nm * 3)
56 range_nm = max_range_nm * 3;
57
58 double rand = sg_random();
59 return range_nm + (range_nm * rand * rand);
60 }
61
62
ADF(SGPropertyNode * node)63 ADF::ADF (SGPropertyNode *node )
64 :
65 _time_before_search_sec(0),
66 _last_frequency_khz(-1),
67 _transmitter_valid(false),
68 _transmitter_pos(SGGeod::fromDeg(0, 0)),
69 _transmitter_cart(0, 0, 0),
70 _transmitter_range_nm(0),
71 _ident_count(0),
72 _last_ident_time(0),
73 _last_volume(-1),
74 _sgr(0)
75 {
76 readConfig(node, "adf");
77 }
78
~ADF()79 ADF::~ADF ()
80 {
81 }
82
83 void
init()84 ADF::init ()
85 {
86 string branch = nodePath();
87 SGPropertyNode *node = fgGetNode(branch.c_str(), true );
88
89 initServicePowerProperties(node);
90
91 // instrument properties
92 _error_node = node->getChild("error-deg", 0, true);
93 _mode_node = node->getChild("mode", 0, true);
94 _volume_node = node->getChild("volume-norm", 0, true);
95 _in_range_node = node->getChild("in-range", 0, true);
96 _bearing_node = node->getChild("indicated-bearing-deg", 0, true);
97 _ident_node = node->getChild("ident", 0, true);
98 _ident_audible_node = node->getChild("ident-audible", 0, true);
99
100 // frequency properties
101 SGPropertyNode *fnode = node->getChild("frequencies", 0, true);
102 _frequency_node = fnode->getChild("selected-khz", 0, true);
103
104 // foreign simulator properties
105 _heading_node = fgGetNode("/orientation/heading-deg", true);
106
107 // sound support (audible ident code)
108 SGSoundMgr *smgr = globals->get_subsystem<SGSoundMgr>();
109 _sgr = smgr->find("avionics", true);
110 _sgr->tie_to_listener();
111
112 std::ostringstream temp;
113 temp << name() << number();
114 _adf_ident = temp.str();
115 }
116
117 void
update(double delta_time_sec)118 ADF::update (double delta_time_sec)
119 {
120 // If it's off, don't waste any time.
121 if (!isServiceableAndPowered()) {
122 _in_range_node->setBoolValue(false);
123 _ident_node->setStringValue("");
124 return;
125 }
126
127 string mode = _mode_node->getStringValue();
128 if (mode == "ant" || mode == "test") set_bearing(delta_time_sec, 90);
129 if (mode != "bfo" && mode != "adf") {
130 _in_range_node->setBoolValue(false);
131 _ident_node->setStringValue("");
132 return;
133 }
134 // Get the frequency
135 int frequency_khz = _frequency_node->getIntValue();
136 if (frequency_khz != _last_frequency_khz) {
137 _time_before_search_sec = 0;
138 _last_frequency_khz = frequency_khz;
139 }
140
141 SGGeod acPos(globals->get_aircraft_position());
142
143 // On timeout, scan again
144 _time_before_search_sec -= delta_time_sec;
145 if (_time_before_search_sec < 0)
146 search(frequency_khz, acPos);
147
148 if (!_transmitter_valid) {
149 _in_range_node->setBoolValue(false);
150 _ident_node->setStringValue("");
151 return;
152 }
153
154 // Calculate the bearing to the transmitter
155 SGVec3d location = globals->get_aircraft_position_cart();
156
157 double distance_nm = dist(_transmitter_cart, location) * SG_METER_TO_NM;
158 double range_nm = adjust_range(_transmitter_pos.getElevationFt(),
159 acPos.getElevationFt(),
160 _transmitter_range_nm);
161
162 if (distance_nm <= range_nm) {
163
164 double bearing, az2, s;
165 double heading = _heading_node->getDoubleValue();
166
167 geo_inverse_wgs_84(acPos, _transmitter_pos,
168 &bearing, &az2, &s);
169 _in_range_node->setBoolValue(true);
170 _ident_node->setStringValue(_last_ident);
171
172 bearing -= heading;
173 if (bearing < 0)
174 bearing += 360;
175 set_bearing(delta_time_sec, bearing);
176
177 // adf ident sound
178 float volume;
179 if ( _ident_audible_node->getBoolValue() )
180 volume = _volume_node->getFloatValue();
181 else
182 volume = 0.0;
183
184 if ( volume != _last_volume ) {
185 _last_volume = volume;
186
187 SGSoundSample *sound;
188 sound = _sgr->find( _adf_ident );
189 if ( sound != NULL )
190 sound->set_volume( volume );
191 else
192 SG_LOG( SG_INSTR, SG_ALERT, "Can't find adf-ident sound" );
193 }
194
195 time_t cur_time = globals->get_time_params()->get_cur_time();
196 if ( _last_ident_time < cur_time - 30 ) {
197 _last_ident_time = cur_time;
198 _ident_count = 0;
199 }
200
201 if ( _ident_count < 4 ) {
202 if ( !_sgr->is_playing(_adf_ident) && (volume > 0.05) ) {
203 _sgr->play_once( _adf_ident );
204 ++_ident_count;
205 }
206 }
207 } else {
208 _in_range_node->setBoolValue(false);
209 _ident_node->setStringValue("");
210 _sgr->stop( _adf_ident );
211 }
212 }
213
214 void
search(double frequency_khz,const SGGeod & pos)215 ADF::search (double frequency_khz, const SGGeod& pos)
216 {
217 string ident = "";
218 // reset search time
219 _time_before_search_sec = 1.0;
220
221 FGNavList::TypeFilter filter(FGPositioned::NDB);
222 FGNavRecord *nav = FGNavList::findByFreq(frequency_khz, pos, &filter);
223
224 _transmitter_valid = (nav != NULL);
225 if ( _transmitter_valid ) {
226 ident = nav->get_trans_ident();
227 if ( ident != _last_ident ) {
228 _transmitter_pos = nav->geod();
229 _transmitter_cart = nav->cart();
230 _transmitter_range_nm = nav->get_range();
231 }
232 }
233
234 if ( _last_ident != ident ) {
235 _last_ident = ident;
236 _ident_node->setStringValue(ident.c_str());
237
238 if ( _sgr->exists( _adf_ident ) ) {
239 // stop is required! -- remove alone wouldn't stop immediately
240 _sgr->stop( _adf_ident );
241 _sgr->remove( _adf_ident );
242 }
243
244 SGSoundSample *sound;
245 sound = FGMorse::instance()->make_ident( ident, FGMorse::LO_FREQUENCY );
246 sound->set_volume(_last_volume = 0);
247 _sgr->add( sound, _adf_ident );
248
249 int offset = (int)(sg_random() * 30.0);
250 _ident_count = offset / 4;
251 _last_ident_time = globals->get_time_params()->get_cur_time() -
252 offset;
253 }
254 }
255
256 void
set_bearing(double dt,double bearing_deg)257 ADF::set_bearing (double dt, double bearing_deg)
258 {
259 double old_bearing_deg = _bearing_node->getDoubleValue();
260
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();
266 bearing_deg =
267 fgGetLowPass(old_bearing_deg, bearing_deg, dt * RESPONSIVENESS);
268
269 _bearing_node->setDoubleValue(bearing_deg);
270 }
271
272
273 // Register the subsystem.
274 #if 0
275 SGSubsystemMgr::InstancedRegistrant<ADF> registrantADF(
276 SGSubsystemMgr::FDM,
277 {{"instrumentation", SGSubsystemMgr::Dependency::HARD}},
278 0.15);
279 #endif
280
281 // end of adf.cxx
282