//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
-// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA.
//
///////////////////////////////////////////////////////////////////////////////
//
#include <simgear/constants.h>
#include <simgear/sg_inlines.h>
#include <simgear/debug/logstream.hxx>
-#include <simgear/math/sg_geodesy.hxx>
+#include <simgear/math/SGMathFwd.hxx>
+#include <simgear/math/SGLimits.hxx>
+#include <simgear/math/SGGeometryFwd.hxx>
+#include <simgear/math/SGGeodesy.hxx>
#include <simgear/math/sg_random.h>
+#include <simgear/math/SGLineSegment.hxx>
+#include <simgear/math/SGIntersect.hxx>
#include <simgear/misc/sg_path.hxx>
#include <simgear/sound/soundmgr_openal.hxx>
+#include <simgear/sound/sample_group.hxx>
#include <simgear/structure/exception.hxx>
using std::string;
#include <Airports/runways.hxx>
-#include <Airports/simple.hxx>
+#include <Airports/airport.hxx>
#if defined( HAVE_VERSION_H ) && HAVE_VERSION_H
# include <Include/version.h>
return amplitude * pow(10.0, dB / 20.0);
}
-static double
-heading_add (double h1, double h2)
-{
- double result = h1 + h2;
- if (result >= 360)
- result -= 360;
- return result;
-}
-
-static double
-heading_substract (double h1, double h2)
-{
- double result = h1 - h2;
- if (result < 0)
- result += 360;
- return result;
-}
-
static double
get_heading_difference (double h1, double h2)
{
return fabs(diff);
}
-static double
-get_reciprocal_heading (double h)
-{
- return heading_add(h, 180);
-}
-
///////////////////////////////////////////////////////////////////////////////
// PropertiesHandler //////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
mk_node(longitude) = fgGetNode("/position/longitude-deg", true);
mk_node(nav0_cdi_serviceable) = fgGetNode("/instrumentation/nav/cdi/serviceable", true);
mk_node(nav0_gs_distance) = fgGetNode("/instrumentation/nav/gs-distance", true);
- mk_node(nav0_gs_needle_deflection) = fgGetNode("/instrumentation/nav/gs-needle-deflection", true);
+ mk_node(nav0_gs_needle_deflection) = fgGetNode("/instrumentation/nav/gs-needle-deflection-deg", true);
mk_node(nav0_gs_serviceable) = fgGetNode("/instrumentation/nav/gs/serviceable", true);
mk_node(nav0_has_gs) = fgGetNode("/instrumentation/nav/has-gs", true);
mk_node(nav0_heading_needle_deflection) = fgGetNode("/instrumentation/nav/heading-needle-deflection", true);
// calculate average
double new_value = 0;
- if (samples.size() > 0)
+ if (! samples.empty())
{
// time consuming loop => queue limited to 75 samples
// (= 15seconds * 5samples/second)
}
-///////////////////////////////////////////////////////////////////////////////
-// FGVoicePlayer //////////////////////////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////////
-
-void
-FGVoicePlayer::Speaker::bind (SGPropertyNode *node)
-{
- // uses xmlsound property names
- tie(node, "volume", &volume);
- tie(node, "pitch", &pitch);
-}
-
-void
-FGVoicePlayer::Speaker::update_configuration ()
-{
- map< string, SGSharedPtr<SGSoundSample> >::iterator iter;
- for (iter = player->samples.begin(); iter != player->samples.end(); iter++)
- {
- SGSoundSample *sample = (*iter).second;
-
- sample->set_pitch(pitch);
- }
-
- if (player->voice)
- player->voice->volume_changed();
-}
-
-FGVoicePlayer::Voice::~Voice ()
-{
- for (iter = elements.begin(); iter != elements.end(); iter++)
- delete *iter; // we owned the element
- elements.clear();
-}
-
-void
-FGVoicePlayer::Voice::play ()
-{
- iter = elements.begin();
- element = *iter;
-
- element->play(get_volume());
-}
-
-void
-FGVoicePlayer::Voice::stop (bool now)
-{
- if (element)
- {
- if (now || element->silence)
- {
- element->stop();
- element = NULL;
- }
- else
- iter = elements.end() - 1; // stop after the current element finishes
- }
-}
-
-void
-FGVoicePlayer::Voice::set_volume (float _volume)
-{
- volume = _volume;
- volume_changed();
-}
-
-void
-FGVoicePlayer::Voice::volume_changed ()
-{
- if (element)
- element->set_volume(get_volume());
-}
-
-void
-FGVoicePlayer::Voice::update ()
-{
- if (element)
- {
- if (! element->is_playing())
- {
- if (++iter == elements.end())
- element = NULL;
- else
- {
- element = *iter;
- element->play(get_volume());
- }
- }
- }
-}
-
-FGVoicePlayer::~FGVoicePlayer ()
-{
- vector<Voice *>::iterator iter1;
- for (iter1 = _voices.begin(); iter1 != _voices.end(); iter1++)
- delete *iter1;
- _voices.clear();
- samples.clear();
-}
-
-void
-FGVoicePlayer::bind (SGPropertyNode *node, const char* default_dir_prefix)
-{
- dir_prefix = node->getStringValue("voice/file-prefix", default_dir_prefix);
- speaker.bind(node);
-}
-
-void
-FGVoicePlayer::init ()
-{
- SGSoundMgr *smgr = globals->get_soundmgr();
- _sgr = smgr->find("avionics", true);
- _sgr->tie_to_listener();
- speaker.update_configuration();
-}
-
-void
-FGVoicePlayer::pause()
-{
- if (paused)
- return;
-
- paused = true;
- if (voice)
- {
- voice->stop(true);
- }
-}
-
-void
-FGVoicePlayer::resume()
-{
- if (!paused)
- return;
- paused = false;
- if (voice)
- {
- voice->play();
- }
-}
-
-SGSoundSample *
-FGVoicePlayer::get_sample (const char *name)
-{
- string refname;
- refname = dev_name + "/" + dir_prefix + name;
-
- SGSoundSample *sample = _sgr->find(refname);
- if (! sample)
- {
- string filename = dir_prefix + string(name) + ".wav";
- try
- {
- sample = new SGSoundSample(filename.c_str(), SGPath());
- }
- catch (const sg_exception &e)
- {
- SG_LOG(SG_INSTR, SG_ALERT, "Error loading sound sample \"" + filename + "\": " + e.getFormattedMessage());
- exit(1);
- }
-
- _sgr->add(sample, refname);
- samples[refname] = sample;
- }
-
- return sample;
-}
-
-void
-FGVoicePlayer::play (Voice *_voice, unsigned int flags)
-{
- if (!_voice)
- return;
- if (test_bits(flags, PLAY_NOW) || ! voice ||
- (voice->element && voice->element->silence))
- {
- if (voice)
- voice->stop(true);
-
- voice = _voice;
- looped = test_bits(flags, PLAY_LOOPED);
-
- next_voice = NULL;
- next_looped = false;
-
- if (!paused)
- voice->play();
- }
- else
- {
- next_voice = _voice;
- next_looped = test_bits(flags, PLAY_LOOPED);
- }
-}
-
-void
-FGVoicePlayer::stop (unsigned int flags)
-{
- if (voice)
- {
- voice->stop(test_bits(flags, STOP_NOW));
- if (voice->element)
- looped = false;
- else
- voice = NULL;
- next_voice = NULL;
- }
-}
-
-void
-FGVoicePlayer::set_volume (float _volume)
-{
- volume = _volume;
- if (voice)
- voice->volume_changed();
-}
-
-void
-FGVoicePlayer::update ()
-{
- if (voice)
- {
- voice->update();
-
- if (next_voice)
- {
- if (! voice->element || voice->element->silence)
- {
- voice = next_voice;
- looped = next_looped;
-
- next_voice = NULL;
- next_looped = false;
-
- voice->play();
- }
- }
- else
- {
- if (! voice->element)
- {
- if (looped)
- voice->play();
- else
- voice = NULL;
- }
- }
- }
-}
-
///////////////////////////////////////////////////////////////////////////////
// MK_VIII::VoicePlayer ///////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
SGGeod::fromDeg(mk_data(gps_longitude).get(), mk_data(gps_latitude).get()));
// get distance to threshold
- double distance, az1, az2;
- SGGeodesy::inverse(pos, _runway->threshold(), az1, az2, distance);
- return distance * SG_METER_TO_NM <= 5;
+ return SGGeodesy::distanceNm(pos, _runway->threshold()) <= 5;
}
bool
// TCFHandler /////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
-// Gets the difference between the azimuth from @from_lat,@from_lon to
-// @to_lat,@to_lon, and @to_heading, in degrees.
-double
-MK_VIII::TCFHandler::get_azimuth_difference (double from_lat,
- double from_lon,
- double to_lat,
- double to_lon,
- double to_heading)
-{
- double az1, az2, distance;
- geo_inverse_wgs_84(0, from_lat, from_lon, to_lat, to_lon, &az1, &az2, &distance);
- return get_heading_difference(az1, to_heading);
-}
-
-// Gets the difference between the azimuth from the current GPS
-// position to the center of @_runway, and the heading of @_runway, in
-// degrees.
-double
-MK_VIII::TCFHandler::get_azimuth_difference (const FGRunway *_runway)
-{
- return get_azimuth_difference(mk_data(gps_latitude).get(),
- mk_data(gps_longitude).get(),
- _runway->latitude(),
- _runway->longitude(),
- _runway->headingDeg());
-}
-
-// Selects the most likely intended destination runway of @airport,
-// and returns it in @_runway. For each runway, the difference between
-// the azimuth from the current GPS position to the center of the
-// runway and its heading is computed. The runway having the smallest
-// difference wins.
-//
-// This selection algorithm is not specified in [SPEC], but
-// http://www.egpws.com/general_information/description/runway_select.htm
-// talks about automatic runway selection.
-FGRunway*
-MK_VIII::TCFHandler::select_runway (const FGAirport *airport)
-{
- FGRunway* _runway = 0;
- double min_diff = 360;
-
- for (unsigned int r=0; r<airport->numRunways(); ++r) {
- FGRunway* rwy(airport->getRunwayByIndex(r));
- double diff = get_azimuth_difference(rwy);
- if (diff < min_diff)
- {
- min_diff = diff;
- _runway = rwy;
- }
- } // of airport runways iteration
- return _runway;
-}
-
bool MK_VIII::TCFHandler::AirportFilter::passAirport(FGAirport* aApt) const
{
return aApt->hasHardRunwayOfLengthFt(mk->conf.runway_database);
// large airports, which may have a runway located far away from
// the airport's reference point.
AirportFilter filter(mk);
- FGAirport* apt = FGAirport::findClosest(
- SGGeod::fromDeg(mk_data(gps_longitude).get(), mk_data(gps_latitude).get()),
- 30.0, &filter);
+ SGGeod apos = SGGeod::fromDeg(mk_data(gps_longitude).get(), mk_data(gps_latitude).get());
+ FGAirport* apt = FGAirport::findClosest(apos, 30.0, &filter);
if (!apt) return;
- FGRunway* _runway = select_runway(apt);
+ FGRunway* _runway = apt->findBestRunwayForPos(apos).get();
if (!_runway) return;
has_runway = true;
- runway.center.latitude = _runway->latitude();
- runway.center.longitude = _runway->longitude();
+ runway.center = _runway->pointOnCenterline(_runway->lengthM() * 0.5);
runway.elevation = apt->elevation();
+ runway.half_width_m = _runway->widthM() * 0.5;
double half_length_m = _runway->lengthM() * 0.5;
runway.half_length = half_length_m * SG_METER_TO_NM;
- // b3 ________________ b0
- // | |
- // h1>>> | e1<<<<<<<<e0 | <<<h0
- // |________________|
- // b2 b1
-
- // get heading to runway threshold (h0) and end (h1)
- runway.edges[0].heading = _runway->headingDeg();
- runway.edges[1].heading = get_reciprocal_heading(_runway->headingDeg());
+ // _________
+ // |
+ // <<<<e0 | <<<h0
+ // _________|
- double az;
+ // get heading of runway end (h0)
+ runway.edge.heading = _runway->headingDeg();
// get position of runway threshold (e0)
- geo_direct_wgs_84(0,
- runway.center.latitude,
- runway.center.longitude,
- runway.edges[1].heading,
- half_length_m,
- &runway.edges[0].position.latitude,
- &runway.edges[0].position.longitude,
- &az);
-
- // get position of runway end (e1)
- geo_direct_wgs_84(0,
- runway.center.latitude,
- runway.center.longitude,
- runway.edges[0].heading,
- half_length_m,
- &runway.edges[1].position.latitude,
- &runway.edges[1].position.longitude,
- &az);
-
- double half_width_m = _runway->widthM() * 0.5;
-
- // get position of threshold bias area edges (b0 and b1)
- get_bias_area_edges(&runway.edges[0].position,
- runway.edges[1].heading,
- half_width_m,
- &runway.bias_area[0],
- &runway.bias_area[1]);
-
- // get position of end bias area edges (b2 and b3)
- get_bias_area_edges(&runway.edges[1].position,
- runway.edges[0].heading,
- half_width_m,
- &runway.bias_area[2],
- &runway.bias_area[3]);
-}
+ runway.edge.position = _runway->begin();
-void
-MK_VIII::TCFHandler::get_bias_area_edges (Position *edge,
- double reciprocal,
- double half_width_m,
- Position *bias_edge1,
- Position *bias_edge2)
-{
- double half_bias_width_m = k * SG_NM_TO_METER + half_width_m;
- double tmp_latitude = 0.0, tmp_longitude = 0.0, az = 0.0;
-
- geo_direct_wgs_84(0,
- edge->latitude,
- edge->longitude,
- reciprocal,
- k * SG_NM_TO_METER,
- &tmp_latitude,
- &tmp_longitude,
- &az);
- geo_direct_wgs_84(0,
- tmp_latitude,
- tmp_longitude,
- heading_substract(reciprocal, 90),
- half_bias_width_m,
- &bias_edge1->latitude,
- &bias_edge1->longitude,
- &az);
- geo_direct_wgs_84(0,
- tmp_latitude,
- tmp_longitude,
- heading_add(reciprocal, 90),
- half_bias_width_m,
- &bias_edge2->latitude,
- &bias_edge2->longitude,
- &az);
+ // get cartesian coordinates of both runway ends
+ runway.bias_points[0] = _runway->cart();
+ runway.bias_points[1] = _runway->reciprocalRunway()->cart();
}
// Returns true if the current GPS position is inside the edge
bool
MK_VIII::TCFHandler::is_inside_edge_triangle (RunwayEdge *edge)
{
- return get_azimuth_difference(mk_data(gps_latitude).get(),
- mk_data(gps_longitude).get(),
- edge->position.latitude,
- edge->position.longitude,
- edge->heading) <= 45;
+ double az = SGGeodesy::courseDeg( SGGeod::fromDeg(mk_data(gps_longitude).get(),
+ mk_data(gps_latitude).get()),
+ edge->position);
+ return get_heading_difference(az, edge->heading) <= 45;
}
// Returns true if the current GPS position is inside the bias area of
bool
MK_VIII::TCFHandler::is_inside_bias_area ()
{
- double az1[4];
- double angles_sum = 0;
-
- for (int i = 0; i < 4; i++)
- {
- double az2, distance;
- geo_inverse_wgs_84(0,
- mk_data(gps_latitude).get(),
- mk_data(gps_longitude).get(),
- runway.bias_area[i].latitude,
- runway.bias_area[i].longitude,
- &az1[i], &az2, &distance);
- }
-
- for (int i = 0; i < 4; i++)
- {
- double angle = az1[i == 3 ? 0 : i + 1] - az1[i];
- if (angle < -180)
- angle += 360;
-
- angles_sum += angle;
- }
-
- return angles_sum > 180;
+ double half_bias_width_m = k * SG_NM_TO_METER + runway.half_width_m;
+ SGVec3d cpos = SGVec3d::fromGeod( SGGeod::fromDegFt(mk_data(gps_longitude).get(),
+ mk_data(gps_latitude).get(),
+ runway.elevation) );
+ SGLineSegmentd bias_line = SGLineSegmentd(runway.bias_points[0], runway.bias_points[1]);
+ return dist(cpos, bias_line) < half_bias_width_m;
}
bool
MK_VIII::TCFHandler::is_tcf ()
{
if (mk_data(radio_altitude).get() > 10)
+ {
+ if (has_runway)
{
- if (has_runway)
- {
- double distance, az1, az2;
+ double distance = SGGeodesy::distanceNm( SGGeod::fromDegFt(mk_data(gps_longitude).get(),
+ mk_data(gps_latitude).get(),
+ runway.elevation),
+ runway.center);
- geo_inverse_wgs_84(0,
- mk_data(gps_latitude).get(),
- mk_data(gps_longitude).get(),
- runway.center.latitude,
- runway.center.longitude,
- &az1, &az2, &distance);
+ // distance to the inner envelope edge
+ double edge_distance = distance - runway.half_length - k;
- distance *= SG_METER_TO_NM;
-
- // distance to the inner envelope edge
- double edge_distance = distance - runway.half_length - k;
-
- if (edge_distance >= 15)
- {
- if (mk_data(radio_altitude).get() < 700)
- return true;
- }
- else if (edge_distance >= 12)
- {
- if (mk_data(radio_altitude).get() < 100 * edge_distance - 800)
- return true;
- }
- else if (edge_distance >= 4)
- {
- if (mk_data(radio_altitude).get() < 400)
- return true;
- }
- else if (edge_distance >= 2.45)
- {
- if (mk_data(radio_altitude).get() < 66.666667 * edge_distance + 133.33333)
- return true;
- }
- else
- {
- if (is_inside_edge_triangle(&runway.edges[0]) || is_inside_edge_triangle(&runway.edges[1]))
- {
- if (edge_distance >= 1)
- {
- if (mk_data(radio_altitude).get() < 66.666667 * edge_distance + 133.33333)
- return true;
- }
- else if (edge_distance >= 0.05)
- {
- if (mk_data(radio_altitude).get() < 200 * edge_distance)
- return true;
- }
- }
- else
- {
- if (! is_inside_bias_area())
- {
- if (mk_data(radio_altitude).get() < 245)
- return true;
- }
- }
- }
- }
- else
- {
- if (mk_data(radio_altitude).get() < 700)
- return true;
- }
+ if (edge_distance > 15)
+ {
+ if (mk_data(radio_altitude).get() < 700)
+ return true;
+ }
+ else if (edge_distance > 12)
+ {
+ if (mk_data(radio_altitude).get() < 100 * edge_distance - 800)
+ return true;
+ }
+ else if (edge_distance > 4)
+ {
+ if (mk_data(radio_altitude).get() < 400)
+ return true;
+ }
+ else if (edge_distance > 2.45)
+ {
+ if (mk_data(radio_altitude).get() < 100 * edge_distance)
+ return true;
+ }
+ else if ( is_inside_edge_triangle(&runway.edge) && (edge_distance > 0.01) )
+ {
+ if (mk_data(radio_altitude).get() < 100 * edge_distance)
+ return true;
+ }
+ else if (! is_inside_bias_area())
+ {
+ if (mk_data(radio_altitude).get() < 245)
+ return true;
+ }
}
-
+ else if (mk_data(radio_altitude).get() < 700)
+ {
+ return true;
+ }
+ }
return false;
}
MK_VIII::TCFHandler::is_rfcf ()
{
if (has_runway)
- {
- double distance, az1, az2;
- geo_inverse_wgs_84(0,
- mk_data(gps_latitude).get(),
- mk_data(gps_longitude).get(),
- runway.center.latitude,
- runway.center.longitude,
- &az1, &az2, &distance);
+ {
+ double distance = SGGeodesy::distanceNm( SGGeod::fromDegFt(mk_data(gps_longitude).get(),
+ mk_data(gps_latitude).get(),
+ runway.elevation),
+ runway.center);
+ distance -= runway.half_length;
+ if (distance < 5.0)
+ {
double krf = k + mk_data(gps_vertical_figure_of_merit).get() / 200;
- distance = distance * SG_METER_TO_NM - runway.half_length - krf;
+ distance -= krf;
+ double altitude_above_field = mk_data(geometric_altitude).get() - runway.elevation;
- if (distance <= 5)
- {
- double altitude_above_field = mk_data(geometric_altitude).get() - runway.elevation;
-
- if (distance >= 1.5)
- {
- if (altitude_above_field < 300)
- return true;
- }
- else if (distance >= 0)
- {
- if (altitude_above_field < 200 * distance)
- return true;
- }
- }
+ if ( (distance > 1.5) && (altitude_above_field < 300.0) )
+ return true;
+ else if ( (distance > 0.0) && (altitude_above_field < 200 * distance) )
+ return true;
}
+ }
return false;
}