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