fly/flightgear
1
0
Fork 0
Code Activity

Make more use of SGGeod

Browse source
This commit is contained in:
frohlich 2006-06-15 19:16:21 +00:00
parent d336553d75
commit ef52b23e3a
15 changed files with 116 additions and 205 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
src/AIModel/AIAircraft.cxx
View file

@ -909,12 +909,6 @@ void FGAIAircraft::getGroundElev(double dt) {
else
dt_elev_count = 0;
// It would be nice if we could set the correct tile center here in order to get a correct
// answer with one call to the function, but what I tried in the two commented-out lines
// below only intermittently worked, and I haven't quite groked why yet.
//SGBucket buck(pos.lon(), pos.lat());
//aip.getSGLocation()->set_tile_center(Point3D(buck.get_center_lon(), buck.get_center_lat(), 0.0));
// Only do the proper hitlist stuff if we are within visible range of the viewer.
if (!invisible) {
double visibility_meters = fgGetDouble("/environment/visibility-m");
@ -922,8 +916,6 @@ void FGAIAircraft::getGroundElev(double dt) {
FGViewer* vw = globals->get_current_view();
double course, distance;
//Point3D currView(vw->getLongitude_deg(),
// vw->getLatitude_deg(), 0.0);
SGWayPoint current(pos.getLongitudeDeg(), pos.getLatitudeDeg(), 0);
SGWayPoint view (vw->getLongitude_deg(), vw->getLatitude_deg(), 0);
view.CourseAndDistance(current, &course, &distance);

28
src/Instrumentation/adf.cxx
View file

@ -61,7 +61,8 @@ ADF::ADF (SGPropertyNode *node )
_time_before_search_sec(0),
_last_frequency_khz(-1),
_transmitter_valid(false),
_transmitter_elevation_ft(0),
_transmitter_pos(SGGeod::fromDeg(0, 0)),
_transmitter_cart(0, 0, 0),
_transmitter_range_nm(0),
_ident_count(0),
_last_ident_time(0),
@ -92,7 +93,8 @@ ADF::ADF ()
: _time_before_search_sec(0),
_last_frequency_khz(-1),
_transmitter_valid(false),
_transmitter_elevation_ft(0),
_transmitter_pos(SGGeod::fromDeg(0, 0)),
_transmitter_cart(0, 0, 0),
_transmitter_range_nm(0),
_ident_count(0),
_last_ident_time(0),
@ -178,11 +180,11 @@ ADF::update (double delta_time_sec)
}
// 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,
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) {
@ -190,11 +192,7 @@ ADF::update (double delta_time_sec)
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,
geo_inverse_wgs_84(geod, _transmitter_pos,
&bearing, &az2, &s);
_in_range_node->setBoolValue(true);
@ -258,10 +256,8 @@ ADF::search (double frequency_khz, double longitude_rad,
if ( _transmitter_valid ) {
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_ft();
_transmitter_pos = nav->get_pos();
_transmitter_cart = nav->get_cart();
_transmitter_range_nm = nav->get_range();
}
}

6
src/Instrumentation/adf.hxx
View file

@ -85,10 +85,8 @@ private:
int _last_frequency_khz;
bool _transmitter_valid;
string _last_ident;
Point3D _transmitter;
double _transmitter_lon_deg;
double _transmitter_lat_deg;
double _transmitter_elevation_ft;
SGGeod _transmitter_pos;
SGVec3d _transmitter_cart;
double _transmitter_range_nm;
FGMorse morse;

9
src/Instrumentation/dme.cxx
View file

@ -152,9 +152,10 @@ DME::update (double delta_time_sec)
}
// Calculate the distance to the transmitter
Point3D location =
sgGeodToCart(Point3D(longitude_rad, latitude_rad, altitude_m));
double distance_nm = _transmitter.distance3D(location) * SG_METER_TO_NM;
SGGeod geod = SGGeod::fromRadM(longitude_rad, latitude_rad, altitude_m);
SGVec3d location = SGVec3d::fromGeod(geod);
double distance_nm = dist(_transmitter, location) * SG_METER_TO_NM;
double range_nm = adjust_range(_transmitter_elevation_ft,
altitude_m * SG_METER_TO_FEET,
@ -198,7 +199,7 @@ DME::search (double frequency_mhz, double longitude_rad,
_transmitter_valid = (dme != NULL);
if ( _transmitter_valid ) {
_transmitter = Point3D(dme->get_x(), dme->get_y(), dme->get_z());
_transmitter = dme->get_cart();
_transmitter_elevation_ft = dme->get_elev_ft();
_transmitter_range_nm = dme->get_range();
_transmitter_bias = dme->get_multiuse();

2
src/Instrumentation/dme.hxx
View file

@ -71,7 +71,7 @@ private:
double _time_before_search_sec;
bool _transmitter_valid;
Point3D _transmitter;
SGVec3d _transmitter;
double _transmitter_elevation_ft;
double _transmitter_range_nm;
double _transmitter_bias;

23
src/Instrumentation/kr_87.cxx
View file

@ -244,14 +244,12 @@ void FGKR_87::unbind () {
// Update the various nav values based on position and valid tuned in navs
void FGKR_87::update( double dt_sec ) {
double acft_lon = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
double acft_lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
double acft_elev = alt_node->getDoubleValue() * SG_FEET_TO_METER;
SGGeod acft = SGGeod::fromDegFt(lon_node->getDoubleValue(),
lat_node->getDoubleValue(),
alt_node->getDoubleValue());
need_update = false;
Point3D aircraft = sgGeodToCart( Point3D( acft_lon, acft_lat, acft_elev ) );
Point3D station;
double az1, az2, s;
// On timeout, scan again
@ -361,20 +359,17 @@ void FGKR_87::update( double dt_sec ) {
if ( valid ) {
// cout << "adf is valid" << endl;
// staightline distance
station = Point3D( x, y, z );
dist = aircraft.distance3D( station );
// What a hack, dist is a class local variable
dist = sqrt(distSqr(SGVec3d::fromGeod(acft), xyz));
// wgs84 heading
geo_inverse_wgs_84( acft_elev,
acft_lat * SGD_RADIANS_TO_DEGREES,
acft_lon * SGD_RADIANS_TO_DEGREES,
stn_lat, stn_lon,
geo_inverse_wgs_84( acft, SGGeod::fromDeg(stn_lon, stn_lat),
&az1, &az2, &s );
heading = az1;
// cout << " heading = " << heading
// << " dist = " << dist << endl;
effective_range = kludgeRange(stn_elev, acft_elev, range);
effective_range = kludgeRange(stn_elev, acft.getElevationFt(), range);
if ( dist < effective_range * SG_NM_TO_METER ) {
inrange = true;
} else if ( dist < 2 * effective_range * SG_NM_TO_METER ) {
@ -530,9 +525,7 @@ void FGKR_87::search() {
stn_elev = adf->get_elev_ft();
range = adf->get_range();
effective_range = kludgeRange(stn_elev, acft_elev, range);
x = adf->get_x();
y = adf->get_y();
z = adf->get_z();
xyz = adf->get_cart();
if ( globals->get_soundmgr()->exists( "adf-ident" ) ) {
globals->get_soundmgr()->remove( "adf-ident" );

4
src/Instrumentation/kr_87.hxx
View file

@ -59,9 +59,7 @@ class FGKR_87 : public SGSubsystem
double effective_range;
double dist;
double heading;
double x;
double y;
double z;
SGVec3d xyz;
double goal_needle_deg;
double et_flash_time;

23
src/Instrumentation/marker_beacon.cxx
View file

@ -201,15 +201,16 @@ FGMarkerBeacon::update(double dt)
}
static bool check_beacon_range( double lon_rad, double lat_rad, double elev_m,
static bool check_beacon_range( const SGGeod& pos,
FGNavRecord *b )
{
Point3D aircraft = sgGeodToCart( Point3D(lon_rad, lat_rad, elev_m) );
Point3D station = Point3D( b->get_x(), b->get_y(), b->get_z() );
SGVec3d aircraft = SGVec3d::fromGeod(pos);
SGVec3d station = b->get_cart();
// cout << " aircraft = " << aircraft << " station = " << station
// << endl;
double d = aircraft.distance3Dsquared( station ); // meters^2
SGVec3d tmp = station - aircraft;
double d = dot(tmp, tmp);
// cout << " distance = " << d << " ("
// << FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER
// * FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER
@ -219,7 +220,7 @@ static bool check_beacon_range( double lon_rad, double lat_rad, double elev_m,
// cout << "elev = " << elev * SG_METER_TO_FEET
// << " current->get_elev() = " << current->get_elev() << endl;
double elev_ft = elev_m * SG_METER_TO_FEET;
double elev_ft = pos.getElevationFt();
double delev = elev_ft - b->get_elev_ft();
// max range is the area under r = 2.4 * alt or r^2 = 4000^2 - alt^2
@ -253,9 +254,9 @@ void FGMarkerBeacon::search()
static fgMkrBeacType last_beacon = NOBEACON;
double lon_rad = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
double lat_rad = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
double elev_m = alt_node->getDoubleValue() * SG_FEET_TO_METER;
SGGeod pos = SGGeod::fromDegFt(lon_node->getDoubleValue(),
lat_node->getDoubleValue(),
alt_node->getDoubleValue());
////////////////////////////////////////////////////////////////////////
// Beacons.
@ -263,7 +264,9 @@ void FGMarkerBeacon::search()
// get closest marker beacon
FGNavRecord *b
= globals->get_mkrlist()->findClosest( lon_rad, lat_rad, elev_m );
= globals->get_mkrlist()->findClosest( pos.getLongitudeRad(),
pos.getLatitudeRad(),
pos.getElevationM() );
// cout << "marker beacon = " << b << " (" << b->get_type() << ")" << endl;
@ -277,7 +280,7 @@ void FGMarkerBeacon::search()
} else if ( b->get_type() == 9 ) {
beacon_type = INNER;
}
inrange = check_beacon_range( lon_rad, lat_rad, elev_m, b );
inrange = check_beacon_range( pos, b );
// cout << " inrange = " << inrange << endl;
}

70
src/Instrumentation/navradio.cxx
View file

@ -323,9 +323,9 @@ FGNavRadio::update(double dt)
}
// cache a few strategic values locally for speed
double lon = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
double lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
double elev = alt_node->getDoubleValue() * SG_FEET_TO_METER;
SGGeod pos = SGGeod::fromDegFt(lon_node->getDoubleValue(),
lat_node->getDoubleValue(),
alt_node->getDoubleValue());
bool power_btn = power_btn_node->getBoolValue();
bool nav_serviceable = nav_serviceable_node->getBoolValue();
bool cdi_serviceable = cdi_serviceable_node->getBoolValue();
@ -335,8 +335,6 @@ FGNavRadio::update(double dt)
bool is_loc = loc_node->getBoolValue();
double loc_dist = loc_dist_node->getDoubleValue();
Point3D aircraft = sgGeodToCart( Point3D( lon, lat, elev ) );
Point3D station;
double az1, az2, s;
// Create "formatted" versions of the nav frequencies for
@ -356,31 +354,22 @@ FGNavRadio::update(double dt)
if ( is_valid && power_btn && (bus_power_node->getDoubleValue() > 1.0)
&& nav_serviceable )
{
station = Point3D( nav_x, nav_y, nav_z );
loc_dist = aircraft.distance3D( station );
SGVec3d aircraft = SGVec3d::fromGeod(pos);
loc_dist = dist(aircraft, nav_xyz);
loc_dist_node->setDoubleValue( loc_dist );
// cout << "station = " << station << " dist = " << loc_dist << endl;
if ( has_gs ) {
// find closest distance to the gs base line
sgdVec3 p;
sgdSetVec3( p, aircraft.x(), aircraft.y(), aircraft.z() );
sgdVec3 p0;
sgdSetVec3( p0, gs_x, gs_y, gs_z );
double dist = sgdClosestPointToLineDistSquared( p, p0,
gs_base_vec );
SGVec3d p = aircraft;
double dist = sgdClosestPointToLineDistSquared(p.sg(), gs_xyz.sg(),
gs_base_vec.sg());
gs_dist_node->setDoubleValue( sqrt( dist ) );
// cout << "gs_dist = " << gs_dist_node->getDoubleValue()
// << endl;
Point3D tmp( gs_x, gs_y, gs_z );
// cout << " (" << aircraft.distance3D( tmp ) << ")" << endl;
// wgs84 heading to glide slope (to determine sign of distance)
geo_inverse_wgs_84( elev,
lat * SGD_RADIANS_TO_DEGREES,
lon * SGD_RADIANS_TO_DEGREES,
gs_lat, gs_lon,
geo_inverse_wgs_84( pos, SGGeod::fromDeg(gs_lon, gs_lat),
&az1, &az2, &s );
double r = az1 - target_radial;
while ( r > 180.0 ) { r -= 360.0;}
@ -403,10 +392,7 @@ FGNavRadio::update(double dt)
// compute forward and reverse wgs84 headings to localizer
//////////////////////////////////////////////////////////
double hdg;
geo_inverse_wgs_84( elev,
lat * SGD_RADIANS_TO_DEGREES,
lon * SGD_RADIANS_TO_DEGREES,
loc_lat, loc_lon,
geo_inverse_wgs_84( pos, SGGeod::fromDeg(loc_lon, loc_lat),
&hdg, &az2, &s );
// cout << "az1 = " << az1 << " magvar = " << nav_magvar << endl;
heading_node->setDoubleValue( hdg );
@ -444,10 +430,10 @@ FGNavRadio::update(double dt)
while ( offset > 180.0 ) { offset -= 360.0; }
// cout << "ils offset = " << offset << endl;
effective_range
= adjustILSRange( nav_elev, elev, offset,
loc_dist * SG_METER_TO_NM );
= adjustILSRange( nav_elev, pos.getElevationM(), offset,
loc_dist * SG_METER_TO_NM );
} else {
effective_range = adjustNavRange( nav_elev, elev, range );
effective_range = adjustNavRange( nav_elev, pos.getElevationM(), range );
}
// cout << "nav range = " << effective_range
// << " (" << range << ")" << endl;
@ -809,9 +795,7 @@ void FGNavRadio::search()
while ( target_radial > 360.0 ) { target_radial -= 360.0; }
loc_lon = loc->get_lon();
loc_lat = loc->get_lat();
nav_x = loc->get_x();
nav_y = loc->get_y();
nav_z = loc->get_z();
nav_xyz = loc->get_cart();
last_nav_id = nav_id;
last_nav_vor = false;
loc_node->setBoolValue( true );
@ -823,32 +807,26 @@ void FGNavRadio::search()
nav_elev = gs->get_elev_ft();
int tmp = (int)(gs->get_multiuse() / 1000.0);
target_gs = (double)tmp / 100.0;
gs_x = gs->get_x();
gs_y = gs->get_y();
gs_z = gs->get_z();
gs_xyz = gs->get_cart();
// derive GS baseline (perpendicular to the runay
// along the ground)
double tlon, tlat, taz;
geo_direct_wgs_84 ( 0.0, gs_lat, gs_lon,
target_radial + 90,
target_radial + 90,
100.0, &tlat, &tlon, &taz );
// cout << "target_radial = " << target_radial << endl;
// cout << "nav_loc = " << loc_node->getBoolValue() << endl;
// cout << gs_lon << "," << gs_lat << " "
// << tlon << "," << tlat << " (" << nav_elev << ")"
// << endl;
Point3D p1 = sgGeodToCart( Point3D(tlon*SGD_DEGREES_TO_RADIANS,
tlat*SGD_DEGREES_TO_RADIANS,
nav_elev*SG_FEET_TO_METER)
);
// cout << gs_x << "," << gs_y << "," << gs_z
// << endl;
SGGeod tpos = SGGeod::fromDegFt(tlon, tlat, nav_elev);
SGVec3d p1 = SGVec3d::fromGeod(tpos);
// cout << gs_xyz << endl;
// cout << p1 << endl;
sgdSetVec3( gs_base_vec,
p1.x()-gs_x, p1.y()-gs_y, p1.z()-gs_z );
// cout << gs_base_vec[0] << "," << gs_base_vec[1] << ","
// << gs_base_vec[2] << endl;
gs_base_vec = p1 - gs_xyz;
// cout << gs_base_vec << endl;
} else {
has_gs_node->setBoolValue( false );
nav_elev = loc->get_elev_ft();
@ -905,9 +883,7 @@ void FGNavRadio::search()
effective_range = adjustNavRange(nav_elev, elev, range);
target_gs = 0.0;
target_radial = sel_radial_node->getDoubleValue();
nav_x = nav->get_x();
nav_y = nav->get_y();
nav_z = nav->get_z();
nav_xyz = nav->get_cart();
if ( globals->get_soundmgr()->exists( nav_fx_name ) ) {
globals->get_soundmgr()->remove( nav_fx_name );

10
src/Instrumentation/navradio.hxx
View file

@ -123,16 +123,12 @@ class FGNavRadio : public SGSubsystem
double target_radial;
double loc_lon;
double loc_lat;
double nav_x;
double nav_y;
double nav_z;
SGVec3d nav_xyz;
double gs_lon;
double gs_lat;
double nav_elev; // use gs elev if available
double gs_x;
double gs_y;
double gs_z;
sgdVec3 gs_base_vec;
SGVec3d gs_xyz;
SGVec3d gs_base_vec;
double gs_dist_signed;
SGTimeStamp prev_time;
SGTimeStamp curr_time;

27
src/Instrumentation/tacan.cxx
View file

@ -51,10 +51,9 @@ TACAN::TACAN ( SGPropertyNode *node )
_time_before_search_sec(0),
_mobile_valid(false),
_transmitter_valid(false),
_transmitter_elevation_ft(0),
_transmitter_pos(SGGeod::fromDeg(0, 0)),
_transmitter_range_nm(0),
_transmitter_bias(0.0),
name("tacan"),
num(0)
{
@ -83,7 +82,7 @@ TACAN::TACAN ()
_time_before_search_sec(0),
_mobile_valid(false),
_transmitter_valid(false),
_transmitter_elevation_ft(0),
_transmitter_pos(SGGeod::fromDeg(0, 0)),
_transmitter_range_nm(0),
_transmitter_bearing_deg(0),
_transmitter_bias(0.0),
@ -227,11 +226,8 @@ TACAN::update (double delta_time_sec)
&mobile_bearing, &mobile_az2, &mobile_distance);
//calculate the bearing and range of the station from the aircraft
geo_inverse_wgs_84(altitude_m,
latitude_deg,
longitude_deg,
_transmitter_lat,
_transmitter_lon,
SGGeod pos = SGGeod::fromDegM(longitude_deg, latitude_deg, altitude_m);
geo_inverse_wgs_84(pos, _transmitter_pos,
&bearing, &az2, &distance);
@ -241,7 +237,7 @@ TACAN::update (double delta_time_sec)
SG_LOG( SG_INSTR, SG_DEBUG, "distance_m " << distance);
bearing = mobile_bearing;
distance = mobile_distance;
_transmitter_elevation_ft = _mobile_elevation_ft;
_transmitter_pos.setElevationFt(_mobile_elevation_ft);
_transmitter_range_nm = _mobile_range_nm;
_transmitter_bias = _mobile_bias;
_transmitter_name = _mobile_name;
@ -278,11 +274,7 @@ TACAN::update (double delta_time_sec)
double rotation = 0;
/*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,
double range_nm = adjust_range(_transmitter_pos.getElevationFt(),
altitude_m * SG_METER_TO_FEET,
_transmitter_range_nm);
@ -489,9 +481,7 @@ TACAN::search (double frequency_mhz, double longitude_rad,
if ( _transmitter_valid ) {
SG_LOG( SG_INSTR, SG_DEBUG, "transmitter valid " << _transmitter_valid );
_transmitter_lat = tacan->get_lat();
_transmitter_lon = tacan->get_lon();
_transmitter_elevation_ft = tacan->get_elev_ft();
_transmitter_pos = tacan->get_pos();
_transmitter_range_nm = tacan->get_range();
_transmitter_bias = tacan->get_multiuse();
_transmitter_name = tacan->get_name();
@ -500,8 +490,7 @@ TACAN::search (double frequency_mhz, double longitude_rad,
_ident_node->setStringValue(_transmitter_ident.c_str());
SG_LOG( SG_INSTR, SG_DEBUG, "name " << _transmitter_name);
SG_LOG( SG_INSTR, SG_DEBUG, "lat " << _transmitter_lat << "lon " << _transmitter_lon);
SG_LOG( SG_INSTR, SG_DEBUG, "elev " << _transmitter_elevation_ft);
SG_LOG( SG_INSTR, SG_DEBUG, _transmitter_pos);
} else {
SG_LOG( SG_INSTR, SG_DEBUG, "transmitter invalid " << _transmitter_valid );

5
src/Instrumentation/tacan.hxx
View file

@ -93,9 +93,8 @@ private:
bool _mobile_valid;
bool _transmitter_valid;
Point3D _transmitter;
double _transmitter_lat, _transmitter_lon;
double _transmitter_elevation_ft;
SGVec3d _transmitter;
SGGeod _transmitter_pos;
double _transmitter_range_nm;
double _transmitter_bearing_deg;
double _transmitter_bias;

50
src/Navaids/navlist.cxx
View file

@ -157,8 +157,10 @@ bool FGTACANList::add( FGTACANRecord *c ) {
FGNavRecord *FGNavList::findByFreq( double freq, double lon, double lat, double elev )
{
nav_list_type stations = navaids[(int)(freq*100.0 + 0.5)];
Point3D aircraft = sgGeodToCart( Point3D(lon, lat, elev) );
const nav_list_type& stations = navaids[(int)(freq*100.0 + 0.5)];
SGGeod geod = SGGeod::fromRadM(lon, lat, elev);
SGVec3d aircraft = SGVec3d::fromGeod(geod);
SG_LOG( SG_INSTR, SG_DEBUG, "findbyFreq " << freq << " size " << stations.size() );
return findNavFromList( aircraft, stations );
@ -168,14 +170,14 @@ FGNavRecord *FGNavList::findByFreq( double freq, double lon, double lat, double
FGNavRecord *FGNavList::findByIdent( const char* ident,
const double lon, const double lat )
{
nav_list_type stations = ident_navaids[ident];
Point3D aircraft = sgGeodToCart( Point3D(lon, lat, 0.0) );
const nav_list_type& stations = ident_navaids[ident];
SGGeod geod = SGGeod::fromRad(lon, lat);
SGVec3d aircraft = SGVec3d::fromGeod(geod);
return findNavFromList( aircraft, stations );
}
nav_list_type FGNavList::findFirstByIdent( string ident, fg_nav_types type, bool exact)
nav_list_type FGNavList::findFirstByIdent( const string& ident, fg_nav_types type, bool exact)
{
nav_list_type n2;
n2.clear();
@ -266,11 +268,10 @@ FGNavRecord *FGNavList::findByIdentAndFreq( const char* ident, const double freq
// Given a point and a list of stations, return the closest one to the
// specified point.
FGNavRecord *FGNavList::findNavFromList( const Point3D &aircraft,
FGNavRecord *FGNavList::findNavFromList( const SGVec3d &aircraft,
const nav_list_type &stations )
{
FGNavRecord *nav = NULL;
Point3D station;
double d2; // in meters squared
double min_dist
= FG_NAV_MAX_RANGE*SG_NM_TO_METER*FG_NAV_MAX_RANGE*SG_NM_TO_METER;
@ -278,11 +279,7 @@ FGNavRecord *FGNavList::findNavFromList( const Point3D &aircraft,
// find the closest station within a sensible range (FG_NAV_MAX_RANGE)
for ( unsigned int i = 0; i < stations.size(); ++i ) {
// cout << "testing " << current->get_ident() << endl;
station = Point3D( stations[i]->get_x(),
stations[i]->get_y(),
stations[i]->get_z() );
d2 = aircraft.distance3Dsquared( station );
d2 = distSqr(stations[i]->get_cart(), aircraft);
// cout << " dist = " << sqrt(d)
// << " range = " << current->get_range() * SG_NM_TO_METER
@ -317,13 +314,8 @@ FGNavRecord *FGNavList::findNavFromList( const Point3D &aircraft,
}
double az1 = 0.0, az2 = 0.0, s = 0.0;
double elev_m = 0.0, lat_rad = 0.0, lon_rad = 0.0;
double xyz[3] = { aircraft.x(), aircraft.y(), aircraft.z() };
sgCartToGeod( xyz, &lat_rad, &lon_rad, &elev_m );
geo_inverse_wgs_84( elev_m,
lat_rad * SG_RADIANS_TO_DEGREES,
lon_rad * SG_RADIANS_TO_DEGREES,
stations[i]->get_lat(), stations[i]->get_lon(),
SGGeod geod = SGGeod::fromCart(aircraft);
geo_inverse_wgs_84( geod, stations[i]->get_pos(),
&az1, &az2, &s);
az1 = az1 - stations[i]->get_multiuse();
if ( az1 > 180.0) az1 -= 360.0;
@ -372,29 +364,23 @@ FGNavRecord *FGNavList::findClosest( double lon_rad, double lat_rad,
int master_index = lonidx * 1000 + latidx;
nav_list_type navs = navaids_by_tile[ master_index ];
const nav_list_type& navs = navaids_by_tile[ master_index ];
// cout << "Master index = " << master_index << endl;
// cout << "beacon search length = " << beacons.size() << endl;
nav_list_const_iterator current = navs.begin();
nav_list_const_iterator last = navs.end();
Point3D aircraft = sgGeodToCart( Point3D(lon_rad,
lat_rad,
elev_m) );
SGGeod geod = SGGeod::fromRadM(lon_rad, lat_rad, elev_m);
SGVec3d aircraft = SGVec3d::fromGeod(geod);
double min_dist = 999999999.0;
for ( ; current != last ; ++current ) {
if(isTypeMatch(*current, type)) {
// cout << " testing " << (*current)->get_ident() << endl;
Point3D station = Point3D( (*current)->get_x(),
(*current)->get_y(),
(*current)->get_z() );
// cout << " aircraft = " << aircraft << " station = " << station
// << endl;
double d = aircraft.distance3Dsquared( station ); // meters^2
double d = distSqr((*current)->get_cart(), aircraft);
// cout << " distance = " << d << " ("
// << FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER
// * FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER
@ -418,7 +404,7 @@ FGNavRecord *FGNavList::findClosest( double lon_rad, double lat_rad,
// Given a TACAN Channel return the first matching frequency
FGTACANRecord *FGTACANList::findByChannel( const string& channel )
{
tacan_list_type stations = ident_channels[channel];
const tacan_list_type& stations = ident_channels[channel];
SG_LOG( SG_INSTR, SG_DEBUG, "findByChannel " << channel<< " size " << stations.size() );
if (stations.size()) {
@ -430,7 +416,7 @@ FGTACANRecord *FGTACANList::findByChannel( const string& channel )
// Given a frequency, return the first matching station.
FGNavRecord *FGNavList::findStationByFreq( double freq )
{
nav_list_type stations = navaids[(int)(freq*100.0 + 0.5)];
const nav_list_type& stations = navaids[(int)(freq*100.0 + 0.5)];
SG_LOG( SG_INSTR, SG_DEBUG, "findStationByFreq " << freq << " size " << stations.size() );

15
src/Navaids/navlist.hxx
View file

@ -56,8 +56,6 @@ typedef map < string, tacan_list_type > tacan_ident_map_type;
class FGNavList {
//nav_list_type navlist; // DCL - this doesn't appear to be used any more
// and can probably be removed.
nav_list_type carrierlist;
nav_map_type navaids;
nav_map_type navaids_by_tile;
@ -65,7 +63,7 @@ class FGNavList {
// Given a point and a list of stations, return the closest one to
// the specified point.
FGNavRecord *findNavFromList( const Point3D &aircraft,
FGNavRecord *findNavFromList( const SGVec3d &aircraft,
const nav_list_type &stations );
public:
@ -78,7 +76,6 @@ public:
// add an entry
bool add( FGNavRecord *n );
//bool add( FGTACANRecord *r );
/** Query the database for the specified station. It is assumed
* that there will be multiple stations with matching frequencies
@ -94,7 +91,7 @@ public:
// (by ASCII code value) to that supplied.
// Supplying true for exact forces only exact matches to be returned (similar to above function)
// Returns an empty list if no match found - calling function should check for this!
nav_list_type findFirstByIdent( string ident, fg_nav_types type, bool exact = false );
nav_list_type findFirstByIdent( const string& ident, fg_nav_types type, bool exact = false );
// Given an Ident and optional freqency, return the first matching
// station.
@ -113,16 +110,9 @@ public:
class FGTACANList {
tacan_list_type channellist;
//nav_list_type carrierlist;
tacan_map_type channels;
//nav_map_type navaids_by_tile;
tacan_ident_map_type ident_channels;
// Given a point and a list of stations, return the closest one to
// the specified point.
/*FGNavRecord *findNavFromList( const Point3D &aircraft,
const nav_list_type &stations );*/
public:
FGTACANList();
@ -132,7 +122,6 @@ public:
bool init();
// add an entry
bool add( FGTACANRecord *r );
// Given a TACAN Channel, return the appropriate frequency.

41
src/Navaids/navrecord.hxx
View file

@ -61,9 +61,8 @@ enum fg_nav_types {
class FGNavRecord {
int type;
double lon, lat; // location in geodetic coords (degrees)
double elev_ft;
double x, y, z; // location in cartesian coords (earth centered)
SGGeod pos; // location in geodetic coords (degrees)
SGVec3d cart; // location in cartesian coords (earth centered)
int freq;
int range;
double multiuse; // can be slaved variation of VOR
@ -85,15 +84,14 @@ public:
inline int get_type() const { return type; }
inline fg_nav_types get_fg_type() const;
inline double get_lon() const { return lon; } // degrees
inline void set_lon( double l ) { lon = l; } // degrees
inline double get_lat() const { return lat; } // degrees
inline void set_lat( double l ) { lat = l; } // degrees
inline double get_elev_ft() const { return elev_ft; }
inline void set_elev_ft( double e ) { elev_ft = e; }
inline double get_x() const { return x; }
inline double get_y() const { return y; }
inline double get_z() const { return z; }
inline double get_lon() const { return pos.getLongitudeDeg(); } // degrees
inline void set_lon( double l ) { pos.setLongitudeDeg(l); } // degrees
inline double get_lat() const { return pos.getLatitudeDeg(); } // degrees
inline void set_lat( double l ) { pos.setLatitudeDeg(l); } // degrees
inline double get_elev_ft() const { return pos.getElevationFt(); }
inline void set_elev_ft( double e ) { pos.setElevationFt(e); }
const SGGeod& get_pos() const { return pos; }
const SGVec3d& get_cart() const { return cart; }
inline int get_freq() const { return freq; }
inline int get_range() const { return range; }
inline double get_multiuse() const { return multiuse; }
@ -111,9 +109,8 @@ public:
inline
FGNavRecord::FGNavRecord(void) :
type(0),
lon(0.0), lat(0.0),
elev_ft(0.0),
x(0.0), y(0.0), z(0.0),
pos(SGGeod::fromDeg(0, 0)),
cart(0, 0, 0),
freq(0),
range(0),
multiuse(0.0),
@ -145,8 +142,12 @@ operator >> ( istream& in, FGNavRecord& n )
return in >> skipeol;
}
in >> n.lat >> n.lon >> n.elev_ft >> n.freq >> n.range >> n.multiuse
double lat, lon, elev_ft;
in >> lat >> lon >> elev_ft >> n.freq >> n.range >> n.multiuse
>> n.ident;
n.pos.setLatitudeDeg(lat);
n.pos.setLongitudeDeg(lon);
n.pos.setElevationFt(elev_ft);
getline( in, n.name );
// silently multiply adf frequencies by 100 so that adf
@ -188,13 +189,7 @@ operator >> ( istream& in, FGNavRecord& n )
n.trans_ident = n.ident;
// generate cartesian coordinates
Point3D geod( n.lon * SGD_DEGREES_TO_RADIANS,
n.lat * SGD_DEGREES_TO_RADIANS,
n.elev_ft * SG_FEET_TO_METER );
Point3D cart = sgGeodToCart( geod );
n.x = cart.x();
n.y = cart.y();
n.z = cart.z();
n.cart = SGVec3d::fromGeod(n.pos);
return in;
}