src/Instrumentation/dme.cxx

   1 // dme.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 <Navaids/navlist.hxx>
  12
  13 #include "dme.hxx"
  14
  15
  16 /**
  17  * Adjust the range.
  18  *
  19  * Start by calculating the radar horizon based on the elevation
  20  * difference, then clamp to the maximum, then add a fudge for
  21  * borderline reception.
  22  */
  23 static double
  24 adjust_range (double transmitter_elevation_ft, double aircraft_altitude_ft,
  25               double max_range_nm)
  26 {
  27     double delta_elevation_ft =
  28         fabs(aircraft_altitude_ft - transmitter_elevation_ft);
  29     double range_nm = 1.23 * sqrt(delta_elevation_ft);
  30     if (range_nm > max_range_nm)
  31         range_nm = max_range_nm;
  32     else if (range_nm < 20.0)
  33         range_nm = 20.0;
  34     double rand = sg_random();
  35     return range_nm + (range_nm * rand * rand);
  36 }
  37
  38
  39 DME::DME ( SGPropertyNode *node )
  40     : _last_distance_nm(0),
  41       _last_frequency_mhz(-1),
  42       _time_before_search_sec(0),
  43       _transmitter_valid(false),
  44       _transmitter_elevation_ft(0),
  45       _transmitter_range_nm(0),
  46       _transmitter_bias(0.0),
  47       name("dme"),
  48       num(0)
  49 {
  50     int i;
  51     for ( i = 0; i < node->nChildren(); ++i ) {
  52         SGPropertyNode *child = node->getChild(i);
  53         string cname = child->getName();
  54         string cval = child->getStringValue();
  55         if ( cname == "name" ) {
  56             name = cval;
  57         } else if ( cname == "number" ) {
  58             num = child->getIntValue();
  59         } else {
  60             SG_LOG( SG_INSTR, SG_WARN, "Error in dme config logic" );
  61             if ( name.length() ) {
  62                 SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
  63             }
  64         }
  65     }
  66 }
  67
  68 DME::DME ()
  69     : _last_distance_nm(0),
  70       _last_frequency_mhz(-1),
  71       _time_before_search_sec(0),
  72       _transmitter_valid(false),
  73       _transmitter_elevation_ft(0),
  74       _transmitter_range_nm(0),
  75       _transmitter_bias(0.0)
  76 {
  77 }
  78
  79 DME::~DME ()
  80 {
  81 }
  82
  83 void
  84 DME::init ()
  85 {
  86     string branch;
  87     branch = "/instrumentation/" + name;
  88
  89     SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
  90
  91     _longitude_node = fgGetNode("/position/longitude-deg", true);
  92     _latitude_node = fgGetNode("/position/latitude-deg", true);
  93     _altitude_node = fgGetNode("/position/altitude-ft", true);
  94     _serviceable_node = node->getChild("serviceable", 0, true);
  95     _electrical_node = fgGetNode("/systems/electrical/outputs/dme", true);
  96     SGPropertyNode *fnode = node->getChild("frequencies", 0, true);
  97     _source_node = fnode->getChild("source", 0, true);
  98     _frequency_node = fnode->getChild("selected-mhz", 0, true);
  99     _in_range_node = node->getChild("in-range", 0, true);
 100     _distance_node = node->getChild("indicated-distance-nm", 0, true);
 101     _speed_node = node->getChild("indicated-ground-speed-kt", 0, true);
 102     _time_node = node->getChild("indicated-time-min", 0, true);
 103 }
 104
 105 void
 106 DME::update (double delta_time_sec)
 107 {
 108                                 // Figure out the source
 109     const char * source = _source_node->getStringValue();
 110     if (source[0] == '\0') {
 111         string branch;
 112         branch = "/instrumentation/" + name + "/frequencies/selected-mhz";
 113         _source_node->setStringValue(branch.c_str());
 114         source = _source_node->getStringValue();
 115     }
 116                                 // Get the frequency
 117
 118     double frequency_mhz = fgGetDouble(source, 108.0);
 119     if (frequency_mhz != _last_frequency_mhz) {
 120         _time_before_search_sec = 0;
 121         _last_frequency_mhz = frequency_mhz;
 122     }
 123
 124                                 // Get the aircraft position
 125     double longitude_rad =
 126         _longitude_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 127     double latitude_rad =
 128         _latitude_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 129     double altitude_m =
 130         _altitude_node->getDoubleValue() * SG_FEET_TO_METER;
 131
 132                                 // On timeout, scan again
 133     _time_before_search_sec -= delta_time_sec;
 134     if (_time_before_search_sec < 0)
 135         search(frequency_mhz, longitude_rad,
 136                latitude_rad, altitude_m);
 137
 138                                 // If it's off, don't bother.
 139     if (!_serviceable_node->getBoolValue() ||
 140         !_electrical_node->getBoolValue() ||
 141         !_transmitter_valid) {
 142         _last_distance_nm = 0;
 143         _in_range_node->setBoolValue(false);
 144         _distance_node->setDoubleValue(0);
 145         _speed_node->setDoubleValue(0);
 146         _time_node->setDoubleValue(0);
 147         return;
 148     }
 149
 150                                 // Calculate the distance to the transmitter
 151     Point3D location =
 152         sgGeodToCart(Point3D(longitude_rad, latitude_rad, altitude_m));
 153     double distance_nm = _transmitter.distance3D(location) * SG_METER_TO_NM;
 154
 155     double range_nm = adjust_range(_transmitter_elevation_ft,
 156                                    altitude_m * SG_METER_TO_FEET,
 157                                    _transmitter_range_nm);
 158
 159     if (distance_nm <= range_nm) {
 160         double speed_kt = (fabs(distance_nm - _last_distance_nm) *
 161                            ((1 / delta_time_sec) * 3600.0));
 162         _last_distance_nm = distance_nm;
 163
 164         _in_range_node->setBoolValue(true);
 165         double tmp_dist = distance_nm - _transmitter_bias;
 166         if ( tmp_dist < 0.0 ) {
 167             tmp_dist = 0.0;
 168         }
 169         _distance_node->setDoubleValue( tmp_dist );
 170         _speed_node->setDoubleValue(speed_kt);
 171         _time_node->setDoubleValue(distance_nm/speed_kt*60.0);
 172
 173     } else {
 174         _last_distance_nm = 0;
 175         _in_range_node->setBoolValue(false);
 176         _distance_node->setDoubleValue(0);
 177         _speed_node->setDoubleValue(0);
 178         _time_node->setDoubleValue(0);
 179     }
 180 }
 181
 182 void
 183 DME::search (double frequency_mhz, double longitude_rad,
 184              double latitude_rad, double altitude_m)
 185 {
 186     // reset search time
 187     _time_before_search_sec = 1.0;
 188
 189     // try the ILS list first
 190     FGNavRecord *dme
 191         = globals->get_dmelist()->findByFreq( frequency_mhz, longitude_rad,
 192                                               latitude_rad, altitude_m);
 193
 194     _transmitter_valid = (dme != NULL);
 195
 196     if ( _transmitter_valid ) {
 197         _transmitter = Point3D(dme->get_x(), dme->get_y(), dme->get_z());
 198         _transmitter_elevation_ft = dme->get_elev_ft();
 199         _transmitter_range_nm = dme->get_range();
 200         _transmitter_bias = dme->get_multiuse();
 201     }
 202 }
 203
 204 // end of dme.cxx