diff options
author | Julian Oes <julian@oes.ch> | 2013-11-27 09:27:08 +0100 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2013-11-27 09:27:08 +0100 |
commit | 3f252987988738d9246eec9716b780d23cb8b0f7 (patch) | |
tree | 9d4ecb7f169b696fd28af22ea496f7c28fb42712 /src/lib | |
parent | 9c1a5be8e16d18612c8e318355fef15e53961da7 (diff) | |
download | px4-firmware-3f252987988738d9246eec9716b780d23cb8b0f7.tar.gz px4-firmware-3f252987988738d9246eec9716b780d23cb8b0f7.tar.bz2 px4-firmware-3f252987988738d9246eec9716b780d23cb8b0f7.zip |
Mavlink and navigator: Disable some functions in mavlink that are taken over by navigator, introduce topic to report mission status from commander back to mavlink
Diffstat (limited to 'src/lib')
-rw-r--r-- | src/lib/geo/geo.c | 15 | ||||
-rw-r--r-- | src/lib/geo/geo.h | 10 |
2 files changed, 25 insertions, 0 deletions
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 614f00186..85b17f9ae 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -411,6 +411,21 @@ __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now 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 */ diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 47f643593..5f92e14cf 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -124,10 +124,20 @@ __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); +/* + * 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); |