From db5a39e3058640c6388819c320277d2a70f70adb Mon Sep 17 00:00:00 2001 From: Christian Schmitt Date: Wed, 27 Mar 2013 20:02:07 +0100 Subject: [PATCH] Issue 1090: Adapt the mk-viii instrument code to the new internal runway scheme. Clean up some unneeded functions as well and make use of standard simgear functions where possible. This whole thing could need a rewrite... --- src/Instrumentation/mk_viii.cxx | 243 +++++--------------------------- src/Instrumentation/mk_viii.hxx | 27 +--- 2 files changed, 39 insertions(+), 231 deletions(-) diff --git a/src/Instrumentation/mk_viii.cxx b/src/Instrumentation/mk_viii.cxx index eb40aee4d..2c2849981 100644 --- a/src/Instrumentation/mk_viii.cxx +++ b/src/Instrumentation/mk_viii.cxx @@ -66,8 +66,13 @@ #include #include #include -#include +#include +#include +#include +#include #include +#include +#include #include #include #include @@ -136,24 +141,6 @@ modify_amplitude (double amplitude, double dB) 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) { @@ -167,12 +154,6 @@ get_heading_difference (double h1, double h2) return fabs(diff); } -static double -get_reciprocal_heading (double h) -{ - return heading_add(h, 180); -} - /////////////////////////////////////////////////////////////////////////////// // PropertiesHandler ////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////// @@ -4068,9 +4049,7 @@ MK_VIII::Mode6Handler::test_runway (const FGRunway *_runway) 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 @@ -4270,60 +4249,6 @@ MK_VIII::Mode6Handler::update () // 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; rnumRunways(); ++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); @@ -4343,109 +4268,43 @@ MK_VIII::TCFHandler::update_runway () // 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<<<<<<<headingDeg(); - runway.edges[1].heading = get_reciprocal_heading(_runway->headingDeg()); - - double az; + runway.edges[1].heading = _runway->reciprocalRunway()->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); + runway.edges[0].position = _runway->begin(); // 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.edges[1].position = _runway->end(); -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 @@ -4457,11 +4316,10 @@ MK_VIII::TCFHandler::get_bias_area_edges (Position *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 @@ -4469,30 +4327,10 @@ MK_VIII::TCFHandler::is_inside_edge_triangle (RunwayEdge *edge) 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::fromDeg(mk_data(gps_longitude).get(), mk_data(gps_latitude).get()) ); + SGLineSegmentd bias_line = SGLineSegmentd(runway.bias_points[0], runway.bias_points[1]); + return dist(cpos, bias_line) < half_bias_width_m; } bool @@ -4502,17 +4340,9 @@ MK_VIII::TCFHandler::is_tcf () { 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); - - distance *= SG_METER_TO_NM; - + double distance = SGGeodesy::distanceNm( SGGeod::fromDeg(mk_data(gps_longitude).get(), + mk_data(gps_latitude).get()), + runway.center); // distance to the inner envelope edge double edge_distance = distance - runway.half_length - k; @@ -4576,16 +4406,13 @@ 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::fromDeg(mk_data(gps_longitude).get(), + mk_data(gps_latitude).get()), + runway.center); double krf = k + mk_data(gps_vertical_figure_of_merit).get() / 200; - distance = distance * SG_METER_TO_NM - runway.half_length - krf; + distance = distance - runway.half_length - krf; if (distance <= 5) { diff --git a/src/Instrumentation/mk_viii.hxx b/src/Instrumentation/mk_viii.hxx index 81a33580b..b3e9b6baa 100644 --- a/src/Instrumentation/mk_viii.hxx +++ b/src/Instrumentation/mk_viii.hxx @@ -1270,13 +1270,7 @@ private: { typedef struct { - double latitude; // latitude in degrees - double longitude; // longitude in degrees - } Position; - - typedef struct - { - Position position; // position of threshold + SGGeod position; // position of threshold double heading; // runway heading } RunwayEdge; @@ -1289,33 +1283,20 @@ private: struct { - Position center; // center point + SGGeod center; // center point double elevation; // elevation in feet double half_length; // runway half length, in nautical miles + double half_width_m; // runway half width, in meters RunwayEdge edges[2]; // runway threshold and end - Position bias_area[4]; // vertices of the bias area + SGVec3d bias_points[2]; // vertices of the bias area } runway; double bias; double *reference; double initial_value; - double get_azimuth_difference (double from_lat, - double from_lon, - double to_lat, - double to_lon, - double to_heading); - double get_azimuth_difference (const FGRunway *_runway); - - FGRunway* select_runway (const FGAirport *airport); void update_runway (); - void get_bias_area_edges (Position *edge, - double reciprocal, - double half_width_m, - Position *bias_edge1, - Position *bias_edge2); - bool is_inside_edge_triangle (RunwayEdge *edge); bool is_inside_bias_area (); -- 2.39.5