1
0
Fork 0

Overhaul of the navaid system to increase efficiency, reduce redundancy, and

also properly model the DME.
This commit is contained in:
curt 2000-05-04 01:18:45 +00:00
parent 977917a612
commit 6bc7ed9ba2
15 changed files with 300 additions and 94 deletions

View file

@ -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

View file

@ -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 );
}

View file

@ -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

View file

@ -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;
}

View file

@ -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( &current_aircraft )) {
// Cockpit initialized ok.

View file

@ -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 );
}

View file

@ -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();

View file

@ -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

View file

@ -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;
}
}

View file

@ -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 );
};

View file

@ -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;
}

View file

@ -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;
}
}

View file

@ -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 );
};

View file

@ -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;
}

View file

@ -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();