← Back to branch summary

~ubuntu-branches/ubuntu/oneiric/flightgear/oneiric

« back to all changes in this revision

Viewing changes to src/Instrumentation/tacan.cxx

  • Committer: Bazaar Package Importer
  • Author(s): Daniel T Chen
  • Date: 2006-01-05 12:48:11 UTC
  • mfrom: (1.1.4 upstream)
  • Revision ID: james.westby@ubuntu.com-20060105124811-xsr6rtcrb88w19c8
Tags: 0.9.9-1ubuntu1
Resynchronise with Debian.

Show diffs side-by-side

added added

removed removed

Lines of Context:
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
#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 <Navaids/navlist.hxx>
 
12
 
 
13
#include "tacan.hxx"
 
14
 
 
15
 
 
16
/**
 
17
 * Adjust the range.
 
18
 *
 
19
 * Start by calculating the radar horizon based on the elevation
 
20
 * difference, then clamp to the maximum, then add a fudge for
 
21
 * borderline reception.
 
22
 */
 
23
static double
 
24
adjust_range (double transmitter_elevation_ft, double aircraft_altitude_ft,
 
25
              double max_range_nm)
 
26
{
 
27
    max_range_nm = 150;
 
28
    double delta_elevation_ft =
 
29
        fabs(aircraft_altitude_ft - transmitter_elevation_ft);
 
30
    double range_nm = 1.23 * sqrt(delta_elevation_ft);
 
31
    if (range_nm > max_range_nm)
 
32
        range_nm = max_range_nm;
 
33
    else if (range_nm < 20.0)
 
34
        range_nm = 20.0;
 
35
    double rand = sg_random();
 
36
    SG_LOG( SG_INSTR, SG_DEBUG, " tacan range " << range_nm << " max range " << max_range_nm);
 
37
    return range_nm + (range_nm * rand * rand);
 
38
}
 
39
 
 
40
 
 
41
TACAN::TACAN ( SGPropertyNode *node )
 
42
    : _last_distance_nm(0),
 
43
      _last_frequency_mhz(-1),
 
44
      _time_before_search_sec(0),
 
45
      _carrier_valid(false),
 
46
      _transmitter_valid(false),
 
47
      _transmitter_elevation_ft(0),
 
48
      _transmitter_range_nm(0),
 
49
      _transmitter_bias(0.0),
 
50
      
 
51
      name("tacan"),
 
52
      num(0)
 
53
{
 
54
    
 
55
    int i;
 
56
    for ( i = 0; i < node->nChildren(); ++i ) {
 
57
        SGPropertyNode *child = node->getChild(i);
 
58
        string cname = child->getName();
 
59
        string cval = child->getStringValue();
 
60
        if ( cname == "name" ) {
 
61
            name = cval;
 
62
        } else if ( cname == "number" ) {
 
63
            num = child->getIntValue();
 
64
        } else {
 
65
            SG_LOG( SG_INSTR, SG_DEBUG, "Error in TACAN config logic" );
 
66
            if ( name.length() ) {
 
67
                SG_LOG( SG_INSTR, SG_DEBUG, "Section = " << name );
 
68
            }
 
69
        }
 
70
    }
 
71
}
 
72
 
 
73
TACAN::TACAN ()
 
74
    : _last_distance_nm(0),
 
75
      _last_frequency_mhz(-1),
 
76
      _time_before_search_sec(0),
 
77
      _carrier_valid(false),
 
78
      _transmitter_valid(false),
 
79
      _transmitter_elevation_ft(0),
 
80
      _transmitter_range_nm(0),
 
81
      _transmitter_bearing_deg(0),
 
82
      _transmitter_bias(0.0),
 
83
      _transmitter_name(""),
 
84
      name("tacan")
 
85
{
 
86
}
 
87
 
 
88
TACAN::~TACAN ()
 
89
{
 
90
}
 
91
 
 
92
void
 
93
TACAN::init ()
 
94
{
 
95
    string branch;
 
96
    branch = "/instrumentation/" + name;
 
97
 
 
98
    SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
 
99
 
 
100
    _longitude_node = fgGetNode("/position/longitude-deg", true);
 
101
    _latitude_node = fgGetNode("/position/latitude-deg", true);
 
102
    _altitude_node = fgGetNode("/position/altitude-ft", true);
 
103
    _serviceable_node = node->getChild("serviceable", 0, true);
 
104
    _electrical_node = fgGetNode("/systems/electrical/outputs/tacan", true);
 
105
    SGPropertyNode *fnode = node->getChild("frequencies", 0, true);
 
106
    _source_node = fnode->getChild("source", 0, true);
 
107
    _frequency_node = fnode->getChild("selected-mhz", 0, true);
 
108
    _channel_node = fnode->getChild("selected-channel", 1, true);
 
109
    _channel_node = fnode->getChild("selected-channel", 2, true);
 
110
    _channel_node = fnode->getChild("selected-channel", 3, true);
 
111
    _channel_node = fnode->getChild("selected-channel", 4, true);
 
112
    _in_range_node = node->getChild("in-range", 0, true);
 
113
    _distance_node = node->getChild("indicated-distance-nm", 0, true);
 
114
    _speed_node = node->getChild("indicated-ground-speed-kt", 0, true);
 
115
    _time_node = node->getChild("indicated-time-min", 0, true);
 
116
    _name_node = node->getChild("name", 0, true);
 
117
    _bearing_node = node->getChild("indicated-bearing-true-deg", 0, true);
 
118
    _carrier_lat_node = fgGetNode("/ai/models/carrier/position/latitude-deg", true);
 
119
    _carrier_lon_node = fgGetNode("/ai/models/carrier/position/longitude-deg", true);
 
120
    _carrier_name_node = fgGetNode("/ai/models/carrier/name", true);
 
121
   
 
122
}
 
123
 
 
124
void
 
125
TACAN::update (double delta_time_sec)
 
126
{   
 
127
    double az2 = 0; 
 
128
    double bearing = 0;
 
129
    double distance = 0; 
 
130
    double carrier_az2= 0; 
 
131
    double carrier_bearing = 0; 
 
132
    double carrier_distance = 0;
 
133
    double frequency_mhz = 0;
 
134
    string _channel, _last_channel, _channel_1, _channel_2,_channel_3, _channel_4;
 
135
 
 
136
                                // If it's off, don't waste any time.    
 
137
    if (!_serviceable_node->getBoolValue() ||
 
138
        !_electrical_node->getBoolValue() 
 
139
        ) {
 
140
            _last_distance_nm = 0;
 
141
            _in_range_node->setBoolValue(false);
 
142
            _distance_node->setDoubleValue(0);
 
143
            _speed_node->setDoubleValue(0);
 
144
            _time_node->setDoubleValue(0);
 
145
            SG_LOG( SG_INSTR, SG_DEBUG, "skip tacan "  );
 
146
            return;
 
147
    }else{
 
148
        //SG_LOG( SG_INSTR, SG_DEBUG, "run tacan "  );
 
149
        }    
 
150
                                
 
151
                                // Figure out the source
 
152
    const char * source = _source_node->getStringValue();
 
153
   
 
154
    if (source[0] == '\0') {
 
155
        string branch;
 
156
        branch = "/instrumentation/" + name + "/frequencies/selected-channel";
 
157
      //  branch = "/instrumentation/" + _name + "/frequencies/selected-mhz";      
 
158
        _source_node->setStringValue(branch.c_str());
 
159
        source = _source_node->getStringValue();
 
160
        SG_LOG( SG_INSTR, SG_DEBUG, "source " << source  ); 
 
161
    }
 
162
                                // Get the channel   
 
163
    _channel_1 = fgGetString("/instrumentation/tacan/frequencies/selected-channel[1]");
 
164
    _channel_2 = fgGetString("/instrumentation/tacan/frequencies/selected-channel[2]");
 
165
    _channel_3 = fgGetString("/instrumentation/tacan/frequencies/selected-channel[3]");
 
166
    _channel_4 = fgGetString("/instrumentation/tacan/frequencies/selected-channel[4]");
 
167
    SG_LOG( SG_INSTR, SG_DEBUG, "channels " << _channel_1 << _channel_2 << _channel_3 << _channel_4);
 
168
    _channel = _channel_1 + _channel_2 + _channel_3 + _channel_4;
 
169
    SG_LOG( SG_INSTR, SG_DEBUG, "channel " << _channel );
 
170
    
 
171
                                // Get the frequecncy
 
172
    if (_channel != _last_channel) {
 
173
        _time_before_search_sec = 0;
 
174
        _last_channel = _channel;
 
175
        frequency_mhz = searchChannel(_channel);
 
176
        SG_LOG( SG_INSTR, SG_DEBUG, "frequency " << frequency_mhz );
 
177
        _frequency_node->setDoubleValue(frequency_mhz);
 
178
    }        
 
179
        
 
180
                                // Get the aircraft position
 
181
     double longitude_deg = _longitude_node->getDoubleValue();
 
182
     double latitude_deg  = _latitude_node->getDoubleValue();
 
183
     double altitude_m    = _altitude_node->getDoubleValue() * SG_FEET_TO_METER;
 
184
     double longitude_rad = longitude_deg * SGD_DEGREES_TO_RADIANS;
 
185
     double latitude_rad  = latitude_deg * SGD_DEGREES_TO_RADIANS;
 
186
                                          
 
187
                                // On timeout, scan again
 
188
    _time_before_search_sec -= delta_time_sec;
 
189
    if (_time_before_search_sec < 0 && frequency_mhz > 0)
 
190
        search(frequency_mhz, longitude_rad,
 
191
               latitude_rad, altitude_m);
 
192
 
 
193
                                 // Calculate the distance to the transmitter
 
194
                                 
 
195
     //calculate the bearing and range of the carrier from the aircraft
 
196
     SG_LOG( SG_INSTR, SG_DEBUG, "carrier_lat " << _carrier_lat);
 
197
     SG_LOG( SG_INSTR, SG_DEBUG, "carrier_lon " << _carrier_lon);
 
198
     SG_LOG( SG_INSTR, SG_DEBUG, "carrier_name " << _carrier_name);
 
199
     SG_LOG( SG_INSTR, SG_DEBUG, "carrier_valid " << _carrier_valid);
 
200
     geo_inverse_wgs_84(altitude_m,
 
201
                        latitude_deg,
 
202
                        longitude_deg,
 
203
                        _carrier_lat,
 
204
                        _carrier_lon,
 
205
                        &carrier_bearing, &carrier_az2, &carrier_distance);
 
206
                         
 
207
      
 
208
     //calculate the bearing and range of the station from the aircraft
 
209
     geo_inverse_wgs_84(altitude_m,
 
210
                        latitude_deg,
 
211
                        longitude_deg,
 
212
                        _transmitter_lat,
 
213
                        _transmitter_lon,
 
214
                        &bearing, &az2, &distance);
 
215
    
 
216
                     
 
217
     //select the nearer
 
218
     if ( carrier_distance <= distance && _carrier_valid) { 
 
219
         SG_LOG( SG_INSTR, SG_DEBUG, "carrier_distance_nm " << carrier_distance);
 
220
         SG_LOG( SG_INSTR, SG_DEBUG, "distance_nm " << distance);
 
221
         bearing = carrier_bearing;
 
222
         distance = carrier_distance;
 
223
         _transmitter_elevation_ft = _carrier_elevation_ft;
 
224
         _transmitter_range_nm = _carrier_range_nm;
 
225
         _transmitter_bias = _carrier_bias;
 
226
         _transmitter_name = _carrier_name;
 
227
         _name_node->setStringValue(_transmitter_name.c_str());
 
228
     }    
 
229
               
 
230
     double distance_nm = distance * SG_METER_TO_NM;        
 
231
     SG_LOG( SG_INSTR, SG_DEBUG, "distance_nm " << distance_nm  << " bearing " << bearing);                          
 
232
 
 
233
    /*Point3D location =
 
234
        sgGeodToCart(Point3D(longitude_rad, latitude_rad, altitude_m));
 
235
    double distance_nm = _transmitter.distance3D(location) * SG_METER_TO_NM;*/
 
236
 
 
237
    double range_nm = adjust_range(_transmitter_elevation_ft,
 
238
                                   altitude_m * SG_METER_TO_FEET,
 
239
                                   _transmitter_range_nm);
 
240
 
 
241
    if (distance_nm <= range_nm) {
 
242
        double speed_kt = (fabs(distance_nm - _last_distance_nm) *
 
243
                           ((1 / delta_time_sec) * 3600.0));
 
244
        _last_distance_nm = distance_nm;
 
245
 
 
246
        _in_range_node->setBoolValue(true);
 
247
        double tmp_dist = distance_nm - _transmitter_bias;
 
248
        if ( tmp_dist < 0.0 ) {
 
249
            tmp_dist = 0.0;
 
250
        }
 
251
        _distance_node->setDoubleValue( tmp_dist );
 
252
        _speed_node->setDoubleValue(speed_kt);
 
253
        _time_node->setDoubleValue(distance_nm/speed_kt*60.0);
 
254
        _bearing_node->setDoubleValue(bearing);
 
255
        
 
256
    } else {
 
257
        _last_distance_nm = 0;
 
258
        _in_range_node->setBoolValue(false);
 
259
        _distance_node->setDoubleValue(0);
 
260
        _speed_node->setDoubleValue(0);
 
261
        _time_node->setDoubleValue(0);
 
262
        _bearing_node->setDoubleValue(0);
 
263
    }
 
264
    
 
265
                                // If we can't find a valid station set everything to zero
 
266
    if (!_transmitter_valid && !_carrier_valid)
 
267
    {
 
268
        _in_range_node->setBoolValue(false);
 
269
        _distance_node->setDoubleValue(0);
 
270
        _speed_node->setDoubleValue(0);
 
271
        _time_node->setDoubleValue(0);
 
272
        _bearing_node->setDoubleValue(0);
 
273
        _transmitter_name = "";
 
274
        _name_node->setStringValue(_transmitter_name.c_str());
 
275
        return;
 
276
    }
 
277
} // end function update
 
278
 
 
279
void
 
280
TACAN::search (double frequency_mhz, double longitude_rad,
 
281
             double latitude_rad, double altitude_m)
 
282
{    
 
283
    
 
284
    SG_LOG( SG_INSTR, SG_DEBUG, "tacan freq " << frequency_mhz );
 
285
     
 
286
    // reset search time
 
287
    _time_before_search_sec = 1.0;
 
288
    
 
289
    //try any carriers first
 
290
     FGNavRecord *carrier_tacan
 
291
          = globals->get_carrierlist()->findStationByFreq( frequency_mhz );
 
292
    _carrier_valid = (carrier_tacan != NULL);
 
293
 
 
294
    if ( _carrier_valid ) {
 
295
        SG_LOG( SG_INSTR, SG_DEBUG, "carrier transmitter valid " << _carrier_valid  );
 
296
         
 
297
        string str1( carrier_tacan->get_name() );
 
298
        string str2 ( _carrier_name_node->getStringValue());
 
299
        SG_LOG( SG_INSTR, SG_DEBUG, "strings 1 " << str1 << " 2 " << str2 );
 
300
        unsigned int loc1= str1.find( str2, 0 );
 
301
          if( loc1 != string::npos && str2 != "" ){
 
302
              SG_LOG( SG_INSTR, SG_DEBUG, " string found" );
 
303
              _carrier_lat = _carrier_lat_node->getDoubleValue();
 
304
              _carrier_lon = _carrier_lon_node->getDoubleValue();
 
305
              _carrier_elevation_ft = carrier_tacan->get_elev_ft();
 
306
              _carrier_range_nm = carrier_tacan->get_range();
 
307
              _carrier_bias = carrier_tacan->get_multiuse();
 
308
              _carrier_name = carrier_tacan->get_name();
 
309
              }
 
310
          else{
 
311
              _carrier_valid = 0;
 
312
              SG_LOG( SG_INSTR, SG_DEBUG, " carrier transmitter invalid " << _carrier_valid );
 
313
          }    
 
314
              
 
315
              
 
316
        //_name_node->setStringValue(_transmitter_name.c_str());
 
317
        
 
318
        SG_LOG( SG_INSTR, SG_DEBUG, "lat " << _carrier_lat << "lon " << _carrier_lon);
 
319
        SG_LOG( SG_INSTR, SG_DEBUG, "elev " << _carrier_elevation_ft);
 
320
        SG_LOG( SG_INSTR, SG_DEBUG, "name " << _carrier_name);
 
321
        
 
322
    } else {
 
323
         SG_LOG( SG_INSTR, SG_DEBUG, " carrier transmitter invalid " << _carrier_valid  );
 
324
     }    
 
325
    
 
326
 
 
327
    // try the TACAN/VORTAC list next
 
328
    FGNavRecord *tacan
 
329
        = globals->get_tacanlist()->findByFreq( frequency_mhz, longitude_rad,
 
330
                                              latitude_rad, altitude_m);
 
331
 
 
332
    _transmitter_valid = (tacan != NULL);
 
333
 
 
334
    if ( _transmitter_valid ) {
 
335
        SG_LOG( SG_INSTR, SG_DEBUG, "transmitter valid " << _transmitter_valid  );
 
336
       
 
337
        _transmitter_lat = tacan->get_lat();
 
338
        _transmitter_lon = tacan->get_lon();
 
339
        _transmitter_elevation_ft = tacan->get_elev_ft();
 
340
        _transmitter_range_nm = tacan->get_range();
 
341
        _transmitter_bias = tacan->get_multiuse();
 
342
        _transmitter_name = tacan->get_name();
 
343
        _name_node->setStringValue(_transmitter_name.c_str());
 
344
        
 
345
        SG_LOG( SG_INSTR, SG_DEBUG, "lat " << _transmitter_lat << "lon " << _transmitter_lon);
 
346
        SG_LOG( SG_INSTR, SG_DEBUG, "elev " << _transmitter_elevation_ft);
 
347
        SG_LOG( SG_INSTR, SG_DEBUG, "name " << _transmitter_name);
 
348
        
 
349
    } else {
 
350
         SG_LOG( SG_INSTR, SG_DEBUG, "transmitter invalid " << _transmitter_valid  );
 
351
     }    
 
352
        
 
353
}
 
354
 
 
355
double
 
356
TACAN::searchChannel (const string& _channel){
 
357
    
 
358
    double frequency_khz = 0;
 
359
    
 
360
    FGTACANRecord *freq
 
361
        = globals->get_channellist()->findByChannel( _channel );
 
362
    double _freq_valid = (freq != NULL); 
 
363
    SG_LOG( SG_INSTR, SG_DEBUG, "freq valid " << _freq_valid  );
 
364
    if ( _freq_valid ) {
 
365
        frequency_khz = freq->get_freq();
 
366
        SG_LOG( SG_INSTR, SG_DEBUG, "freq output " << frequency_khz  );
 
367
        //check sanity
 
368
        if (frequency_khz >9620 && frequency_khz <= 12130)return frequency_khz/100;
 
369
    }
 
370
    return frequency_khz = 0;
 
371
} // end TACAN::searchChannel
 
372
   
 
373
// end of TACAN.cxx