Added support for dynamic turn radii

7 files changed, 86 insertions, 26 deletions

diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 7a5c2dab2..7c10e297b 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -690,25 +690,25 @@ int mavlink_thread_main(int argc, char *argv[])
 
 		lowspeed_counter++;
 
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
+		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
 
 		/* sleep quarter the time */
 		usleep(25000);
 
 		/* check if waypoint has been reached against the last positions */
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
+		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
 
 		/* sleep quarter the time */
 		usleep(25000);
 
 		/* send parameters at 20 Hz (if queued for sending) */
 		mavlink_pm_queued_send();
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
+		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
 
 		/* sleep quarter the time */
 		usleep(25000);
 
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
+		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
 
 		if (baudrate > 57600) {
 			mavlink_pm_queued_send();
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
index 3175e64ce..e8d707948 100644
--- a/src/modules/mavlink/missionlib.c
+++ b/src/modules/mavlink/missionlib.c
@@ -170,6 +170,28 @@ bool set_special_fields(float param1, float param2, float param3, float param4,
 	sp->param2 = param2;
 	sp->param3 = param3;
 	sp->param4 = param4;
+
+
+	/* define the turn distance */
+	float orbit = 15.0f;
+
+	if (command == (int)MAV_CMD_NAV_WAYPOINT) {
+
+		orbit = param2;
+
+	} else if (command == (int)MAV_CMD_NAV_LOITER_TURNS ||
+			command == (int)MAV_CMD_NAV_LOITER_TIME ||
+			command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
+
+		orbit = param3;
+	} else {
+
+		// XXX set default orbit via param
+		// 15 initialized above
+	}
+
+	sp->turn_distance_xy = orbit;
+	sp->turn_distance_z = orbit;
 }
 
 /**
@@ -223,10 +245,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		int last_setpoint_index = -1;
 		bool last_setpoint_valid = false;
 
-		/* at first waypoint, but cycled once through mission */
-		if (index == 0 && last_waypoint_index > 0) {
-			last_setpoint_index = last_waypoint_index;
-		} else {
+		if (index > 0) {
 			last_setpoint_index = index - 1;
 		}
 
@@ -251,10 +270,8 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		int next_setpoint_index = -1;
 		bool next_setpoint_valid = false;
 
-		/* at last waypoint, try to re-loop through mission as default */
-		if (index == (wpm->size - 1) && wpm->size > 1) {
-			next_setpoint_index = 0;
-		} else if (wpm->size > 1) {
+		/* next waypoint */
+		if (wpm->size > 1) {
 			next_setpoint_index = index + 1;
 		}
 
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index cec2fdc43..0e0ce2723 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -67,6 +67,7 @@ extern bool gcs_link;
 
 struct vehicle_global_position_s global_pos;
 struct vehicle_local_position_s local_pos;
+struct navigation_capabilities_s nav_cap;
 struct vehicle_status_s v_status;
 struct rc_channels_s rc;
 struct rc_input_values rc_raw;
@@ -122,6 +123,7 @@ static void	l_optical_flow(const struct listener *l);
 static void	l_vehicle_rates_setpoint(const struct listener *l);
 static void	l_home(const struct listener *l);
 static void	l_airspeed(const struct listener *l);
+static void	l_nav_cap(const struct listener *l);
 
 static const struct listener listeners[] = {
 	{l_sensor_combined,		&mavlink_subs.sensor_sub,	0},
@@ -148,6 +150,7 @@ static const struct listener listeners[] = {
 	{l_vehicle_rates_setpoint,	&mavlink_subs.rates_setpoint_sub,	0},
 	{l_home,			&mavlink_subs.home_sub,		0},
 	{l_airspeed,			&mavlink_subs.airspeed_sub,		0},
+	{l_nav_cap,			&mavlink_subs.navigation_capabilities_sub,		0},
 };
 
 static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
@@ -691,6 +694,19 @@ l_airspeed(const struct listener *l)
 	mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb);
 }
 
+void
+l_nav_cap(const struct listener *l)
+{
+
+	orb_copy(ORB_ID(navigation_capabilities), mavlink_subs.navigation_capabilities_sub, &nav_cap);
+
+	mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+				   hrt_absolute_time() / 1000,
+				   "turn dist",
+				   nav_cap.turn_distance);
+
+}
+
 static void *
 uorb_receive_thread(void *arg)
 {
@@ -837,6 +853,11 @@ uorb_receive_start(void)
 	mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	orb_set_interval(mavlink_subs.airspeed_sub, 200); 	/* 5Hz updates */
 
+	/* --- NAVIGATION CAPABILITIES --- */
+	mavlink_subs.navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
+	orb_set_interval(mavlink_subs.navigation_capabilities_sub, 500); 	/* 2Hz updates */
+	nav_cap.turn_distance = 0.0f;
+
 	/* start the listener loop */
 	pthread_attr_t uorb_attr;
 	pthread_attr_init(&uorb_attr);
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index c5cd0d03e..91773843b 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -67,6 +67,7 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 #include <drivers/drv_rc_input.h>
+#include <uORB/topics/navigation_capabilities.h>
 
 struct mavlink_subscriptions {
 	int sensor_sub;
@@ -92,6 +93,7 @@ struct mavlink_subscriptions {
 	int rates_setpoint_sub;
 	int home_sub;
 	int airspeed_sub;
+	int navigation_capabilities_sub;
 };
 
 extern struct mavlink_subscriptions mavlink_subs;
@@ -102,6 +104,9 @@ extern struct vehicle_global_position_s global_pos;
 /** Local position */
 extern struct vehicle_local_position_s local_pos;
 
+/** navigation capabilities */
+extern struct navigation_capabilities_s nav_cap;
+
 /** Vehicle status */
 extern struct vehicle_status_s v_status;
 
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index f03fe4fdf..ddad5f0df 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -251,9 +251,8 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
  *
  * The distance calculation is based on the WGS84 geoid (GPS)
  */
-float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt)
+float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt, float *dist_xy, float *dist_z)
 {
-	static uint16_t counter;
 
 	if (seq < wpm->size) {
 		mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
@@ -274,20 +273,21 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float
 		float dxy = radius_earth * c;
 		float dz = alt - wp->z;
 
+		*dist_xy = fabsf(dxy);
+		*dist_z = fabsf(dz);
+
 		return sqrtf(dxy * dxy + dz * dz);
 
 	} else {
 		return -1.0f;
 	}
 
-	counter++;
-
 }
 
 /*
  * Calculate distance in local frame (NED)
  */
-float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z)
+float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z, float *dist_xy, float *dist_z)
 {
 	if (seq < wpm->size) {
 		mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
@@ -296,6 +296,9 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float
 		float dy = (cur->y - y);
 		float dz = (cur->z - z);
 
+		*dist_xy = sqrtf(dx * dx + dy * dy);
+		*dist_z = fabsf(dz);
+
 		return sqrtf(dx * dx + dy * dy + dz * dz);
 
 	} else {
@@ -303,7 +306,7 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float
 	}
 }
 
-void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos)
+void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance)
 {
 	static uint16_t counter;
 
@@ -332,26 +335,37 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 			orbit = 15.0f;
 		}
 
+		/* keep vertical orbit */
+		float vertical_switch_distance = orbit;
+
+		/* Take the larger turn distance - orbit or turn_distance */
+		if (orbit < turn_distance)
+			orbit = turn_distance;
+
 		int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame;
 		float dist = -1.0f;
 
+		float dist_xy = -1.0f;
+		float dist_z = -1.0f;
+
 		if (coordinate_frame == (int)MAV_FRAME_GLOBAL) {
-			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt);
+			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt, &dist_xy, &dist_z);
 
 		} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
-			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt);
+			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z);
 
 		} else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) {
-			dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z);
+			dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z);
 
 		} else if (coordinate_frame == (int)MAV_FRAME_MISSION) {
 			/* Check if conditions of mission item are satisfied */
 			// XXX TODO
 		}
 
-		if (dist >= 0.f && dist <= orbit) {
+		if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) {
 			wpm->pos_reached = true;
 		}
+
 		// check if required yaw reached
 		float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI);
 		float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw);
@@ -465,7 +479,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 }
 
 
-int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position)
+int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position, struct navigation_capabilities_s *nav_cap)
 {
 	/* check for timed-out operations */
 	if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
@@ -488,7 +502,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio
 		}
 	}
 
-	check_waypoints_reached(now, global_position, local_position);
+	check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance);
 
 	return OK;
 }
@@ -1011,5 +1025,5 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 		}
 	}
 
-	check_waypoints_reached(now, global_pos, local_pos);
+	// check_waypoints_reached(now, global_pos, local_pos);
 }
diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h
index 96a0ecd30..d7d6b31dc 100644
--- a/src/modules/mavlink/waypoints.h
+++ b/src/modules/mavlink/waypoints.h
@@ -55,6 +55,7 @@
 #include <stdbool.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/navigation_capabilities.h>
 
 // FIXME XXX - TO BE MOVED TO XML
 enum MAVLINK_WPM_STATES {
@@ -120,7 +121,7 @@ typedef struct mavlink_wpm_storage mavlink_wpm_storage;
 
 void mavlink_wpm_init(mavlink_wpm_storage *state);
 int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position,
-			       struct vehicle_local_position_s *local_pos);
+			       struct vehicle_local_position_s *local_pos, struct navigation_capabilities_s *nav_cap);
 void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos ,
 				 struct vehicle_local_position_s *local_pos);
 
diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h
index 5c8ce1e4d..a56434d3b 100644
--- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h
@@ -72,6 +72,8 @@ struct vehicle_global_position_setpoint_s
 	float param2;
 	float param3;
 	float param4;
+	float turn_distance_xy;		/**< The distance on the plane which will mark this as reached */
+	float turn_distance_z;		/**< The distance in Z direction which will mark this as reached */
 };
 
 /**