diff options
Diffstat (limited to 'src/lib')
-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); |