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