utils.cpp
590 lines
| 15.6 KiB
| text/x-c
|
CppLexer
|
r96 | /* | ||
* utils.cpp | ||||
* | ||||
* Created on: Oct 21, 2014 | ||||
* Author: Alan Aguilar Sologuren | ||||
*/ | ||||
#include <stdlib.h> | ||||
#include <stdarg.h> | ||||
#include "utils.h" | ||||
/** | ||||
* \fn code_degree2radian | ||||
* \brief Convert degree to radian | ||||
*/ | ||||
double code_degree2radian(double val) { | ||||
return (val * code_PI180); | ||||
} | ||||
/** | ||||
* \fn code_radian2degree | ||||
* \brief Convert radian to degree | ||||
*/ | ||||
double code_radian2degree(double val) { | ||||
return (val / code_PI180); | ||||
} | ||||
/** | ||||
* \brief Convert NDEG (NMEA degree) to fractional degree | ||||
*/ | ||||
double code_ndeg2degree(double val) { | ||||
double deg = ((int)(val / 100)); | ||||
val = deg + (val - deg * 100) / 60; | ||||
return val; | ||||
} | ||||
/** | ||||
* \brief Convert fractional degree to NDEG (NMEA degree) | ||||
*/ | ||||
double code_degree2ndeg(double val) { | ||||
double int_part; | ||||
double fra_part; | ||||
fra_part = modf(val, &int_part); | ||||
val = int_part * 100 + fra_part * 60; | ||||
return val; | ||||
} | ||||
/** | ||||
* \fn code_ndeg2radian | ||||
* \brief Convert NDEG (NMEA degree) to radian | ||||
*/ | ||||
double code_ndeg2radian(double val) { | ||||
return code_degree2radian(code_ndeg2degree(val)); | ||||
} | ||||
/** | ||||
* \fn code_radian2ndeg | ||||
* \brief Convert radian to NDEG (NMEA degree) | ||||
*/ | ||||
double code_radian2ndeg(double val) { | ||||
return code_degree2ndeg(code_radian2degree(val)); | ||||
} | ||||
/** | ||||
* \brief Calculate PDOP (Position Dilution Of Precision) factor | ||||
*/ | ||||
double code_calc_pdop(double hdop, double vdop) { | ||||
return sqrt(pow(hdop, 2) + pow(vdop, 2)); | ||||
} | ||||
double code_dop2meters(double dop) { | ||||
return (dop * code_DOP_FACTOR); | ||||
} | ||||
double code_meters2dop(double meters) { | ||||
return (meters / code_DOP_FACTOR); | ||||
} | ||||
/** | ||||
* \brief Calculate distance between two points | ||||
* \return Distance in meters | ||||
*/ | ||||
double code_distance( | ||||
const nmeaPOS *from_pos, /**< From position in radians */ | ||||
const nmeaPOS *to_pos /**< To position in radians */ | ||||
) | ||||
{ | ||||
double dist = ((double)code_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 | ||||
*/ | ||||
double code_distance_ellipsoid( | ||||
const nmeaPOS *from_pos, /**< From position in radians */ | ||||
const nmeaPOS *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 */ | ||||
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 != 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 = code_EARTH_FLATTENING; | ||||
a = code_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, 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 ) | ||||
{ | ||||
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); | ||||
} | ||||
/** | ||||
* \brief Horizontal move of point position | ||||
*/ | ||||
int code_move_horz( | ||||
const nmeaPOS *start_pos, /**< Start position in radians */ | ||||
nmeaPOS *end_pos, /**< Result position in radians */ | ||||
double azimuth, /**< Azimuth (degree) [0, 359] */ | ||||
double distance /**< Distance (km) */ | ||||
) | ||||
{ | ||||
nmeaPOS p1 = *start_pos; | ||||
int RetVal = 1; | ||||
distance /= code_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */ | ||||
azimuth = code_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 code_move_horz_ellipsoid( | ||||
const nmeaPOS *start_pos, /**< Start position in radians */ | ||||
nmeaPOS *end_pos, /**< (O) Result position 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 tan_sigma1, sigma1, sin_alpha, cos_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 != 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 = code_EARTH_FLATTENING; | ||||
a = code_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 * code_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 Calculate control sum of binary buffer | ||||
*/ | ||||
int code_calc_crc(const char *buff, int buff_sz) | ||||
{ | ||||
int chsum = 0, | ||||
it; | ||||
for(it = 0; it < buff_sz; ++it) | ||||
chsum ^= (int)buff[it]; | ||||
return chsum; | ||||
} | ||||
/** | ||||
* \brief Convert string to number | ||||
*/ | ||||
int code_atoi(const char *str, int str_sz, int radix) | ||||
{ | ||||
char *tmp_ptr; | ||||
char buff[NMEA_CONVSTR_BUF]; | ||||
int res = 0; | ||||
if(str_sz < NMEA_CONVSTR_BUF) | ||||
{ | ||||
memcpy(&buff[0], str, str_sz); | ||||
buff[str_sz] = '\0'; | ||||
res = strtol(&buff[0], &tmp_ptr, radix); | ||||
} | ||||
return res; | ||||
} | ||||
/** | ||||
* \brief Convert string to fraction number | ||||
*/ | ||||
double code_atof(const char *str, int str_sz) | ||||
{ | ||||
char *tmp_ptr; | ||||
char buff[NMEA_CONVSTR_BUF]; | ||||
double res = 0; | ||||
if(str_sz < NMEA_CONVSTR_BUF) | ||||
{ | ||||
memcpy(&buff[0], str, str_sz); | ||||
buff[str_sz] = '\0'; | ||||
res = strtod(&buff[0], &tmp_ptr); | ||||
} | ||||
return res; | ||||
} | ||||
/** | ||||
* \brief Formating string (like standart printf) with CRC tail (*CRC) | ||||
*/ | ||||
int code_printf(char *buff, int buff_sz, const char *format, ...) | ||||
{ | ||||
int retval, add = 0; | ||||
va_list arg_ptr; | ||||
if(buff_sz <= 0) | ||||
return 0; | ||||
va_start(arg_ptr, format); | ||||
retval = NMEA_POSIX(vsnprintf)(buff, buff_sz, format, arg_ptr); | ||||
if(retval > 0) | ||||
{ | ||||
add = NMEA_POSIX(snprintf)( | ||||
buff + retval, buff_sz - retval, "*%02x\r\n", | ||||
code_calc_crc(buff + 1, retval - 1)); | ||||
} | ||||
retval += add; | ||||
if(retval < 0 || retval > buff_sz) | ||||
{ | ||||
memset(buff, ' ', buff_sz); | ||||
retval = buff_sz; | ||||
} | ||||
va_end(arg_ptr); | ||||
return retval; | ||||
} | ||||
/** | ||||
* \brief Analyse string (specificate for NMEA sentences) | ||||
*/ | ||||
int code_scanf(const char *buff, int buff_sz, const char *format, ...) | ||||
{ | ||||
const char *beg_tok; | ||||
const char *end_buf = buff + buff_sz; | ||||
va_list arg_ptr; | ||||
int tok_type = NMEA_TOKS_COMPARE; | ||||
int width = 0; | ||||
const char *beg_fmt = 0; | ||||
int snum = 0, unum = 0; | ||||
int tok_count = 0; | ||||
void *parg_target; | ||||
va_start(arg_ptr, format); | ||||
for(; *format && buff < end_buf; ++format) | ||||
{ | ||||
switch(tok_type) | ||||
{ | ||||
case NMEA_TOKS_COMPARE: | ||||
if('%' == *format) | ||||
tok_type = NMEA_TOKS_PERCENT; | ||||
else if(*buff++ != *format) | ||||
goto fail; | ||||
break; | ||||
case NMEA_TOKS_PERCENT: | ||||
width = 0; | ||||
beg_fmt = format; | ||||
tok_type = NMEA_TOKS_WIDTH; | ||||
case NMEA_TOKS_WIDTH: | ||||
if(isdigit(*format)) | ||||
break; | ||||
{ | ||||
tok_type = NMEA_TOKS_TYPE; | ||||
if(format > beg_fmt) | ||||
width = code_atoi(beg_fmt, (int)(format - beg_fmt), 10); | ||||
} | ||||
case NMEA_TOKS_TYPE: | ||||
beg_tok = buff; | ||||
if(!width && ('c' == *format || 'C' == *format) && *buff != format[1]) | ||||
width = 1; | ||||
if(width) | ||||
{ | ||||
if(buff + width <= end_buf) | ||||
buff += width; | ||||
else | ||||
goto fail; | ||||
} | ||||
else | ||||
{ | ||||
if(!format[1] || (0 == (buff = (char *)memchr(buff, format[1], end_buf - buff)))) | ||||
buff = end_buf; | ||||
} | ||||
if(buff > end_buf) | ||||
goto fail; | ||||
tok_type = NMEA_TOKS_COMPARE; | ||||
tok_count++; | ||||
parg_target = 0; width = (int)(buff - beg_tok); | ||||
switch(*format) | ||||
{ | ||||
case 'c': | ||||
case 'C': | ||||
parg_target = (void *)va_arg(arg_ptr, char *); | ||||
if(width && 0 != (parg_target)) | ||||
*((char *)parg_target) = *beg_tok; | ||||
break; | ||||
case 's': | ||||
case 'S': | ||||
parg_target = (void *)va_arg(arg_ptr, char *); | ||||
if(width && 0 != (parg_target)) | ||||
{ | ||||
memcpy(parg_target, beg_tok, width); | ||||
((char *)parg_target)[width] = '\0'; | ||||
} | ||||
break; | ||||
case 'f': | ||||
case 'g': | ||||
case 'G': | ||||
case 'e': | ||||
case 'E': | ||||
parg_target = (void *)va_arg(arg_ptr, double *); | ||||
if(width && 0 != (parg_target)) | ||||
*((double *)parg_target) = code_atof(beg_tok, width); | ||||
break; | ||||
}; | ||||
if(parg_target) | ||||
break; | ||||
if(0 == (parg_target = (void *)va_arg(arg_ptr, int *))) | ||||
break; | ||||
if(!width) | ||||
break; | ||||
switch(*format) | ||||
{ | ||||
case 'd': | ||||
case 'i': | ||||
snum = code_atoi(beg_tok, width, 10); | ||||
memcpy(parg_target, &snum, sizeof(int)); | ||||
break; | ||||
case 'u': | ||||
unum = code_atoi(beg_tok, width, 10); | ||||
memcpy(parg_target, &unum, sizeof(unsigned int)); | ||||
break; | ||||
case 'x': | ||||
case 'X': | ||||
unum = code_atoi(beg_tok, width, 16); | ||||
memcpy(parg_target, &unum, sizeof(unsigned int)); | ||||
break; | ||||
case 'o': | ||||
unum = code_atoi(beg_tok, width, 8); | ||||
memcpy(parg_target, &unum, sizeof(unsigned int)); | ||||
break; | ||||
default: | ||||
goto fail; | ||||
}; | ||||
break; | ||||
}; | ||||
} | ||||
fail: | ||||
va_end(arg_ptr); | ||||
return tok_count; | ||||
} | ||||
double nmeaGenerator::nmea_random(double min, double max) | ||||
{ | ||||
static double rand_max = RAND_MAX; | ||||
double rand_val = rand(); | ||||
double bounds = max - min; | ||||
return min + (rand_val * bounds) / rand_max; | ||||
} | ||||