From 8a3c4aa6ad7e41e56b87847614cb90f3808a9313 Mon Sep 17 00:00:00 2001 From: Villuton Date: Thu, 27 Mar 2025 15:07:51 +0300 Subject: [PATCH] =?UTF-8?q?=D0=98=D0=BD=D1=82=D0=B5=D0=B3=D1=80=D0=B8?= =?UTF-8?q?=D1=80=D0=BE=D0=B2=D0=B0=D0=BD=D0=BD=D1=8B=20=D1=81=D1=82=D1=80?= =?UTF-8?q?=D1=83=D0=BA=D1=82=D1=83=D1=80=D1=8B=20=D0=B8=D0=B7=20=D1=84?= =?UTF-8?q?=D0=B0=D0=B9=D0=BB=D0=B0=20Nmea0183=5FSentence.h?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Inc/Nmea0183_Math.h | 40 +++++++--------- Src/Nmea0183_Math.c | 113 +++++++++++++++----------------------------- 2 files changed, 54 insertions(+), 99 deletions(-) diff --git a/Inc/Nmea0183_Math.h b/Inc/Nmea0183_Math.h index 0c8e4e3..37d3ef2 100644 --- a/Inc/Nmea0183_Math.h +++ b/Inc/Nmea0183_Math.h @@ -8,6 +8,8 @@ /* * G math units */ +#include "Nmea0183_Sentence.h" + #define NMEA_PI (3.141592653589793) /**< PI value */ #define NMEA_PI180 (NMEA_PI / 180) /**< PI division by 180 */ #define NMEA_EARTHRADIUS_KM (6378) /**< Earth's mean radius in km */ @@ -43,34 +45,26 @@ double nmeaMetersToDop(double meters); * positions work */ double nmeaCalcDistance( - const double *from_pos_lat, // /**< From position latitude in radians */ - const double *from_pos_lon, // /**< From position longitude in radians */ - const double *to_pos_lat, // /**< To position latitude in radians */ - const double *to_pos_lon // /**< To position longitude in radians */ + tNmeaPositionDouble *from_pos, // /**< From position in radians */ + tNmeaPositionDouble *to_pos // /**< To position in radians */ ); double nmeaCalcDistanceEllipsoid( - const double *from_pos_lat, // /**< From position latitude in radians */ - const double *from_pos_lon, // /**< From position longitude in radians */ - const double *to_pos_lat, // /**< To position latitude in radians */ - const double *to_pos_lon, // /**< To position longitude in radians */ - double *from_azimuth, // /**< (O) azimuth at "from" position in radians */ - double *to_azimuth // /**< (O) azimuth at "to" position in radians */ + tNmeaPositionDouble *from_pos, // /**< From position in radians */ + tNmeaPositionDouble *to_pos, // /**< To position in radians */ + double *from_azimuth, // /**< (O) azimuth at "from" position in radians */ + double *to_azimuth // /**< (O) azimuth at "to" position in radians */ ); int nmeaMoveHorz( - const double *start_pos_lat, // /**< Start position latitude in radians */ - const double *start_pos_lon, // /**< Start position longitude in radians */ - double *end_pos_lat, // /**< Result position latitude in radians */ - double *end_pos_lon, // /**< Result position longitude in radians */ - double azimuth, // /**< Azimuth (degree) [0, 359] */ - double distance // /**< Distance (km) */ + tNmeaPositionDouble *start_pos, // /**< Start position in radians */ + tNmeaPositionDouble *end_pos, // /**< Result position in radians */ + double azimuth, // /**< Azimuth (degree) [0, 359] */ + double distance // /**< Distance (km) */ ); int nmeaMoveHorzEllipsoid( - const double *start_pos_lat, // /**< Start position latitude in radians */ - const double *start_pos_lon, // /**< Start position longitude in radians */ - double *end_pos_lat, // /**< (O) Result position latitude in radians */ - double *end_pos_lon, // /**< (O) Result position longitude in radians */ - double azimuth, // /**< Azimuth in radians */ - double distance, // /**< Distance (km) */ - double *end_azimuth // /**< (O) Azimuth at end position in radians */ + tNmeaPositionDouble *start_pos, // /**< Start position in radians */ + tNmeaPositionDouble *end_pos, // /**< Result position in radians */ + double azimuth, // /**< Azimuth in radians */ + double distance, // /**< Distance (km) */ + double *end_azimuth // /**< (O) Azimuth at end position in radians */ ); #endif //NMEA0183_MATH_H diff --git a/Src/Nmea0183_Math.c b/Src/Nmea0183_Math.c index ee4f557..43462f5 100644 --- a/Src/Nmea0183_Math.c +++ b/Src/Nmea0183_Math.c @@ -5,14 +5,6 @@ #include "Nmea0183Parser_Private.h" #include -/** - * \struct tNmeaMathPos - * \brief Position data in fractional degrees or radians - */ -typedef struct { - double lat; - double lon; -}tNmeaMathPos; /** * \fn nmeaDegreeToRadian @@ -99,23 +91,14 @@ double nmeaMetersToDop(double meters) * @return Distance in meters */ double nmeaCalcDistance( - const double *from_pos_lat, // /**< From position latitude in radians */ - const double *from_pos_lon, // /**< From position longitude in radians */ - const double *to_pos_lat, // /**< To position latitude in radians */ - const double *to_pos_lon // /**< To position longitude in radians */ + tNmeaPositionDouble *from_pos, // /**< From position in radians */ + tNmeaPositionDouble *to_pos // /**< To position in radians */ ) { - tNmeaMathPos from_pos = { - .lat = *from_pos_lat, - .lon = *from_pos_lon - }; - tNmeaMathPos to_pos = { - .lat = *to_pos_lat, - .lon = *to_pos_lon - }; + double dist = ((double)NMEA_EARTHRADIUS_M) * acos( - sin(to_pos.lat) * sin(from_pos.lat) + - cos(to_pos.lat) * cos(from_pos.lat) * cos(to_pos.lon - from_pos.lon) + sin(to_pos->lat) * sin(from_pos->lat) + + cos(to_pos->lat) * cos(from_pos->lat) * cos(to_pos->lon - from_pos->lon) ); return dist; } @@ -135,23 +118,13 @@ double nmeaCalcDistance( * @return Distance in meters */ double nmeaCalcDistanceEllipsoid( - const double *from_pos_lat, // /**< From position latitude in radians */ - const double *from_pos_lon, // /**< From position longitude in radians */ - const double *to_pos_lat, // /**< To position latitude in radians */ - const double *to_pos_lon, // /**< To position longitude in radians */ + tNmeaPositionDouble *from_pos, // /**< From position in radians */ + tNmeaPositionDouble *to_pos, // /**< To position in radians */ double *from_azimuth, // /**< (O) azimuth at "from" position in radians */ double *to_azimuth // /**< (O) azimuth at "to" position in radians */ ) { /* All variables */ - tNmeaMathPos from_pos = { - .lat = *from_pos_lat, - .lon = *from_pos_lon - }; - tNmeaMathPos to_pos = { - .lat = *to_pos_lat, - .lon = *to_pos_lon - }; double f, a, b, sqr_a, sqr_b; double L, phi1, phi2, U1, U2, sin_U1, sin_U2, cos_U1, cos_U2; double sigma, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, sqr_cos_alpha, lambda, sin_lambda, cos_lambda, delta_lambda; @@ -159,14 +132,12 @@ double nmeaCalcDistanceEllipsoid( double sqr_u, A, B, delta_sigma; /* Check input */ - NMEA_ASSERT(from_pos_lat != NULL) - NMEA_ASSERT(from_pos_lon != NULL) + NMEA_ASSERT(from_pos != NULL) - NMEA_ASSERT(to_pos_lat != NULL) - NMEA_ASSERT(to_pos_lon != NULL) + NMEA_ASSERT(to_pos != NULL) - if ((from_pos.lat == to_pos.lat) && (from_pos.lon == to_pos.lon)) + if ((from_pos->lat == to_pos->lat) && (from_pos->lon == to_pos->lon)) { /* Identical points */ if ( from_azimuth != 0 ) *from_azimuth = 0; @@ -183,9 +154,9 @@ double nmeaCalcDistanceEllipsoid( sqr_b = b * b; /* Calculation */ - L = to_pos.lon - from_pos.lon; - phi1 = from_pos.lat; - phi2 = to_pos.lat; + L = to_pos->lon - from_pos->lon; + phi1 = from_pos->lat; + phi2 = to_pos->lat; U1 = atan((1 - f) * tan(phi1)); U2 = atan((1 - f) * tan(phi2)); sin_U1 = sin(U1); @@ -271,31 +242,26 @@ double nmeaCalcDistanceEllipsoid( * @return */ int nmeaMoveHorz( - const double *start_pos_lat, // /**< Start position latitude in radians */ - const double *start_pos_lon, // /**< Start position longitude in radians */ - double *end_pos_lat, // /**< Result position latitude in radians */ - double *end_pos_lon, // /**< Result position longitude in radians */ - double azimuth, // /**< Azimuth (degree) [0, 359] */ - double distance // /**< Distance (km) */ + tNmeaPositionDouble *start_pos, // /**< Start position in radians */ + tNmeaPositionDouble *end_pos, // /**< Result position in radians */ + double azimuth, // /**< Azimuth (degree) [0, 359] */ + double distance // /**< Distance (km) */ ) { - tNmeaMathPos p1 = { - .lat = *start_pos_lat, - .lon = *start_pos_lon - }; + tNmeaPositionDouble *p1 = start_pos; int RetVal = 1; distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */ azimuth = nmeaDegreeToRadian(azimuth); - *end_pos_lat = asin( - sin(p1.lat) * cos(distance) + cos(p1.lat) * sin(distance) * cos(azimuth)); - *end_pos_lon = p1.lon + atan2( - sin(azimuth) * sin(distance) * cos(p1.lat), cos(distance) - sin(p1.lat) * sin(*end_pos_lat)); + end_pos->lat = asin( + sin(p1->lat) * cos(distance) + cos(p1->lat) * sin(distance) * cos(azimuth)); + end_pos->lon = p1->lon + atan2( + sin(azimuth) * sin(distance) * cos(p1->lat), cos(distance) - sin(p1->lat) * sin(end_pos->lat)); - if(isnan(*end_pos_lat) || isnan(*end_pos_lon)) + if(isnan(end_pos->lat) || isnan(end_pos->lon)) { - *end_pos_lat = 0; *end_pos_lon = 0; + end_pos->lat = 0; end_pos->lon = 0; RetVal = 0; } @@ -318,13 +284,11 @@ int nmeaMoveHorz( * @return */ int nmeaMoveHorzEllipsoid( - const double *start_pos_lat, // /**< Start position latitude in radians */ - const double *start_pos_lon, // /**< Start position longitude in radians */ - double *end_pos_lat, // /**< (O) Result position latitude in radians */ - double *end_pos_lon, // /**< (O) Result position longitude in radians */ - double azimuth, // /**< Azimuth in radians */ - double distance, // /**< Distance (km) */ - double *end_azimuth // /**< (O) Azimuth at end position in radians */ + tNmeaPositionDouble *start_pos, // /**< Start position in radians */ + tNmeaPositionDouble *end_pos, // /**< Result position in radians */ + double azimuth, // /**< Azimuth in radians */ + double distance, // /**< Distance (km) */ + double *end_azimuth // /**< (O) Azimuth at end position in radians */ ) { /* Variables */ @@ -336,17 +300,14 @@ int nmeaMoveHorzEllipsoid( double tmp1, phi2, lambda, C, L; /* Check input */ - NMEA_ASSERT(start_pos_lat != NULL) - NMEA_ASSERT(start_pos_lon != NULL) - NMEA_ASSERT(end_pos_lat != NULL) - NMEA_ASSERT(end_pos_lon != NULL) + NMEA_ASSERT(start_pos != NULL) + NMEA_ASSERT(end_pos != NULL) if (fabs(distance) < 1e-12) { /* No move */ - *end_pos_lat = *start_pos_lat; - *end_pos_lon = *start_pos_lon; + end_pos = start_pos; if ( end_azimuth != 0 ) *end_azimuth = azimuth; - return ! (isnan(*end_pos_lat) || isnan(*end_pos_lon)); + return ! (isnan(end_pos->lat) || isnan(end_pos->lon)); } /* No move */ /* Earth geometry */ @@ -357,7 +318,7 @@ int nmeaMoveHorzEllipsoid( sqr_b = b * b; /* Calculation */ - phi1 = *start_pos_lat; + phi1 = start_pos->lat; tan_U1 = (1 - f) * tan(phi1); cos_U1 = 1 / sqrt(1 + tan_U1 * tan_U1); sin_U1 = tan_U1 * cos_U1; @@ -416,15 +377,15 @@ int nmeaMoveHorzEllipsoid( ); /* Result */ - *end_pos_lon = *start_pos_lon + L; - *end_pos_lat = phi2; + end_pos->lon = start_pos->lon + L; + end_pos->lat = phi2; if ( end_azimuth != 0 ) { *end_azimuth = atan2( sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1 ); } - return ! (isnan(*end_pos_lat) || isnan(*end_pos_lon)); + return ! (isnan(end_pos->lat) || isnan(end_pos->lon)); } /** todo перонести в другой модуль */