--- /dev/null
+// adf.cxx - distance-measuring equipment.
+// Written by David Megginson, started 2003.
+//
+// This file is in the Public Domain and comes with no warranty.
+
+#include <simgear/compiler.h>
+#include <simgear/math/sg_geodesy.hxx>
+#include <simgear/math/sg_random.h>
+
+#include <Main/fg_props.hxx>
+#include <Main/util.hxx>
+#include <Navaids/navlist.hxx>
+
+#include "adf.hxx"
+
+
+// Use a bigger number to be more responsive, or a smaller number
+// to be more sluggish.
+#define RESPONSIVENESS 0.5
+
+
+/**
+ * Fiddle with the reception range a bit.
+ *
+ * TODO: better reception at night (??).
+ */
+static double
+adjust_range (double transmitter_elevation_ft, double aircraft_altitude_ft,
+ double max_range_nm)
+{
+ double delta_elevation_ft =
+ aircraft_altitude_ft - transmitter_elevation_ft;
+ double range_nm = max_range_nm;
+
+ // kludge slightly better reception at
+ // altitude
+ if (delta_elevation_ft < 0)
+ delta_elevation_ft = 200;
+ if (delta_elevation_ft <= 1000)
+ range_nm *= sqrt(delta_elevation_ft / 1000);
+ else if (delta_elevation_ft >= 5000)
+ range_nm *= sqrt(delta_elevation_ft / 5000);
+ if (range_nm >= max_range_nm * 3)
+ range_nm = max_range_nm * 3;
+
+ double rand = sg_random();
+ return range_nm + (range_nm * rand * rand);
+}
+
+
+ADF::ADF ()
+ : _last_frequency_khz(-1),
+ _time_before_search_sec(0),
+ _transmitter_valid(false),
+ _transmitter_elevation_ft(0),
+ _transmitter_range_nm(0)
+{
+}
+
+ADF::~ADF ()
+{
+}
+
+void
+ADF::init ()
+{
+ _longitude_node = fgGetNode("/position/longitude-deg", true);
+ _latitude_node = fgGetNode("/position/latitude-deg", true);
+ _altitude_node = fgGetNode("/position/altitude-ft", true);
+ _heading_node = fgGetNode("/orientation/heading-deg", true);
+ _serviceable_node = fgGetNode("/instrumentation/adf/serviceable", true);
+ _error_node = fgGetNode("/instrumentation/adf/error-deg", true);
+ _electrical_node = fgGetNode("/systems/electrical/outputs/adf", true);
+ _frequency_node =
+ fgGetNode("/instrumentation/adf/frequencies/selected-khz", true);
+ _mode_node = fgGetNode("/instrumentation/adf/mode", true);
+
+ _in_range_node = fgGetNode("/instrumentation/adf/in-range", true);
+ _bearing_node =
+ fgGetNode("/instrumentation/adf/indicated-bearing-deg", true);
+ _ident_node = fgGetNode("/instrumentation/adf/ident", true);
+}
+
+void
+ADF::update (double delta_time_sec)
+{
+ // If it's off, don't waste any time.
+ if (!_electrical_node->getBoolValue() ||
+ !_serviceable_node->getBoolValue()) {
+ set_bearing(delta_time_sec, 90);
+ _ident_node->setStringValue("");
+ return;
+ }
+
+ // Get the frequency
+ int frequency_khz = _frequency_node->getIntValue();
+ if (frequency_khz != _last_frequency_khz) {
+ _time_before_search_sec = 0;
+ _last_frequency_khz = frequency_khz;
+ }
+
+ // Get the aircraft position
+ double longitude_deg = _longitude_node->getDoubleValue();
+ double latitude_deg = _latitude_node->getDoubleValue();
+ double altitude_m = _altitude_node->getDoubleValue();
+
+ double longitude_rad = longitude_deg * SGD_DEGREES_TO_RADIANS;
+ double latitude_rad = latitude_deg * SGD_DEGREES_TO_RADIANS;
+
+ // On timeout, scan again
+ _time_before_search_sec -= delta_time_sec;
+ if (_time_before_search_sec < 0)
+ search(frequency_khz, longitude_rad, latitude_rad, altitude_m);
+
+ // If it's off, don't bother.
+ string mode = _mode_node->getStringValue();
+ if (!_transmitter_valid || (mode != "bfo" && mode != "adf")) {
+ set_bearing(delta_time_sec, 90);
+ _ident_node->setStringValue("");
+ return;
+ }
+
+ // Calculate the bearing to the transmitter
+ Point3D location =
+ sgGeodToCart(Point3D(longitude_rad, latitude_rad, altitude_m));
+
+ double distance_nm = _transmitter.distance3D(location) * SG_METER_TO_NM;
+ double range_nm = adjust_range(_transmitter_elevation_ft,
+ altitude_m * SG_METER_TO_FEET,
+ _transmitter_range_nm);
+ if (distance_nm <= range_nm) {
+
+ double bearing, az2, s;
+ double heading = _heading_node->getDoubleValue();
+
+ geo_inverse_wgs_84(altitude_m,
+ latitude_deg,
+ longitude_deg,
+ _transmitter_lat_deg,
+ _transmitter_lon_deg,
+ &bearing, &az2, &s);
+ _in_range_node->setBoolValue(true);
+
+ bearing -= heading;
+ if (bearing < 0)
+ bearing += 360;
+ set_bearing(delta_time_sec, bearing);
+ } else {
+ _in_range_node->setBoolValue(false);
+ set_bearing(delta_time_sec, 90);
+ _ident_node->setStringValue("");
+ }
+}
+
+void
+ADF::search (double frequency_khz, double longitude_rad,
+ double latitude_rad, double altitude_m)
+{
+ string ident = "";
+
+ // reset search time
+ _time_before_search_sec = 1.0;
+
+ // try the ILS list first
+ FGNav * nav =
+ current_navlist->findByFreq(frequency_khz, longitude_rad,
+ latitude_rad, altitude_m);
+
+ if (nav !=0) {
+ _transmitter_valid = true;
+ ident = nav->get_trans_ident();
+ if (ident !=_last_ident) {
+ _transmitter_lon_deg = nav->get_lon();
+ _transmitter_lat_deg = nav->get_lat();
+ _transmitter = Point3D(nav->get_x(), nav->get_y(), nav->get_z());
+ _transmitter_elevation_ft = nav->get_elev() * SG_METER_TO_FEET;
+ _transmitter_range_nm = nav->get_range();
+ }
+ } else {
+ _transmitter_valid = false;
+ }
+ _last_ident = ident;
+ _ident_node->setStringValue(ident.c_str());
+}
+
+void
+ADF::set_bearing (double dt, double bearing_deg)
+{
+ double old_bearing_deg = _bearing_node->getDoubleValue();
+
+ bearing_deg += _error_node->getDoubleValue();
+
+ while ((bearing_deg - old_bearing_deg) >= 180)
+ old_bearing_deg += 360;
+ while ((bearing_deg - old_bearing_deg) <= -180)
+ old_bearing_deg -= 360;
+
+ bearing_deg =
+ fgGetLowPass(old_bearing_deg, bearing_deg, dt * RESPONSIVENESS);
+
+ _bearing_node->setDoubleValue(bearing_deg);
+}
+
+
+// end of adf.cxx
--- /dev/null
+// adf.hxx - automatic direction finder.
+// Written by David Megginson, started 2003.
+//
+// This file is in the Public Domain and comes with no warranty.
+
+
+#ifndef __INSTRUMENTS_ADF_HXX
+#define __INSTRUMENTS_ADF_HXX 1
+
+#ifndef __cplusplus
+# error This library requires C++
+#endif
+
+#include <string>
+
+#include <simgear/math/point3d.hxx>
+#include <simgear/props/props.hxx>
+
+#include <simgear/structure/subsystem_mgr.hxx>
+
+SG_USING_STD(string);
+
+
+/**
+ * Model an ADF radio.
+ *
+ * Input properties:
+ *
+ * /position/longitude-deg
+ * /position/latitude-deg
+ * /position/altitude-ft
+ * /orientation/heading-deg
+ * /systems/electrical/outputs/adf
+ * /instrumentation/adf/serviceable
+ * /instrumentation/adf/error-deg
+ * /instrumentation/adf/frequencies/selected-khz
+ * /instrumentation/adf/mode
+ *
+ * Output properties:
+ *
+ * /instrumentation/adf/in-range
+ * /instrumentation/adf/indicated-bearing-deg
+ * /instrumentation/adf/ident
+ */
+class ADF : public SGSubsystem
+{
+
+public:
+
+ ADF ();
+ virtual ~ADF ();
+
+ virtual void init ();
+ virtual void update (double delta_time_sec);
+
+private:
+
+ void set_bearing (double delta_time_sec, double bearing);
+
+ void search (double frequency, double longitude_rad,
+ double latitude_rad, double altitude_m);
+
+ SGPropertyNode_ptr _longitude_node;
+ SGPropertyNode_ptr _latitude_node;
+ SGPropertyNode_ptr _altitude_node;
+ SGPropertyNode_ptr _heading_node;
+ SGPropertyNode_ptr _serviceable_node;
+ SGPropertyNode_ptr _error_node;
+ SGPropertyNode_ptr _electrical_node;
+ SGPropertyNode_ptr _frequency_node;
+ SGPropertyNode_ptr _mode_node;
+
+ SGPropertyNode_ptr _in_range_node;
+ SGPropertyNode_ptr _bearing_node;
+ SGPropertyNode_ptr _ident_node;
+
+ double _time_before_search_sec;
+
+ int _last_frequency_khz;
+ bool _transmitter_valid;
+ string _last_ident;
+ Point3D _transmitter;
+ double _transmitter_lon_deg;
+ double _transmitter_lat_deg;
+ double _transmitter_elevation_ft;
+ double _transmitter_range_nm;
+
+};
+
+
+#endif // __INSTRUMENTS_ADF_HXX
projects / flightgear.git / commitdiff