diff options
Diffstat (limited to 'src/lib/geo/geo.h')
| -rw-r--r-- | src/lib/geo/geo.h | 23 |
1 files changed, 21 insertions, 2 deletions
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 123ff80f1..94afb4df0 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -47,6 +47,9 @@ #pragma once +#include "uORB/topics/fence.h" +#include "uORB/topics/vehicle_global_position.h" + __BEGIN_DECLS #include <stdbool.h> @@ -112,15 +115,31 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou */ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -__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); + +__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); -__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 add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res); __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_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); +/* + * Calculate distance in global frame + */ +__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); + +/* + * Calculate distance in local frame (NED) + */ +__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); + __EXPORT float _wrap_180(float bearing); __EXPORT float _wrap_360(float bearing); __EXPORT float _wrap_pi(float bearing); |