An improved ADF, but no sound yet (and not the default).
This commit is contained in:
david
parent
81d86fbe27
commit
3c764462e6
4 changed files with 299 additions and 0 deletions
|
|
@ -5,6 +5,7 @@ libInstrumentation_a_SOURCES = \
|
|||
annunciator.cxx annunciator.hxx \
|
||||
dme.cxx dme.hxx \
|
||||
gps.cxx gps.hxx \
|
||||
adf.cxx adf.hxx \
|
||||
gyro.cxx gyro.hxx \
|
||||
airspeed_indicator.cxx airspeed_indicator.hxx \
|
||||
attitude_indicator.cxx attitude_indicator.hxx \
|
||||
|
|
|
|||
205
src/Instrumentation/adf.cxx
Normal file
205
src/Instrumentation/adf.cxx
Normal file
|
|
@ -0,0 +1,205 @@
|
|||
// 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
|
||||
91
src/Instrumentation/adf.hxx
Normal file
91
src/Instrumentation/adf.hxx
Normal file
|
|
@ -0,0 +1,91 @@
|
|||
// 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
|
||||
|
|
@ -16,6 +16,7 @@
|
|||
#include "mag_compass.hxx"
|
||||
|
||||
#include "dme.hxx"
|
||||
#include "adf.hxx"
|
||||
#include "gps.hxx"
|
||||
#include "clock.hxx"
|
||||
|
||||
|
|
@ -32,6 +33,7 @@ FGInstrumentMgr::FGInstrumentMgr ()
|
|||
set_subsystem("vsi", new VerticalSpeedIndicator);
|
||||
set_subsystem("compass", new MagCompass);
|
||||
set_subsystem("dme", new DME, 1.0);
|
||||
set_subsystem("adf", new ADF, 0.15);
|
||||
set_subsystem("gps", new GPS, 0.45);
|
||||
set_subsystem("clock", new Clock, 0.25);
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue