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

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);