/** * \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 */ double azimuth, /**< Azimuth (degree) [0, 359] */ double 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; }
void nmea_error(const char *str, ...) { int size; va_list arg_list; char buff[NMEA_DEF_PARSEBUFF]; nmeaErrorFunc func = nmea_property()->error_func; if(func) { va_start(arg_list, str); size = NMEA_POSIX(vsnprintf)(&buff[0], NMEA_DEF_PARSEBUFF - 1, str, arg_list); va_end(arg_list); if(size > 0) (*func)(&buff[0], size); } }
void nmea_error(const char *str, ...) { // FILE *file; // file = fopen("nmeaError.txt","a+"); // fprintf(file,"%s\n",str); // fclose(file); int size; va_list arg_list; char buff[NMEA_DEF_PARSEBUFF]; nmeaErrorFunc func = nmea_property()->error_func; if(func) { va_start(arg_list, str); size = NMEA_POSIX(vsnprintf)(&buff[0], NMEA_DEF_PARSEBUFF - 1, str, arg_list); va_end(arg_list); if(size > 0) (*func)(&buff[0], size); } }
/** * \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 */ 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 = 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)); }