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 else
dt_elev_count = 0; 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. // Only do the proper hitlist stuff if we are within visible range of the viewer.
if (!invisible) { if (!invisible) {
double visibility_meters = fgGetDouble("/environment/visibility-m"); double visibility_meters = fgGetDouble("/environment/visibility-m");
@ -922,8 +916,6 @@ void FGAIAircraft::getGroundElev(double dt) {
FGViewer* vw = globals->get_current_view(); FGViewer* vw = globals->get_current_view();
double course, distance; double course, distance;
//Point3D currView(vw->getLongitude_deg(),
// vw->getLatitude_deg(), 0.0);
SGWayPoint current(pos.getLongitudeDeg(), pos.getLatitudeDeg(), 0); SGWayPoint current(pos.getLongitudeDeg(), pos.getLatitudeDeg(), 0);
SGWayPoint view (vw->getLongitude_deg(), vw->getLatitude_deg(), 0); SGWayPoint view (vw->getLongitude_deg(), vw->getLatitude_deg(), 0);
view.CourseAndDistance(current, &course, &distance); 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), _time_before_search_sec(0),
_last_frequency_khz(-1), _last_frequency_khz(-1),
_transmitter_valid(false), _transmitter_valid(false),
_transmitter_elevation_ft(0), _transmitter_pos(SGGeod::fromDeg(0, 0)),
_transmitter_cart(0, 0, 0),
_transmitter_range_nm(0), _transmitter_range_nm(0),
_ident_count(0), _ident_count(0),
_last_ident_time(0), _last_ident_time(0),
@ -92,7 +93,8 @@ ADF::ADF ()
: _time_before_search_sec(0), : _time_before_search_sec(0),
_last_frequency_khz(-1), _last_frequency_khz(-1),
_transmitter_valid(false), _transmitter_valid(false),
_transmitter_elevation_ft(0), _transmitter_pos(SGGeod::fromDeg(0, 0)),
_transmitter_cart(0, 0, 0),
_transmitter_range_nm(0), _transmitter_range_nm(0),
_ident_count(0), _ident_count(0),
_last_ident_time(0), _last_ident_time(0),
@ -178,11 +180,11 @@ ADF::update (double delta_time_sec)
} }
// Calculate the bearing to the transmitter // Calculate the bearing to the transmitter
Point3D location = SGGeod geod = SGGeod::fromRadM(longitude_rad, latitude_rad, altitude_m);
sgGeodToCart(Point3D(longitude_rad, latitude_rad, altitude_m)); SGVec3d location = SGVec3d::fromGeod(geod);
double distance_nm = _transmitter.distance3D(location) * SG_METER_TO_NM; double distance_nm = dist(_transmitter_cart, 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, altitude_m * SG_METER_TO_FEET,
_transmitter_range_nm); _transmitter_range_nm);
if (distance_nm <= range_nm) { if (distance_nm <= range_nm) {
@ -190,11 +192,7 @@ ADF::update (double delta_time_sec)
double bearing, az2, s; double bearing, az2, s;
double heading = _heading_node->getDoubleValue(); double heading = _heading_node->getDoubleValue();
geo_inverse_wgs_84(altitude_m, geo_inverse_wgs_84(geod, _transmitter_pos,
latitude_deg,
longitude_deg,
_transmitter_lat_deg,
_transmitter_lon_deg,
&bearing, &az2, &s); &bearing, &az2, &s);
_in_range_node->setBoolValue(true); _in_range_node->setBoolValue(true);
@ -258,10 +256,8 @@ ADF::search (double frequency_khz, double longitude_rad,
if ( _transmitter_valid ) { if ( _transmitter_valid ) {
ident = nav->get_trans_ident(); ident = nav->get_trans_ident();
if ( ident != _last_ident ) { if ( ident != _last_ident ) {
_transmitter_lon_deg = nav->get_lon(); _transmitter_pos = nav->get_pos();
_transmitter_lat_deg = nav->get_lat(); _transmitter_cart = nav->get_cart();
_transmitter = Point3D(nav->get_x(), nav->get_y(), nav->get_z());
_transmitter_elevation_ft = nav->get_elev_ft();
_transmitter_range_nm = nav->get_range(); _transmitter_range_nm = nav->get_range();
} }
} }

6
src/Instrumentation/adf.hxx
View file

@ -85,10 +85,8 @@ private:
int _last_frequency_khz; int _last_frequency_khz;
bool _transmitter_valid; bool _transmitter_valid;
string _last_ident; string _last_ident;
Point3D _transmitter; SGGeod _transmitter_pos;
double _transmitter_lon_deg; SGVec3d _transmitter_cart;
double _transmitter_lat_deg;
double _transmitter_elevation_ft;
double _transmitter_range_nm; double _transmitter_range_nm;
FGMorse morse; 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 // Calculate the distance to the transmitter
Point3D location = SGGeod geod = SGGeod::fromRadM(longitude_rad, latitude_rad, altitude_m);
sgGeodToCart(Point3D(longitude_rad, latitude_rad, altitude_m)); SGVec3d location = SGVec3d::fromGeod(geod);
double distance_nm = _transmitter.distance3D(location) * SG_METER_TO_NM;
double distance_nm = dist(_transmitter, location) * SG_METER_TO_NM;
double range_nm = adjust_range(_transmitter_elevation_ft, double range_nm = adjust_range(_transmitter_elevation_ft,
altitude_m * SG_METER_TO_FEET, altitude_m * SG_METER_TO_FEET,
@ -198,7 +199,7 @@ DME::search (double frequency_mhz, double longitude_rad,
_transmitter_valid = (dme != NULL); _transmitter_valid = (dme != NULL);
if ( _transmitter_valid ) { 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_elevation_ft = dme->get_elev_ft();
_transmitter_range_nm = dme->get_range(); _transmitter_range_nm = dme->get_range();
_transmitter_bias = dme->get_multiuse(); _transmitter_bias = dme->get_multiuse();

2
src/Instrumentation/dme.hxx
View file

@ -71,7 +71,7 @@ private:
double _time_before_search_sec; double _time_before_search_sec;
bool _transmitter_valid; bool _transmitter_valid;
Point3D _transmitter; SGVec3d _transmitter;
double _transmitter_elevation_ft; double _transmitter_elevation_ft;
double _transmitter_range_nm; double _transmitter_range_nm;
double _transmitter_bias; 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 // Update the various nav values based on position and valid tuned in navs
void FGKR_87::update( double dt_sec ) { void FGKR_87::update( double dt_sec ) {
double acft_lon = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS; SGGeod acft = SGGeod::fromDegFt(lon_node->getDoubleValue(),
double acft_lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS; lat_node->getDoubleValue(),
double acft_elev = alt_node->getDoubleValue() * SG_FEET_TO_METER; alt_node->getDoubleValue());
need_update = false; need_update = false;
Point3D aircraft = sgGeodToCart( Point3D( acft_lon, acft_lat, acft_elev ) );
Point3D station;
double az1, az2, s; double az1, az2, s;
// On timeout, scan again // On timeout, scan again
@ -361,20 +359,17 @@ void FGKR_87::update( double dt_sec ) {
if ( valid ) { if ( valid ) {
// cout << "adf is valid" << endl; // cout << "adf is valid" << endl;
// staightline distance // staightline distance
station = Point3D( x, y, z ); // What a hack, dist is a class local variable
dist = aircraft.distance3D( station ); dist = sqrt(distSqr(SGVec3d::fromGeod(acft), xyz));
// wgs84 heading // wgs84 heading
geo_inverse_wgs_84( acft_elev, geo_inverse_wgs_84( acft, SGGeod::fromDeg(stn_lon, stn_lat),
acft_lat * SGD_RADIANS_TO_DEGREES,
acft_lon * SGD_RADIANS_TO_DEGREES,
stn_lat, stn_lon,
&az1, &az2, &s ); &az1, &az2, &s );
heading = az1; heading = az1;
// cout << " heading = " << heading // cout << " heading = " << heading
// << " dist = " << dist << endl; // << " 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 ) { if ( dist < effective_range * SG_NM_TO_METER ) {
inrange = true; inrange = true;
} else if ( dist < 2 * effective_range * SG_NM_TO_METER ) { } else if ( dist < 2 * effective_range * SG_NM_TO_METER ) {
@ -530,9 +525,7 @@ void FGKR_87::search() {
stn_elev = adf->get_elev_ft(); stn_elev = adf->get_elev_ft();
range = adf->get_range(); range = adf->get_range();
effective_range = kludgeRange(stn_elev, acft_elev, range); effective_range = kludgeRange(stn_elev, acft_elev, range);
x = adf->get_x(); xyz = adf->get_cart();
y = adf->get_y();
z = adf->get_z();
if ( globals->get_soundmgr()->exists( "adf-ident" ) ) { if ( globals->get_soundmgr()->exists( "adf-ident" ) ) {
globals->get_soundmgr()->remove( "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 effective_range;
double dist; double dist;
double heading; double heading;
double x; SGVec3d xyz;
double y;
double z;
double goal_needle_deg; double goal_needle_deg;
double et_flash_time; 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 ) FGNavRecord *b )
{ {
Point3D aircraft = sgGeodToCart( Point3D(lon_rad, lat_rad, elev_m) ); SGVec3d aircraft = SGVec3d::fromGeod(pos);
Point3D station = Point3D( b->get_x(), b->get_y(), b->get_z() ); SGVec3d station = b->get_cart();
// cout << " aircraft = " << aircraft << " station = " << station // cout << " aircraft = " << aircraft << " station = " << station
// << endl; // << endl;
double d = aircraft.distance3Dsquared( station ); // meters^2 SGVec3d tmp = station - aircraft;
double d = dot(tmp, tmp);
// cout << " distance = " << d << " (" // cout << " distance = " << d << " ("
// << FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER // << FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER
// * 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 // cout << "elev = " << elev * SG_METER_TO_FEET
// << " current->get_elev() = " << current->get_elev() << endl; // << " 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(); 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 // 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; static fgMkrBeacType last_beacon = NOBEACON;
double lon_rad = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS; SGGeod pos = SGGeod::fromDegFt(lon_node->getDoubleValue(),
double lat_rad = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS; lat_node->getDoubleValue(),
double elev_m = alt_node->getDoubleValue() * SG_FEET_TO_METER; alt_node->getDoubleValue());
//////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////
// Beacons. // Beacons.
@ -263,7 +264,9 @@ void FGMarkerBeacon::search()
// get closest marker beacon // get closest marker beacon
FGNavRecord *b 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; // cout << "marker beacon = " << b << " (" << b->get_type() << ")" << endl;
@ -277,7 +280,7 @@ void FGMarkerBeacon::search()
} else if ( b->get_type() == 9 ) { } else if ( b->get_type() == 9 ) {
beacon_type = INNER; beacon_type = INNER;
} }
inrange = check_beacon_range( lon_rad, lat_rad, elev_m, b ); inrange = check_beacon_range( pos, b );
// cout << " inrange = " << inrange << endl; // 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 // cache a few strategic values locally for speed
double lon = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS; SGGeod pos = SGGeod::fromDegFt(lon_node->getDoubleValue(),
double lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS; lat_node->getDoubleValue(),
double elev = alt_node->getDoubleValue() * SG_FEET_TO_METER; alt_node->getDoubleValue());
bool power_btn = power_btn_node->getBoolValue(); bool power_btn = power_btn_node->getBoolValue();
bool nav_serviceable = nav_serviceable_node->getBoolValue(); bool nav_serviceable = nav_serviceable_node->getBoolValue();
bool cdi_serviceable = cdi_serviceable_node->getBoolValue(); bool cdi_serviceable = cdi_serviceable_node->getBoolValue();
@ -335,8 +335,6 @@ FGNavRadio::update(double dt)
bool is_loc = loc_node->getBoolValue(); bool is_loc = loc_node->getBoolValue();
double loc_dist = loc_dist_node->getDoubleValue(); double loc_dist = loc_dist_node->getDoubleValue();
Point3D aircraft = sgGeodToCart( Point3D( lon, lat, elev ) );
Point3D station;
double az1, az2, s; double az1, az2, s;
// Create "formatted" versions of the nav frequencies for // 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) if ( is_valid && power_btn && (bus_power_node->getDoubleValue() > 1.0)
&& nav_serviceable ) && nav_serviceable )
{ {
station = Point3D( nav_x, nav_y, nav_z ); SGVec3d aircraft = SGVec3d::fromGeod(pos);
loc_dist = aircraft.distance3D( station ); loc_dist = dist(aircraft, nav_xyz);
loc_dist_node->setDoubleValue( loc_dist ); loc_dist_node->setDoubleValue( loc_dist );
// cout << "station = " << station << " dist = " << loc_dist << endl; // cout << "station = " << station << " dist = " << loc_dist << endl;
if ( has_gs ) { if ( has_gs ) {
// find closest distance to the gs base line // find closest distance to the gs base line
sgdVec3 p; SGVec3d p = aircraft;
sgdSetVec3( p, aircraft.x(), aircraft.y(), aircraft.z() ); double dist = sgdClosestPointToLineDistSquared(p.sg(), gs_xyz.sg(),
sgdVec3 p0; gs_base_vec.sg());
sgdSetVec3( p0, gs_x, gs_y, gs_z );
double dist = sgdClosestPointToLineDistSquared( p, p0,
gs_base_vec );
gs_dist_node->setDoubleValue( sqrt( dist ) ); gs_dist_node->setDoubleValue( sqrt( dist ) );
// cout << "gs_dist = " << gs_dist_node->getDoubleValue() // cout << "gs_dist = " << gs_dist_node->getDoubleValue()
// << endl; // << endl;
Point3D tmp( gs_x, gs_y, gs_z );
// cout << " (" << aircraft.distance3D( tmp ) << ")" << endl;
// wgs84 heading to glide slope (to determine sign of distance) // wgs84 heading to glide slope (to determine sign of distance)
geo_inverse_wgs_84( elev, geo_inverse_wgs_84( pos, SGGeod::fromDeg(gs_lon, gs_lat),
lat * SGD_RADIANS_TO_DEGREES,
lon * SGD_RADIANS_TO_DEGREES,
gs_lat, gs_lon,
&az1, &az2, &s ); &az1, &az2, &s );
double r = az1 - target_radial; double r = az1 - target_radial;
while ( r > 180.0 ) { r -= 360.0;} while ( r > 180.0 ) { r -= 360.0;}
@ -403,10 +392,7 @@ FGNavRadio::update(double dt)
// compute forward and reverse wgs84 headings to localizer // compute forward and reverse wgs84 headings to localizer
////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////
double hdg; double hdg;
geo_inverse_wgs_84( elev, geo_inverse_wgs_84( pos, SGGeod::fromDeg(loc_lon, loc_lat),
lat * SGD_RADIANS_TO_DEGREES,
lon * SGD_RADIANS_TO_DEGREES,
loc_lat, loc_lon,
&hdg, &az2, &s ); &hdg, &az2, &s );
// cout << "az1 = " << az1 << " magvar = " << nav_magvar << endl; // cout << "az1 = " << az1 << " magvar = " << nav_magvar << endl;
heading_node->setDoubleValue( hdg ); heading_node->setDoubleValue( hdg );
@ -444,10 +430,10 @@ FGNavRadio::update(double dt)
while ( offset > 180.0 ) { offset -= 360.0; } while ( offset > 180.0 ) { offset -= 360.0; }
// cout << "ils offset = " << offset << endl; // cout << "ils offset = " << offset << endl;
effective_range effective_range
= adjustILSRange( nav_elev, elev, offset, = adjustILSRange( nav_elev, pos.getElevationM(), offset,
loc_dist * SG_METER_TO_NM ); loc_dist * SG_METER_TO_NM );
} else { } else {
effective_range = adjustNavRange( nav_elev, elev, range ); effective_range = adjustNavRange( nav_elev, pos.getElevationM(), range );
} }
// cout << "nav range = " << effective_range // cout << "nav range = " << effective_range
// << " (" << range << ")" << endl; // << " (" << range << ")" << endl;
@ -809,9 +795,7 @@ void FGNavRadio::search()
while ( target_radial > 360.0 ) { target_radial -= 360.0; } while ( target_radial > 360.0 ) { target_radial -= 360.0; }
loc_lon = loc->get_lon(); loc_lon = loc->get_lon();
loc_lat = loc->get_lat(); loc_lat = loc->get_lat();
nav_x = loc->get_x(); nav_xyz = loc->get_cart();
nav_y = loc->get_y();
nav_z = loc->get_z();
last_nav_id = nav_id; last_nav_id = nav_id;
last_nav_vor = false; last_nav_vor = false;
loc_node->setBoolValue( true ); loc_node->setBoolValue( true );
@ -823,32 +807,26 @@ void FGNavRadio::search()
nav_elev = gs->get_elev_ft(); nav_elev = gs->get_elev_ft();
int tmp = (int)(gs->get_multiuse() / 1000.0); int tmp = (int)(gs->get_multiuse() / 1000.0);
target_gs = (double)tmp / 100.0; target_gs = (double)tmp / 100.0;
gs_x = gs->get_x(); gs_xyz = gs->get_cart();
gs_y = gs->get_y();
gs_z = gs->get_z();
// derive GS baseline (perpendicular to the runay // derive GS baseline (perpendicular to the runay
// along the ground) // along the ground)
double tlon, tlat, taz; double tlon, tlat, taz;
geo_direct_wgs_84 ( 0.0, gs_lat, gs_lon, geo_direct_wgs_84 ( 0.0, gs_lat, gs_lon,
target_radial + 90, target_radial + 90,
100.0, &tlat, &tlon, &taz ); 100.0, &tlat, &tlon, &taz );
// cout << "target_radial = " << target_radial << endl; // cout << "target_radial = " << target_radial << endl;
// cout << "nav_loc = " << loc_node->getBoolValue() << endl; // cout << "nav_loc = " << loc_node->getBoolValue() << endl;
// cout << gs_lon << "," << gs_lat << " " // cout << gs_lon << "," << gs_lat << " "
// << tlon << "," << tlat << " (" << nav_elev << ")" // << tlon << "," << tlat << " (" << nav_elev << ")"
// << endl; // << endl;
Point3D p1 = sgGeodToCart( Point3D(tlon*SGD_DEGREES_TO_RADIANS, SGGeod tpos = SGGeod::fromDegFt(tlon, tlat, nav_elev);
tlat*SGD_DEGREES_TO_RADIANS, SGVec3d p1 = SGVec3d::fromGeod(tpos);
nav_elev*SG_FEET_TO_METER)
); // cout << gs_xyz << endl;
// cout << gs_x << "," << gs_y << "," << gs_z
// << endl;
// cout << p1 << endl; // cout << p1 << endl;
sgdSetVec3( gs_base_vec, gs_base_vec = p1 - gs_xyz;
p1.x()-gs_x, p1.y()-gs_y, p1.z()-gs_z ); // cout << gs_base_vec << endl;
// cout << gs_base_vec[0] << "," << gs_base_vec[1] << ","
// << gs_base_vec[2] << endl;
} else { } else {
has_gs_node->setBoolValue( false ); has_gs_node->setBoolValue( false );
nav_elev = loc->get_elev_ft(); nav_elev = loc->get_elev_ft();
@ -905,9 +883,7 @@ void FGNavRadio::search()
effective_range = adjustNavRange(nav_elev, elev, range); effective_range = adjustNavRange(nav_elev, elev, range);
target_gs = 0.0; target_gs = 0.0;
target_radial = sel_radial_node->getDoubleValue(); target_radial = sel_radial_node->getDoubleValue();
nav_x = nav->get_x(); nav_xyz = nav->get_cart();
nav_y = nav->get_y();
nav_z = nav->get_z();
if ( globals->get_soundmgr()->exists( nav_fx_name ) ) { if ( globals->get_soundmgr()->exists( nav_fx_name ) ) {
globals->get_soundmgr()->remove( 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 target_radial;
double loc_lon; double loc_lon;
double loc_lat; double loc_lat;
double nav_x; SGVec3d nav_xyz;
double nav_y;
double nav_z;
double gs_lon; double gs_lon;
double gs_lat; double gs_lat;
double nav_elev; // use gs elev if available double nav_elev; // use gs elev if available
double gs_x; SGVec3d gs_xyz;
double gs_y; SGVec3d gs_base_vec;
double gs_z;
sgdVec3 gs_base_vec;
double gs_dist_signed; double gs_dist_signed;
SGTimeStamp prev_time; SGTimeStamp prev_time;
SGTimeStamp curr_time; SGTimeStamp curr_time;

27
src/Instrumentation/tacan.cxx
View file

@ -51,10 +51,9 @@ TACAN::TACAN ( SGPropertyNode *node )
_time_before_search_sec(0), _time_before_search_sec(0),
_mobile_valid(false), _mobile_valid(false),
_transmitter_valid(false), _transmitter_valid(false),
_transmitter_elevation_ft(0), _transmitter_pos(SGGeod::fromDeg(0, 0)),
_transmitter_range_nm(0), _transmitter_range_nm(0),
_transmitter_bias(0.0), _transmitter_bias(0.0),
name("tacan"), name("tacan"),
num(0) num(0)
{ {
@ -83,7 +82,7 @@ TACAN::TACAN ()
_time_before_search_sec(0), _time_before_search_sec(0),
_mobile_valid(false), _mobile_valid(false),
_transmitter_valid(false), _transmitter_valid(false),
_transmitter_elevation_ft(0), _transmitter_pos(SGGeod::fromDeg(0, 0)),
_transmitter_range_nm(0), _transmitter_range_nm(0),
_transmitter_bearing_deg(0), _transmitter_bearing_deg(0),
_transmitter_bias(0.0), _transmitter_bias(0.0),
@ -227,11 +226,8 @@ TACAN::update (double delta_time_sec)
&mobile_bearing, &mobile_az2, &mobile_distance); &mobile_bearing, &mobile_az2, &mobile_distance);
//calculate the bearing and range of the station from the aircraft //calculate the bearing and range of the station from the aircraft
geo_inverse_wgs_84(altitude_m, SGGeod pos = SGGeod::fromDegM(longitude_deg, latitude_deg, altitude_m);
latitude_deg, geo_inverse_wgs_84(pos, _transmitter_pos,
longitude_deg,
_transmitter_lat,
_transmitter_lon,
&bearing, &az2, &distance); &bearing, &az2, &distance);
@ -241,7 +237,7 @@ TACAN::update (double delta_time_sec)
SG_LOG( SG_INSTR, SG_DEBUG, "distance_m " << distance); SG_LOG( SG_INSTR, SG_DEBUG, "distance_m " << distance);
bearing = mobile_bearing; bearing = mobile_bearing;
distance = mobile_distance; distance = mobile_distance;
_transmitter_elevation_ft = _mobile_elevation_ft; _transmitter_pos.setElevationFt(_mobile_elevation_ft);
_transmitter_range_nm = _mobile_range_nm; _transmitter_range_nm = _mobile_range_nm;
_transmitter_bias = _mobile_bias; _transmitter_bias = _mobile_bias;
_transmitter_name = _mobile_name; _transmitter_name = _mobile_name;
@ -278,11 +274,7 @@ TACAN::update (double delta_time_sec)
double rotation = 0; double rotation = 0;
/*Point3D location = double range_nm = adjust_range(_transmitter_pos.getElevationFt(),
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, altitude_m * SG_METER_TO_FEET,
_transmitter_range_nm); _transmitter_range_nm);
@ -489,9 +481,7 @@ TACAN::search (double frequency_mhz, double longitude_rad,
if ( _transmitter_valid ) { if ( _transmitter_valid ) {
SG_LOG( SG_INSTR, SG_DEBUG, "transmitter valid " << _transmitter_valid ); SG_LOG( SG_INSTR, SG_DEBUG, "transmitter valid " << _transmitter_valid );
_transmitter_lat = tacan->get_lat(); _transmitter_pos = tacan->get_pos();
_transmitter_lon = tacan->get_lon();
_transmitter_elevation_ft = tacan->get_elev_ft();
_transmitter_range_nm = tacan->get_range(); _transmitter_range_nm = tacan->get_range();
_transmitter_bias = tacan->get_multiuse(); _transmitter_bias = tacan->get_multiuse();
_transmitter_name = tacan->get_name(); _transmitter_name = tacan->get_name();
@ -500,8 +490,7 @@ TACAN::search (double frequency_mhz, double longitude_rad,
_ident_node->setStringValue(_transmitter_ident.c_str()); _ident_node->setStringValue(_transmitter_ident.c_str());
SG_LOG( SG_INSTR, SG_DEBUG, "name " << _transmitter_name); 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, _transmitter_pos);
SG_LOG( SG_INSTR, SG_DEBUG, "elev " << _transmitter_elevation_ft);
} else { } else {
SG_LOG( SG_INSTR, SG_DEBUG, "transmitter invalid " << _transmitter_valid ); 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 _mobile_valid;
bool _transmitter_valid; bool _transmitter_valid;
Point3D _transmitter; SGVec3d _transmitter;
double _transmitter_lat, _transmitter_lon; SGGeod _transmitter_pos;
double _transmitter_elevation_ft;
double _transmitter_range_nm; double _transmitter_range_nm;
double _transmitter_bearing_deg; double _transmitter_bearing_deg;
double _transmitter_bias; 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 ) FGNavRecord *FGNavList::findByFreq( double freq, double lon, double lat, double elev )
{ {
nav_list_type stations = navaids[(int)(freq*100.0 + 0.5)]; const nav_list_type& stations = navaids[(int)(freq*100.0 + 0.5)];
Point3D aircraft = sgGeodToCart( Point3D(lon, lat, elev) );
SGGeod geod = SGGeod::fromRadM(lon, lat, elev);
SGVec3d aircraft = SGVec3d::fromGeod(geod);
SG_LOG( SG_INSTR, SG_DEBUG, "findbyFreq " << freq << " size " << stations.size() ); SG_LOG( SG_INSTR, SG_DEBUG, "findbyFreq " << freq << " size " << stations.size() );
return findNavFromList( aircraft, stations ); return findNavFromList( aircraft, stations );
@ -168,14 +170,14 @@ FGNavRecord *FGNavList::findByFreq( double freq, double lon, double lat, double
FGNavRecord *FGNavList::findByIdent( const char* ident, FGNavRecord *FGNavList::findByIdent( const char* ident,
const double lon, const double lat ) const double lon, const double lat )
{ {
nav_list_type stations = ident_navaids[ident]; const nav_list_type& stations = ident_navaids[ident];
Point3D aircraft = sgGeodToCart( Point3D(lon, lat, 0.0) ); SGGeod geod = SGGeod::fromRad(lon, lat);
SGVec3d aircraft = SGVec3d::fromGeod(geod);
return findNavFromList( aircraft, stations ); 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; nav_list_type n2;
n2.clear(); 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 // Given a point and a list of stations, return the closest one to the
// specified point. // specified point.
FGNavRecord *FGNavList::findNavFromList( const Point3D &aircraft, FGNavRecord *FGNavList::findNavFromList( const SGVec3d &aircraft,
const nav_list_type &stations ) const nav_list_type &stations )
{ {
FGNavRecord *nav = NULL; FGNavRecord *nav = NULL;
Point3D station;
double d2; // in meters squared double d2; // in meters squared
double min_dist double min_dist
= FG_NAV_MAX_RANGE*SG_NM_TO_METER*FG_NAV_MAX_RANGE*SG_NM_TO_METER; = 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) // find the closest station within a sensible range (FG_NAV_MAX_RANGE)
for ( unsigned int i = 0; i < stations.size(); ++i ) { for ( unsigned int i = 0; i < stations.size(); ++i ) {
// cout << "testing " << current->get_ident() << endl; // cout << "testing " << current->get_ident() << endl;
station = Point3D( stations[i]->get_x(), d2 = distSqr(stations[i]->get_cart(), aircraft);
stations[i]->get_y(),
stations[i]->get_z() );
d2 = aircraft.distance3Dsquared( station );
// cout << " dist = " << sqrt(d) // cout << " dist = " << sqrt(d)
// << " range = " << current->get_range() * SG_NM_TO_METER // << " 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 az1 = 0.0, az2 = 0.0, s = 0.0;
double elev_m = 0.0, lat_rad = 0.0, lon_rad = 0.0; SGGeod geod = SGGeod::fromCart(aircraft);
double xyz[3] = { aircraft.x(), aircraft.y(), aircraft.z() }; geo_inverse_wgs_84( geod, stations[i]->get_pos(),
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(),
&az1, &az2, &s); &az1, &az2, &s);
az1 = az1 - stations[i]->get_multiuse(); az1 = az1 - stations[i]->get_multiuse();
if ( az1 > 180.0) az1 -= 360.0; 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; 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 << "Master index = " << master_index << endl;
// cout << "beacon search length = " << beacons.size() << endl; // cout << "beacon search length = " << beacons.size() << endl;
nav_list_const_iterator current = navs.begin(); nav_list_const_iterator current = navs.begin();
nav_list_const_iterator last = navs.end(); nav_list_const_iterator last = navs.end();
Point3D aircraft = sgGeodToCart( Point3D(lon_rad, SGGeod geod = SGGeod::fromRadM(lon_rad, lat_rad, elev_m);
lat_rad, SGVec3d aircraft = SGVec3d::fromGeod(geod);
elev_m) );
double min_dist = 999999999.0; double min_dist = 999999999.0;
for ( ; current != last ; ++current ) { for ( ; current != last ; ++current ) {
if(isTypeMatch(*current, type)) { if(isTypeMatch(*current, type)) {
// cout << " testing " << (*current)->get_ident() << endl; // 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 << " (" // cout << " distance = " << d << " ("
// << FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER // << FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER
// * 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 // Given a TACAN Channel return the first matching frequency
FGTACANRecord *FGTACANList::findByChannel( const string& channel ) 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() ); SG_LOG( SG_INSTR, SG_DEBUG, "findByChannel " << channel<< " size " << stations.size() );
if (stations.size()) { if (stations.size()) {
@ -430,7 +416,7 @@ FGTACANRecord *FGTACANList::findByChannel( const string& channel )
// Given a frequency, return the first matching station. // Given a frequency, return the first matching station.
FGNavRecord *FGNavList::findStationByFreq( double freq ) 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() ); 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 { 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_list_type carrierlist;
nav_map_type navaids; nav_map_type navaids;
nav_map_type navaids_by_tile; 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 // Given a point and a list of stations, return the closest one to
// the specified point. // the specified point.
FGNavRecord *findNavFromList( const Point3D &aircraft, FGNavRecord *findNavFromList( const SGVec3d &aircraft,
const nav_list_type &stations ); const nav_list_type &stations );
public: public:
@ -78,7 +76,6 @@ public:
// add an entry // add an entry
bool add( FGNavRecord *n ); bool add( FGNavRecord *n );
//bool add( FGTACANRecord *r );
/** Query the database for the specified station. It is assumed /** Query the database for the specified station. It is assumed
* that there will be multiple stations with matching frequencies * that there will be multiple stations with matching frequencies
@ -94,7 +91,7 @@ public:
// (by ASCII code value) to that supplied. // (by ASCII code value) to that supplied.
// Supplying true for exact forces only exact matches to be returned (similar to above function) // 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! // 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 // Given an Ident and optional freqency, return the first matching
// station. // station.
@ -113,16 +110,9 @@ public:
class FGTACANList { class FGTACANList {
tacan_list_type channellist; tacan_list_type channellist;
//nav_list_type carrierlist;
tacan_map_type channels; tacan_map_type channels;
//nav_map_type navaids_by_tile;
tacan_ident_map_type ident_channels; 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: public:
FGTACANList(); FGTACANList();
@ -132,7 +122,6 @@ public:
bool init(); bool init();
// add an entry // add an entry
bool add( FGTACANRecord *r ); bool add( FGTACANRecord *r );
// Given a TACAN Channel, return the appropriate frequency. // 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 { class FGNavRecord {
int type; int type;
double lon, lat; // location in geodetic coords (degrees) SGGeod pos; // location in geodetic coords (degrees)
double elev_ft; SGVec3d cart; // location in cartesian coords (earth centered)
double x, y, z; // location in cartesian coords (earth centered)
int freq; int freq;
int range; int range;
double multiuse; // can be slaved variation of VOR double multiuse; // can be slaved variation of VOR
@ -85,15 +84,14 @@ public:
inline int get_type() const { return type; } inline int get_type() const { return type; }
inline fg_nav_types get_fg_type() const; inline fg_nav_types get_fg_type() const;
inline double get_lon() const { return lon; } // degrees inline double get_lon() const { return pos.getLongitudeDeg(); } // degrees
inline void set_lon( double l ) { lon = l; } // degrees inline void set_lon( double l ) { pos.setLongitudeDeg(l); } // degrees
inline double get_lat() const { return lat; } // degrees inline double get_lat() const { return pos.getLatitudeDeg(); } // degrees
inline void set_lat( double l ) { lat = l; } // degrees inline void set_lat( double l ) { pos.setLatitudeDeg(l); } // degrees
inline double get_elev_ft() const { return elev_ft; } inline double get_elev_ft() const { return pos.getElevationFt(); }
inline void set_elev_ft( double e ) { elev_ft = e; } inline void set_elev_ft( double e ) { pos.setElevationFt(e); }
inline double get_x() const { return x; } const SGGeod& get_pos() const { return pos; }
inline double get_y() const { return y; } const SGVec3d& get_cart() const { return cart; }
inline double get_z() const { return z; }
inline int get_freq() const { return freq; } inline int get_freq() const { return freq; }
inline int get_range() const { return range; } inline int get_range() const { return range; }
inline double get_multiuse() const { return multiuse; } inline double get_multiuse() const { return multiuse; }
@ -111,9 +109,8 @@ public:
inline inline
FGNavRecord::FGNavRecord(void) : FGNavRecord::FGNavRecord(void) :
type(0), type(0),
lon(0.0), lat(0.0), pos(SGGeod::fromDeg(0, 0)),
elev_ft(0.0), cart(0, 0, 0),
x(0.0), y(0.0), z(0.0),
freq(0), freq(0),
range(0), range(0),
multiuse(0.0), multiuse(0.0),
@ -145,8 +142,12 @@ operator >> ( istream& in, FGNavRecord& n )
return in >> skipeol; 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.ident;
n.pos.setLatitudeDeg(lat);
n.pos.setLongitudeDeg(lon);
n.pos.setElevationFt(elev_ft);
getline( in, n.name ); getline( in, n.name );
// silently multiply adf frequencies by 100 so that adf // silently multiply adf frequencies by 100 so that adf
@ -188,13 +189,7 @@ operator >> ( istream& in, FGNavRecord& n )
n.trans_ident = n.ident; n.trans_ident = n.ident;
// generate cartesian coordinates // generate cartesian coordinates
Point3D geod( n.lon * SGD_DEGREES_TO_RADIANS, n.cart = SGVec3d::fromGeod(n.pos);
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();
return in; return in;
} }