src/Instrumentation/adf.cxx

   1 // adf.cxx - distance-measuring equipment.
   2 // Written by David Megginson, started 2003.
   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 #include <simgear/timing/sg_time.hxx>
  14
  15 #include <Main/fg_props.hxx>
  16 #include <Main/util.hxx>
  17 #include <Navaids/navlist.hxx>
  18
  19 #include "adf.hxx"
  20 #include <Sound/morse.hxx>
  21
  22
  23 #include <iostream>
  24 #include <string>
  25 #include <sstream>
  26
  27
  28 // Use a bigger number to be more responsive, or a smaller number
  29 // to be more sluggish.
  30 #define RESPONSIVENESS 0.5
  31
  32
  33 /**
  34  * Fiddle with the reception range a bit.
  35  *
  36  * TODO: better reception at night (??).
  37  */
  38 static double
  39 adjust_range (double transmitter_elevation_ft, double aircraft_altitude_ft,
  40               double max_range_nm)
  41 {
  42     double delta_elevation_ft =
  43         aircraft_altitude_ft - transmitter_elevation_ft;
  44     double range_nm = max_range_nm;
  45
  46                                 // kludge slightly better reception at
  47                                 // altitude
  48     if (delta_elevation_ft < 0)
  49         delta_elevation_ft = 200;
  50     if (delta_elevation_ft <= 1000)
  51         range_nm *= sqrt(delta_elevation_ft / 1000);
  52     else if (delta_elevation_ft >= 5000)
  53         range_nm *= sqrt(delta_elevation_ft / 5000);
  54     if (range_nm >= max_range_nm * 3)
  55         range_nm = max_range_nm * 3;
  56
  57     double rand = sg_random();
  58     return range_nm + (range_nm * rand * rand);
  59 }
  60
  61
  62 ADF::ADF (SGPropertyNode *node )
  63     :
  64     _name(node->getStringValue("name", "adf")),
  65     _num(node->getIntValue("number", 0)),
  66     _time_before_search_sec(0),
  67     _last_frequency_khz(-1),
  68     _transmitter_valid(false),
  69     _transmitter_pos(SGGeod::fromDeg(0, 0)),
  70     _transmitter_cart(0, 0, 0),
  71     _transmitter_range_nm(0),
  72     _ident_count(0),
  73     _last_ident_time(0),
  74     _last_volume(-1),
  75     _sgr(0)
  76 {
  77 }
  78
  79 ADF::~ADF ()
  80 {
  81 }
  82
  83 void
  84 ADF::init ()
  85 {
  86     string branch;
  87     branch = "/instrumentation/" + _name;
  88
  89     SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
  90     _longitude_node = fgGetNode("/position/longitude-deg", true);
  91     _latitude_node = fgGetNode("/position/latitude-deg", true);
  92     _altitude_node = fgGetNode("/position/altitude-ft", true);
  93     _heading_node = fgGetNode("/orientation/heading-deg", true);
  94     _serviceable_node = node->getChild("serviceable", 0, true);
  95     _error_node = node->getChild("error-deg", 0, true);
  96     _electrical_node = fgGetNode("/systems/electrical/outputs/adf", true);
  97     branch = branch + "/frequencies";
  98     SGPropertyNode *fnode = node->getChild("frequencies", 0, true);
  99     _frequency_node = fnode->getChild("selected-khz", 0, true);
 100     _mode_node = node->getChild("mode", 0, true);
 101     _volume_node = node->getChild("volume-norm", 0, true);
 102     _in_range_node = node->getChild("in-range", 0, true);
 103     _bearing_node = node->getChild("indicated-bearing-deg", 0, true);
 104     _ident_node = node->getChild("ident", 0, true);
 105     _ident_audible_node = node->getChild("ident-audible", 0, true);
 106     _power_btn_node = node->getChild("power-btn", 0, true);
 107     _operable_node      = node->getChild("operable", 0, true);
 108
 109     if (_power_btn_node->getType() == simgear::props::NONE)
 110       _power_btn_node->setBoolValue(true); // front end didn't implement a power button
 111
 112     SGSoundMgr *smgr = globals->get_soundmgr();
 113     _sgr = smgr->find("avionics", true);
 114     _sgr->tie_to_listener();
 115
 116     std::ostringstream temp;
 117     temp << _name << _num;
 118     _adf_ident = temp.str();
 119 }
 120
 121 void
 122 ADF::update (double delta_time_sec)
 123 {
 124                                 // If it's off, don't waste any time.
 125     if (_electrical_node->getDoubleValue() < 8.0
 126             || !_serviceable_node->getBoolValue()
 127             || !_power_btn_node->getBoolValue()     ) {
 128         _in_range_node->setBoolValue(false);
 129         _operable_node->setBoolValue(false);
 130         _ident_node->setStringValue("");
 131         return;
 132     }
 133
 134     _operable_node->setBoolValue(true);
 135     string mode = _mode_node->getStringValue();
 136     if (mode == "ant" || mode == "test") set_bearing(delta_time_sec, 90);
 137     if (mode != "bfo" && mode != "adf") {
 138         _in_range_node->setBoolValue(false);
 139         _ident_node->setStringValue("");
 140         return;
 141     }
 142                                 // Get the frequency
 143     int frequency_khz = _frequency_node->getIntValue();
 144     if (frequency_khz != _last_frequency_khz) {
 145         _time_before_search_sec = 0;
 146         _last_frequency_khz = frequency_khz;
 147     }
 148
 149                                 // Get the aircraft position
 150     double longitude_deg = _longitude_node->getDoubleValue();
 151     double latitude_deg = _latitude_node->getDoubleValue();
 152     double altitude_m = _altitude_node->getDoubleValue();
 153
 154     double longitude_rad = longitude_deg * SGD_DEGREES_TO_RADIANS;
 155     double latitude_rad = latitude_deg * SGD_DEGREES_TO_RADIANS;
 156
 157                                 // On timeout, scan again
 158     _time_before_search_sec -= delta_time_sec;
 159     if (_time_before_search_sec < 0)
 160         search(frequency_khz, longitude_rad, latitude_rad, altitude_m);
 161
 162     if (!_transmitter_valid) {
 163         _in_range_node->setBoolValue(false);
 164         _ident_node->setStringValue("");
 165         return;
 166     }
 167
 168                                 // Calculate the bearing to the transmitter
 169     SGGeod geod = SGGeod::fromRadM(longitude_rad, latitude_rad, altitude_m);
 170     SGVec3d location = SGVec3d::fromGeod(geod);
 171
 172     double distance_nm = dist(_transmitter_cart, location) * SG_METER_TO_NM;
 173     double range_nm = adjust_range(_transmitter_pos.getElevationFt(),
 174                                    altitude_m * SG_METER_TO_FEET,
 175                                    _transmitter_range_nm);
 176
 177     if (distance_nm <= range_nm) {
 178
 179         double bearing, az2, s;
 180         double heading = _heading_node->getDoubleValue();
 181
 182         geo_inverse_wgs_84(geod, _transmitter_pos,
 183                            &bearing, &az2, &s);
 184         _in_range_node->setBoolValue(true);
 185
 186         bearing -= heading;
 187         if (bearing < 0)
 188             bearing += 360;
 189         set_bearing(delta_time_sec, bearing);
 190
 191         // adf ident sound
 192         float volume;
 193         if ( _ident_audible_node->getBoolValue() )
 194             volume = _volume_node->getFloatValue();
 195         else
 196             volume = 0.0;
 197
 198         if ( volume != _last_volume ) {
 199             _last_volume = volume;
 200
 201             SGSoundSample *sound;
 202             sound = _sgr->find( _adf_ident );
 203             if ( sound != NULL )
 204                 sound->set_volume( volume );
 205             else
 206                 SG_LOG( SG_INSTR, SG_ALERT, "Can't find adf-ident sound" );
 207         }
 208
 209         time_t cur_time = globals->get_time_params()->get_cur_time();
 210         if ( _last_ident_time < cur_time - 30 ) {
 211             _last_ident_time = cur_time;
 212             _ident_count = 0;
 213         }
 214
 215         if ( _ident_count < 4 ) {
 216             if ( !_sgr->is_playing(_adf_ident) && (volume > 0.05) ) {
 217                 _sgr->play_once( _adf_ident );
 218                 ++_ident_count;
 219             }
 220         }
 221     } else {
 222         _in_range_node->setBoolValue(false);
 223         _ident_node->setStringValue("");
 224         _sgr->stop( _adf_ident );
 225     }
 226 }
 227
 228 void
 229 ADF::search (double frequency_khz, double longitude_rad,
 230              double latitude_rad, double altitude_m)
 231 {
 232     string ident = "";
 233                                 // reset search time
 234     _time_before_search_sec = 1.0;
 235
 236                                 // try the ILS list first
 237     FGNavRecord *nav = globals->get_navlist()->findByFreq(frequency_khz,
 238       SGGeod::fromRadM(longitude_rad, latitude_rad, altitude_m));
 239
 240     _transmitter_valid = (nav != NULL);
 241     if ( _transmitter_valid ) {
 242         ident = nav->get_trans_ident();
 243         if ( ident != _last_ident ) {
 244             _transmitter_pos = nav->geod();
 245             _transmitter_cart = nav->cart();
 246             _transmitter_range_nm = nav->get_range();
 247         }
 248     }
 249
 250     if ( _last_ident != ident ) {
 251         _last_ident = ident;
 252         _ident_node->setStringValue(ident.c_str());
 253
 254         if ( _sgr->exists( _adf_ident ) ) {
 255             // stop is required! -- remove alone wouldn't stop immediately
 256             _sgr->stop( _adf_ident );
 257             _sgr->remove( _adf_ident );
 258         }
 259
 260         SGSoundSample *sound;
 261         sound = FGMorse::instance()->make_ident( ident, FGMorse::LO_FREQUENCY );
 262         sound->set_volume(_last_volume = 0);
 263         _sgr->add( sound, _adf_ident );
 264
 265         int offset = (int)(sg_random() * 30.0);
 266         _ident_count = offset / 4;
 267         _last_ident_time = globals->get_time_params()->get_cur_time() -
 268             offset;
 269     }
 270 }
 271
 272 void
 273 ADF::set_bearing (double dt, double bearing_deg)
 274 {
 275     double old_bearing_deg = _bearing_node->getDoubleValue();
 276
 277     while ((bearing_deg - old_bearing_deg) >= 180)
 278         old_bearing_deg += 360;
 279     while ((bearing_deg - old_bearing_deg) <= -180)
 280         old_bearing_deg -= 360;
 281     bearing_deg += _error_node->getDoubleValue();
 282     bearing_deg =
 283         fgGetLowPass(old_bearing_deg, bearing_deg, dt * RESPONSIVENESS);
 284
 285     _bearing_node->setDoubleValue(bearing_deg);
 286 }
 287
 288
 289 // end of adf.cxx