aboutsummaryrefslogtreecommitdiff
path: root/apps/gps/nmealib/gmath.c
diff options
context:
space:
mode:
Diffstat (limited to 'apps/gps/nmealib/gmath.c')
-rw-r--r--apps/gps/nmealib/gmath.c376
1 files changed, 0 insertions, 376 deletions
diff --git a/apps/gps/nmealib/gmath.c b/apps/gps/nmealib/gmath.c
deleted file mode 100644
index 327b982ef..000000000
--- a/apps/gps/nmealib/gmath.c
+++ /dev/null
@@ -1,376 +0,0 @@
-/*
- *
- * NMEA library
- * URL: http://nmea.sourceforge.net
- * Author: Tim (xtimor@gmail.com)
- * Licence: http://www.gnu.org/licenses/lgpl.html
- * $Id: gmath.c 17 2008-03-11 11:56:11Z xtimor $
- *
- */
-
-/*! \file gmath.h */
-#include "nmea/gmath.h"
-
-#include <math.h>
-#include <float.h>
-
-/**
- * \fn nmea_degree2radian
- * \brief Convert degree to radian
- */
-float nmea_degree2radian(float val)
-{ return (val * NMEA_PI180); }
-
-/**
- * \fn nmea_radian2degree
- * \brief Convert radian to degree
- */
-float nmea_radian2degree(float val)
-{ return (val / NMEA_PI180); }
-
-/**
- * \brief Convert NDEG (NMEA degree) to fractional degree
- */
-float nmea_ndeg2degree(float val)
-{
- float deg = ((int)(val / 100));
- val = deg + (val - deg * 100) / 60;
- return val;
-}
-
-/**
- * \brief Convert fractional degree to NDEG (NMEA degree)
- */
-float nmea_degree2ndeg(float val)
-{
- float int_part;
- float fra_part;
- fra_part = modf(val, &int_part);
- val = int_part * 100 + fra_part * 60;
- return val;
-}
-
-/**
- * \fn nmea_ndeg2radian
- * \brief Convert NDEG (NMEA degree) to radian
- */
-float nmea_ndeg2radian(float val)
-{ return nmea_degree2radian(nmea_ndeg2degree(val)); }
-
-/**
- * \fn nmea_radian2ndeg
- * \brief Convert radian to NDEG (NMEA degree)
- */
-float nmea_radian2ndeg(float val)
-{ return nmea_degree2ndeg(nmea_radian2degree(val)); }
-
-/**
- * \brief Calculate PDOP (Position Dilution Of Precision) factor
- */
-float nmea_calc_pdop(float hdop, float vdop)
-{
- return sqrt(pow(hdop, 2) + pow(vdop, 2));
-}
-
-float nmea_dop2meters(float dop)
-{ return (dop * NMEA_DOP_FACTOR); }
-
-float nmea_meters2dop(float meters)
-{ return (meters / NMEA_DOP_FACTOR); }
-
-/**
- * \brief Calculate distance between two points
- * \return Distance in meters
- */
-float nmea_distance(
- const nmeaPOS *from_pos, /**< From position in radians */
- const nmeaPOS *to_pos /**< To position in radians */
- )
-{
- float dist = ((float)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;
-}
-
-/**
- * \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
- * \return Distance in meters
- */
-float nmea_distance_ellipsoid(
- const nmeaPOS *from_pos, /**< From position in radians */
- const nmeaPOS *to_pos, /**< To position in radians */
- float *from_azimuth, /**< (O) azimuth at "from" position in radians */
- float *to_azimuth /**< (O) azimuth at "to" position in radians */
- )
-{
- /* All variables */
- float f, a, b, sqr_a, sqr_b;
- float L, phi1, phi2, U1, U2, sin_U1, sin_U2, cos_U1, cos_U2;
- float 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;
- float sqr_u, A, B, delta_sigma;
-
- /* Check input */
- //NMEA_ASSERT(from_pos != 0);
- //NMEA_ASSERT(to_pos != 0);
-
- 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 */
- float tmp1, tmp2, tan_sigma, 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;
- tan_sigma = sin_sigma / cos_sigma;
- 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 )
- {
- float 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 )
- {
- float 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);
-}
-
-/**
- * \brief Horizontal move of point position
- */
-int nmea_move_horz(
- const nmeaPOS *start_pos, /**< Start position in radians */
- nmeaPOS *end_pos, /**< Result position in radians */
- float azimuth, /**< Azimuth (degree) [0, 359] */
- float distance /**< Distance (km) */
- )
-{
- nmeaPOS p1 = *start_pos;
- int RetVal = 1;
-
- distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */
- azimuth = nmea_degree2radian(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(NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(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
- */
-int nmea_move_horz_ellipsoid(
- const nmeaPOS *start_pos, /**< Start position in radians */
- nmeaPOS *end_pos, /**< (O) Result position in radians */
- float azimuth, /**< Azimuth in radians */
- float distance, /**< Distance (km) */
- float *end_azimuth /**< (O) Azimuth at end position in radians */
- )
-{
- /* Variables */
- float f, a, b, sqr_a, sqr_b;
- float phi1, tan_U1, sin_U1, cos_U1, s, alpha1, sin_alpha1, cos_alpha1;
- float tan_sigma1, sigma1, sin_alpha, cos_alpha, sqr_cos_alpha, sqr_u, A, B;
- float sigma_initial, sigma, sigma_prev, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, delta_sigma;
- int remaining_steps;
- float tmp1, phi2, lambda, C, L;
-
- /* Check input */
- //NMEA_ASSERT(start_pos != 0);
- //NMEA_ASSERT(end_pos != 0);
-
- if (fabs(distance) < 1e-12)
- { /* No move */
- *end_pos = *start_pos;
- if ( end_azimuth != 0 ) *end_azimuth = azimuth;
- return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(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);
- tan_sigma1 = tan_U1 / cos_alpha1;
- sigma1 = atan2(tan_U1, cos_alpha1);
- sin_alpha = cos_U1 * sin_alpha1;
- sqr_cos_alpha = 1 - sin_alpha * sin_alpha;
- cos_alpha = sqrt(sqr_cos_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;
- delta_sigma = 0;
- 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 ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon));
-}
-
-/**
- * \brief Convert position from INFO to radians position
- */
-void nmea_info2pos(const nmeaINFO *info, nmeaPOS *pos)
-{
- pos->lat = nmea_ndeg2radian(info->lat);
- pos->lon = nmea_ndeg2radian(info->lon);
-}
-
-/**
- * \brief Convert radians position to INFOs position
- */
-void nmea_pos2info(const nmeaPOS *pos, nmeaINFO *info)
-{
- info->lat = nmea_radian2ndeg(pos->lat);
- info->lon = nmea_radian2ndeg(pos->lon);
-}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-12 11:37:19 +0000