diff options
Diffstat (limited to 'src/lib/geo/geo.c')
-rw-r--r-- | src/lib/geo/geo.c | 316 |
1 files changed, 161 insertions, 155 deletions
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 43105fdba..e600976ce 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> + * Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -42,6 +39,7 @@ * @author Thomas Gubler <thomasgubler@student.ethz.ch> * @author Julian Oes <joes@student.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> */ #include <geo/geo.h> @@ -52,125 +50,58 @@ #include <math.h> #include <stdbool.h> +/* + * Azimuthal Equidistant Projection + * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html + */ -/* values for map projection */ -static double phi_1; -static double sin_phi_1; -static double cos_phi_1; -static double lambda_0; -static double scale; - -__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { - /* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - phi_1 = lat_0 / 180.0 * M_PI; - lambda_0 = lon_0 / 180.0 * M_PI; - - sin_phi_1 = sin(phi_1); - cos_phi_1 = cos(phi_1); - - /* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale - - /* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */ - const double r_earth = 6371000; - - double lat1 = phi_1; - double lon1 = lambda_0; - - double lat2 = phi_1 + 0.5 / 180 * M_PI; - double lon2 = lambda_0 + 0.5 / 180 * M_PI; - double sin_lat_2 = sin(lat2); - double cos_lat_2 = cos(lat2); - double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth; - - /* 2) calculate distance rho on plane */ - double k_bar = 0; - double c = acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)); - - if (0 != c) - k_bar = c / sin(c); - - double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane - double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0))); - double rho = sqrt(pow(x2, 2) + pow(y2, 2)); - - scale = d / rho; + ref->lat = lat_0 / 180.0 * M_PI; + ref->lon = lon_0 / 180.0 * M_PI; + ref->sin_lat = sin(ref->lat); + ref->cos_lat = cos(ref->lat); } -__EXPORT void map_projection_project(double lat, double lon, float *x, float *y) +__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) { - /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - double phi = lat / 180.0 * M_PI; - double lambda = lon / 180.0 * M_PI; - - double sin_phi = sin(phi); - double cos_phi = cos(phi); - - double k_bar = 0; - /* using small angle approximation (formula in comment is without aproximation) */ - double c = acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c = acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) ); + double lat_rad = lat / 180.0 * M_PI; + double lon_rad = lon / 180.0 * M_PI; - if (0 != c) - k_bar = c / sin(c); + double sin_lat = sin(lat_rad); + double cos_lat = cos(lat_rad); + double cos_d_lon = cos(lon_rad - ref->lon); - /* using small angle approximation (formula in comment is without aproximation) */ - *y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale; - *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; // *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale; + double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon); + double k = (c == 0.0) ? 1.0 : (c / sin(c)); -// printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0); + *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; + *y = k * cos_lat * sin(lon_rad - ref->lon) * CONSTANTS_RADIUS_OF_EARTH; } -__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon) +__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) { - /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - - double x_descaled = x / scale; - double y_descaled = y / scale; - - double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2)); + double x_rad = x / CONSTANTS_RADIUS_OF_EARTH; + double y_rad = y / CONSTANTS_RADIUS_OF_EARTH; + double c = sqrtf(x_rad * x_rad + y_rad * y_rad); double sin_c = sin(c); double cos_c = cos(c); - double lat_sphere = 0; - - if (c != 0) - lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c); - else - lat_sphere = asin(cos_c * sin_phi_1); - -// printf("lat_sphere = %.10f\n",lat_sphere); - - double lon_sphere = 0; - - if (phi_1 == M_PI / 2) { - //using small angle approximation (formula in comment is without aproximation) - lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled)); + double lat_rad; + double lon_rad; - } else if (phi_1 == -M_PI / 2) { - //using small angle approximation (formula in comment is without aproximation) - lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled)); + if (c != 0.0) { + lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c); + lon_rad = (ref->lon + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c)); } else { - - lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c)); - //using small angle approximation -// double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c); -// if(denominator != 0) -// { -// lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator); -// } -// else -// { -// ... -// } + lat_rad = ref->lat; + lon_rad = ref->lon; } -// printf("lon_sphere = %.10f\n",lon_sphere); - - *lat = lat_sphere * 180.0 / M_PI; - *lon = lon_sphere * 180.0 / M_PI; - + *lat = lat_rad * 180.0 / M_PI; + *lon = lon_rad * 180.0 / M_PI; } @@ -188,8 +119,7 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad); double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a)); - const double radius_earth = 6371000.0d; - return radius_earth * c; + return CONSTANTS_RADIUS_OF_EARTH * c; } __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) @@ -199,7 +129,6 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub double lat_next_rad = lat_next * M_DEG_TO_RAD; double lon_next_rad = lon_next * M_DEG_TO_RAD; - double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ @@ -210,22 +139,21 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub return theta; } -__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy) +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; double lat_next_rad = lat_next * M_DEG_TO_RAD; double lon_next_rad = lon_next * M_DEG_TO_RAD; - double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ - *vy = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad); - *vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon); + *v_n = CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); + *v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad); } -__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy) +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -236,13 +164,22 @@ __EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, d double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ - *vy = CONSTANTS_RADIUS_OF_EARTH * d_lon; - *vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad); + *v_n = CONSTANTS_RADIUS_OF_EARTH * d_lat; + *v_e = CONSTANTS_RADIUS_OF_EARTH * d_lon * cos(lat_now_rad); +} + +__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res) +{ + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + + *lat_res = (lat_now_rad + (double)v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG; + *lon_res = (lon_now_rad + (double)v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG; } // Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> -__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) +__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) { // This function returns the distance to the nearest point on the track line. Distance is positive if current // position is right of the track and negative if left of the track as seen from a point on the track line @@ -259,7 +196,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, crosstrack_error->bearing = 0.0f; // Return error if arguments are bad - if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_value; + if (lat_now == 0.0 || lon_now == 0.0 || lat_start == 0.0 || lon_start == 0.0 || lat_end == 0.0d || lon_end == 0.0d) { return return_value; } bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); @@ -274,7 +211,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, } dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); - crosstrack_error->distance = (dist_to_end) * sin(bearing_diff); + crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff); if (sin(bearing_diff) >= 0) { crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F); @@ -290,8 +227,8 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, } -__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, - float radius, float arc_start_bearing, float arc_sweep) +__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep) { // This function returns the distance to the nearest point on the track arc. Distance is positive if current // position is right of the arc and negative if left of the arc as seen from the closest point on the arc and @@ -310,29 +247,29 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d crosstrack_error->bearing = 0.0f; // Return error if arguments are bad - if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_value; + if (lat_now == 0.0 || lon_now == 0.0 || lat_center == 0.0 || lon_center == 0.0 || radius == 0.0f) { return return_value; } if (arc_sweep >= 0) { bearing_sector_start = arc_start_bearing; bearing_sector_end = arc_start_bearing + arc_sweep; - if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F; + if (bearing_sector_end > 2.0f * M_PI_F) { bearing_sector_end -= M_TWOPI_F; } } else { bearing_sector_end = arc_start_bearing; bearing_sector_start = arc_start_bearing - arc_sweep; - if (bearing_sector_start < 0.0f) bearing_sector_start += M_TWOPI_F; + if (bearing_sector_start < 0.0f) { bearing_sector_start += M_TWOPI_F; } } in_sector = false; // Case where sector does not span zero - if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true; + if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) { in_sector = true; } // Case where sector does span zero - if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true; + if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) { in_sector = true; } // If in the sector then calculate distance and bearing to closest point if (in_sector) { @@ -357,23 +294,21 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d // calculate the position of the start and end points. We should not be doing this often // as this function generally will not be called repeatedly when we are out of the sector. - // TO DO - this is messed up and won't compile - float start_disp_x = radius * sin(arc_start_bearing); - float start_disp_y = radius * cos(arc_start_bearing); - float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep)); - float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep)); - float lon_start = lon_now + start_disp_x / 111111.0d; - float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d; - float lon_end = lon_now + end_disp_x / 111111.0d; - float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d; - float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); - float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + double start_disp_x = (double)radius * sin(arc_start_bearing); + double start_disp_y = (double)radius * cos(arc_start_bearing); + double end_disp_x = (double)radius * sin(_wrapPI((double)(arc_start_bearing + arc_sweep))); + double end_disp_y = (double)radius * cos(_wrapPI((double)(arc_start_bearing + arc_sweep))); + double lon_start = lon_now + start_disp_x / 111111.0; + double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0; + double lon_end = lon_now + end_disp_x / 111111.0; + double lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0; + double dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + double dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); if (dist_to_start < dist_to_end) { crosstrack_error->distance = dist_to_start; crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); - } else { crosstrack_error->past_end = true; crosstrack_error->distance = dist_to_end; @@ -382,30 +317,73 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d } - crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing); + crosstrack_error->bearing = _wrapPI((double)crosstrack_error->bearing); return_value = OK; return return_value; } +__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now, + double lat_next, double lon_next, float alt_next, + float *dist_xy, float *dist_z) +{ + double current_x_rad = lat_next / 180.0 * M_PI; + double current_y_rad = lon_next / 180.0 * M_PI; + double x_rad = lat_now / 180.0 * M_PI; + double y_rad = lon_now / 180.0 * M_PI; + + double d_lat = x_rad - current_x_rad; + double d_lon = y_rad - current_y_rad; + + double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(current_x_rad) * cos(x_rad); + double c = 2 * atan2(sqrt(a), sqrt(1 - a)); + + float dxy = CONSTANTS_RADIUS_OF_EARTH * c; + float dz = alt_now - alt_next; + + *dist_xy = fabsf(dxy); + *dist_z = fabsf(dz); + + return sqrtf(dxy * dxy + dz * dz); +} + + +__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now, + float x_next, float y_next, float z_next, + float *dist_xy, float *dist_z) +{ + float dx = x_now - x_next; + float dy = y_now - y_next; + float dz = z_now - z_next; + + *dist_xy = sqrtf(dx * dx + dy * dy); + *dist_z = fabsf(dz); + + return sqrtf(dx * dx + dy * dy + dz * dz); +} + __EXPORT float _wrap_pi(float bearing) { /* value is inf or NaN */ - if (!isfinite(bearing) || bearing == 0) { + if (!isfinite(bearing)) { return bearing; } int c = 0; - - while (bearing > M_PI_F && c < 30) { + while (bearing >= M_PI_F) { bearing -= M_TWOPI_F; - c++; + + if (c++ > 3) { + return NAN; + } } c = 0; - - while (bearing <= -M_PI_F && c < 30) { + while (bearing < -M_PI_F) { bearing += M_TWOPI_F; - c++; + + if (c++ > 3) { + return NAN; + } } return bearing; @@ -418,12 +396,22 @@ __EXPORT float _wrap_2pi(float bearing) return bearing; } + int c = 0; while (bearing >= M_TWOPI_F) { - bearing = bearing - M_TWOPI_F; + bearing -= M_TWOPI_F; + + if (c++ > 3) { + return NAN; + } } - while (bearing < 0.0f) { - bearing = bearing + M_TWOPI_F; + c = 0; + while (bearing < 0.0f) { + bearing += M_TWOPI_F; + + if (c++ > 3) { + return NAN; + } } return bearing; @@ -436,12 +424,22 @@ __EXPORT float _wrap_180(float bearing) return bearing; } - while (bearing > 180.0f) { - bearing = bearing - 360.0f; + int c = 0; + while (bearing >= 180.0f) { + bearing -= 360.0f; + + if (c++ > 3) { + return NAN; + } } - while (bearing <= -180.0f) { - bearing = bearing + 360.0f; + c = 0; + while (bearing < -180.0f) { + bearing += 360.0f; + + if (c++ > 3) { + return NAN; + } } return bearing; @@ -454,15 +452,23 @@ __EXPORT float _wrap_360(float bearing) return bearing; } + int c = 0; while (bearing >= 360.0f) { - bearing = bearing - 360.0f; + bearing -= 360.0f; + + if (c++ > 3) { + return NAN; + } } - while (bearing < 0.0f) { - bearing = bearing + 360.0f; + c = 0; + while (bearing < 0.0f) { + bearing += 360.0f; + + if (c++ > 3) { + return NAN; + } } return bearing; } - - |