Villuton
/
Nmea0183Parser
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Интегрированны структуры из файла Nmea0183_Sentence.h

This commit is contained in:
Степанов Станислав 2025-03-27 15:07:51 +03:00
parent 33f4d048e9
commit 8a3c4aa6ad
2 changed files with 54 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
Inc/Nmea0183_Math.h
View File

@ -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

113
Src/Nmea0183_Math.c
View File

@ -5,14 +5,6 @@
#include "Nmea0183Parser_Private.h"
#include <math.h>
/**
* \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 перонести в другой модуль */