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     : _last_distance_nm(0),
  50       _last_frequency_mhz(-1),
  51       _time_before_search_sec(0),
  52       _carrier_valid(false),
  53       _transmitter_valid(false),
  54       _transmitter_elevation_ft(0),
  55       _transmitter_range_nm(0),
  56       _transmitter_bias(0.0),
  57
  58       name("tacan"),
  59       num(0)
  60 {
  61
  62     int i;
  63     for ( i = 0; i < node->nChildren(); ++i ) {
  64         SGPropertyNode *child = node->getChild(i);
  65         string cname = child->getName();
  66         string cval = child->getStringValue();
  67         if ( cname == "name" ) {
  68             name = cval;
  69         } else if ( cname == "number" ) {
  70             num = child->getIntValue();
  71         } else {
  72             SG_LOG( SG_INSTR, SG_DEBUG, "Error in TACAN config logic" );
  73             if ( name.length() ) {
  74                 SG_LOG( SG_INSTR, SG_DEBUG, "Section = " << name );
  75             }
  76         }
  77     }
  78 }
  79
  80 TACAN::TACAN ()
  81     : _last_distance_nm(0),
  82       _last_frequency_mhz(-1),
  83       _time_before_search_sec(0),
  84       _carrier_valid(false),
  85       _transmitter_valid(false),
  86       _transmitter_elevation_ft(0),
  87       _transmitter_range_nm(0),
  88       _transmitter_bearing_deg(0),
  89       _transmitter_bias(0.0),
  90       _transmitter_name(""),
  91       name("tacan")
  92 {
  93 }
  94
  95 TACAN::~TACAN ()
  96 {
  97 }
  98
  99 void
 100 TACAN::init ()
 101 {
 102     string branch;
 103     branch = "/instrumentation/" + name;
 104
 105     SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
 106
 107     _longitude_node = fgGetNode("/position/longitude-deg", true);
 108     _latitude_node = fgGetNode("/position/latitude-deg", true);
 109     _altitude_node = fgGetNode("/position/altitude-ft", true);
 110     _serviceable_node = node->getChild("serviceable", 0, true);
 111     _electrical_node = fgGetNode("/systems/electrical/outputs/tacan", true);
 112     SGPropertyNode *fnode = node->getChild("frequencies", 0, true);
 113     _source_node = fnode->getChild("source", 0, true);
 114     _frequency_node = fnode->getChild("selected-mhz", 0, true);
 115     _channel_node = fnode->getChild("selected-channel", 1, true);
 116     _channel_node = fnode->getChild("selected-channel", 2, true);
 117     _channel_node = fnode->getChild("selected-channel", 3, true);
 118     _channel_node = fnode->getChild("selected-channel", 4, true);
 119     _in_range_node = node->getChild("in-range", 0, true);
 120     _distance_node = node->getChild("indicated-distance-nm", 0, true);
 121     _speed_node = node->getChild("indicated-ground-speed-kt", 0, true);
 122     _time_node = node->getChild("indicated-time-min", 0, true);
 123     _name_node = node->getChild("name", 0, true);
 124     _bearing_node = node->getChild("indicated-bearing-true-deg", 0, true);
 125     SGPropertyNode *cnode = fgGetNode("/ai/models/carrier", num, true );
 126     _carrier_name_node = cnode->getChild("name", 0, true);
 127
 128 }
 129
 130 void
 131 TACAN::update (double delta_time_sec)
 132 {
 133     double az2 = 0;
 134     double bearing = 0;
 135     double distance = 0;
 136     double carrier_az2= 0;
 137     double carrier_bearing = 0;
 138     double carrier_distance = 0;
 139     double frequency_mhz = 0;
 140     string _channel, _last_channel, _channel_1, _channel_2,_channel_3, _channel_4;
 141
 142                                 // If it's off, don't waste any time.
 143     if (!_serviceable_node->getBoolValue() || !_electrical_node->getBoolValue()) {
 144         _last_distance_nm = 0;
 145         _in_range_node->setBoolValue(false);
 146         _distance_node->setDoubleValue(0);
 147         _speed_node->setDoubleValue(0);
 148         _time_node->setDoubleValue(0);
 149         SG_LOG( SG_INSTR, SG_DEBUG, "skip tacan" );
 150         return;
 151     }
 152
 153                                 // Figure out the source
 154     const char * source = _source_node->getStringValue();
 155
 156     if (source[0] == '\0') {
 157         string branch;
 158         branch = "/instrumentation/" + name + "/frequencies/selected-channel";
 159         _source_node->setStringValue(branch.c_str());
 160         source = _source_node->getStringValue();
 161         SG_LOG( SG_INSTR, SG_DEBUG, "source " << source  );
 162     }
 163                                 // Get the channel
 164     _channel_1 = fgGetString("/instrumentation/tacan/frequencies/selected-channel[1]");
 165     _channel_2 = fgGetString("/instrumentation/tacan/frequencies/selected-channel[2]");
 166     _channel_3 = fgGetString("/instrumentation/tacan/frequencies/selected-channel[3]");
 167     _channel_4 = fgGetString("/instrumentation/tacan/frequencies/selected-channel[4]");
 168     SG_LOG( SG_INSTR, SG_DEBUG, "channels " << _channel_1 << _channel_2 << _channel_3 << _channel_4);
 169     _channel = _channel_1 + _channel_2 + _channel_3 + _channel_4;
 170     SG_LOG( SG_INSTR, SG_DEBUG, "channel " << _channel );
 171
 172                                 // Get the frequecncy
 173     if (_channel != _last_channel) {
 174         _time_before_search_sec = 0;
 175         _last_channel = _channel;
 176         frequency_mhz = searchChannel(_channel);
 177         SG_LOG( SG_INSTR, SG_DEBUG, "frequency " << frequency_mhz );
 178         _frequency_node->setDoubleValue(frequency_mhz);
 179     }
 180
 181                                 // Get the aircraft position
 182     double longitude_deg = _longitude_node->getDoubleValue();
 183     double latitude_deg  = _latitude_node->getDoubleValue();
 184     double altitude_m    = _altitude_node->getDoubleValue() * SG_FEET_TO_METER;
 185     double longitude_rad = longitude_deg * SGD_DEGREES_TO_RADIANS;
 186     double latitude_rad  = latitude_deg * SGD_DEGREES_TO_RADIANS;
 187
 188                                 // On timeout, scan again
 189     _time_before_search_sec -= delta_time_sec;
 190     if (_time_before_search_sec < 0 && frequency_mhz > 0)
 191         search(frequency_mhz, longitude_rad, 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         _in_range_node->setBoolValue(false);
 268         _distance_node->setDoubleValue(0);
 269         _speed_node->setDoubleValue(0);
 270         _time_node->setDoubleValue(0);
 271         _bearing_node->setDoubleValue(0);
 272         _transmitter_name = "";
 273         _name_node->setStringValue(_transmitter_name.c_str());
 274         return;
 275     }
 276 } // end function update
 277
 278 void
 279 TACAN::search (double frequency_mhz, double longitude_rad,
 280                double latitude_rad, double altitude_m)
 281 {
 282
 283     SG_LOG( SG_INSTR, SG_DEBUG, "tacan freq " << frequency_mhz );
 284
 285     // reset search time
 286     _time_before_search_sec = 1.0;
 287
 288     //try any carriers first
 289      FGNavRecord *carrier_tacan
 290           = globals->get_carrierlist()->findStationByFreq( frequency_mhz );
 291     _carrier_valid = (carrier_tacan != NULL);
 292
 293     if ( _carrier_valid ) {
 294         SG_LOG( SG_INSTR, SG_DEBUG, "carrier transmitter valid " << _carrier_valid  );
 295
 296         string str1( carrier_tacan->get_name() );
 297
 298         SGPropertyNode * branch = fgGetNode("ai/models", true);
 299         vector<SGPropertyNode_ptr> carrier = branch->getChildren("carrier");
 300
 301         int number = carrier.size();
 302
 303         SG_LOG( SG_INSTR, SG_DEBUG, "carrier " << number );
 304
 305         int i;
 306         for ( i = 0; i < number; ++i ) {
 307             string str2 ( carrier[i]->getStringValue("name", ""));
 308             // SG_LOG( SG_INSTR, SG_DEBUG, "carrier name " << str2 );
 309
 310             SG_LOG( SG_INSTR, SG_DEBUG, "strings 1 " << str1 << " 2 " << str2 );
 311             unsigned int loc1= str1.find( str2, 0 );
 312             if ( loc1 != string::npos && str2 != "" ) {
 313                 SG_LOG( SG_INSTR, SG_DEBUG, " string found" );
 314                 _carrier_lat = carrier[i]->getDoubleValue("position/latitude-deg");
 315                 _carrier_lon = carrier[i]->getDoubleValue("position/longitude-deg");
 316                 _carrier_elevation_ft = carrier_tacan->get_elev_ft();
 317                 _carrier_range_nm = carrier_tacan->get_range();
 318                 _carrier_bias = carrier_tacan->get_multiuse();
 319                 _carrier_name = carrier_tacan->get_name();
 320                 _carrier_valid = 1;
 321                 SG_LOG( SG_INSTR, SG_DEBUG, " carrier transmitter valid " << _carrier_valid );
 322                 break;
 323             } else {
 324                 _carrier_valid = 0;
 325                 SG_LOG( SG_INSTR, SG_DEBUG, " carrier transmitter invalid " << _carrier_valid );
 326             }
 327         }
 328
 329         SG_LOG( SG_INSTR, SG_DEBUG, "name " << _carrier_name);
 330         SG_LOG( SG_INSTR, SG_DEBUG, "lat " << _carrier_lat << "lon " << _carrier_lon);
 331         SG_LOG( SG_INSTR, SG_DEBUG, "elev " << _carrier_elevation_ft);
 332
 333     } else {
 334         SG_LOG( SG_INSTR, SG_DEBUG, " carrier transmitter invalid " << _carrier_valid  );
 335     }
 336
 337
 338     // try the TACAN/VORTAC list next
 339     FGNavRecord *tacan
 340         = globals->get_tacanlist()->findByFreq( frequency_mhz, longitude_rad,
 341                                                 latitude_rad, altitude_m);
 342
 343     _transmitter_valid = (tacan != NULL);
 344
 345     if ( _transmitter_valid ) {
 346         SG_LOG( SG_INSTR, SG_DEBUG, "transmitter valid " << _transmitter_valid  );
 347
 348         _transmitter_lat = tacan->get_lat();
 349         _transmitter_lon = tacan->get_lon();
 350         _transmitter_elevation_ft = tacan->get_elev_ft();
 351         _transmitter_range_nm = tacan->get_range();
 352         _transmitter_bias = tacan->get_multiuse();
 353         _transmitter_name = tacan->get_name();
 354         _name_node->setStringValue(_transmitter_name.c_str());
 355
 356         SG_LOG( SG_INSTR, SG_DEBUG, "name " << _transmitter_name);
 357         SG_LOG( SG_INSTR, SG_DEBUG, "lat " << _transmitter_lat << "lon " << _transmitter_lon);
 358         SG_LOG( SG_INSTR, SG_DEBUG, "elev " << _transmitter_elevation_ft);
 359
 360     } else {
 361         SG_LOG( SG_INSTR, SG_DEBUG, "transmitter invalid " << _transmitter_valid  );
 362     }
 363 }
 364
 365 double
 366 TACAN::searchChannel (const string& _channel){
 367
 368     double frequency_khz = 0;
 369
 370     FGTACANRecord *freq
 371         = globals->get_channellist()->findByChannel( _channel );
 372     double _freq_valid = (freq != NULL);
 373     SG_LOG( SG_INSTR, SG_DEBUG, "freq valid " << _freq_valid  );
 374     if ( _freq_valid ) {
 375         frequency_khz = freq->get_freq();
 376         SG_LOG( SG_INSTR, SG_DEBUG, "freq output " << frequency_khz  );
 377         //check sanity
 378         if (frequency_khz >=9620 && frequency_khz <= 12130)
 379             return frequency_khz/100;
 380     }
 381     return frequency_khz = 0;
 382 } // end TACAN::searchChannel
 383
 384 // end of TACAN.cxx