fly/flightgear
1
0
Fork 0
Code Activity
flightgear/src/Instrumentation/adf.cxx
ehofman c4a38e10ec James Turner: 
Attached patch + new file make FGNavRecord have a .cxx file, and a constructor w
hich allows all the parameters to be supplied. Along the way I also cleaned up t
he navrecord.hxx header, lots more header pollution has been killed.

Some long methods are no longer inline, but were all suspiciously long to meet c
ompiler inlining criteria (I'm not clear if the 'inline' keyword is advisory or
mandatory in this situation) - I don't expect this to affect performance in any
way whatsoever.

The constructor addition is to support some hacking I'm doing improving the star
tup performance of the navDB by lazily loading the data, and caching it in a mor
e efficient format than text. I'm submitting this change (and probably some othe
r small tweaks in the future) since they are worthwhile as cleanups regardless o
f how my current experiments work out.
2008-08-03 14:34:42 +00:00

273 lines
8.5 KiB
C++
Raw Blame History

// adf.cxx - distance-measuring equipment.
// Written by David Megginson, started 2003.
//
// This file is in the Public Domain and comes with no warranty.
#ifdef HAVE_CONFIG_H
# include <config.h>
#endif
#include <simgear/compiler.h>
#include <simgear/math/sg_geodesy.hxx>
#include <simgear/math/sg_random.h>
#include <simgear/timing/sg_time.hxx>
#include <Main/fg_props.hxx>
#include <Main/util.hxx>
#include <Navaids/navlist.hxx>
#include "adf.hxx"
#include <iostream>
#include <string>
#include <sstream>
// 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 (SGPropertyNode *node )
:
_name(node->getStringValue("name", "adf")),
_num(node->getIntValue("number", 0)),
_time_before_search_sec(0),
_last_frequency_khz(-1),
_transmitter_valid(false),
_transmitter_pos(SGGeod::fromDeg(0, 0)),
_transmitter_cart(0, 0, 0),
_transmitter_range_nm(0),
_ident_count(0),
_last_ident_time(0),
_last_volume(-1)
{
}
ADF::~ADF ()
{
}
void
ADF::init ()
{
string branch;
branch = "/instrumentation/" + _name;
SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
_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 = node->getChild("serviceable", 0, true);
_error_node = node->getChild("error-deg", 0, true);
_electrical_node = fgGetNode("/systems/electrical/outputs/adf", true);
branch = branch + "/frequencies";
SGPropertyNode *fnode = node->getChild("frequencies", 0, true);
_frequency_node = fnode->getChild("selected-khz", 0, true);
_mode_node = node->getChild("mode", 0, true);
_volume_node = node->getChild("volume-norm", 0, true);
_in_range_node = node->getChild("in-range", 0, true);
_bearing_node = node->getChild("indicated-bearing-deg", 0, true);
_ident_node = node->getChild("ident", 0, true);
_ident_audible_node = node->getChild("ident-audible", 0, true);
morse.init();
std::ostringstream temp;
temp << _name << _num;
_adf_ident = temp.str();
}
void
ADF::update (double delta_time_sec)
{
// If it's off, don't waste any time.
if (_electrical_node->getDoubleValue() < 1.0
|| !_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
SGGeod geod = SGGeod::fromRadM(longitude_rad, latitude_rad, altitude_m);
SGVec3d location = SGVec3d::fromGeod(geod);
double distance_nm = dist(_transmitter_cart, location) * SG_METER_TO_NM;
double range_nm = adjust_range(_transmitter_pos.getElevationFt(),
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(geod, _transmitter_pos,
&bearing, &az2, &s);
_in_range_node->setBoolValue(true);
bearing -= heading;
if (bearing < 0)
bearing += 360;
set_bearing(delta_time_sec, bearing);
// adf ident sound
double volume;
if ( _ident_audible_node->getBoolValue() )
volume = _volume_node->getDoubleValue();
else
volume = 0.0;
if ( volume != _last_volume ) {
_last_volume = volume;
SGSoundSample *sound;
sound = globals->get_soundmgr()->find( _adf_ident );
if ( sound != NULL )
sound->set_volume( volume );
else
SG_LOG( SG_GENERAL, SG_ALERT, "Can't find adf-ident sound" );
}
time_t cur_time = globals->get_time_params()->get_cur_time();
if ( _last_ident_time < cur_time - 30 ) {
_last_ident_time = cur_time;
_ident_count = 0;
}
if ( _ident_count < 4 ) {
if ( !globals->get_soundmgr()->is_playing(_adf_ident) ) {
globals->get_soundmgr()->play_once( _adf_ident );
++_ident_count;
}
}
} else {
_in_range_node->setBoolValue(false);
set_bearing(delta_time_sec, 90);
_ident_node->setStringValue("");
globals->get_soundmgr()->stop( _adf_ident );
}
}
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
FGNavRecord *nav =
globals->get_navlist()->findByFreq(frequency_khz, longitude_rad,
latitude_rad, altitude_m);
_transmitter_valid = (nav != NULL);
if ( _transmitter_valid ) {
ident = nav->get_trans_ident();
if ( ident != _last_ident ) {
_transmitter_pos = nav->get_pos();
_transmitter_cart = nav->get_cart();
_transmitter_range_nm = nav->get_range();
}
}
if ( _last_ident != ident ) {
_last_ident = ident;
_ident_node->setStringValue(ident.c_str());
if ( globals->get_soundmgr()->exists( _adf_ident ) ) {
// stop is required! -- remove alone wouldn't stop immediately
globals->get_soundmgr()->stop( _adf_ident );
globals->get_soundmgr()->remove( _adf_ident );
}
SGSoundSample *sound;
sound = morse.make_ident( ident, LO_FREQUENCY );
sound->set_volume(_last_volume = 0);
globals->get_soundmgr()->add( sound, _adf_ident );
int offset = (int)(sg_random() * 30.0);
_ident_count = offset / 4;
_last_ident_time = globals->get_time_params()->get_cur_time() -
offset;
}
}
void
ADF::set_bearing (double dt, double bearing_deg)
{
double old_bearing_deg = _bearing_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 += _error_node->getDoubleValue();
bearing_deg =
fgGetLowPass(old_bearing_deg, bearing_deg, dt * RESPONSIVENESS);
_bearing_node->setDoubleValue(bearing_deg);
}
// end of adf.cxx
View git blame Copy permalink