diff --git a/Inc/Nmea0183_Math.h b/Inc/Nmea0183_Math.h new file mode 100644 index 0000000..0c8e4e3 --- /dev/null +++ b/Inc/Nmea0183_Math.h @@ -0,0 +1,76 @@ +// +// Created by villuton on 24.03.25. +// + +#ifndef NMEA0183_MATH_H +#define NMEA0183_MATH_H + +/* + * G math units + */ +#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 */ +#define NMEA_EARTHRADIUS_M (NMEA_EARTHRADIUS_KM * 1000) /**< Earth's mean radius in m */ +#define NMEA_EARTH_SEMIMAJORAXIS_M (6378137.0) /**< Earth's semi-major axis in m according WGS84 */ +#define NMEA_EARTH_SEMIMAJORAXIS_KM (NMEA_EARTHMAJORAXIS_KM / 1000) /**< Earth's semi-major axis in km according WGS 84 */ +#define NMEA_EARTH_FLATTENING (1 / 298.257223563) /**< Earth's flattening according WGS 84 */ +#define NMEA_DOP_FACTOR (5) /**< Factor for translating DOP to meters */ + +/* + * degree VS radian + */ +double nmeaDegreeToRadian(double val); +double nmeaRadianToDegree(double val); + +/* + * NDEG (NMEA degree) + */ +double nmeaNdegToDegree(double val); +double nmeaDegreeToNdeg(double val); + +double nmeaNdegToRadian(double val); +double nmeaRadianToNdeg(double val); + +/* + * DOP + */ +double nmeaCalcPdop(double hdop, double vdop); +double nmeaDopToMeters(double dop); +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 */ +); +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 */ +); +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) */ +); +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 */ +); +#endif //NMEA0183_MATH_H diff --git a/Inc/Nmea0183_Sentence.h b/Inc/Nmea0183_Sentence.h new file mode 100644 index 0000000..d98e2fb --- /dev/null +++ b/Inc/Nmea0183_Sentence.h @@ -0,0 +1,34 @@ +// +// Created by villuton on 26.03.25. +// + +#ifndef NMEA0183_SENTENCE_H +#define NMEA0183_SENTENCE_H + +/** + * Sentence location data in fractional NDEG and Cardinal directions + */ +typedef struct { + double lat; /**< Latitude in NDEG - [degree][min].[sec/60] */ + char ns; /**< [N]orth or [S]outh */ + double lon; /**< Longitude in NDEG - [degree][min].[sec/60] */ + char ew; /**< [E]ast or [W]est */ +}tNmeaLocation; + +/** + * Position data in fractional degrees or radians + */ +typedef struct { + double lat; /**< Latitude */ + double lon; /**< Longitude */ +}tNmeaPositionDouble; + +/** + * Position data in fractional degrees or radians + */ +typedef struct { + float lat; /**< Latitude */ + float lon; /**< Longitude */ +}tNmeaPositionFloat; + +#endif //NMEA0183_SENTENCE_H diff --git a/Src/Nmea0183_Math.c b/Src/Nmea0183_Math.c new file mode 100644 index 0000000..ee4f557 --- /dev/null +++ b/Src/Nmea0183_Math.c @@ -0,0 +1,448 @@ +// +// Created by villuton on 24.03.25. +// +#include "Nmea0183_Math.h" +#include "Nmea0183Parser_Private.h" +#include + +/** + * \struct tNmeaMathPos + * \brief Position data in fractional degrees or radians + */ +typedef struct { + double lat; + double lon; +}tNmeaMathPos; + +/** + * \fn nmeaDegreeToRadian + * \brief Convert degree to radian + */ +double nmeaDegreeToRadian(double val) +{ return (val * NMEA_PI180); } + +/** + * \fn nmeaRadianToDegree + * \brief Convert radian to degree + */ +double nmeaRadianToDegree(double val) +{ return (val / NMEA_PI180); } + +/** + * \fn nmeaNdegToDegree + * \brief Convert NDEG (NMEA degree) to fractional degree + */ +double nmeaNdegToDegree(double val) +{ + double deg = ((int)(val / 100)); + val = deg + (val - deg * 100) / 60; + return val; +} + +/** + * \fn nmeaDegreeToNdeg + * \brief Convert fractional degree to NDEG (NMEA degree) + */ +double nmeaDegreeToNdeg(double val) +{ + double int_part; + double fra_part; + fra_part = modf(val, &int_part); + val = int_part * 100 + fra_part * 60; + return val; +} + +/** + * \fn nmeaNdegToRadian + * \brief Convert NDEG (NMEA degree) to radian + */ +double nmeaNdegToRadian(double val) +{ return nmeaDegreeToRadian(nmeaNdegToDegree(val)); } + +/** + * \fn nmeaRadianToNdeg + * \brief Convert radian to NDEG (NMEA degree) + */ +double nmeaRadianToNdeg(double val) +{ return nmeaDegreeToNdeg(nmeaRadianToDegree(val)); } + +/** + * \fn nmeaCalcPdop + * \brief Calculate PDOP (Position Dilution Of Precision) factor + */ +double nmeaCalcPdop(double hdop, double vdop) +{ + return sqrt(pow(hdop, 2) + pow(vdop, 2)); +} + +/** + * \fn nmeaDopToMeters + * \brief Calculate DOP to meters + */ +double nmeaDopToMeters(double dop) +{ return (dop * NMEA_DOP_FACTOR); } + +/** + * \fn nmeaMetersToDop + * \brief Calculate meters to DOP + */ +double nmeaMetersToDop(double meters) +{ return (meters / NMEA_DOP_FACTOR); } + +/** + * \fn nmeaCalcDistance + * \brief Calculate distance between two points + * @param from_pos_lat < From position latitude in radians + * @param from_pos_lon < From position longitude in radians + * @param to_pos_lat < To position latitude in radians + * @param to_pos_lon < To position longitude in radians + * @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 */ +) +{ + 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) + ); + return dist; +} + +/** + * \fn nmeaCalcDistanceEllipsoid + * \brief Calculate distance between two points + * This function uses an algorithm for an oblate spheroid earth model. + * The algorithm is described here: + * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf + * @param from_pos_lat < From position latitude in radians + * @param from_pos_lon < From position longitude in radians + * @param to_pos_lat < To position latitude in radians + * @param to_pos_lon < To position longitude in radians + * @param from_azimuth < (O) azimuth at "from" position in radians + * @param to_azimuth < (O) azimuth at "to" position in radians + * @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 */ + 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; + int remaining_steps; + double sqr_u, A, B, delta_sigma; + + /* Check input */ + NMEA_ASSERT(from_pos_lat != NULL) + NMEA_ASSERT(from_pos_lon != NULL) + + NMEA_ASSERT(to_pos_lat != NULL) + NMEA_ASSERT(to_pos_lon != NULL) + + + if ((from_pos.lat == to_pos.lat) && (from_pos.lon == to_pos.lon)) + { /* Identical points */ + if ( from_azimuth != 0 ) + *from_azimuth = 0; + if ( to_azimuth != 0 ) + *to_azimuth = 0; + return 0; + } /* Identical points */ + + /* Earth geometry */ + f = NMEA_EARTH_FLATTENING; + a = NMEA_EARTH_SEMIMAJORAXIS_M; + b = (1 - f) * a; + sqr_a = a * a; + sqr_b = b * b; + + /* Calculation */ + 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); + sin_U2 = sin(U2); + cos_U1 = cos(U1); + cos_U2 = cos(U2); + + /* Initialize iteration */ + sigma = 0; + sin_sigma = sin(sigma); + cos_sigma = cos(sigma); + cos_2_sigmam = 0; + sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; + sqr_cos_alpha = 0; + lambda = L; + sin_lambda = sin(lambda); + cos_lambda = cos(lambda); + delta_lambda = lambda; + remaining_steps = 20; + + while ((delta_lambda > 1e-12) && (remaining_steps > 0)) + { /* Iterate */ + /* Variables */ + double tmp1, tmp2, sin_alpha, cos_alpha, C, lambda_prev; + + /* Calculation */ + tmp1 = cos_U2 * sin_lambda; + tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda; + sin_sigma = sqrt(tmp1 * tmp1 + tmp2 * tmp2); + cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda; + sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma; + cos_alpha = cos(asin(sin_alpha)); + sqr_cos_alpha = cos_alpha * cos_alpha; + cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha; + sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; + C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha)); + lambda_prev = lambda; + sigma = asin(sin_sigma); + lambda = L + + (1 - C) * f * sin_alpha + * (sigma + C * sin_sigma * (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam))); + delta_lambda = lambda_prev - lambda; + if ( delta_lambda < 0 ) delta_lambda = -delta_lambda; + sin_lambda = sin(lambda); + cos_lambda = cos(lambda); + remaining_steps--; + } /* Iterate */ + + /* More calculation */ + sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b; + A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u))); + B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u))); + delta_sigma = B * sin_sigma * ( + cos_2_sigmam + B / 4 * ( + cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) - + B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam) + )); + + /* Calculate result */ + if ( from_azimuth != 0 ) + { + double tan_alpha_1 = cos_U2 * sin_lambda / (cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda); + *from_azimuth = atan(tan_alpha_1); + } + if ( to_azimuth != 0 ) + { + double tan_alpha_2 = cos_U1 * sin_lambda / (-sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda); + *to_azimuth = atan(tan_alpha_2); + } + + return b * A * (sigma - delta_sigma); +} + +/** + * \fn nmeaMoveHorz + * \brief Horizontal move of point position + * @param start_pos_lat < Start position latitude in radians + * @param start_pos_lon < Start position longitude in radians + * @param end_pos_lat < Result position latitude in radians + * @param end_pos_lon < Result position longitude in radians + * @param azimuth < Azimuth (degree) [0, 359] + * @param distance < Distance (km) + * @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) */ +) +{ + tNmeaMathPos p1 = { + .lat = *start_pos_lat, + .lon = *start_pos_lon + }; + 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)); + + if(isnan(*end_pos_lat) || isnan(*end_pos_lon)) + { + *end_pos_lat = 0; *end_pos_lon = 0; + RetVal = 0; + } + + return RetVal; +} + +/** + * + * \brief Horizontal move of point position + * This function uses an algorithm for an oblate spheroid earth model. + * The algorithm is described here: + * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf + * @param start_pos_lat < Start position latitude in radians + * @param start_pos_lon < Start position longitude in radians + * @param end_pos_lat < (O) Result position latitude in radians + * @param end_pos_lon < (O) Result position longitude in radians + * @param azimuth < Azimuth in radians + * @param distance < Distance (km) + * @param end_azimuth < (O) Azimuth at end position in radians + * @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 */ +) +{ + /* Variables */ + double f, a, b, sqr_a, sqr_b; + double phi1, tan_U1, sin_U1, cos_U1, s, alpha1, sin_alpha1, cos_alpha1; + double sigma1, sin_alpha, sqr_cos_alpha, sqr_u, A, B; + double sigma_initial, sigma, sigma_prev, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, delta_sigma; + int remaining_steps; + 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) + + if (fabs(distance) < 1e-12) + { /* No move */ + *end_pos_lat = *start_pos_lat; + *end_pos_lon = *start_pos_lon; + if ( end_azimuth != 0 ) *end_azimuth = azimuth; + return ! (isnan(*end_pos_lat) || isnan(*end_pos_lon)); + } /* No move */ + + /* Earth geometry */ + f = NMEA_EARTH_FLATTENING; + a = NMEA_EARTH_SEMIMAJORAXIS_M; + b = (1 - f) * a; + sqr_a = a * a; + sqr_b = b * b; + + /* Calculation */ + 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; + s = distance; + alpha1 = azimuth; + sin_alpha1 = sin(alpha1); + cos_alpha1 = cos(alpha1); + sigma1 = atan2(tan_U1, cos_alpha1); + sin_alpha = cos_U1 * sin_alpha1; + sqr_cos_alpha = 1 - sin_alpha * sin_alpha; + sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b; + A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u))); + B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u))); + + /* Initialize iteration */ + sigma_initial = s / (b * A); + sigma = sigma_initial; + sin_sigma = sin(sigma); + cos_sigma = cos(sigma); + cos_2_sigmam = cos(2 * sigma1 + sigma); + sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; + sigma_prev = 2 * NMEA_PI; + remaining_steps = 20; + + while ((fabs(sigma - sigma_prev) > 1e-12) && (remaining_steps > 0)) + { /* Iterate */ + cos_2_sigmam = cos(2 * sigma1 + sigma); + sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; + sin_sigma = sin(sigma); + cos_sigma = cos(sigma); + delta_sigma = B * sin_sigma * ( + cos_2_sigmam + B / 4 * ( + cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) - + B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam) + )); + sigma_prev = sigma; + sigma = sigma_initial + delta_sigma; + remaining_steps --; + } /* Iterate */ + + /* Calculate result */ + tmp1 = (sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1); + phi2 = atan2( + sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_alpha1, + (1 - f) * sqrt(sin_alpha * sin_alpha + tmp1 * tmp1) + ); + lambda = atan2( + sin_sigma * sin_alpha1, + cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1 + ); + C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha)); + L = lambda - + (1 - C) * f * sin_alpha * ( + sigma + C * sin_sigma * + (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam)) + ); + + /* Result */ + *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)); +} + +/** todo перонести в другой модуль */ + +///** +// * \brief Convert position from INFO to radians position +// */ +//void nmeaInfoToPos(const nmeaINFO *info, const nmeaPOS *pos) +//{ +// pos->lat = nmeaNdegToRadian(info->lat); +// pos->lon = nmeaNdegToRadian(info->lon); +//} +// +///** +// * \brief Convert radians position to INFOs position +// */ +//void nmeaPosToInfo(const nmeaPOS *pos, const nmeaINFO *info) +//{ +// info->lat = nmeaRadianToNdeg(pos->lat); +// info->lon = nmeaRadianToNdeg(pos->lon); +//} \ No newline at end of file