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