]> git.mxchange.org Git - flightgear.git/blob - src/Instrumentation/adf.cxx
Melchior FRANZ:
[flightgear.git] / src / Instrumentation / adf.cxx
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 #include <simgear/compiler.h>
7 #include <simgear/math/sg_geodesy.hxx>
8 #include <simgear/math/sg_random.h>
9
10 #include <Main/fg_props.hxx>
11 #include <Main/util.hxx>
12 #include <Navaids/navlist.hxx>
13
14 #include "adf.hxx"
15
16 #include <iostream>
17 #include <string>
18 #include <sstream>
19
20
21 // Use a bigger number to be more responsive, or a smaller number
22 // to be more sluggish.
23 #define RESPONSIVENESS 0.5
24
25
26 /**
27  * Fiddle with the reception range a bit.
28  *
29  * TODO: better reception at night (??).
30  */
31 static double
32 adjust_range (double transmitter_elevation_ft, double aircraft_altitude_ft,
33               double max_range_nm)
34 {
35     double delta_elevation_ft =
36         aircraft_altitude_ft - transmitter_elevation_ft;
37     double range_nm = max_range_nm;
38
39                                 // kludge slightly better reception at
40                                 // altitude
41     if (delta_elevation_ft < 0)
42         delta_elevation_ft = 200;
43     if (delta_elevation_ft <= 1000)
44         range_nm *= sqrt(delta_elevation_ft / 1000);
45     else if (delta_elevation_ft >= 5000)
46         range_nm *= sqrt(delta_elevation_ft / 5000);
47     if (range_nm >= max_range_nm * 3)
48         range_nm = max_range_nm * 3;
49
50     double rand = sg_random();
51     return range_nm + (range_nm * rand * rand);
52 }
53
54
55 ADF::ADF (SGPropertyNode *node )
56     :
57     _time_before_search_sec(0),
58     _last_frequency_khz(-1),
59     _transmitter_valid(false),
60     _transmitter_elevation_ft(0),
61     _transmitter_range_nm(0),
62     _ident_count(0),
63     _last_ident_time(0),
64     _last_volume(-1),
65     name("adf"),
66     num(0)
67 {
68     int i;
69     for ( i = 0; i < node->nChildren(); ++i ) {
70         SGPropertyNode *child = node->getChild(i);
71         string cname = child->getName();
72         string cval = child->getStringValue();
73         if ( cname == "name" ) {
74             name = cval;
75         } else if ( cname == "number" ) {
76             num = child->getIntValue();
77         } else {
78             SG_LOG( SG_INSTR, SG_WARN, "Error in adf config logic" );
79             if ( name.length() ) {
80                 SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
81             }
82         }
83     }
84 }
85
86
87 ADF::ADF ()
88     : _time_before_search_sec(0),
89       _last_frequency_khz(-1),
90       _transmitter_valid(false),
91       _transmitter_elevation_ft(0),
92       _transmitter_range_nm(0),
93       _ident_count(0),
94       _last_ident_time(0),
95       _last_volume(-1),
96       name("adf"),
97       num(0)
98 {
99 }
100
101 ADF::~ADF ()
102 {
103 }
104
105 void
106 ADF::init ()
107 {
108     string branch;
109     branch = "/instrumentation/" + name;
110
111     SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
112     _longitude_node = fgGetNode("/position/longitude-deg", true);
113     _latitude_node = fgGetNode("/position/latitude-deg", true);
114     _altitude_node = fgGetNode("/position/altitude-ft", true);
115     _heading_node = fgGetNode("/orientation/heading-deg", true);
116     _serviceable_node = node->getChild("serviceable", 0, true);
117     _error_node = node->getChild("error-deg", 0, true);
118     _electrical_node = fgGetNode("/systems/electrical/outputs/adf", true);
119     branch = branch + "/frequencies";
120     SGPropertyNode *fnode = node->getChild("frequencies", 0, true);
121     _frequency_node = fnode->getChild("selected-khz", 0, true);
122     _mode_node = node->getChild("mode", 0, true);
123     _volume_node = node->getChild("volume-norm", 0, true);
124     _in_range_node = node->getChild("in-range", 0, true);
125     _bearing_node = node->getChild("indicated-bearing-deg", 0, true);
126     _ident_node = node->getChild("ident", 0, true);
127     _ident_audible_node = node->getChild("ident-audible", 0, true);
128
129     morse.init();
130
131     std::ostringstream temp;
132     temp << name << num;
133     adf_ident = temp.str();
134 }
135
136 void
137 ADF::update (double delta_time_sec)
138 {
139                                 // If it's off, don't waste any time.
140     if (!_electrical_node->getBoolValue() ||
141         !_serviceable_node->getBoolValue()) {
142         set_bearing(delta_time_sec, 90);
143         _ident_node->setStringValue("");
144         return;
145     }
146
147                                 // Get the frequency
148     int frequency_khz = _frequency_node->getIntValue();
149     if (frequency_khz != _last_frequency_khz) {
150         _time_before_search_sec = 0;
151         _last_frequency_khz = frequency_khz;
152     }
153
154                                 // Get the aircraft position
155     double longitude_deg = _longitude_node->getDoubleValue();
156     double latitude_deg = _latitude_node->getDoubleValue();
157     double altitude_m = _altitude_node->getDoubleValue();
158
159     double longitude_rad = longitude_deg * SGD_DEGREES_TO_RADIANS;
160     double latitude_rad = latitude_deg * SGD_DEGREES_TO_RADIANS;
161
162                                 // On timeout, scan again
163     _time_before_search_sec -= delta_time_sec;
164     if (_time_before_search_sec < 0)
165         search(frequency_khz, longitude_rad, latitude_rad, altitude_m);
166
167                                 // If it's off, don't bother.
168     string mode = _mode_node->getStringValue();
169     if (!_transmitter_valid || (mode != "bfo" && mode != "adf"))
170     {
171         set_bearing(delta_time_sec, 90);
172         _ident_node->setStringValue("");
173         return;
174     }
175
176                                 // Calculate the bearing to the transmitter
177     Point3D location =
178         sgGeodToCart(Point3D(longitude_rad, latitude_rad, altitude_m));
179
180     double distance_nm = _transmitter.distance3D(location) * SG_METER_TO_NM;
181     double range_nm = adjust_range(_transmitter_elevation_ft,
182                                    altitude_m * SG_METER_TO_FEET,
183                                    _transmitter_range_nm);
184     if (distance_nm <= range_nm) {
185
186         double bearing, az2, s;
187         double heading = _heading_node->getDoubleValue();
188
189         geo_inverse_wgs_84(altitude_m,
190                            latitude_deg,
191                            longitude_deg,
192                            _transmitter_lat_deg,
193                            _transmitter_lon_deg,
194                            &bearing, &az2, &s);
195         _in_range_node->setBoolValue(true);
196
197         bearing -= heading;
198         if (bearing < 0)
199             bearing += 360;
200         set_bearing(delta_time_sec, bearing);
201
202         // adf ident sound
203         double volume;
204         if ( _ident_audible_node->getBoolValue() )
205             volume = _volume_node->getDoubleValue();
206         else
207             volume = 0.0;
208
209         if ( volume != _last_volume ) {
210             _last_volume = volume;
211
212             SGSoundSample *sound;
213             sound = globals->get_soundmgr()->find( adf_ident );
214             if ( sound != NULL )
215                 sound->set_volume( volume );
216             else
217                 SG_LOG( SG_GENERAL, SG_ALERT, "Can't find adf-ident sound" );
218         }
219
220         time_t cur_time = globals->get_time_params()->get_cur_time();
221         if ( _last_ident_time < cur_time - 30 ) {
222             _last_ident_time = cur_time;
223             _ident_count = 0;
224         }
225
226         if ( _ident_count < 4 ) {
227             if ( !globals->get_soundmgr()->is_playing(adf_ident) ) {
228                 globals->get_soundmgr()->play_once( adf_ident );
229                 ++_ident_count;
230             }
231         }
232     } else {
233         _in_range_node->setBoolValue(false);
234         set_bearing(delta_time_sec, 90);
235         _ident_node->setStringValue("");
236         globals->get_soundmgr()->stop( adf_ident );
237     }
238 }
239
240 void
241 ADF::search (double frequency_khz, double longitude_rad,
242              double latitude_rad, double altitude_m)
243 {
244     string ident = "";
245                                 // reset search time
246     _time_before_search_sec = 1.0;
247
248                                 // try the ILS list first
249     FGNavRecord *nav =
250         globals->get_navlist()->findByFreq(frequency_khz, longitude_rad,
251                                            latitude_rad, altitude_m);
252
253     _transmitter_valid = (nav != NULL);
254     if ( _transmitter_valid ) {
255         ident = nav->get_trans_ident();
256         if ( ident != _last_ident ) {
257             _transmitter_lon_deg = nav->get_lon();
258             _transmitter_lat_deg = nav->get_lat();
259             _transmitter = Point3D(nav->get_x(), nav->get_y(), nav->get_z());
260             _transmitter_elevation_ft = nav->get_elev_ft();
261             _transmitter_range_nm = nav->get_range();
262         }
263     }
264
265     if ( _last_ident != ident ) {
266         _last_ident = ident;
267         _ident_node->setStringValue(ident.c_str());
268
269         if ( globals->get_soundmgr()->exists( adf_ident ) ) {
270             // stop is required! -- remove alone wouldn't stop immediately
271             globals->get_soundmgr()->stop( adf_ident );
272             globals->get_soundmgr()->remove( adf_ident );
273         }
274
275         SGSoundSample *sound;
276         sound = morse.make_ident( ident, LO_FREQUENCY );
277         sound->set_volume(_last_volume = 0);
278         globals->get_soundmgr()->add( sound, adf_ident );
279
280         int offset = (int)(sg_random() * 30.0);
281         _ident_count = offset / 4;
282         _last_ident_time = globals->get_time_params()->get_cur_time() -
283             offset;
284     }
285 }
286
287 void
288 ADF::set_bearing (double dt, double bearing_deg)
289 {
290     double old_bearing_deg = _bearing_node->getDoubleValue();
291
292     while ((bearing_deg - old_bearing_deg) >= 180)
293         old_bearing_deg += 360;
294     while ((bearing_deg - old_bearing_deg) <= -180)
295         old_bearing_deg -= 360;
296     bearing_deg += _error_node->getDoubleValue();
297     bearing_deg =
298         fgGetLowPass(old_bearing_deg, bearing_deg, dt * RESPONSIVENESS);
299
300     _bearing_node->setDoubleValue(bearing_deg);
301 }
302
303
304 // end of adf.cxx