Overhaul of the navaid system to increase efficiency, reduce redundancy, and
also properly model the DME.
This commit is contained in:
parent
977917a612
commit
6bc7ed9ba2
15 changed files with 300 additions and 94 deletions
|
@ -301,21 +301,30 @@ int FGAutopilot::run() {
|
|||
if ( heading_mode == FG_HEADING_LOCK ) {
|
||||
// leave target heading alone
|
||||
} else if ( heading_mode == FG_HEADING_NAV1 ) {
|
||||
double tgt_radial = current_radiostack->get_nav1_radial();
|
||||
double cur_radial = current_radiostack->get_nav1_heading() + 180.0;
|
||||
// cout << "target rad = " << tgt_radial
|
||||
// << " current rad = " << cur_radial
|
||||
// << endl;
|
||||
double tgt_radial;
|
||||
double cur_radial;
|
||||
if ( current_radiostack->get_nav1_loc() ) {
|
||||
tgt_radial = current_radiostack->get_nav1_radial() + 180.0;
|
||||
} else {
|
||||
tgt_radial = current_radiostack->get_nav1_radial();
|
||||
}
|
||||
cur_radial = current_radiostack->get_nav1_heading();
|
||||
cout << "target rad = " << tgt_radial
|
||||
<< " current rad = " << cur_radial
|
||||
<< endl;
|
||||
|
||||
double diff = (tgt_radial - cur_radial)
|
||||
* (current_radiostack->get_nav1_dist() * METER_TO_NM);
|
||||
double diff = (tgt_radial - cur_radial);
|
||||
while ( diff < -180.0 ) { diff += 360.0; }
|
||||
while ( diff > 180.0 ) { diff -= 360.0; }
|
||||
|
||||
diff *= (current_radiostack->get_nav1_dist() * METER_TO_NM);
|
||||
if ( diff < -30.0 ) { diff = -30.0; }
|
||||
if ( diff > 30.0 ) { diff = 30.0; }
|
||||
|
||||
TargetHeading = cur_radial - diff;
|
||||
while ( TargetHeading < 0.0 ) { TargetHeading += 360.0; }
|
||||
while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; }
|
||||
// cout << "target course = " << TargetHeading << endl;
|
||||
cout << "target course = " << TargetHeading << endl;
|
||||
} else if ( heading_mode == FG_HEADING_WAYPOINT ) {
|
||||
// update target heading to waypoint
|
||||
|
||||
|
|
|
@ -21,6 +21,7 @@
|
|||
// $Id$
|
||||
|
||||
|
||||
#include <Aircraft/aircraft.hxx>
|
||||
#include <Navaids/ilslist.hxx>
|
||||
#include <Navaids/navlist.hxx>
|
||||
|
||||
|
@ -41,73 +42,200 @@ FGRadioStack::~FGRadioStack() {
|
|||
}
|
||||
|
||||
|
||||
// Update nav/adf radios based on current postition
|
||||
// Search the database for the current frequencies given current location
|
||||
void FGRadioStack::update( double lon, double lat, double elev ) {
|
||||
need_update = false;
|
||||
|
||||
// nav1
|
||||
FGILS ils;
|
||||
FGNav nav;
|
||||
Point3D aircraft = fgGeodToCart( Point3D( lon, lat, elev ) );
|
||||
Point3D station;
|
||||
double az1, az2, s;
|
||||
|
||||
if ( current_ilslist->query( lon, lat, elev, nav1_freq,
|
||||
&ils, &nav1_heading, &nav1_dist) ) {
|
||||
nav1_inrange = true;
|
||||
nav1_loc = true;
|
||||
nav1_lon = ils.get_loclon();
|
||||
nav1_lat = ils.get_loclat();
|
||||
nav1_elev = ils.get_gselev();
|
||||
nav1_target_gs = ils.get_gsangle();
|
||||
nav1_radial = ils.get_locheading() + 180.0;
|
||||
while ( nav1_radial < 0.0 ) { nav1_radial += 360.0; }
|
||||
while ( nav1_radial > 360.0 ) { nav1_radial -= 360.0; }
|
||||
// cout << "Found a vor station in range" << endl;
|
||||
// cout << " id = " << ils.get_locident() << endl;
|
||||
// cout << " heading = " << nav1_heading
|
||||
// << " dist = " << nav1_dist << endl;
|
||||
} else if ( current_navlist->query( lon, lat, elev, nav1_freq,
|
||||
&nav, &nav1_heading, &nav1_dist) ) {
|
||||
// not ILS
|
||||
nav1_inrange = true;
|
||||
nav1_loc = false;
|
||||
nav1_lon = nav.get_lon();
|
||||
nav1_lat = nav.get_lat();
|
||||
nav1_elev = nav.get_elev();
|
||||
nav1_target_gs = 0.0;
|
||||
nav1_radial = nav1_sel_radial;
|
||||
if ( nav1_valid ) {
|
||||
// staightline distance
|
||||
station = Point3D( nav1_x, nav1_y, nav1_z );
|
||||
nav1_dist = aircraft.distance3D( station );
|
||||
|
||||
if ( nav1_dist < nav1_effective_range * NM_TO_METER ) {
|
||||
nav1_inrange = true;
|
||||
|
||||
// wgs84 heading
|
||||
geo_inverse_wgs_84( elev, lat * RAD_TO_DEG, lon * RAD_TO_DEG,
|
||||
nav1_lat, nav1_lon,
|
||||
&az1, &az2, &s );
|
||||
nav1_heading = az1;
|
||||
|
||||
// cout << " heading = " << nav1_heading
|
||||
// << " dist = " << nav1_dist << endl;
|
||||
} else {
|
||||
nav1_inrange = false;
|
||||
}
|
||||
|
||||
if ( nav1_loc ) {
|
||||
} else {
|
||||
nav1_radial = nav1_sel_radial;
|
||||
}
|
||||
} else {
|
||||
nav1_inrange = false;
|
||||
// cout << "not picking up vor. :-(" << endl;
|
||||
}
|
||||
|
||||
// nav2
|
||||
if ( current_navlist->query( lon, lat, elev, nav2_freq,
|
||||
&nav, &nav2_heading, &nav2_dist) ) {
|
||||
nav2_inrange = true;
|
||||
nav2_loc = false;
|
||||
nav2_lon = nav.get_lon();
|
||||
nav2_lat = nav.get_lat();
|
||||
nav2_elev = nav.get_elev();
|
||||
nav2_radial = nav2_sel_radial;
|
||||
if ( nav2_valid ) {
|
||||
// staightline distance
|
||||
station = Point3D( nav2_x, nav2_y, nav2_z );
|
||||
nav2_dist = aircraft.distance3D( station );
|
||||
|
||||
if ( nav2_dist < nav2_effective_range * NM_TO_METER ) {
|
||||
nav2_inrange = true;
|
||||
|
||||
// wgs84 heading
|
||||
geo_inverse_wgs_84( elev, lat * RAD_TO_DEG, lon * RAD_TO_DEG,
|
||||
nav2_lat, nav2_lon,
|
||||
&az1, &az2, &s );
|
||||
nav2_heading = az1;
|
||||
|
||||
// cout << " heading = " << nav2_heading
|
||||
// << " dist = " << nav2_dist << endl;
|
||||
} else {
|
||||
nav2_inrange = false;
|
||||
}
|
||||
|
||||
if ( !nav2_loc ) {
|
||||
nav2_radial = nav2_sel_radial;
|
||||
}
|
||||
} else {
|
||||
nav2_inrange = false;
|
||||
// cout << "not picking up vor. :-(" << endl;
|
||||
}
|
||||
|
||||
// adf
|
||||
double junk;
|
||||
if ( current_navlist->query( lon, lat, elev, adf_freq,
|
||||
&nav, &adf_heading, &junk) ) {
|
||||
adf_inrange = true;
|
||||
if ( adf_valid ) {
|
||||
// staightline distance
|
||||
station = Point3D( adf_x, adf_y, adf_z );
|
||||
adf_dist = aircraft.distance3D( station );
|
||||
|
||||
if ( adf_dist < adf_effective_range * NM_TO_METER ) {
|
||||
adf_inrange = true;
|
||||
|
||||
// wgs84 heading
|
||||
geo_inverse_wgs_84( elev, lat * RAD_TO_DEG, lon * RAD_TO_DEG,
|
||||
adf_lat, adf_lon,
|
||||
&az1, &az2, &s );
|
||||
adf_heading = az1;
|
||||
|
||||
// cout << " heading = " << nav2_heading
|
||||
// << " dist = " << nav2_dist << endl;
|
||||
} else {
|
||||
adf_inrange = false;
|
||||
}
|
||||
} else {
|
||||
adf_inrange = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Update current nav/adf radio stations based on current postition
|
||||
void FGRadioStack::search( double lon, double lat, double elev ) {
|
||||
// nav1
|
||||
FGILS ils;
|
||||
FGNav nav;
|
||||
|
||||
if ( current_ilslist->query( lon, lat, elev, nav1_freq, &ils ) ) {
|
||||
nav1_valid = true;
|
||||
nav1_loc = true;
|
||||
|
||||
nav1_lon = ils.get_loclon();
|
||||
nav1_lat = ils.get_loclat();
|
||||
nav1_elev = ils.get_gselev();
|
||||
nav1_effective_range = FG_ILS_DEFAULT_RANGE;
|
||||
nav1_target_gs = ils.get_gsangle();
|
||||
nav1_radial = ils.get_locheading();
|
||||
while ( nav1_radial < 0.0 ) { nav1_radial += 360.0; }
|
||||
while ( nav1_radial > 360.0 ) { nav1_radial -= 360.0; }
|
||||
nav1_x = ils.get_x();
|
||||
nav1_y = ils.get_y();
|
||||
nav1_z = ils.get_z();
|
||||
// cout << "Found an ils station in range" << endl;
|
||||
// cout << " id = " << ils.get_locident() << endl;
|
||||
} else if ( current_navlist->query( lon, lat, elev, nav1_freq, &nav ) ) {
|
||||
nav1_valid = true;
|
||||
nav1_loc = false;
|
||||
|
||||
nav1_lon = nav.get_lon();
|
||||
nav1_lat = nav.get_lat();
|
||||
nav1_elev = nav.get_elev();
|
||||
nav1_effective_range = nav.get_range();
|
||||
nav1_target_gs = 0.0;
|
||||
nav1_radial = nav1_sel_radial;
|
||||
nav1_x = nav.get_x();
|
||||
nav1_y = nav.get_y();
|
||||
nav1_z = nav.get_z();
|
||||
// cout << "Found a vor station in range" << endl;
|
||||
// cout << " id = " << nav.get_ident() << endl;
|
||||
} else {
|
||||
nav1_valid = false;
|
||||
// cout << "not picking up vor1. :-(" << endl;
|
||||
}
|
||||
|
||||
if ( current_ilslist->query( lon, lat, elev, nav2_freq, &ils ) ) {
|
||||
nav2_valid = true;
|
||||
nav2_loc = true;
|
||||
|
||||
nav2_lon = ils.get_loclon();
|
||||
nav2_lat = ils.get_loclat();
|
||||
nav2_elev = ils.get_gselev();
|
||||
nav2_effective_range = FG_ILS_DEFAULT_RANGE;
|
||||
nav2_target_gs = ils.get_gsangle();
|
||||
nav2_radial = ils.get_locheading();
|
||||
while ( nav2_radial < 0.0 ) { nav2_radial += 360.0; }
|
||||
while ( nav2_radial > 360.0 ) { nav2_radial -= 360.0; }
|
||||
nav2_x = ils.get_x();
|
||||
nav2_y = ils.get_y();
|
||||
nav2_z = ils.get_z();
|
||||
// cout << "Found an ils station in range" << endl;
|
||||
// cout << " id = " << ils.get_locident() << endl;
|
||||
} else if ( current_navlist->query( lon, lat, elev, nav2_freq, &nav ) ) {
|
||||
nav2_valid = true;
|
||||
nav2_loc = false;
|
||||
|
||||
nav2_lon = nav.get_lon();
|
||||
nav2_lat = nav.get_lat();
|
||||
nav2_elev = nav.get_elev();
|
||||
nav2_effective_range = nav.get_range();
|
||||
nav2_target_gs = 0.0;
|
||||
nav2_radial = nav2_sel_radial;
|
||||
nav2_x = nav.get_x();
|
||||
nav2_y = nav.get_y();
|
||||
nav2_z = nav.get_z();
|
||||
// cout << "Found a vor station in range" << endl;
|
||||
// cout << " id = " << nav.get_ident() << endl;
|
||||
} else {
|
||||
nav2_valid = false;
|
||||
// cout << "not picking up vor2. :-(" << endl;
|
||||
}
|
||||
|
||||
// adf
|
||||
if ( current_navlist->query( lon, lat, elev, adf_freq, &nav ) ) {
|
||||
adf_valid = true;
|
||||
|
||||
adf_lon = nav.get_lon();
|
||||
adf_lat = nav.get_lat();
|
||||
adf_elev = nav.get_elev();
|
||||
adf_effective_range = nav.get_range();
|
||||
adf_x = nav.get_x();
|
||||
adf_y = nav.get_y();
|
||||
adf_z = nav.get_z();
|
||||
// cout << "Found an adf station in range" << endl;
|
||||
// cout << " id = " << nav.get_ident() << endl;
|
||||
// cout << " heading = " << adf_heading
|
||||
// << " dist = " << junk << endl;
|
||||
} else {
|
||||
adf_inrange = false;
|
||||
adf_valid = false;
|
||||
// cout << "not picking up adf. :-(" << endl;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// periodic radio station search wrapper
|
||||
void fgRadioSearch( void ) {
|
||||
current_radiostack->search( cur_fdm_state->get_Longitude(),
|
||||
cur_fdm_state->get_Latitude(),
|
||||
cur_fdm_state->get_Altitude() * FEET_TO_METER );
|
||||
}
|
||||
|
|
|
@ -27,11 +27,15 @@
|
|||
|
||||
#include <simgear/compiler.h>
|
||||
|
||||
#include <Navaids/ilslist.hxx>
|
||||
#include <Navaids/navlist.hxx>
|
||||
|
||||
|
||||
class FGRadioStack {
|
||||
|
||||
bool need_update;
|
||||
|
||||
bool nav1_valid;
|
||||
bool nav1_inrange;
|
||||
bool nav1_loc;
|
||||
double nav1_freq;
|
||||
|
@ -41,10 +45,15 @@ class FGRadioStack {
|
|||
double nav1_lon;
|
||||
double nav1_lat;
|
||||
double nav1_elev;
|
||||
double nav1_effective_range;
|
||||
double nav1_dist;
|
||||
double nav1_heading;
|
||||
double nav1_target_gs;
|
||||
double nav1_x;
|
||||
double nav1_y;
|
||||
double nav1_z;
|
||||
|
||||
bool nav2_valid;
|
||||
bool nav2_inrange;
|
||||
bool nav2_loc;
|
||||
double nav2_freq;
|
||||
|
@ -54,10 +63,15 @@ class FGRadioStack {
|
|||
double nav2_lon;
|
||||
double nav2_lat;
|
||||
double nav2_elev;
|
||||
double nav2_effective_range;
|
||||
double nav2_dist;
|
||||
double nav2_heading;
|
||||
double nav2_target_gs;
|
||||
double nav2_x;
|
||||
double nav2_y;
|
||||
double nav2_z;
|
||||
|
||||
bool adf_valid;
|
||||
bool adf_inrange;
|
||||
double adf_freq;
|
||||
double adf_alt_freq;
|
||||
|
@ -65,13 +79,21 @@ class FGRadioStack {
|
|||
double adf_lon;
|
||||
double adf_lat;
|
||||
double adf_elev;
|
||||
double adf_effective_range;
|
||||
double adf_dist;
|
||||
double adf_heading;
|
||||
double adf_x;
|
||||
double adf_y;
|
||||
double adf_z;
|
||||
|
||||
public:
|
||||
|
||||
FGRadioStack();
|
||||
~FGRadioStack();
|
||||
|
||||
// Update nav/adf radios based on current postition
|
||||
void search( double lon, double lat, double elev );
|
||||
|
||||
// Update nav/adf radios based on current postition
|
||||
void update( double lon, double lat, double elev );
|
||||
|
||||
|
@ -147,4 +169,8 @@ public:
|
|||
extern FGRadioStack *current_radiostack;
|
||||
|
||||
|
||||
// periodic radio station search wrapper
|
||||
void fgRadioSearch( void );
|
||||
|
||||
|
||||
#endif // _FG_RADIOSTACK_HXX
|
||||
|
|
|
@ -349,8 +349,8 @@ double FGSteam::get_HackVOR1_deg () {
|
|||
double r;
|
||||
|
||||
if ( current_radiostack->get_nav1_inrange() ) {
|
||||
r = current_radiostack->get_nav1_radial() -
|
||||
current_radiostack->get_nav1_heading();
|
||||
r = current_radiostack->get_nav1_heading() -
|
||||
current_radiostack->get_nav1_radial();
|
||||
// cout << "Radial = " << current_radiostack->get_nav1_radial()
|
||||
// << " Bearing = " << current_radiostack->get_nav1_heading()
|
||||
// << endl;
|
||||
|
@ -359,7 +359,7 @@ double FGSteam::get_HackVOR1_deg () {
|
|||
if (r<-180.0) r+=360.0;
|
||||
if ( fabs(r) > 90.0 )
|
||||
r = ( r<0.0 ? -r-180.0 : -r+180.0 );
|
||||
if ( current_radiostack->get_nav1_loc() ) r*=5.0;
|
||||
if ( current_radiostack->get_nav1_loc() ) r *= -5.0;
|
||||
} else {
|
||||
r = 0.0;
|
||||
}
|
||||
|
|
|
@ -460,16 +460,19 @@ bool fgInitSubsystems( void ) {
|
|||
// Initialize vor/ndb/ils/fix list management and query systems
|
||||
FG_LOG(FG_GENERAL, FG_INFO, "Loading Navaids");
|
||||
|
||||
FG_LOG(FG_GENERAL, FG_INFO, " VOR/NDB");
|
||||
current_navlist = new FGNavList;
|
||||
FGPath p_nav( current_options.get_fg_root() );
|
||||
p_nav.append( "Navaids/default.nav" );
|
||||
current_navlist->init( p_nav );
|
||||
|
||||
FG_LOG(FG_GENERAL, FG_INFO, " ILS");
|
||||
current_ilslist = new FGILSList;
|
||||
FGPath p_ils( current_options.get_fg_root() );
|
||||
p_ils.append( "Navaids/default.ils" );
|
||||
current_ilslist->init( p_ils );
|
||||
|
||||
FG_LOG(FG_GENERAL, FG_INFO, " Fixes");
|
||||
current_fixlist = new FGFixList;
|
||||
FGPath p_fix( current_options.get_fg_root() );
|
||||
p_fix.append( "Navaids/default.fix" );
|
||||
|
@ -478,7 +481,7 @@ bool fgInitSubsystems( void ) {
|
|||
// Initialize the underlying radio stack model
|
||||
current_radiostack = new FGRadioStack;
|
||||
|
||||
current_radiostack->set_nav1_freq( 110.30 );
|
||||
current_radiostack->set_nav1_freq( 117.30 );
|
||||
current_radiostack->set_nav1_alt_freq( 110.30 );
|
||||
current_radiostack->set_nav1_sel_radial( 299.0 );
|
||||
|
||||
|
@ -488,10 +491,19 @@ bool fgInitSubsystems( void ) {
|
|||
|
||||
current_radiostack->set_adf_freq( 266.0 );
|
||||
|
||||
current_radiostack->search( cur_fdm_state->get_Longitude(),
|
||||
cur_fdm_state->get_Latitude(),
|
||||
cur_fdm_state->get_Altitude() * FEET_TO_METER );
|
||||
|
||||
current_radiostack->update( cur_fdm_state->get_Longitude(),
|
||||
cur_fdm_state->get_Latitude(),
|
||||
cur_fdm_state->get_Altitude() * FEET_TO_METER );
|
||||
|
||||
// Search radio database once per second
|
||||
global_events.Register( "fgRadioSearch()", fgRadioSearch,
|
||||
fgEVENT::FG_EVENT_READY, 1000);
|
||||
|
||||
|
||||
// Initialize the Cockpit subsystem
|
||||
if( fgCockpitInit( ¤t_aircraft )) {
|
||||
// Cockpit initialized ok.
|
||||
|
|
|
@ -745,8 +745,8 @@ void fgUpdateTimeDepCalcs(int multi_loop, int remainder) {
|
|||
ephem->update( t, cur_fdm_state->get_Latitude() );
|
||||
|
||||
// Update radio stack model
|
||||
current_radiostack->update( cur_fdm_state->get_Longitude() * RAD_TO_DEG,
|
||||
cur_fdm_state->get_Latitude() * RAD_TO_DEG,
|
||||
current_radiostack->update( cur_fdm_state->get_Longitude(),
|
||||
cur_fdm_state->get_Latitude(),
|
||||
cur_fdm_state->get_Altitude() * FEET_TO_METER );
|
||||
}
|
||||
|
||||
|
|
|
@ -248,7 +248,8 @@ fgOPTIONS::toggle_panel() {
|
|||
} else {
|
||||
fov *= (1.0 / 0.4232);
|
||||
}
|
||||
fgReshape( xsize, ysize);
|
||||
// fgReshape( xsize, ysize);
|
||||
fgReshape( current_view.get_winWidth(), current_view.get_winHeight() );
|
||||
|
||||
if( !toggle_pause )
|
||||
t->togglePauseMode();
|
||||
|
|
|
@ -26,6 +26,7 @@
|
|||
|
||||
|
||||
#include <simgear/compiler.h>
|
||||
#include <simgear/math/fg_geodesy.hxx>
|
||||
#include <simgear/misc/fgstream.hxx>
|
||||
|
||||
#ifdef FG_HAVE_STD_INCLUDES
|
||||
|
@ -56,6 +57,7 @@ class FGILS {
|
|||
double locheading;
|
||||
double loclat;
|
||||
double loclon;
|
||||
double x, y, z;
|
||||
double gselev;
|
||||
double gsangle;
|
||||
double gslat;
|
||||
|
@ -84,6 +86,9 @@ public:
|
|||
inline double get_loclat() const { return loclat; }
|
||||
inline double get_loclon() const { return loclon; }
|
||||
inline double get_gselev() const { return gselev; }
|
||||
inline double get_x() const { return x; }
|
||||
inline double get_y() const { return y; }
|
||||
inline double get_z() const { return z; }
|
||||
inline double get_gsangle() const { return gsangle; }
|
||||
inline double get_gslat() const { return gslat; }
|
||||
inline double get_gslon() const { return gslon; }
|
||||
|
@ -115,9 +120,16 @@ operator >> ( istream& in, FGILS& i )
|
|||
|
||||
i.locfreq = (int)(f*100.0 + 0.5);
|
||||
|
||||
// return in >> skipeol;
|
||||
// generate cartesian coordinates
|
||||
Point3D geod( i.loclon * DEG_TO_RAD, i.loclat * DEG_TO_RAD, i.gselev );
|
||||
Point3D cart = fgGeodToCart( geod );
|
||||
i.x = cart.x();
|
||||
i.y = cart.y();
|
||||
i.z = cart.z();
|
||||
|
||||
// return in >> skipeol;
|
||||
return in;
|
||||
}
|
||||
|
||||
|
||||
#endif // _FG_NAVAID_HXX
|
||||
#endif // _FG_ILS_HXX
|
||||
|
|
|
@ -97,8 +97,8 @@ bool FGILSList::init( FGPath path ) {
|
|||
}
|
||||
}
|
||||
|
||||
cout << "min freq = " << min << endl;
|
||||
cout << "max freq = " << max << endl;
|
||||
// cout << "min freq = " << min << endl;
|
||||
// cout << "max freq = " << max << endl;
|
||||
|
||||
#endif
|
||||
|
||||
|
@ -109,24 +109,33 @@ bool FGILSList::init( FGPath path ) {
|
|||
// query the database for the specified frequency, lon and lat are in
|
||||
// degrees, elev is in meters
|
||||
bool FGILSList::query( double lon, double lat, double elev, double freq,
|
||||
FGILS *ils, double *heading, double *dist )
|
||||
FGILS *ils )
|
||||
{
|
||||
ils_list_type stations = ilslist[(int)(freq*100.0 + 0.5)];
|
||||
|
||||
ils_list_iterator current = stations.begin();
|
||||
ils_list_iterator last = stations.end();
|
||||
|
||||
double az1, az2, s;
|
||||
// double az1, az2, s;
|
||||
Point3D aircraft = fgGeodToCart( Point3D(lon, lat, elev) );
|
||||
Point3D station;
|
||||
double d;
|
||||
for ( ; current != last ; ++current ) {
|
||||
// cout << "testing " << current->get_ident() << endl;
|
||||
geo_inverse_wgs_84( elev, lat, lon,
|
||||
current->get_loclat(), current->get_loclon(),
|
||||
&az1, &az2, &s );
|
||||
// cout << " testing " << current->get_locident() << endl;
|
||||
station = Point3D(current->get_x(), current->get_y(), current->get_z());
|
||||
// cout << " aircraft = " << aircraft << " station = " << station
|
||||
// << endl;
|
||||
|
||||
d = aircraft.distance3Dsquared( station );
|
||||
// cout << " distance = " << d << " ("
|
||||
// << FG_ILS_DEFAULT_RANGE * NM_TO_METER
|
||||
// * FG_ILS_DEFAULT_RANGE * NM_TO_METER
|
||||
// << ")" << endl;
|
||||
|
||||
// cout << " dist = " << s << endl;
|
||||
if ( s < ( FG_ILS_DEFAULT_RANGE * NM_TO_METER ) ) {
|
||||
if ( d < (FG_ILS_DEFAULT_RANGE * NM_TO_METER
|
||||
* FG_ILS_DEFAULT_RANGE * NM_TO_METER) ) {
|
||||
*ils = *current;
|
||||
*heading = az2;
|
||||
*dist = s;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -60,8 +60,7 @@ public:
|
|||
|
||||
// query the database for the specified frequency, lon and lat are
|
||||
// in degrees, elev is in meters
|
||||
bool query( double lon, double lat, double elev, double freq,
|
||||
FGILS *i, double *heading, double *dist);
|
||||
bool query( double lon, double lat, double elev, double freq, FGILS *i );
|
||||
};
|
||||
|
||||
|
||||
|
|
|
@ -26,6 +26,7 @@
|
|||
|
||||
|
||||
#include <simgear/compiler.h>
|
||||
#include <simgear/math/fg_geodesy.hxx>
|
||||
#include <simgear/misc/fgstream.hxx>
|
||||
|
||||
#ifdef FG_HAVE_STD_INCLUDES
|
||||
|
@ -48,6 +49,7 @@ class FGNav {
|
|||
char type;
|
||||
double lon, lat;
|
||||
double elev;
|
||||
double x, y, z;
|
||||
int freq;
|
||||
int range;
|
||||
bool dme;
|
||||
|
@ -62,6 +64,9 @@ public:
|
|||
inline double get_lon() const { return lon; }
|
||||
inline double get_lat() const { return lat; }
|
||||
inline double get_elev() const { return elev; }
|
||||
inline double get_x() const { return x; }
|
||||
inline double get_y() const { return y; }
|
||||
inline double get_z() const { return z; }
|
||||
inline int get_freq() const { return freq; }
|
||||
inline int get_range() const { return range; }
|
||||
inline bool get_dme() const { return dme; }
|
||||
|
@ -95,6 +100,13 @@ operator >> ( istream& in, FGNav& n )
|
|||
n.dme = false;
|
||||
}
|
||||
|
||||
// generate cartesian coordinates
|
||||
Point3D geod( n.lon * DEG_TO_RAD, n.lat * DEG_TO_RAD, n.elev );
|
||||
Point3D cart = fgGeodToCart( geod );
|
||||
n.x = cart.x();
|
||||
n.y = cart.y();
|
||||
n.z = cart.z();
|
||||
|
||||
return in >> skipeol;
|
||||
}
|
||||
|
||||
|
|
|
@ -99,8 +99,8 @@ bool FGNavList::init( FGPath path ) {
|
|||
}
|
||||
}
|
||||
|
||||
cout << "min freq = " << min << endl;
|
||||
cout << "max freq = " << max << endl;
|
||||
// cout << "min freq = " << min << endl;
|
||||
// cout << "max freq = " << max << endl;
|
||||
|
||||
#endif
|
||||
|
||||
|
@ -111,24 +111,27 @@ bool FGNavList::init( FGPath path ) {
|
|||
// query the database for the specified frequency, lon and lat are in
|
||||
// degrees, elev is in meters
|
||||
bool FGNavList::query( double lon, double lat, double elev, double freq,
|
||||
FGNav *n, double *heading, double *dist )
|
||||
FGNav *n )
|
||||
{
|
||||
nav_list_type stations = navaids[(int)(freq*100.0 + 0.5)];
|
||||
|
||||
nav_list_iterator current = stations.begin();
|
||||
nav_list_iterator last = stations.end();
|
||||
|
||||
double az1, az2, s;
|
||||
// double az1, az2, s;
|
||||
Point3D aircraft = fgGeodToCart( Point3D(lon, lat, elev) );
|
||||
Point3D station;
|
||||
double d;
|
||||
for ( ; current != last ; ++current ) {
|
||||
// cout << "testing " << current->get_ident() << endl;
|
||||
geo_inverse_wgs_84( elev, lat, lon,
|
||||
current->get_lat(), current->get_lon(),
|
||||
&az1, &az2, &s );
|
||||
station = Point3D(current->get_x(), current->get_y(), current->get_z());
|
||||
|
||||
d = aircraft.distance3Dsquared( station );
|
||||
|
||||
// cout << " dist = " << s << endl;
|
||||
if ( s < ( current->get_range() * NM_TO_METER ) ) {
|
||||
if ( d < (current->get_range() * NM_TO_METER
|
||||
* current->get_range() * NM_TO_METER) ) {
|
||||
*n = *current;
|
||||
*heading = az2;
|
||||
*dist = s;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -60,8 +60,7 @@ public:
|
|||
|
||||
// query the database for the specified frequency, lon and lat are
|
||||
// in degrees, elev is in meters
|
||||
bool query( double lon, double lat, double elev, double freq,
|
||||
FGNav *n, double *heading, double *dist);
|
||||
bool query( double lon, double lat, double elev, double freq, FGNav *n );
|
||||
};
|
||||
|
||||
|
||||
|
|
|
@ -11,11 +11,9 @@ int main() {
|
|||
FGPath p_nav( "/home/curt/FlightGear/Navaids/default.nav" );
|
||||
current_navlist->init( p_nav );
|
||||
FGNav n;
|
||||
if ( current_navlist->query( -93.2, 45.14, 3000, 117.30,
|
||||
&n, &heading, &dist) ) {
|
||||
if ( current_navlist->query( -93.2, 45.14, 3000, 117.30, &n) ) {
|
||||
cout << "Found a vor station in range" << endl;
|
||||
cout << " id = " << n.get_ident() << endl;
|
||||
cout << " heading = " << heading << " dist = " << dist << endl;
|
||||
} else {
|
||||
cout << "not picking up vor. :-(" << endl;
|
||||
}
|
||||
|
@ -24,12 +22,10 @@ int main() {
|
|||
FGPath p_ils( "/home/curt/FlightGear/Navaids/default.ils" );
|
||||
current_ilslist->init( p_ils );
|
||||
FGILS i;
|
||||
if ( current_ilslist->query( -93.1, 45.24, 3000, 110.30,
|
||||
&i, &heading, &dist) ) {
|
||||
if ( current_ilslist->query( -93.1, 45.24, 3000, 110.30, &i) ) {
|
||||
cout << "Found an ils station in range" << endl;
|
||||
cout << " apt = " << i.get_aptcode() << endl;
|
||||
cout << " rwy = " << i.get_rwyno() << endl;
|
||||
cout << " heading = " << heading << " dist = " << dist << endl;
|
||||
} else {
|
||||
cout << "not picking up ils. :-(" << endl;
|
||||
}
|
||||
|
|
|
@ -124,7 +124,7 @@ fgEVENT::~fgEVENT()
|
|||
void
|
||||
fgEVENT::run()
|
||||
{
|
||||
FG_LOG(FG_EVENT, FG_INFO, "Running " << description );
|
||||
FG_LOG(FG_EVENT, FG_DEBUG, "Running " << description );
|
||||
|
||||
// record starting time
|
||||
last_run.stamp();
|
||||
|
|
Loading…
Add table
Reference in a new issue