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 /**
  21  * Adjust the range.
  22  *
  23  * Start by calculating the radar horizon based on the elevation
  24  * difference, then clamp to the maximum, then add a fudge for
  25  * borderline reception.
  26  */
  27 static double
  28 adjust_range( double station_elevation_ft,
  29               double aircraft_altitude_ft,
  30               double max_range_nm )
  31 {
  32   // See http://en.wikipedia.org/wiki/Line-of-sight_propagation for approximate
  33   // line-of-sight distance to the horizon
  34
  35   double range_nm = 0;
  36   if( station_elevation_ft > 5 )
  37     range_nm += 1.23 * sqrt(station_elevation_ft);
  38
  39   if( aircraft_altitude_ft > 5 )
  40     range_nm += 1.23 * sqrt(aircraft_altitude_ft);
  41
  42   return std::max(20., std::min(range_nm, max_range_nm))
  43        * (1 + SGMiscd::pow<2>(0.3 * sg_random()));
  44 }
  45
  46 //------------------------------------------------------------------------------
  47 TACAN::TACAN( SGPropertyNode *node ):
  48   _name(node->getStringValue("name", "tacan")),
  49   _num(node->getIntValue("number", 0)),
  50   _was_disabled(true),
  51   _new_frequency(false),
  52   _channel("0000"),
  53   _last_distance_nm(0),
  54   _frequency_mhz(-1),
  55   _time_before_search_sec(0),
  56   _listener_active(0)
  57 {
  58
  59 }
  60
  61 //------------------------------------------------------------------------------
  62 TACAN::~TACAN()
  63 {
  64
  65 }
  66
  67 //------------------------------------------------------------------------------
  68 void TACAN::init()
  69 {
  70     std::string branch = "/instrumentation/" + _name;
  71
  72     SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
  73
  74     _serviceable_node = node->getChild("serviceable", 0, true);
  75     _ident_node = node->getChild("ident", 0, true);
  76
  77     SGPropertyNode *fnode = node->getChild("frequencies", 0, true);
  78     _frequency_node = fnode->getChild("selected-mhz", 0, true);
  79
  80     _channel_in0_node = fnode->getChild("selected-channel", 0, true);
  81     _channel_in1_node = fnode->getChild("selected-channel", 1, true);
  82     _channel_in2_node = fnode->getChild("selected-channel", 2, true);
  83     _channel_in3_node = fnode->getChild("selected-channel", 3, true);
  84     _channel_in4_node = fnode->getChild("selected-channel", 4, true);
  85
  86     _in_range_node = node->getChild("in-range", 0, true);
  87     _distance_node = node->getChild("indicated-distance-nm", 0, true);
  88     _speed_node = node->getChild("indicated-ground-speed-kt", 0, true);
  89     _time_node = node->getChild("indicated-time-min", 0, true);
  90     _name_node = node->getChild("name", 0, true);
  91     _bearing_node = node->getChild("indicated-bearing-true-deg", 0, true);
  92
  93     SGPropertyNode *dnode = node->getChild("display", 0, true);
  94     _x_shift_node = dnode->getChild("x-shift", 0, true);
  95     _y_shift_node = dnode->getChild("y-shift", 0, true);
  96     _channel_node = dnode->getChild("channel", 0, true);
  97
  98     _heading_node = fgGetNode("/orientation/heading-deg", true);
  99     _electrical_node = fgGetNode("/systems/electrical/outputs/tacan", true);
 100
 101     // Add/trigger change listener after creating all nodes
 102     _channel_in0_node->addChangeListener(this);
 103     _channel_in1_node->addChangeListener(this);
 104     _channel_in2_node->addChangeListener(this);
 105     _channel_in3_node->addChangeListener(this);
 106     _channel_in4_node->addChangeListener(this, true);
 107
 108     disabled(true);
 109 }
 110
 111 //------------------------------------------------------------------------------
 112 void TACAN::reinit()
 113 {
 114   _time_before_search_sec = 0;
 115 }
 116
 117 //------------------------------------------------------------------------------
 118 void TACAN::update(double delta_time_sec)
 119 {
 120   // don't do anything when paused
 121   if( delta_time_sec == 0 )
 122     return;
 123
 124   if(   !_serviceable_node->getBoolValue()
 125      || !_electrical_node->getBoolValue() )
 126     return disabled();
 127
 128   SGGeod pos(globals->get_aircraft_position());
 129
 130   // On timeout, scan again
 131   _time_before_search_sec -= delta_time_sec;
 132   if ((_time_before_search_sec < 0 || _new_frequency) && _frequency_mhz >= 0)
 133       search(_frequency_mhz, pos);
 134
 135   if( !_active_station || !_active_station->get_serviceable() )
 136     return disabled();
 137
 138   const SGGeod& nav_pos = _active_station->geod();
 139
 140   // Calculate the bearing and range of the station from the aircraft
 141   double az = 0;
 142   double bearing = 0;
 143   double distance = SGLimitsd::max();
 144   if( !SGGeodesy::inverse(pos, nav_pos, bearing, az, distance) )
 145     return disabled();
 146
 147   // Increase distance due to difference in altitude
 148   distance =
 149     sqrt( SGMiscd::pow<2>(distance)
 150         + SGMiscd::pow<2>(pos.getElevationM() - nav_pos.getElevationM()) );
 151   double distance_nm = distance * SG_METER_TO_NM;
 152
 153   double range_nm = adjust_range( nav_pos.getElevationFt(),
 154                                   pos.getElevationFt(),
 155                                   _active_station->get_range() );
 156
 157   if( distance_nm > range_nm )
 158     return disabled();
 159
 160   //// calculate look left/right to target, without yaw correction
 161   // double horiz_offset = bearing - heading;
 162   //
 163   // if (horiz_offset > 180.0) horiz_offset -= 360.0;
 164   // if (horiz_offset < -180.0) horiz_offset += 360.0;
 165
 166   //// now correct look left/right for yaw
 167   // horiz_offset += yaw;
 168
 169   // use the bearing for a plan position indicator display
 170   double horiz_offset = bearing;
 171
 172   // calculate values for radar display
 173   double y_shift = distance_nm * cos(horiz_offset * SG_DEGREES_TO_RADIANS);
 174   double x_shift = distance_nm * sin(horiz_offset * SG_DEGREES_TO_RADIANS);
 175
 176   // TODO probably not the best way to do this (especially with mobile stations)
 177   double speed_kt = fabs(distance_nm - _last_distance_nm)
 178                   * (3600 / delta_time_sec);
 179   _speed_node->setDoubleValue(speed_kt);
 180   _last_distance_nm = distance_nm;
 181
 182   double bias = _active_station->get_multiuse();
 183   _distance_node->setDoubleValue( SGMiscd::max(0.0, distance_nm - bias) );
 184
 185   _time_node->setDoubleValue(speed_kt > 0 ? (distance_nm/speed_kt*60.0) : 0);
 186   _bearing_node->setDoubleValue(bearing);
 187   _x_shift_node->setDoubleValue(x_shift);
 188   _y_shift_node->setDoubleValue(y_shift);
 189
 190   _name_node->setStringValue(_active_station->name());
 191   _ident_node->setStringValue(_active_station->ident());
 192   _in_range_node->setBoolValue(true);
 193
 194   _was_disabled = false;
 195 }
 196
 197 //------------------------------------------------------------------------------
 198 void TACAN::disabled(bool force)
 199 {
 200   if( _was_disabled && !force )
 201     return;
 202
 203   _last_distance_nm = 0;
 204
 205   _in_range_node->setBoolValue(false);
 206   _distance_node->setDoubleValue(0);
 207   _speed_node->setDoubleValue(0);
 208   _time_node->setDoubleValue(0);
 209   _bearing_node->setDoubleValue(0);
 210   _x_shift_node->setDoubleValue(0);
 211   _y_shift_node->setDoubleValue(0);
 212   _name_node->setStringValue("");
 213   _ident_node->setStringValue("");
 214
 215   _was_disabled = true;
 216 }
 217
 218 //------------------------------------------------------------------------------
 219 void TACAN::search (double frequency_mhz,const SGGeod& pos)
 220 {
 221   // reset search time
 222   _time_before_search_sec = 5;
 223
 224   // Get first matching mobile station (carriers/tankers/etc.)
 225   // TODO do we need to check for mobile stations with same frequency? Currently
 226   //      the distance is not taken into account.
 227   FGNavRecordRef mobile_tacan =
 228     FGNavList::findByFreq(frequency_mhz, FGNavList::mobileTacanFilter());
 229
 230   double mobile_dist = (mobile_tacan && mobile_tacan->get_serviceable())
 231                      ? SGGeodesy::distanceM(pos, mobile_tacan->geod())
 232                      : SGLimitsd::max();
 233
 234   // No get nearest TACAN/VORTAC
 235   FGNavRecordRef tacan =
 236     FGNavList::findByFreq(frequency_mhz, pos, FGNavList::tacanFilter());
 237
 238   double tacan_dist = tacan
 239                     ? SGGeodesy::distanceM(pos, tacan->geod())
 240                     : SGLimitsd::max();
 241
 242   // Select nearer station as active
 243   // TODO do we need to take more care of stations with same frequency?
 244   _active_station = mobile_dist < tacan_dist
 245                   ? mobile_tacan
 246                   : tacan;
 247 }
 248
 249 //------------------------------------------------------------------------------
 250 double TACAN::searchChannel(const std::string& channel)
 251 {
 252   FGTACANRecord *freq = globals->get_channellist()->findByChannel(channel);
 253   if( !freq )
 254   {
 255     SG_LOG(SG_INSTR, SG_DEBUG, "Invalid TACAN channel: " << channel);
 256     return 0;
 257   }
 258
 259   double frequency_khz = freq->get_freq();
 260   if(frequency_khz < 9620 || frequency_khz > 121300)
 261   {
 262     SG_LOG
 263     (
 264       SG_INSTR,
 265       SG_DEBUG,
 266       "TACAN frequency out of range: " << channel
 267                                << ": " << frequency_khz << "kHz"
 268     );
 269     return 0;
 270   }
 271
 272   return frequency_khz/100;
 273 }
 274
 275 /*
 276  * Listener callback. Maintains channel input properties,
 277  * searches new channel frequency, updates _channel and
 278  * _frequency and sets boolean _new_frequency appropriately.
 279  */
 280 void
 281 TACAN::valueChanged(SGPropertyNode *prop)
 282 {
 283     if (_listener_active)
 284         return;
 285     _listener_active++;
 286
 287     int index = prop->getIndex();
 288     std::string channel = _channel;
 289
 290     if (index) {  // channel digit or X/Y input
 291         int c;
 292         if (isdigit(c = _channel_in1_node->getStringValue()[0]))
 293             channel[0] = c;
 294         if (isdigit(c = _channel_in2_node->getStringValue()[0]))
 295             channel[1] = c;
 296         if (isdigit(c = _channel_in3_node->getStringValue()[0]))
 297             channel[2] = c;
 298         c = _channel_in4_node->getStringValue()[0];
 299         if (c == 'X' || c == 'Y')
 300             channel[3] = c;
 301
 302     } else {      // channel number input
 303         unsigned int f = prop->getIntValue();
 304         if (f >= 1 && f <= 126) {
 305             channel[0] = '0' + (f / 100) % 10;
 306             channel[1] = '0' + (f / 10) % 10;
 307             channel[2] = '0' + f % 10;
 308         }
 309     }
 310
 311     if (channel != _channel) {
 312         SG_LOG(SG_INSTR, SG_DEBUG, "new channel " << channel);
 313
 314         // write back result
 315         _channel_in0_node->setIntValue((channel[0] - '0') * 100
 316                 + (channel[1] - '0') * 10 + (channel[2] - '0'));
 317         char s[2] = "0";
 318         s[0] = channel[0], _channel_in1_node->setStringValue(s);
 319         s[0] = channel[1], _channel_in2_node->setStringValue(s);
 320         s[0] = channel[2], _channel_in3_node->setStringValue(s);
 321         s[0] = channel[3], _channel_in4_node->setStringValue(s);
 322
 323         // search channel frequency
 324         double freq = searchChannel(channel);
 325         if (freq != _frequency_mhz) {
 326             SG_LOG(SG_INSTR, SG_DEBUG, "new frequency " << freq);
 327             _frequency_node->setDoubleValue(freq);
 328             _frequency_mhz = freq;
 329             _new_frequency = true;
 330         }
 331
 332         _channel = channel;
 333         _channel_node->setStringValue(_channel);
 334         _time_before_search_sec = 0;
 335     }
 336
 337     _listener_active--;
 338 }
 339
 340 // end of TACAN.cxx