navigator: added NAV_CMD_IDLE, added RTL_STATE_LOITER and RTL_STATE_LANDED instead of FINISHED

4 files changed, 82 insertions, 31 deletions

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 41d81ad9b..5e545706d 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -69,10 +69,10 @@ MissionBlock::~MissionBlock()
 bool
 MissionBlock::is_mission_item_reached()
 {
-	/* don't check landed WPs */
 	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
-		return false;
+		return _navigator_priv->get_vstatus()->condition_landed;
 	}
+
 	/* TODO: count turns */
 #if 0
 	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
@@ -178,19 +178,28 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 	sp->loiter_direction = item->loiter_direction;
 	sp->pitch_min = item->pitch_min;
 
-	if (item->nav_cmd == NAV_CMD_TAKEOFF) {
+	switch (item->nav_cmd) {
+	case NAV_CMD_IDLE:
+		sp->type = SETPOINT_TYPE_IDLE;
+		break;
+
+	case NAV_CMD_TAKEOFF:
 		sp->type = SETPOINT_TYPE_TAKEOFF;
+		break;
 
-	} else if (item->nav_cmd == NAV_CMD_LAND) {
+	case NAV_CMD_LAND:
 		sp->type = SETPOINT_TYPE_LAND;
+		break;
 
-	} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-		   item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-		   item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+	case NAV_CMD_LOITER_TIME_LIMIT:
+	case NAV_CMD_LOITER_TURN_COUNT:
+	case NAV_CMD_LOITER_UNLIMITED:
 		sp->type = SETPOINT_TYPE_LOITER;
+		break;
 
-	} else {
+	default:
 		sp->type = SETPOINT_TYPE_POSITION;
+		break;
 	}
 }
 
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index c5bb06c3b..b7961a260 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -53,6 +53,8 @@
 #include "navigator.h"
 #include "rtl.h"
 
+#define DELAY_SIGMA	0.01f
+
 RTL::RTL(Navigator *navigator, const char *name) :
 	NavigatorMode(navigator, name),
 	MissionBlock(navigator),
@@ -95,7 +97,7 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 		if (_rtl_state == RTL_STATE_NONE) {
 			/* for safety reasons don't go into RTL if landed */
 			if (_navigator->get_vstatus()->condition_landed) {
-				_rtl_state = RTL_STATE_FINISHED;
+				_rtl_state = RTL_STATE_LANDED;
 				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
 
 			/* if lower than return altitude, climb up first */
@@ -113,11 +115,9 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 		}
 
 		set_rtl_item(pos_sp_triplet);
-
 		updated = true;
-	}
 
-	if ((_rtl_state != RTL_STATE_FINISHED) && is_mission_item_reached()) {
+	} else if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
 		advance_rtl();
 		set_rtl_item(pos_sp_triplet);
 		updated = true;
@@ -132,19 +132,11 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 	/* make sure we have the latest params */
 	updateParams();
 
-	if (_rtl_state == RTL_STATE_FINISHED) {
-		/* RTL finished, nothing to do */
-		pos_sp_triplet->next.valid = false;
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed");
-		return;
-	}
-
 	set_previous_pos_setpoint(pos_sp_triplet);
 	_navigator->set_can_loiter_at_sp(false);
 
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB: {
-
 		float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
 
 		_mission_item.lat = _navigator->get_global_position()->lat;
@@ -165,8 +157,8 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 			(int)(climb_alt - _navigator->get_home_position()->alt));
 		break;
 	}
-	case RTL_STATE_RETURN: {
 
+	case RTL_STATE_RETURN: {
 		_mission_item.lat = _navigator->get_home_position()->lat;
 		_mission_item.lon = _navigator->get_home_position()->lon;
 		 // don't change altitude
@@ -196,8 +188,8 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
 	}
-	case RTL_STATE_DESCEND: {
 
+	case RTL_STATE_DESCEND: {
 		_mission_item.lat = _navigator->get_home_position()->lat;
 		_mission_item.lon = _navigator->get_home_position()->lon;
 		_mission_item.altitude_is_relative = false;
@@ -207,9 +199,9 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
 		_mission_item.acceptance_radius = _param_acceptance_radius.get();
-		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
+		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
-		_mission_item.autocontinue = _param_land_delay.get() > -0.001f;
+		_mission_item.autocontinue = false;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
@@ -217,8 +209,35 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		break;
 	}
 
-	case RTL_STATE_LAND: {
+	case RTL_STATE_LOITER: {
+		bool autoland = _param_land_delay.get() > -DELAY_SIGMA;
+
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_home_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
+		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = autoland;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_can_loiter_at_sp(true);
 
+		if (autoland) {
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", _mission_item.time_inside);
+
+		} else {
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter");
+		}
+		break;
+	}
+
+	case RTL_STATE_LAND: {
 		_mission_item.lat = _navigator->get_home_position()->lat;
 		_mission_item.lon = _navigator->get_home_position()->lon;
 		_mission_item.altitude_is_relative = false;
@@ -237,6 +256,25 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		break;
 	}
 
+	case RTL_STATE_LANDED: {
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_home_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _navigator->get_home_position()->alt;
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_IDLE;
+		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.time_inside = 0.0f;
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed");
+		break;
+	}
+
 	default:
 		break;
 	}
@@ -262,18 +300,20 @@ RTL::advance_rtl()
 
 	case RTL_STATE_DESCEND:
 		/* only go to land if autoland is enabled */
-		if (_param_land_delay.get() < 0) {
-			_rtl_state = RTL_STATE_FINISHED;
+		if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
+			_rtl_state = RTL_STATE_LOITER;
+
 		} else {
 			_rtl_state = RTL_STATE_LAND;
 		}
 		break;
 
-	case RTL_STATE_LAND:
-		_rtl_state = RTL_STATE_FINISHED;
+	case RTL_STATE_LOITER:
+		_rtl_state = RTL_STATE_LAND;
 		break;
 
-	case RTL_STATE_FINISHED:
+	case RTL_STATE_LAND:
+		_rtl_state = RTL_STATE_LANDED;
 		break;
 
 	default:
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index bcccf7454..c4286b391 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -97,8 +97,9 @@ private:
 		RTL_STATE_CLIMB,
 		RTL_STATE_RETURN,
 		RTL_STATE_DESCEND,
+		RTL_STATE_LOITER,
 		RTL_STATE_LAND,
-		RTL_STATE_FINISHED,
+		RTL_STATE_LANDED,
 	} _rtl_state;
 
 	control::BlockParamFloat _param_return_alt;
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index d25db3a77..d9dd61df1 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -50,6 +50,7 @@
 
 /* compatible to mavlink MAV_CMD */
 enum NAV_CMD {
+	NAV_CMD_IDLE=0,
 	NAV_CMD_WAYPOINT=16,
 	NAV_CMD_LOITER_UNLIMITED=17,
 	NAV_CMD_LOITER_TURN_COUNT=18,