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