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