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