diff options
author | Julian Oes <julian@oes.ch> | 2013-11-20 22:28:05 +0100 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2013-11-20 22:28:05 +0100 |
commit | f351afe3f69c81e4f1ee43f24531728e4b518cea (patch) | |
tree | 0b9342b7432aaab3eae0a972cd5fc2d85462f69b | |
parent | 344705396d94602b5c3b3b34c61131602bc03fde (diff) | |
download | px4-firmware-f351afe3f69c81e4f1ee43f24531728e4b518cea.tar.gz px4-firmware-f351afe3f69c81e4f1ee43f24531728e4b518cea.tar.bz2 px4-firmware-f351afe3f69c81e4f1ee43f24531728e4b518cea.zip |
Geo: Copy distance function over from mavlink
-rw-r--r-- | src/lib/geo/geo.c | 24 | ||||
-rw-r--r-- | src/lib/geo/geo.h | 4 |
2 files changed, 28 insertions, 0 deletions
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 8aeca5be7..614f00186 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -387,6 +387,30 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d 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.0f) * 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 _wrap_pi(float bearing) { /* value is inf or NaN */ diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 6bce9309b..47f643593 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -124,6 +124,10 @@ __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 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); + __EXPORT float _wrap_180(float bearing); __EXPORT float _wrap_360(float bearing); __EXPORT float _wrap_pi(float bearing); |