]> git.mxchange.org Git - flightgear.git/blob - src/Instrumentation/tacan.cxx
4bde59c9cb3f45a9d58f2aa93a6f1ea8273706d9
[flightgear.git] / src / Instrumentation / tacan.cxx
1 // tacan.cxx - Tactical Navigation Beacon.
2 // Written by Vivian Meazaa, started 2005.
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
14 #include <Main/fg_props.hxx>
15 #include <Navaids/navlist.hxx>
16 #include <vector>
17
18 #include "tacan.hxx"
19
20 SG_USING_STD(vector);
21
22
23 /**
24  * Adjust the range.
25  *
26  * Start by calculating the radar horizon based on the elevation
27  * difference, then clamp to the maximum, then add a fudge for
28  * borderline reception.
29  */
30 static double
31 adjust_range (double transmitter_elevation_ft, double aircraft_altitude_ft,
32               double max_range_nm)
33 {
34     max_range_nm = 150;
35     double delta_elevation_ft =
36         fabs(aircraft_altitude_ft - transmitter_elevation_ft);
37     double range_nm = 1.23 * sqrt(delta_elevation_ft);
38     if (range_nm > max_range_nm)
39         range_nm = max_range_nm;
40     else if (range_nm < 20.0)
41         range_nm = 20.0;
42     double rand = sg_random();
43     SG_LOG( SG_INSTR, SG_DEBUG, " tacan range " << range_nm << " max range " << max_range_nm);
44     return range_nm + (range_nm * rand * rand);
45 }
46
47
48 TACAN::TACAN ( SGPropertyNode *node ) :
49     _name(node->getStringValue("name", "tacan")),
50     _num(node->getIntValue("number", 0)),
51     _new_frequency(false),
52     _channel("0000"),
53     _last_distance_nm(0),
54     _frequency_mhz(-1),
55     _time_before_search_sec(0),
56     _mobile_valid(false),
57     _transmitter_valid(false),
58     _transmitter_pos(SGGeod::fromDeg(0, 0)),
59     _transmitter_range_nm(0),
60     _transmitter_bias(0.0),
61     _listener_active(0)
62 {
63 }
64
65 TACAN::~TACAN ()
66 {
67 }
68
69 void
70 TACAN::init ()
71 {
72     string branch;
73     branch = "/instrumentation/" + _name;
74
75     SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
76
77     _serviceable_node = node->getChild("serviceable", 0, true);
78     _ident_node = node->getChild("ident", 0, true);
79
80     SGPropertyNode *fnode = node->getChild("frequencies", 0, true);
81     _frequency_node = fnode->getChild("selected-mhz", 0, true);
82
83     _channel_in0_node = fnode->getChild("selected-channel", 0, true);
84     _channel_in1_node = fnode->getChild("selected-channel", 1, true);
85     _channel_in2_node = fnode->getChild("selected-channel", 2, true);
86     _channel_in3_node = fnode->getChild("selected-channel", 3, true);
87     _channel_in4_node = fnode->getChild("selected-channel", 4, true);
88
89     _channel_in0_node->addChangeListener(this);
90     _channel_in1_node->addChangeListener(this);
91     _channel_in2_node->addChangeListener(this);
92     _channel_in3_node->addChangeListener(this);
93     _channel_in4_node->addChangeListener(this, true);
94
95     _in_range_node = node->getChild("in-range", 0, true);
96     _distance_node = node->getChild("indicated-distance-nm", 0, true);
97     _speed_node = node->getChild("indicated-ground-speed-kt", 0, true);
98     _time_node = node->getChild("indicated-time-min", 0, true);
99     _name_node = node->getChild("name", 0, true);
100     _bearing_node = node->getChild("indicated-bearing-true-deg", 0, true);
101
102     SGPropertyNode *dnode = node->getChild("display", 0, true);
103     _x_shift_node = dnode->getChild("x-shift", 0, true);
104     _y_shift_node = dnode->getChild("y-shift", 0, true);
105     _rotation_node = dnode->getChild("rotation", 0, true);
106     _channel_node = dnode->getChild("channel", 0, true);
107
108     SGPropertyNode *cnode = fgGetNode("/ai/models/carrier", _num, false );
109     _carrier_name_node = cnode ? cnode->getChild("name", 0, false) : 0;
110
111     SGPropertyNode *tnode = fgGetNode("/ai/models/aircraft", _num, false);
112     _tanker_callsign_node = tnode ? tnode->getChild("callsign", 0, false) : 0;
113
114     SGPropertyNode *mnode = fgGetNode("/ai/models/multiplayer", _num, false);
115     _mp_callsign_node = mnode ? mnode->getChild("callsign", 0, false) : 0;
116
117     _longitude_node = fgGetNode("/position/longitude-deg", true);
118     _latitude_node = fgGetNode("/position/latitude-deg", true);
119     _altitude_node = fgGetNode("/position/altitude-ft", true);
120     _heading_node = fgGetNode("/orientation/heading-deg", true);
121     _yaw_node = fgGetNode("/orientation/side-slip-deg", true);
122     _electrical_node = fgGetNode("/systems/electrical/outputs/tacan", true);
123 }
124
125 void
126 TACAN::update (double delta_time_sec)
127 {
128     if (!_serviceable_node->getBoolValue() || !_electrical_node->getBoolValue()) {
129         _last_distance_nm = 0;
130         _in_range_node->setBoolValue(false);
131         _distance_node->setDoubleValue(0);
132         _speed_node->setDoubleValue(0);
133         _time_node->setDoubleValue(0);
134         SG_LOG( SG_INSTR, SG_DEBUG, "skip tacan" );
135         return;
136     }
137
138                                 // Get the aircraft position
139     double longitude_deg = _longitude_node->getDoubleValue();
140     double latitude_deg  = _latitude_node->getDoubleValue();
141     double altitude_m    = _altitude_node->getDoubleValue() * SG_FEET_TO_METER;
142     double longitude_rad = longitude_deg * SGD_DEGREES_TO_RADIANS;
143     double latitude_rad  = latitude_deg * SGD_DEGREES_TO_RADIANS;
144
145                                 // On timeout, scan again
146     _time_before_search_sec -= delta_time_sec;
147     if ((_time_before_search_sec < 0 || _new_frequency) && _frequency_mhz >= 0)
148         search(_frequency_mhz, longitude_rad, latitude_rad, altitude_m);
149
150                                  // Calculate the distance to the transmitter
151
152     //calculate the bearing and range of the mobile from the aircraft
153     double mobile_az2 = 0;
154     double mobile_bearing = 0;
155     double mobile_distance = 0;
156
157     SG_LOG( SG_INSTR, SG_DEBUG, "mobile_lat " << _mobile_lat);
158     SG_LOG( SG_INSTR, SG_DEBUG, "mobile_lon " << _mobile_lon);
159     SG_LOG( SG_INSTR, SG_DEBUG, "mobile_name " << _mobile_name);
160     SG_LOG( SG_INSTR, SG_DEBUG, "mobile_valid " << _mobile_valid);
161     geo_inverse_wgs_84(altitude_m,
162                        latitude_deg,
163                        longitude_deg,
164                        _mobile_lat,
165                        _mobile_lon,
166                        &mobile_bearing, &mobile_az2, &mobile_distance);
167
168
169     //calculate the bearing and range of the station from the aircraft
170     double az2 = 0;
171     double bearing = 0;
172     double distance = 0;
173
174     SGGeod pos = SGGeod::fromDegM(longitude_deg, latitude_deg, altitude_m);
175     geo_inverse_wgs_84(pos, _transmitter_pos,
176                        &bearing, &az2, &distance);
177
178
179     //select the nearer
180     if ( mobile_distance <= distance && _mobile_valid) {
181         SG_LOG( SG_INSTR, SG_DEBUG, "mobile_distance_m " << mobile_distance);
182         SG_LOG( SG_INSTR, SG_DEBUG, "distance_m " << distance);
183         bearing = mobile_bearing;
184         distance = mobile_distance;
185         _transmitter_pos.setElevationFt(_mobile_elevation_ft);
186         _transmitter_range_nm = _mobile_range_nm;
187         _transmitter_bias = _mobile_bias;
188         _transmitter_name = _mobile_name;
189         _name_node->setStringValue(_transmitter_name.c_str());
190         _transmitter_ident = _mobile_ident;
191         _ident_node->setStringValue(_transmitter_ident.c_str());
192         _channel_node->setStringValue(_channel.c_str());
193     }
194
195     //// calculate some values for boresight display
196     double distance_nm = distance * SG_METER_TO_NM;
197
198     //// calculate look left/right to target, without yaw correction
199     // double horiz_offset = bearing - heading;
200     //
201     // if (horiz_offset > 180.0) horiz_offset -= 360.0;
202     // if (horiz_offset < -180.0) horiz_offset += 360.0;
203
204     //// now correct look left/right for yaw
205     // horiz_offset += yaw;
206
207     // use the bearing for a plan position indicator display
208
209     double horiz_offset = bearing;
210
211     SG_LOG( SG_INSTR, SG_DEBUG, "distance_nm " << distance_nm << " bearing "
212             << bearing << " horiz_offset " << horiz_offset);
213
214     // calculate values for radar display
215     double y_shift = distance_nm * cos( horiz_offset * SG_DEGREES_TO_RADIANS);
216     double x_shift = distance_nm * sin( horiz_offset * SG_DEGREES_TO_RADIANS);
217
218     SG_LOG( SG_INSTR, SG_DEBUG, "y_shift " << y_shift  << " x_shift " << x_shift);
219
220     double rotation = 0;
221
222     double range_nm = adjust_range(_transmitter_pos.getElevationFt(),
223                                    altitude_m * SG_METER_TO_FEET,
224                                    _transmitter_range_nm);
225
226     if (distance_nm <= range_nm) {
227         double speed_kt = (fabs(distance_nm - _last_distance_nm) *
228                            ((1 / delta_time_sec) * 3600.0));
229         _last_distance_nm = distance_nm;
230
231         _in_range_node->setBoolValue(true);
232         double tmp_dist = distance_nm - _transmitter_bias;
233         if ( tmp_dist < 0.0 ) {
234             tmp_dist = 0.0;
235         }
236         _distance_node->setDoubleValue( tmp_dist );
237         _speed_node->setDoubleValue(speed_kt);
238         _time_node->setDoubleValue(distance_nm/speed_kt*60.0);
239         _bearing_node->setDoubleValue(bearing);
240         _x_shift_node->setDoubleValue(x_shift);
241         _y_shift_node->setDoubleValue(y_shift);
242         _rotation_node->setDoubleValue(rotation);
243     } else {
244         _last_distance_nm = 0;
245         _in_range_node->setBoolValue(false);
246         _distance_node->setDoubleValue(0);
247         _speed_node->setDoubleValue(0);
248         _time_node->setDoubleValue(0);
249         _bearing_node->setDoubleValue(0);
250         _x_shift_node->setDoubleValue(0);
251         _y_shift_node->setDoubleValue(0);
252         _rotation_node->setDoubleValue(0);
253     }
254
255                                 // If we can't find a valid station set everything to zero
256     if (!_transmitter_valid && !_mobile_valid ) {
257         _in_range_node->setBoolValue(false);
258         _distance_node->setDoubleValue(0);
259         _speed_node->setDoubleValue(0);
260         _time_node->setDoubleValue(0);
261         _bearing_node->setDoubleValue(0);
262         _x_shift_node->setDoubleValue(0);
263         _y_shift_node->setDoubleValue(0);
264         _rotation_node->setDoubleValue(0);
265         _transmitter_name = "";
266         _name_node->setStringValue(_transmitter_name.c_str());
267         _transmitter_ident = "";
268         _ident_node->setStringValue(_transmitter_ident.c_str());
269         _channel_node->setStringValue(_channel.c_str());
270         return;
271     }
272 } // end function update
273
274 void
275 TACAN::search (double frequency_mhz, double longitude_rad,
276                double latitude_rad, double altitude_m)
277 {
278     int number, i;
279     _mobile_valid = false;
280
281     SG_LOG( SG_INSTR, SG_DEBUG, "tacan freq " << frequency_mhz );
282
283     // reset search time
284     _time_before_search_sec = 1.0;
285
286     //try any carriers first
287     FGNavRecord *mobile_tacan
288           = globals->get_carrierlist()->findStationByFreq( frequency_mhz );
289     bool freq_valid = (mobile_tacan != NULL);
290     SG_LOG( SG_INSTR, SG_DEBUG, "mobile freqency valid " << freq_valid );
291
292     if ( freq_valid ) {
293
294         string str1( mobile_tacan->get_name() );
295
296         SGPropertyNode * branch = fgGetNode("ai/models", true);
297         vector<SGPropertyNode_ptr> carrier = branch->getChildren("carrier");
298
299         number = carrier.size();
300
301         SG_LOG( SG_INSTR, SG_DEBUG, "carrier " << number );
302         if ( number > 0 ) {       // don't do this if there are no carriers
303             for ( i = 0; i < number; ++i ) {
304                 string str2 ( carrier[i]->getStringValue("name", ""));
305                 SG_LOG( SG_INSTR, SG_DEBUG, "carrier name " << str2 );
306
307                 SG_LOG( SG_INSTR, SG_DEBUG, "strings 1 " << str1 << " 2 " << str2 );
308                 string::size_type loc1= str1.find( str2, 0 );
309                 if ( loc1 != string::npos && str2 != "" ) {
310                     SG_LOG( SG_INSTR, SG_DEBUG, " string found" );
311                     _mobile_lat = carrier[i]->getDoubleValue("position/latitude-deg");
312                     _mobile_lon = carrier[i]->getDoubleValue("position/longitude-deg");
313                     _mobile_elevation_ft = mobile_tacan->get_elev_ft();
314                     _mobile_range_nm = mobile_tacan->get_range();
315                     _mobile_bias = mobile_tacan->get_multiuse();
316                     _mobile_name = mobile_tacan->get_name();
317                     _mobile_ident = mobile_tacan->get_trans_ident();
318                     _mobile_valid = true;
319                     SG_LOG( SG_INSTR, SG_DEBUG, " carrier transmitter valid " << _mobile_valid );
320                     break;
321                 } else {
322                     _mobile_valid = false;
323                     SG_LOG( SG_INSTR, SG_DEBUG, " carrier transmitter invalid " << _mobile_valid );
324                 }
325             }
326         }
327
328         SG_LOG( SG_INSTR, SG_DEBUG, "name " << _mobile_name);
329         SG_LOG( SG_INSTR, SG_DEBUG, "lat " << _mobile_lat << "lon " << _mobile_lon);
330         SG_LOG( SG_INSTR, SG_DEBUG, "elev " << _mobile_elevation_ft);
331
332         //try any AI tankers second
333
334         if ( !_mobile_valid) {
335             SG_LOG( SG_INSTR, SG_DEBUG, "tanker transmitter valid start " << _mobile_valid );
336
337         SGPropertyNode * branch = fgGetNode("ai/models", true);
338         vector<SGPropertyNode_ptr> tanker = branch->getChildren("aircraft");
339
340         number = tanker.size();
341
342         SG_LOG( SG_INSTR, SG_DEBUG, "tanker number " << number );
343
344         if ( number > 0 ) {       // don't do this if there are no AI aircraft
345             for ( i = 0; i < number; ++i ) {
346                 string str4 ( tanker[i]->getStringValue("callsign", ""));
347                 SG_LOG( SG_INSTR, SG_DEBUG, "tanker callsign " << str4 );
348
349                 SG_LOG( SG_INSTR, SG_DEBUG, "strings 1 " << str1 << " 4 " << str4 );
350                 string::size_type loc1= str1.find( str4, 0 );
351                 if ( loc1 != string::npos && str4 != "" ) {
352                     SG_LOG( SG_INSTR, SG_DEBUG, " string found" );
353                     _mobile_lat = tanker[i]->getDoubleValue("position/latitude-deg");
354                     _mobile_lon = tanker[i]->getDoubleValue("position/longitude-deg");
355                     _mobile_elevation_ft = tanker[i]->getDoubleValue("position/altitude-ft");
356                     _mobile_range_nm = mobile_tacan->get_range();
357                     _mobile_bias = mobile_tacan->get_multiuse();
358                     _mobile_name = mobile_tacan->get_name();
359                     _mobile_ident = mobile_tacan->get_trans_ident();
360                     _mobile_valid = true;
361                     SG_LOG( SG_INSTR, SG_DEBUG, " tanker transmitter valid " << _mobile_valid );
362                     break;
363                 } else {
364                     _mobile_valid = false;
365                     SG_LOG( SG_INSTR, SG_DEBUG, " tanker transmitter invalid " << _mobile_valid );
366                 }
367             }
368         }
369
370         SG_LOG( SG_INSTR, SG_DEBUG, "tanker name " << _mobile_name);
371         SG_LOG( SG_INSTR, SG_DEBUG, "lat " << _mobile_lat << "lon " << _mobile_lon);
372         SG_LOG( SG_INSTR, SG_DEBUG, "elev " << _mobile_elevation_ft);
373         SG_LOG( SG_INSTR, SG_DEBUG, "range " << _mobile_range_nm);
374     }
375
376     //try any mp tankers third, if we haven't found the tanker in the ai aircraft
377
378     if ( !_mobile_valid ) {
379         SG_LOG( SG_INSTR, SG_DEBUG, " mp tanker transmitter valid start " << _mobile_valid );
380
381         SGPropertyNode * branch = fgGetNode("ai/models", true);
382         vector<SGPropertyNode_ptr> mp_tanker = branch->getChildren("multiplayer");
383
384         number = mp_tanker.size();
385
386         SG_LOG( SG_INSTR, SG_DEBUG, " mp tanker number " << number );
387
388         if ( number > 0 ) {       // don't do this if there are no MP aircraft
389             for ( i = 0; i < number; ++i ) {
390                 string str6 ( mp_tanker[i]->getStringValue("callsign", ""));
391                 SG_LOG( SG_INSTR, SG_DEBUG, "mp tanker callsign " << str6 );
392
393                 SG_LOG( SG_INSTR, SG_DEBUG, "strings 1 " << str1 << " 5 " << str6 );
394                 string::size_type loc1= str1.find( str6, 0 );
395                 if ( loc1 != string::npos && str6 != "" ) {
396                     SG_LOG( SG_INSTR, SG_DEBUG, " string found" );
397                     _mobile_lat = mp_tanker[i]->getDoubleValue("position/latitude-deg");
398                     _mobile_lon = mp_tanker[i]->getDoubleValue("position/longitude-deg");
399                     _mobile_elevation_ft = mp_tanker[i]->getDoubleValue("position/altitude-ft");
400                     _mobile_range_nm = mobile_tacan->get_range();
401                     _mobile_bias = mobile_tacan->get_multiuse();
402                     _mobile_name = mobile_tacan->get_name();
403                     _mobile_ident = mobile_tacan->get_trans_ident();
404                     _mobile_valid = true;
405
406                     SG_LOG( SG_INSTR, SG_DEBUG, " mp tanker transmitter valid " << _mobile_valid );
407                     SG_LOG( SG_INSTR, SG_DEBUG, " mp tanker name " << _mobile_name);
408                     SG_LOG( SG_INSTR, SG_DEBUG, " mp lat " << _mobile_lat << "lon " << _mobile_lon);
409                     SG_LOG( SG_INSTR, SG_DEBUG, " mp elev " << _mobile_elevation_ft);
410                     SG_LOG( SG_INSTR, SG_DEBUG, " mp range " << _mobile_range_nm);
411                     break;
412                 } else {
413                     _mobile_valid = false;
414                     SG_LOG( SG_INSTR, SG_DEBUG, " mp tanker transmitter invalid " << _mobile_valid );
415                     }
416                 }
417             }
418         }
419     } else {
420         _mobile_valid = false;
421         SG_LOG( SG_INSTR, SG_DEBUG, " mobile transmitter invalid " << _mobile_valid );
422     }
423
424     // try the TACAN/VORTAC list next
425     FGNavRecord *tacan
426         = globals->get_tacanlist()->findByFreq( frequency_mhz, longitude_rad,
427                                                 latitude_rad, altitude_m);
428
429     _transmitter_valid = (tacan != NULL);
430
431     if ( _transmitter_valid ) {
432         SG_LOG( SG_INSTR, SG_DEBUG, "transmitter valid " << _transmitter_valid );
433
434         _transmitter_pos = tacan->get_pos();
435         _transmitter_range_nm = tacan->get_range();
436         _transmitter_bias = tacan->get_multiuse();
437         _transmitter_name = tacan->get_name();
438         _name_node->setStringValue(_transmitter_name.c_str());
439         _transmitter_ident = tacan->get_trans_ident();
440         _ident_node->setStringValue(_transmitter_ident.c_str());
441
442         SG_LOG( SG_INSTR, SG_DEBUG, "name " << _transmitter_name);
443         SG_LOG( SG_INSTR, SG_DEBUG, _transmitter_pos);
444
445     } else {
446         SG_LOG( SG_INSTR, SG_DEBUG, "transmitter invalid " << _transmitter_valid );
447     }
448 }
449
450 double
451 TACAN::searchChannel (const string& channel)
452 {
453     double frequency_khz = 0;
454
455     FGTACANRecord *freq
456         = globals->get_channellist()->findByChannel( channel );
457     bool _freq_valid = (freq != NULL);
458     SG_LOG( SG_INSTR, SG_DEBUG, "freq valid " << _freq_valid );
459     if ( _freq_valid ) {
460         frequency_khz = freq->get_freq();
461         SG_LOG( SG_INSTR, SG_DEBUG, "freq output " << frequency_khz );
462         //check sanity
463         if (frequency_khz >= 9620 && frequency_khz <= 121300)
464             return frequency_khz/100;
465     }
466     return frequency_khz = 0;
467 } // end TACAN::searchChannel
468
469 /*
470  * Listener callback. Maintains channel input properties,
471  * searches new channel frequency, updates _channel and
472  * _frequency and sets boolean _new_frequency appropriately.
473  */
474 void
475 TACAN::valueChanged(SGPropertyNode *prop)
476 {
477     if (_listener_active)
478         return;
479     _listener_active++;
480
481     int index = prop->getIndex();
482     string channel = _channel;
483
484     if (index) {  // channel digit or X/Y input
485         int c;
486         if (isdigit(c = _channel_in1_node->getStringValue()[0]))
487             channel[0] = c;
488         if (isdigit(c = _channel_in2_node->getStringValue()[0]))
489             channel[1] = c;
490         if (isdigit(c = _channel_in3_node->getStringValue()[0]))
491             channel[2] = c;
492         c = _channel_in4_node->getStringValue()[0];
493         if (c == 'X' || c == 'Y')
494             channel[3] = c;
495
496     } else {      // channel number input
497         unsigned int f = prop->getIntValue();
498         if (f >= 1 && f <= 126) {
499             channel[0] = '0' + (f / 100) % 10;
500             channel[1] = '0' + (f / 10) % 10;
501             channel[2] = '0' + f % 10;
502         }
503     }
504
505     if (channel != _channel) {
506         SG_LOG(SG_INSTR, SG_DEBUG, "new channel " << channel);
507
508         // write back result
509         _channel_in0_node->setIntValue((channel[0] - '0') * 100
510                 + (channel[1] - '0') * 10 + (channel[2] - '0'));
511         char s[2] = "0";
512         s[0] = channel[0], _channel_in1_node->setStringValue(s);
513         s[0] = channel[1], _channel_in2_node->setStringValue(s);
514         s[0] = channel[2], _channel_in3_node->setStringValue(s);
515         s[0] = channel[3], _channel_in4_node->setStringValue(s);
516
517         // search channel frequency
518         double freq = searchChannel(channel);
519         if (freq != _frequency_mhz) {
520             SG_LOG(SG_INSTR, SG_DEBUG, "new frequency " << freq);
521             _frequency_node->setDoubleValue(freq);
522             _frequency_mhz = freq;
523             _new_frequency = true;
524         }
525
526         _channel = channel;
527         _time_before_search_sec = 0;
528     }
529
530     _listener_active--;
531 }
532
533 // end of TACAN.cxx