#include <simgear/compiler.h>
#include <simgear/math/sg_random.h>
+#include <simgear/math/vector.hxx>
#include <Aircraft/aircraft.hxx>
#include <Navaids/ilslist.hxx>
nav_last_time(0),
need_update(true),
power_btn(true),
+ audio_btn(true),
comm_freq(0.0),
comm_alt_freq(0.0),
comm_vol_btn(0.0),
{
morse.init();
- search();
+ // We assume that index is valid now (it must be set before init()
+ // is called.)
+ char propname[256];
+
+ sprintf( propname, "/systems/electrical/outputs/navcom[%d]", index );
+ // default to true in case no electrical system defined.
+ fgSetDouble( propname, 60.0 );
+ bus_power = fgGetNode( propname, true );
+
+ sprintf( propname, "/instrumentation/comm[%d]/servicable", index );
+ com_servicable = fgGetNode( propname, true );
+ com_servicable->setBoolValue( true );
+
+ sprintf( propname, "/instrumentation/nav[%d]/servicable", index );
+ nav_servicable = fgGetNode( propname, true );
+ nav_servicable->setBoolValue( true );
+
+ sprintf( propname, "/instrumentation/vor[%d]/cdi/servicable", index );
+ cdi_servicable = fgGetNode( propname, true );
+ cdi_servicable->setBoolValue( true );
+
+ sprintf( propname, "/instrumentation/vor[%d]/gs/servicable", index );
+ gs_servicable = fgGetNode( propname, true );
+ gs_servicable->setBoolValue( true );
- update(0); // FIXME: use dt
+ sprintf( propname, "/instrumentation/vor[%d]/to-from/servicable", index );
+ tofrom_servicable = fgGetNode( propname, true );
+ tofrom_servicable->setBoolValue( true );
}
void
fgSetArchivable( propname );
// Radio outputs
+ sprintf( propname, "/radios/nav[%d]/audio-btn", index );
+ fgTie( propname, this,
+ &FGNavCom::get_audio_btn, &FGNavCom::set_audio_btn );
+ fgSetArchivable( propname );
+
sprintf( propname, "/radios/nav[%d]/radials/actual-deg", index );
fgTie( propname, this, &FGNavCom::get_nav_radial );
sprintf( propname, "/radios/nav[%d]/heading-needle-deflection", index );
fgTie( propname, this, &FGNavCom::get_nav_heading_needle_deflection );
+ sprintf( propname, "/radios/nav[%d]/has-gs", index );
+ fgTie( propname, this, &FGNavCom::get_nav_has_gs );
+
sprintf( propname, "/radios/nav[%d]/gs-needle-deflection", index );
fgTie( propname, this, &FGNavCom::get_nav_gs_needle_deflection );
// end of binding
-
- // We know index is valid now so lets bind to the bus property
- // here.
- sprintf( propname, "/systems/electrical/outputs/navcom[%d]", index );
- // default to true in case no electrical system defined.
- fgSetDouble( propname, 60.0 );
- bus_power = fgGetNode( propname, true );
}
// model standard VOR/DME/TACAN service volumes as per AIM 1-1-8
double FGNavCom::adjustNavRange( double stationElev, double aircraftElev,
- double nominalRange )
+ double nominalRange )
{
// extend out actual usable range to be 1.3x the published safe range
const double usability_factor = 1.3;
// altitude difference
// double alt = ( aircraftElev * SG_METER_TO_FEET - stationElev );
- double offset = fabs( offsetDegrees );
-
- if ( offset < 10 ) {
- return FG_ILS_DEFAULT_RANGE;
- } else if ( offset < 35 ) {
- return 10 + (35 - offset) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
- } else if ( offset < 45 ) {
- return (45 - offset);
- } else if ( offset > 170 ) {
- return FG_ILS_DEFAULT_RANGE;
- } else if ( offset > 145 ) {
- return 10 + (offset - 145) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
- } else if ( offset > 135 ) {
- return (offset - 135);
- } else {
- return 0;
- }
+// double offset = fabs( offsetDegrees );
+
+// if ( offset < 10 ) {
+// return FG_ILS_DEFAULT_RANGE;
+// } else if ( offset < 35 ) {
+// return 10 + (35 - offset) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
+// } else if ( offset < 45 ) {
+// return (45 - offset);
+// } else if ( offset > 170 ) {
+// return FG_ILS_DEFAULT_RANGE;
+// } else if ( offset > 145 ) {
+// return 10 + (offset - 145) * (FG_ILS_DEFAULT_RANGE - 10) / 25;
+// } else if ( offset > 135 ) {
+// return (offset - 135);
+// } else {
+// return 0;
+// }
+ return FG_ILS_DEFAULT_RANGE;
}
// Nav.
////////////////////////////////////////////////////////////////////////
- if ( nav_valid && power_btn && (bus_power->getDoubleValue() > 1.0) ) {
+ if ( nav_valid && power_btn && (bus_power->getDoubleValue() > 1.0)
+ && nav_servicable->getBoolValue() )
+ {
station = Point3D( nav_x, nav_y, nav_z );
nav_loc_dist = aircraft.distance3D( station );
if ( nav_has_gs ) {
- station = Point3D( nav_gs_x, nav_gs_y, nav_gs_z );
- nav_gs_dist = aircraft.distance3D( station );
+ // find closest distance to the gs base line
+ sgdVec3 p;
+ sgdSetVec3( p, aircraft.x(), aircraft.y(), aircraft.z() );
+ sgdVec3 p0;
+ sgdSetVec3( p0, nav_gs_x, nav_gs_y, nav_gs_z );
+ double dist = sgdClosestPointToLineDistSquared( p, p0,
+ gs_base_vec );
+ nav_gs_dist = sqrt( dist );
+ // cout << nav_gs_dist;
+
+ // Point3D tmp( nav_gs_x, nav_gs_y, nav_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,
+ nav_gslat, nav_gslon,
+ &az1, &az2, &s );
+ double r = az1 - nav_radial;
+ while ( r > 180.0 ) { r -= 360.0;}
+ while ( r < -180.0 ) { r += 360.0;}
+ if ( r >= -90.0 && r <= 90.0 ) {
+ nav_gs_dist_signed = nav_gs_dist;
+ } else {
+ nav_gs_dist_signed = -nav_gs_dist;
+ }
+ /* cout << "Target Radial = " << nav_radial
+ << " Bearing = " << az1
+ << " dist (signed) = " << nav_gs_dist_signed
+ << endl; */
+
} else {
nav_gs_dist = 0.0;
}
- // wgs84 heading
- geo_inverse_wgs_84( elev, lat * SGD_RADIANS_TO_DEGREES, lon * SGD_RADIANS_TO_DEGREES,
+ // wgs84 heading to localizer
+ geo_inverse_wgs_84( elev,
+ lat * SGD_RADIANS_TO_DEGREES,
+ lon * SGD_RADIANS_TO_DEGREES,
nav_loclat, nav_loclon,
&az1, &az2, &s );
// cout << "az1 = " << az1 << " magvar = " << nav_magvar << endl;
// cout << "not picking up vor. :-(" << endl;
}
-#ifdef ENABLE_AUDIO_SUPPORT
- if ( nav_valid && nav_inrange ) {
+ if ( nav_valid && nav_inrange && nav_servicable->getBoolValue() ) {
// play station ident via audio system if on + ident,
// otherwise turn it off
if ( power_btn && (bus_power->getDoubleValue() > 1.0)
- && nav_ident_btn )
+ && nav_ident_btn && audio_btn )
{
- FGSimpleSound *sound;
+ SGSimpleSound *sound;
sound = globals->get_soundmgr()->find( nav_fx_name );
if ( sound != NULL ) {
sound->set_volume( nav_vol_btn );
globals->get_soundmgr()->stop( dme_fx_name );
}
}
-#endif
-
}
double lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
double elev = alt_node->getDoubleValue() * SG_FEET_TO_METER;
- FGILS ils;
- FGNav nav;
+ FGILS *ils;
+ FGNav *nav;
////////////////////////////////////////////////////////////////////////
// Nav.
////////////////////////////////////////////////////////////////////////
- if ( current_ilslist->query( lon, lat, elev, nav_freq, &ils ) ) {
- nav_id = ils.get_locident();
+ if ( (ils = current_ilslist->findByFreq(nav_freq, lon, lat, elev)) != NULL ) {
+ nav_id = ils->get_locident();
nav_valid = true;
if ( last_nav_id != nav_id || last_nav_vor ) {
- nav_trans_ident = ils.get_trans_ident();
+ nav_trans_ident = ils->get_trans_ident();
last_nav_id = nav_id;
last_nav_vor = false;
nav_loc = true;
- nav_has_dme = ils.get_has_dme();
- nav_has_gs = ils.get_has_gs();
-
- nav_loclon = ils.get_loclon();
- nav_loclat = ils.get_loclat();
- nav_gslon = ils.get_gslon();
- nav_gslat = ils.get_gslat();
- nav_elev = ils.get_gselev();
+ nav_has_dme = ils->get_has_dme();
+ nav_has_gs = ils->get_has_gs();
+
+ nav_loclon = ils->get_loclon();
+ nav_loclat = ils->get_loclat();
+ nav_gslon = ils->get_gslon();
+ nav_gslat = ils->get_gslat();
+ nav_elev = ils->get_gselev();
nav_magvar = 0;
nav_range = FG_ILS_DEFAULT_RANGE;
nav_effective_range = nav_range;
- nav_target_gs = ils.get_gsangle();
- nav_radial = ils.get_locheading();
+ nav_target_gs = ils->get_gsangle();
+ nav_radial = ils->get_locheading();
while ( nav_radial < 0.0 ) { nav_radial += 360.0; }
while ( nav_radial > 360.0 ) { nav_radial -= 360.0; }
- nav_x = ils.get_x();
- nav_y = ils.get_y();
- nav_z = ils.get_z();
- nav_gs_x = ils.get_gs_x();
- nav_gs_y = ils.get_gs_y();
- nav_gs_z = ils.get_gs_z();
-
-#ifdef ENABLE_AUDIO_SUPPORT
+ nav_x = ils->get_x();
+ nav_y = ils->get_y();
+ nav_z = ils->get_z();
+ nav_gs_x = ils->get_gs_x();
+ nav_gs_y = ils->get_gs_y();
+ nav_gs_z = ils->get_gs_z();
+
+ // derive GS baseline
+ double tlon, tlat, taz;
+ geo_direct_wgs_84 ( 0.0, nav_gslat, nav_gslon, nav_radial + 90,
+ 100.0, &tlat, &tlon, &taz );
+ // cout << nav_gslon << "," << nav_gslat << " "
+ // << 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 << nav_gs_x << "," << nav_gs_y << "," << nav_gs_z << endl;
+ // cout << p1 << endl;
+ sgdSetVec3( gs_base_vec,
+ p1.x()-nav_gs_x, p1.y()-nav_gs_y, p1.z()-nav_gs_z );
+ // cout << gs_base_vec[0] << "," << gs_base_vec[1] << ","
+ // << gs_base_vec[2] << endl;
+
if ( globals->get_soundmgr()->exists( nav_fx_name ) ) {
globals->get_soundmgr()->remove( nav_fx_name );
}
- FGSimpleSound *sound;
+ SGSimpleSound *sound;
sound = morse.make_ident( nav_trans_ident, LO_FREQUENCY );
sound->set_volume( 0.3 );
globals->get_soundmgr()->add( sound, nav_fx_name );
// << " nav_last_time = " << nav_last_time
// << " current time = "
// << globals->get_time_params()->get_cur_time() << endl;
-#endif
// cout << "Found an ils station in range" << endl;
- // cout << " id = " << ils.get_locident() << endl;
+ // cout << " id = " << ils->get_locident() << endl;
}
- } else if ( current_navlist->query( lon, lat, elev, nav_freq, &nav ) ) {
- nav_id = nav.get_ident();
- nav_valid = true;
+ } else if ( (nav = current_navlist->findByFreq(nav_freq, lon, lat, elev)) != NULL ) {
+ nav_id = nav->get_ident();
+ nav_valid = (nav->get_type() == 'V');
if ( last_nav_id != nav_id || !last_nav_vor ) {
last_nav_id = nav_id;
last_nav_vor = true;
- nav_trans_ident = nav.get_trans_ident();
+ nav_trans_ident = nav->get_trans_ident();
nav_loc = false;
- nav_has_dme = nav.get_has_dme();
+ nav_has_dme = nav->get_has_dme();
nav_has_gs = false;
- nav_loclon = nav.get_lon();
- nav_loclat = nav.get_lat();
- nav_elev = nav.get_elev();
- nav_magvar = nav.get_magvar();
- nav_range = nav.get_range();
+ nav_loclon = nav->get_lon();
+ nav_loclat = nav->get_lat();
+ nav_elev = nav->get_elev();
+ nav_magvar = nav->get_magvar();
+ nav_range = nav->get_range();
nav_effective_range = adjustNavRange(nav_elev, elev, nav_range);
nav_target_gs = 0.0;
nav_radial = nav_sel_radial;
- nav_x = nav.get_x();
- nav_y = nav.get_y();
- nav_z = nav.get_z();
+ nav_x = nav->get_x();
+ nav_y = nav->get_y();
+ nav_z = nav->get_z();
-#ifdef ENABLE_AUDIO_SUPPORT
if ( globals->get_soundmgr()->exists( nav_fx_name ) ) {
globals->get_soundmgr()->remove( nav_fx_name );
}
- FGSimpleSound *sound;
+ SGSimpleSound *sound;
sound = morse.make_ident( nav_trans_ident, LO_FREQUENCY );
sound->set_volume( 0.3 );
if ( globals->get_soundmgr()->add( sound, nav_fx_name ) ) {
// cout << "Added nav-vor-ident sound" << endl;
} else {
- cout << "Failed to add v1-vor-ident sound" << endl;
+ SG_LOG(SG_COCKPIT, SG_WARN, "Failed to add v1-vor-ident sound");
}
if ( globals->get_soundmgr()->exists( dme_fx_name ) ) {
// << nav_play_count << " nav_last_time = "
// << nav_last_time << " current time = "
// << globals->get_time_params()->get_cur_time() << endl;
-#endif
// cout << "Found a vor station in range" << endl;
- // cout << " id = " << nav.get_ident() << endl;
+ // cout << " id = " << nav->get_ident() << endl;
}
} else {
nav_valid = false;
nav_radial = 0;
nav_trans_ident = "";
last_nav_id = "";
-#ifdef ENABLE_AUDIO_SUPPORT
if ( ! globals->get_soundmgr()->remove( nav_fx_name ) ) {
- cout << "Failed to remove nav-vor-ident sound" << endl;
+ SG_LOG(SG_COCKPIT, SG_WARN, "Failed to remove nav-vor-ident sound");
}
globals->get_soundmgr()->remove( dme_fx_name );
-#endif
// cout << "not picking up vor1. :-(" << endl;
}
}
double FGNavCom::get_nav_heading_needle_deflection() const {
double r;
- std::cout << "Nav heading is " << nav_heading << std::endl;
- std::cout << "Nav radial is " << nav_radial << std::endl;
-
- if ( nav_inrange ) {
+ if ( nav_inrange
+ && nav_servicable->getBoolValue() && cdi_servicable->getBoolValue() )
+ {
r = nav_heading - nav_radial;
// cout << "Radial = " << nav_radial
// << " Bearing = " << nav_heading << endl;
// return the amount of glide slope needle deflection (.i.e. the
// number of degrees we are off the glide slope * 5.0
double FGNavCom::get_nav_gs_needle_deflection() const {
- if ( nav_inrange && nav_has_gs ) {
+ if ( nav_inrange && nav_has_gs
+ && nav_servicable->getBoolValue() && gs_servicable->getBoolValue() )
+ {
double x = nav_gs_dist;
double y = (fgGetDouble("/position/altitude-ft") - nav_elev)
* SG_FEET_TO_METER;
- double angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
+ // cout << "dist = " << x << " height = " << y << endl;
+ double angle = asin( y / x ) * SGD_RADIANS_TO_DEGREES;
return (nav_target_gs - angle) * 5.0;
} else {
return 0.0;
bool
FGNavCom::get_nav_to_flag () const
{
- if (nav_inrange) {
- double offset = fabs(nav_heading - nav_radial);
- if (nav_loc)
- return true;
- else
- return (offset <= 90.0 || offset >= 270.0);
- } else {
- return false;
- }
+ if ( nav_inrange
+ && nav_servicable->getBoolValue()
+ && tofrom_servicable->getBoolValue() )
+ {
+ double offset = fabs(nav_heading - nav_radial);
+ if (nav_loc) {
+ return true;
+ } else {
+ return (offset <= 90.0 || offset >= 270.0);
+ }
+ } else {
+ return false;
+ }
}
bool
FGNavCom::get_nav_from_flag () const
{
- if (nav_inrange) {
- double offset = fabs(nav_heading - nav_radial);
- if (nav_loc)
- return false;
- else
- return (offset > 90.0 && offset < 270.0);
- } else {
- return false;
- }
+ if ( nav_inrange
+ && nav_servicable->getBoolValue()
+ && tofrom_servicable->getBoolValue() ) {
+ double offset = fabs(nav_heading - nav_radial);
+ if (nav_loc) {
+ return false;
+ } else {
+ return (offset > 90.0 && offset < 270.0);
+ }
+ } else {
+ return false;
+ }
}
projects / flightgear.git / blobdiff