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 (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 and  frequency_khz <= 12130)return frequency_khz/100;
 369     }
 370     return frequency_khz = 0;
 371 } // end TACAN::searchChannel
 372
 373 // end of TACAN.cxx