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_AUTOPILOT, SG_WARN, "Error in dme config logic" );
  61             if ( name.length() ) {
  62                 SG_LOG( SG_AUTOPILOT, 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", 0, true);
 102     _time_node = node->getChild("indicated-time-min", 0, true);
 103
 104     _serviceable_node->setBoolValue(true);
 105 }
 106
 107 void
 108 DME::update (double delta_time_sec)
 109 {
 110                                 // Figure out the source
 111     const char * source = _source_node->getStringValue();
 112     if (source[0] == '\0') {
 113         string branch;
 114         branch = "/instrumentation/" + name + "/frequencies/selected-mhz";
 115         source = branch.c_str();
 116         _source_node->setStringValue(source);
 117     }
 118
 119                                 // Get the frequency
 120     double frequency_mhz = fgGetDouble(source, 108.0);
 121     if (frequency_mhz != _last_frequency_mhz) {
 122         _time_before_search_sec = 0;
 123         _last_frequency_mhz = frequency_mhz;
 124     }
 125
 126                                 // Get the aircraft position
 127     double longitude_rad =
 128         _longitude_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 129     double latitude_rad =
 130         _latitude_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 131     double altitude_m =
 132         _altitude_node->getDoubleValue() * SG_FEET_TO_METER;
 133
 134                                 // On timeout, scan again
 135     _time_before_search_sec -= delta_time_sec;
 136     if (_time_before_search_sec < 0)
 137         search(frequency_mhz, longitude_rad,
 138                latitude_rad, altitude_m);
 139
 140                                 // If it's off, don't bother.
 141     if (!_serviceable_node->getBoolValue() ||
 142         !_electrical_node->getBoolValue() ||
 143         !_transmitter_valid) {
 144         _last_distance_nm = 0;
 145         _in_range_node->setBoolValue(false);
 146         _distance_node->setDoubleValue(0);
 147         _speed_node->setDoubleValue(0);
 148         _time_node->setDoubleValue(0);
 149         return;
 150     }
 151
 152                                 // Calculate the distance to the transmitter
 153     Point3D location =
 154         sgGeodToCart(Point3D(longitude_rad, latitude_rad, altitude_m));
 155     double distance_nm = _transmitter.distance3D(location) * SG_METER_TO_NM;
 156     double range_nm = adjust_range(_transmitter_elevation_ft,
 157                                    altitude_m * SG_METER_TO_FEET,
 158                                    _transmitter_range_nm);
 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