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     : _last_frequency_khz(-1),
  53       _time_before_search_sec(0),
  54       _transmitter_valid(false),
  55       _transmitter_elevation_ft(0),
  56       _transmitter_range_nm(0)
  57 {
  58 }
  59
  60 ADF::~ADF ()
  61 {
  62 }
  63
  64 void
  65 ADF::init ()
  66 {
  67     _longitude_node = fgGetNode("/position/longitude-deg", true);
  68     _latitude_node = fgGetNode("/position/latitude-deg", true);
  69     _altitude_node = fgGetNode("/position/altitude-ft", true);
  70     _heading_node = fgGetNode("/orientation/heading-deg", true);
  71     _serviceable_node = fgGetNode("/instrumentation/adf/serviceable", true);
  72     _error_node = fgGetNode("/instrumentation/adf/error-deg", true);
  73     _electrical_node = fgGetNode("/systems/electrical/outputs/adf", true);
  74     _frequency_node =
  75         fgGetNode("/instrumentation/adf/frequencies/selected-khz", true);
  76     _mode_node = fgGetNode("/instrumentation/adf/mode", true);
  77
  78     _in_range_node = fgGetNode("/instrumentation/adf/in-range", true);
  79     _bearing_node =
  80         fgGetNode("/instrumentation/adf/indicated-bearing-deg", true);
  81     _ident_node = fgGetNode("/instrumentation/adf/ident", true);
  82 }
  83
  84 void
  85 ADF::update (double delta_time_sec)
  86 {
  87                                 // If it's off, don't waste any time.
  88     if (!_electrical_node->getBoolValue() ||
  89         !_serviceable_node->getBoolValue()) {
  90         set_bearing(delta_time_sec, 90);
  91         _ident_node->setStringValue("");
  92         return;
  93     }
  94
  95                                 // Get the frequency
  96     int frequency_khz = _frequency_node->getIntValue();
  97     if (frequency_khz != _last_frequency_khz) {
  98         _time_before_search_sec = 0;
  99         _last_frequency_khz = frequency_khz;
 100     }
 101
 102                                 // Get the aircraft position
 103     double longitude_deg = _longitude_node->getDoubleValue();
 104     double latitude_deg = _latitude_node->getDoubleValue();
 105     double altitude_m = _altitude_node->getDoubleValue();
 106
 107     double longitude_rad = longitude_deg * SGD_DEGREES_TO_RADIANS;
 108     double latitude_rad = latitude_deg * SGD_DEGREES_TO_RADIANS;
 109
 110                                 // On timeout, scan again
 111     _time_before_search_sec -= delta_time_sec;
 112     if (_time_before_search_sec < 0)
 113         search(frequency_khz, longitude_rad, latitude_rad, altitude_m);
 114
 115                                 // If it's off, don't bother.
 116     string mode = _mode_node->getStringValue();
 117     if (!_transmitter_valid || (mode != "bfo" && mode != "adf"))
 118     {
 119         set_bearing(delta_time_sec, 90);
 120         _ident_node->setStringValue("");
 121         return;
 122     }
 123
 124                                 // Calculate the bearing to the transmitter
 125     Point3D location =
 126         sgGeodToCart(Point3D(longitude_rad, latitude_rad, altitude_m));
 127
 128     double distance_nm = _transmitter.distance3D(location) * SG_METER_TO_NM;
 129     double range_nm = adjust_range(_transmitter_elevation_ft,
 130                                    altitude_m * SG_METER_TO_FEET,
 131                                    _transmitter_range_nm);
 132     if (distance_nm <= range_nm) {
 133
 134         double bearing, az2, s;
 135         double heading = _heading_node->getDoubleValue();
 136
 137         geo_inverse_wgs_84(altitude_m,
 138                            latitude_deg,
 139                            longitude_deg,
 140                            _transmitter_lat_deg,
 141                            _transmitter_lon_deg,
 142                            &bearing, &az2, &s);
 143         _in_range_node->setBoolValue(true);
 144
 145         bearing -= heading;
 146         if (bearing < 0)
 147             bearing += 360;
 148         set_bearing(delta_time_sec, bearing);
 149     } else {
 150         _in_range_node->setBoolValue(false);
 151         set_bearing(delta_time_sec, 90);
 152         _ident_node->setStringValue("");
 153     }
 154 }
 155
 156 void
 157 ADF::search (double frequency_khz, double longitude_rad,
 158              double latitude_rad, double altitude_m)
 159 {
 160     string ident = "";
 161
 162                                 // reset search time
 163     _time_before_search_sec = 1.0;
 164
 165                                 // try the ILS list first
 166     FGNav * nav =
 167         current_navlist->findByFreq(frequency_khz, longitude_rad,
 168                                     latitude_rad, altitude_m);
 169
 170     if (nav !=0) {
 171         _transmitter_valid = true;
 172         ident = nav->get_trans_ident();
 173         if (ident !=_last_ident) {
 174             _transmitter_lon_deg = nav->get_lon();
 175             _transmitter_lat_deg = nav->get_lat();
 176             _transmitter = Point3D(nav->get_x(), nav->get_y(), nav->get_z());
 177             _transmitter_elevation_ft = nav->get_elev() * SG_METER_TO_FEET;
 178             _transmitter_range_nm = nav->get_range();
 179         }
 180     } else {
 181         _transmitter_valid = false;
 182     }
 183     _last_ident = ident;
 184     _ident_node->setStringValue(ident.c_str());
 185 }
 186
 187 void
 188 ADF::set_bearing (double dt, double bearing_deg)
 189 {
 190     double old_bearing_deg = _bearing_node->getDoubleValue();
 191
 192     bearing_deg += _error_node->getDoubleValue();
 193
 194     while ((bearing_deg - old_bearing_deg) >= 180)
 195         old_bearing_deg += 360;
 196     while ((bearing_deg - old_bearing_deg) <= -180)
 197         old_bearing_deg -= 360;
 198
 199     bearing_deg =
 200         fgGetLowPass(old_bearing_deg, bearing_deg, dt * RESPONSIVENESS);
 201
 202     _bearing_node->setDoubleValue(bearing_deg);
 203 }
 204
 205
 206 // end of adf.cxx