]> git.mxchange.org Git - flightgear.git/blob - src/Instrumentation/tacan.cxx
a6781d7d3477ca1a93b1e0582b1556a5f9a42049
[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, "carrier_lat " << _mobile_lat);
158     SG_LOG( SG_INSTR, SG_DEBUG, "carrier_lon " << _mobile_lon);
159     SG_LOG( SG_INSTR, SG_DEBUG, "carrier_name " << _mobile_name);
160     SG_LOG( SG_INSTR, SG_DEBUG, "carrier_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
280     SG_LOG( SG_INSTR, SG_DEBUG, "tacan freq " << frequency_mhz );
281
282     // reset search time
283     _time_before_search_sec = 1.0;
284
285     //try any carriers first
286     FGNavRecord *mobile_tacan
287           = globals->get_carrierlist()->findStationByFreq( frequency_mhz );
288     bool freq_valid = (mobile_tacan != NULL);
289     SG_LOG( SG_INSTR, SG_DEBUG, "mobile freqency valid " << freq_valid );
290
291     if ( freq_valid ) {
292
293         string str1( mobile_tacan->get_name() );
294
295         SGPropertyNode * branch = fgGetNode("ai/models", true);
296         vector<SGPropertyNode_ptr> carrier = branch->getChildren("carrier");
297
298         number = carrier.size();
299
300         SG_LOG( SG_INSTR, SG_DEBUG, "carrier " << number );
301         if ( number > 0 ) {       // don't do this if there are no carriers
302             for ( i = 0; i < number; ++i ) {
303                 string str2 ( carrier[i]->getStringValue("name", ""));
304                 SG_LOG( SG_INSTR, SG_DEBUG, "carrier name " << str2 );
305
306                 SG_LOG( SG_INSTR, SG_DEBUG, "strings 1 " << str1 << " 2 " << str2 );
307                 string::size_type loc1= str1.find( str2, 0 );
308                 if ( loc1 != string::npos && str2 != "" ) {
309                     SG_LOG( SG_INSTR, SG_DEBUG, " string found" );
310                     _mobile_lat = carrier[i]->getDoubleValue("position/latitude-deg");
311                     _mobile_lon = carrier[i]->getDoubleValue("position/longitude-deg");
312                     _mobile_elevation_ft = mobile_tacan->get_elev_ft();
313                     _mobile_range_nm = mobile_tacan->get_range();
314                     _mobile_bias = mobile_tacan->get_multiuse();
315                     _mobile_name = mobile_tacan->get_name();
316                     _mobile_ident = mobile_tacan->get_trans_ident();
317                     _mobile_valid = true;
318                     SG_LOG( SG_INSTR, SG_DEBUG, " carrier transmitter valid " << _mobile_valid );
319                     break;
320                 } else {
321                     _mobile_valid = false;
322                     SG_LOG( SG_INSTR, SG_DEBUG, " carrier transmitter invalid " << _mobile_valid );
323                 }
324             }
325         }
326
327         SG_LOG( SG_INSTR, SG_DEBUG, "name " << _mobile_name);
328         SG_LOG( SG_INSTR, SG_DEBUG, "lat " << _mobile_lat << "lon " << _mobile_lon);
329         SG_LOG( SG_INSTR, SG_DEBUG, "elev " << _mobile_elevation_ft);
330
331         //try any AI tankers second
332
333         if ( !_mobile_valid) {
334             SG_LOG( SG_INSTR, SG_DEBUG, "tanker transmitter valid start " << _mobile_valid );
335
336         SGPropertyNode * branch = fgGetNode("ai/models", true);
337         vector<SGPropertyNode_ptr> tanker = branch->getChildren("aircraft");
338
339         number = tanker.size();
340
341         SG_LOG( SG_INSTR, SG_DEBUG, "tanker number " << number );
342
343         if ( number > 0 ) {       // don't do this if there are no AI aircraft
344             for ( i = 0; i < number; ++i ) {
345                 string str4 ( tanker[i]->getStringValue("callsign", ""));
346                 SG_LOG( SG_INSTR, SG_DEBUG, "tanker callsign " << str4 );
347
348                 SG_LOG( SG_INSTR, SG_DEBUG, "strings 1 " << str1 << " 4 " << str4 );
349                 string::size_type loc1= str1.find( str4, 0 );
350                 if ( loc1 != string::npos && str4 != "" ) {
351                     SG_LOG( SG_INSTR, SG_DEBUG, " string found" );
352                     _mobile_lat = tanker[i]->getDoubleValue("position/latitude-deg");
353                     _mobile_lon = tanker[i]->getDoubleValue("position/longitude-deg");
354                     _mobile_elevation_ft = tanker[i]->getDoubleValue("position/altitude-ft");
355                     _mobile_range_nm = mobile_tacan->get_range();
356                     _mobile_bias = mobile_tacan->get_multiuse();
357                     _mobile_name = mobile_tacan->get_name();
358                     _mobile_ident = mobile_tacan->get_trans_ident();
359                     _mobile_valid = true;
360                     SG_LOG( SG_INSTR, SG_DEBUG, " tanker transmitter valid " << _mobile_valid );
361                     break;
362                 } else {
363                     _mobile_valid = false;
364                     SG_LOG( SG_INSTR, SG_DEBUG, " tanker transmitter invalid " << _mobile_valid );
365                 }
366             }
367         }
368
369         SG_LOG( SG_INSTR, SG_DEBUG, "tanker name " << _mobile_name);
370         SG_LOG( SG_INSTR, SG_DEBUG, "lat " << _mobile_lat << "lon " << _mobile_lon);
371         SG_LOG( SG_INSTR, SG_DEBUG, "elev " << _mobile_elevation_ft);
372         SG_LOG( SG_INSTR, SG_DEBUG, "range " << _mobile_range_nm);
373     }
374
375     //try any mp tankers third, if we haven't found the tanker in the ai aircraft
376
377     if ( !_mobile_valid ) {
378         SG_LOG( SG_INSTR, SG_DEBUG, " mp tanker transmitter valid start " << _mobile_valid );
379
380         SGPropertyNode * branch = fgGetNode("ai/models", true);
381         vector<SGPropertyNode_ptr> mp_tanker = branch->getChildren("multiplayer");
382
383         number = mp_tanker.size();
384
385         SG_LOG( SG_INSTR, SG_DEBUG, " mp tanker number " << number );
386
387         if ( number > 0 ) {       // don't do this if there are no MP aircraft
388             for ( i = 0; i < number; ++i ) {
389                 string str6 ( mp_tanker[i]->getStringValue("callsign", ""));
390                 SG_LOG( SG_INSTR, SG_DEBUG, "mp tanker callsign " << str6 );
391
392                 SG_LOG( SG_INSTR, SG_DEBUG, "strings 1 " << str1 << " 5 " << str6 );
393                 string::size_type loc1= str1.find( str6, 0 );
394                 if ( loc1 != string::npos && str6 != "" ) {
395                     SG_LOG( SG_INSTR, SG_DEBUG, " string found" );
396                     _mobile_lat = mp_tanker[i]->getDoubleValue("position/latitude-deg");
397                     _mobile_lon = mp_tanker[i]->getDoubleValue("position/longitude-deg");
398                     _mobile_elevation_ft = mp_tanker[i]->getDoubleValue("position/altitude-ft");
399                     _mobile_range_nm = mobile_tacan->get_range();
400                     _mobile_bias = mobile_tacan->get_multiuse();
401                     _mobile_name = mobile_tacan->get_name();
402                     _mobile_ident = mobile_tacan->get_trans_ident();
403                     _mobile_valid = true;
404
405                     SG_LOG( SG_INSTR, SG_DEBUG, " mp tanker transmitter valid " << _mobile_valid );
406                     SG_LOG( SG_INSTR, SG_DEBUG, " mp tanker name " << _mobile_name);
407                     SG_LOG( SG_INSTR, SG_DEBUG, " mp lat " << _mobile_lat << "lon " << _mobile_lon);
408                     SG_LOG( SG_INSTR, SG_DEBUG, " mp elev " << _mobile_elevation_ft);
409                     SG_LOG( SG_INSTR, SG_DEBUG, " mp range " << _mobile_range_nm);
410                     break;
411                 } else {
412                     _mobile_valid = false;
413                     SG_LOG( SG_INSTR, SG_DEBUG, " mp tanker transmitter invalid " << _mobile_valid );
414                     }
415                 }
416             }
417         }
418     } else {
419         _mobile_valid = false;
420         SG_LOG( SG_INSTR, SG_DEBUG, " mobile transmitter invalid " << _mobile_valid );
421     }
422
423     // try the TACAN/VORTAC list next
424     FGNavRecord *tacan
425         = globals->get_tacanlist()->findByFreq( frequency_mhz, longitude_rad,
426                                                 latitude_rad, altitude_m);
427
428     _transmitter_valid = (tacan != NULL);
429
430     if ( _transmitter_valid ) {
431         SG_LOG( SG_INSTR, SG_DEBUG, "transmitter valid " << _transmitter_valid );
432
433         _transmitter_pos = tacan->get_pos();
434         _transmitter_range_nm = tacan->get_range();
435         _transmitter_bias = tacan->get_multiuse();
436         _transmitter_name = tacan->get_name();
437         _name_node->setStringValue(_transmitter_name.c_str());
438         _transmitter_ident = tacan->get_trans_ident();
439         _ident_node->setStringValue(_transmitter_ident.c_str());
440
441         SG_LOG( SG_INSTR, SG_DEBUG, "name " << _transmitter_name);
442         SG_LOG( SG_INSTR, SG_DEBUG, _transmitter_pos);
443
444     } else {
445         SG_LOG( SG_INSTR, SG_DEBUG, "transmitter invalid " << _transmitter_valid );
446     }
447 }
448
449 double
450 TACAN::searchChannel (const string& channel)
451 {
452     double frequency_khz = 0;
453
454     FGTACANRecord *freq
455         = globals->get_channellist()->findByChannel( channel );
456     bool _freq_valid = (freq != NULL);
457     SG_LOG( SG_INSTR, SG_DEBUG, "freq valid " << _freq_valid );
458     if ( _freq_valid ) {
459         frequency_khz = freq->get_freq();
460         SG_LOG( SG_INSTR, SG_DEBUG, "freq output " << frequency_khz );
461         //check sanity
462         if (frequency_khz >= 9620 && frequency_khz <= 121300)
463             return frequency_khz/100;
464     }
465     return frequency_khz = 0;
466 } // end TACAN::searchChannel
467
468 /*
469  * Listener callback. Maintains channel input properties,
470  * searches new channel frequency, updates _channel and
471  * _frequency and sets boolean _new_frequency appropriately.
472  */
473 void
474 TACAN::valueChanged(SGPropertyNode *prop)
475 {
476     if (_listener_active)
477         return;
478     _listener_active++;
479
480     int index = prop->getIndex();
481     string channel = _channel;
482
483     if (index) {  // channel digit or X/Y input
484         int c;
485         if (isdigit(c = _channel_in1_node->getStringValue()[0]))
486             channel[0] = c;
487         if (isdigit(c = _channel_in2_node->getStringValue()[0]))
488             channel[1] = c;
489         if (isdigit(c = _channel_in3_node->getStringValue()[0]))
490             channel[2] = c;
491         c = _channel_in4_node->getStringValue()[0];
492         if (c == 'X' || c == 'Y')
493             channel[3] = c;
494
495     } else {      // channel number input
496         unsigned int f = prop->getIntValue();
497         if (f >= 1 && f <= 126) {
498             channel[0] = '0' + (f / 100) % 10;
499             channel[1] = '0' + (f / 10) % 10;
500             channel[2] = '0' + f % 10;
501         }
502     }
503
504     if (channel != _channel) {
505         SG_LOG(SG_INSTR, SG_DEBUG, "new channel " << channel);
506
507         // write back result
508         _channel_in0_node->setIntValue((channel[0] - '0') * 100
509                 + (channel[1] - '0') * 10 + (channel[2] - '0'));
510         char s[2] = "0";
511         s[0] = channel[0], _channel_in1_node->setStringValue(s);
512         s[0] = channel[1], _channel_in2_node->setStringValue(s);
513         s[0] = channel[2], _channel_in3_node->setStringValue(s);
514         s[0] = channel[3], _channel_in4_node->setStringValue(s);
515
516         // search channel frequency
517         double freq = searchChannel(channel);
518         if (freq != _frequency_mhz) {
519             SG_LOG(SG_INSTR, SG_DEBUG, "new frequency " << freq);
520             _frequency_node->setDoubleValue(freq);
521             _frequency_mhz = freq;
522             _new_frequency = true;
523         }
524
525         _channel = channel;
526         _time_before_search_sec = 0;
527     }
528
529     _listener_active--;
530 }
531
532 // end of TACAN.cxx