navigator: lot's of cleanup (WIP)

src/modules/navigator/geofence_params.c

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

src/modules/navigator/mission.cpp

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Julian Oes <joes@student.ethz.ch>
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,22 +32,22 @@
  ****************************************************************************/
 /**
  * @file navigator_mission.cpp
+ *
  * Helper class to access missions
+ *
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #include <string.h>
 #include <stdlib.h>
+
 #include <dataman/dataman.h>
 #include <systemlib/err.h>
+
 #include <uORB/uORB.h>
 #include <uORB/topics/mission_result.h>
-#include "navigator_mission.h"
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
+#include "mission.h"
 
 
 Mission::Mission() :
@@ -60,27 +59,43 @@ Mission::Mission() :
 	_onboard_mission_item_count(0),
 	_onboard_mission_allowed(false),
 	_current_mission_type(MISSION_TYPE_NONE),
-	_mission_result_pub(-1)
+	_mission_result_pub(-1),
+	_mission_result({})
 {
-	memset(&_mission_result, 0, sizeof(struct mission_result_s));
 }
 
 Mission::~Mission()
 {
-
 }
 
 void
-Mission::set_offboard_dataman_id(int new_id)
+Mission::set_offboard_dataman_id(const int new_id)
 {
 	_offboard_dataman_id = new_id;
 }
 
 void
-Mission::set_current_offboard_mission_index(int new_index)
+Mission::set_offboard_mission_count(int new_count)
 {
-	if (new_index != -1) {
-		warnx("specifically set to %d", new_index);
+	_offboard_mission_item_count = new_count;
+}
+
+void
+Mission::set_onboard_mission_count(int new_count)
+{
+	_onboard_mission_item_count = new_count;
+}
+
+void
+Mission::set_onboard_mission_allowed(bool allowed)
+{
+	_onboard_mission_allowed = allowed;
+}
+
+bool
+Mission::command_current_offboard_mission_index(const int new_index)
+{
+	if (new_index >= 0) {
 		_current_offboard_mission_index = (unsigned)new_index;
 	} else {
 
@@ -92,8 +107,8 @@ Mission::set_current_offboard_mission_index(int new_index)
 	report_current_offboard_mission_item();
 }
 
-void
-Mission::set_current_onboard_mission_index(int new_index)
+bool
+Mission::command_current_onboard_mission_index(int new_index)
 {
 	if (new_index != -1) {
 		_current_onboard_mission_index = (unsigned)new_index;
@@ -108,153 +123,130 @@ Mission::set_current_onboard_mission_index(int new_index)
 	// report_current_mission_item();
 }
 
-void
-Mission::set_offboard_mission_count(unsigned new_count)
-{
-	_offboard_mission_item_count = new_count;
-}
-
-void
-Mission::set_onboard_mission_count(unsigned new_count)
-{
-	_onboard_mission_item_count = new_count;
-}
-
-void
-Mission::set_onboard_mission_allowed(bool allowed)
-{
-	_onboard_mission_allowed = allowed;
-}
-
 bool
-Mission::current_mission_available()
+Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, int *index)
 {
-	return (current_onboard_mission_available() || current_offboard_mission_available());
-
-}
-
-bool
-Mission::next_mission_available()
-{
-	return (next_onboard_mission_available() || next_offboard_mission_available());
-}
-
-int
-Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index)
-{
-	int ret = ERROR;
+	*onboard = false;
+	*index = -1;
 
 	/* try onboard mission first */
-	if (current_onboard_mission_available()) {
-
-		ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &_current_onboard_mission_index, new_mission_item);
-		if (ret == OK) {
+	if (_current_onboard_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) {
+		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, new_mission_item)) {
 			_current_mission_type = MISSION_TYPE_ONBOARD;
 			*onboard = true;
 			*index = _current_onboard_mission_index;
-		}
 
+			return true;
+		}
+	}
+	
 	/* otherwise fallback to offboard */
-	} else if (current_offboard_mission_available()) {
+	if (_current_offboard_mission_index < _offboard_mission_item_count) {
 
 		dm_item_t dm_current;
-
 		if (_offboard_dataman_id == 0) {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
-
 		} else {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
 		}
+		if (read_mission_item(dm_current, true, &_current_offboard_mission_index, new_mission_item)) {
 
-		ret = get_mission_item(dm_current, &_current_offboard_mission_index, new_mission_item);
-		if (ret == OK) {
 			_current_mission_type = MISSION_TYPE_OFFBOARD;
 			*onboard = false;
 			*index = _current_offboard_mission_index;
-		}
 
-	} else {
-		/* happens when no more mission items can be added as a next item */
-		_current_mission_type = MISSION_TYPE_NONE;
-		ret == ERROR;
+			return true;
+		}
 	}
 
-	return ret;
+	/* happens when no more mission items can be added as a next item */
+	_current_mission_type = MISSION_TYPE_NONE;
+
+	return false;
 }
 
-int
+bool
 Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
 {
-	int ret = ERROR;
+	int next_temp_mission_index = _current_onboard_mission_index + 1;
 
 	/* try onboard mission first */
-	if (next_onboard_mission_available()) {
+	if (next_temp_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) {
+		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, new_mission_item)) {
+			return true;
+		}
+	}
 
-		unsigned next_onboard_mission_index = _current_onboard_mission_index + 1;
-		ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &next_onboard_mission_index, new_mission_item);
+	/* then try offboard mission */
+	dm_item_t dm_current;
+	if (_offboard_dataman_id == 0) {
+		dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+	} else {
+		dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+	}
+	next_temp_mission_index = _current_offboard_mission_index + 1;
+	if (next_temp_mission_index < _offboard_mission_item_count) {
+		if (read_mission_item(dm_current, false, &next_temp_mission_index, new_mission_item)) {
+			return true;
+		}
+	}
 
-	/* otherwise fallback to offboard */
-	} else if (next_offboard_mission_available()) {
+	/* both failed, bail out */
+	return false;
+}
 
-		dm_item_t dm_current;
+bool
+Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, struct mission_item_s *new_mission_item)
+{
+	/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
+	for (int i=0; i<10; i++) {
+		const ssize_t len = sizeof(struct mission_item_s);
 
-		if (_offboard_dataman_id == 0) {
-			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
-
-		} else {
-			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		/* read mission item from datamanager */
+		if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			return false;
 		}
 
-		unsigned next_offboard_mission_index = _current_offboard_mission_index + 1;
-		ret = get_mission_item(dm_current, &next_offboard_mission_index, new_mission_item);
+		/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
+		if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) {
 
-	} else {
-		/* happens when no more mission items can be added as a next item */
-		ret = ERROR;
-	}
-	return ret;
-}
+			if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) {
 
+				/* only raise the repeat count if this is for the current mission item
+				 * but not for the next mission item */
+				if (is_current) {
+					(new_mission_item->do_jump_current_count)++;
 
-bool
-Mission::current_onboard_mission_available()
-{
-	return _onboard_mission_item_count > _current_onboard_mission_index && _onboard_mission_allowed;
-}
+					/* save repeat count */
+					if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) {
+						/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+						return false;
+					}
+				}
+				/* set new mission item index and repeat
+				 * we don't have to validate here, if it's invalid, we should realize this later .*/
+				*mission_index = new_mission_item->do_jump_mission_index;
+			} else {
+				return false;
+			}
 
-bool
-Mission::current_offboard_mission_available()
-{
-	return _offboard_mission_item_count > _current_offboard_mission_index;
-}
-
-bool
-Mission::next_onboard_mission_available()
-{
-	unsigned next = 0;
-
-	if (_current_mission_type != MISSION_TYPE_ONBOARD) {
-		next = 1;
+		} else {
+			/* if it's not a DO_JUMP, then we were successful */
+			return true;
+		}
 	}
 
-	return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed;
-}
-
-bool
-Mission::next_offboard_mission_available()
-{
-	unsigned next = 0;
-
-	if (_current_mission_type != MISSION_TYPE_OFFBOARD) {
-		next = 1;
-	}
-
-	return _offboard_mission_item_count > (_current_offboard_mission_index + next);
+	/* we have given up, we don't want to cycle forever */
+	warnx("ERROR: cycling through mission items without success");
+	return false;
 }
 
 void
 Mission::move_to_next()
 {
+	report_mission_item_reached();
+	
 	switch (_current_mission_type) {
 	case MISSION_TYPE_ONBOARD:
 		_current_onboard_mission_index++;
@@ -277,12 +269,14 @@ Mission::report_mission_item_reached()
 		_mission_result.mission_reached = true;
 		_mission_result.mission_index_reached = _current_offboard_mission_index;
 	}
+	publish_mission_result();
 }
 
 void
 Mission::report_current_offboard_mission_item()
 {
 	_mission_result.index_current_mission = _current_offboard_mission_index;
+	publish_mission_result();
 }
 
 void
@@ -301,32 +295,3 @@ Mission::publish_mission_result()
 	_mission_result.mission_reached = false;
 }
 
-int
-Mission::get_mission_item(const dm_item_t dm_item, unsigned *mission_index, struct mission_item_s *new_mission_item)
-{
-	/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
-	for (int i=0; i<10; i++) {
-
-		const ssize_t len = sizeof(struct mission_item_s);
-
-		/* read mission item from datamanager */
-		if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) {
-			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-			return ERROR;
-		}
-
-		/* check for DO_JUMP item */
-		if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) {
-			/* set new mission item index and repeat
-			 * we don't have to validate here, if it's invalid, we should realize this later .*/
-			*mission_index = new_mission_item->do_jump_mission_index;
-		} else {
-			/* if it's not a DO_JUMP, then we were successful */
-			return OK;
-		}
-	}
-
-	/* we have given up, we don't want to cycle forever */
-	warnx("ERROR: cycling through mission items without success");
-	return ERROR;
-}

src/modules/navigator/mission.h

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Julian Oes <joes@student.ethz.ch>
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,6 +33,7 @@
 /**
  * @file navigator_mission.h
  * Helper class to access missions
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #ifndef NAVIGATOR_MISSION_H
@@ -41,6 +41,7 @@
 
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
+#include <dataman/dataman.h>
 
 
 class __EXPORT Mission
@@ -52,44 +53,36 @@ public:
 	Mission();
 
 	/**
-	 * Destructor, also kills the sensors task.
+	 * Destructor
 	 */
 	~Mission();
 
-	void		set_offboard_dataman_id(int new_id);
-	void		set_current_offboard_mission_index(int new_index);
-	void		set_current_onboard_mission_index(int new_index);
-	void		set_offboard_mission_count(unsigned new_count);
-	void		set_onboard_mission_count(unsigned new_count);
+	void		set_offboard_dataman_id(const int new_id);
+	void		set_offboard_mission_count(const int new_count);
+	void		set_onboard_mission_count(const int new_count);
+	void		set_onboard_mission_allowed(const bool allowed);
 
-	void		set_onboard_mission_allowed(bool allowed);
+	bool		command_current_offboard_mission_index(const int new_index);
+	bool		command_current_onboard_mission_index(const int new_index);
 
-	bool		current_mission_available();
-	bool		next_mission_available();
-
-	int		get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, unsigned *index);
-	int		get_next_mission_item(struct mission_item_s *mission_item);
+	bool		get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, int *index);
+	bool		get_next_mission_item(struct mission_item_s *mission_item);
 
 	void		move_to_next();
 
+private:
+	bool		read_mission_item(const dm_item_t dm_item, const bool is_current, int *mission_index, struct mission_item_s *new_mission_item);
+
 	void		report_mission_item_reached();
 	void		report_current_offboard_mission_item();
+
 	void		publish_mission_result();
 
-private:
-	int		get_mission_item(const dm_item_t dm_item, unsigned *mission_index, struct mission_item_s *new_mission_item);
-
-	bool		current_onboard_mission_available();
-	bool		current_offboard_mission_available();
-	bool		next_onboard_mission_available();
-	bool		next_offboard_mission_available();
-
 	int 		_offboard_dataman_id;
-	unsigned	_current_offboard_mission_index;
-	unsigned	_current_onboard_mission_index;
-	unsigned	_offboard_mission_item_count;		/** number of offboard mission items available */
-	unsigned	_onboard_mission_item_count;		/** number of onboard mission items available */
-
+	int		_current_offboard_mission_index;
+	int		_current_onboard_mission_index;
+	int		_offboard_mission_item_count;		/** number of offboard mission items available */
+	int		_onboard_mission_item_count;		/** number of onboard mission items available */
 	bool		_onboard_mission_allowed;
 
 	enum {
@@ -98,9 +91,8 @@ private:
 		MISSION_TYPE_OFFBOARD,
 	} 		_current_mission_type;
 
-	int		_mission_result_pub;
-
-	struct mission_result_s _mission_result;
+	orb_advert_t	 _mission_result_pub;			/**< publish mission result topic */
+	mission_result_s _mission_result;			/**< mission result for commander/mavlink */
 };
 
 #endif

src/modules/navigator/module.mk

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -39,7 +39,9 @@ MODULE_COMMAND	= navigator
 
 SRCS		= navigator_main.cpp \
 		  navigator_params.c \
-		  navigator_mission.cpp \
+		  mission.cpp \
+		  rtl.cpp \
+		  rtl_params.c \
 		  mission_feasibility_checker.cpp \
 		  geofence.cpp \
 		  geofence_params.c

src/modules/navigator/navigator_params.c

@@ -52,16 +52,6 @@
  * Navigator parameters, accessible via MAVLink
  */
 
-/**
- * Minimum altitude (fixed wing only)
- *
- * Minimum altitude above home for LOITER.
- *
- * @unit meters
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
-
 /**
  * Waypoint acceptance radius
  *
@@ -101,37 +91,6 @@ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
  */
 PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f);
 
-/**
- * Landing altitude
- *
- * Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed.
- *
- * @unit meters
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f);
-
-/**
- * Return-To-Launch altitude
- *
- * Minimum altitude above home position for going home in RTL mode.
- *
- * @unit meters
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f);
-
-/**
- * Return-To-Launch delay
- *
- * Delay after descend before landing in RTL mode.
- * If set to -1 the system will not land but loiter at NAV_LAND_ALT.
- *
- * @unit seconds
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f);
-
 /**
  * Enable parachute deployment
  *

src/modules/navigator/navigator_state.h

@@ -1,21 +0,0 @@
-/*
- * navigator_state.h
- *
- *  Created on: 27.01.2014
- *      Author: ton
- */
-
-#ifndef NAVIGATOR_STATE_H_
-#define NAVIGATOR_STATE_H_
-
-typedef enum {
-	NAV_STATE_NONE = 0,
-	NAV_STATE_READY,
-	NAV_STATE_LOITER,
-	NAV_STATE_MISSION,
-	NAV_STATE_RTL,
-	NAV_STATE_LAND,
-	NAV_STATE_MAX
-} nav_state_t;
-
-#endif /* NAVIGATOR_STATE_H_ */

src/modules/navigator/rtl.cpp

@@ -0,0 +1,253 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file navigator_rtl.cpp
+ * Helper class to access RTL
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/home_position.h>
+
+#include "rtl.h"
+
+
+RTL::RTL() :
+	SuperBlock(NULL, "RTL"),
+	_mavlink_fd(-1),
+	_rtl_state(RTL_STATE_NONE),
+	_home_position({}),
+	_param_return_alt(this, "RETURN_ALT"),
+	_param_descend_alt(this, "DESCEND_ALT"),
+	_param_land_delay(this, "LAND_DELAY"),
+	_loiter_radius(50),
+	_acceptance_radius(50)
+{
+	/* load initial params */
+	updateParams();
+}
+
+RTL::~RTL()
+{
+}
+
+void
+RTL::set_home_position(const home_position_s *new_home_position)
+{
+	memcpy(&_home_position, new_home_position, sizeof(home_position_s));
+}
+
+bool
+RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item)
+{
+	/* Open mavlink fd */
+	if (_mavlink_fd < 0) {
+		/* try to open the mavlink log device every once in a while */
+		_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	}
+
+	/* decide if we need climb */
+	if (_rtl_state == RTL_STATE_NONE) {
+		if (global_position->alt < _home_position.alt + _param_return_alt.get()) {
+			_rtl_state = RTL_STATE_CLIMB;
+
+		} else {
+			_rtl_state = RTL_STATE_RETURN;
+		}
+	}
+
+	/* if switching directly to return state, set altitude setpoint to current altitude */
+	if (_rtl_state == RTL_STATE_RETURN) {
+		new_mission_item->altitude_is_relative = false;
+		new_mission_item->altitude = global_position->alt;
+	}
+
+	switch (_rtl_state) {
+	case RTL_STATE_CLIMB: {
+
+		float climb_alt = _home_position.alt + _param_return_alt.get();
+
+		/* TODO understand and fix this */
+		// if (_vstatus.condition_landed) {
+		// 	climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt);
+		// }
+
+		new_mission_item->lat = global_position->lat;
+		new_mission_item->lon = global_position->lon;
+		new_mission_item->altitude_is_relative = false;
+		new_mission_item->altitude = climb_alt;
+		new_mission_item->yaw = NAN;
+		new_mission_item->loiter_radius = _loiter_radius;
+		new_mission_item->loiter_direction = 1;
+		new_mission_item->nav_cmd = NAV_CMD_TAKEOFF;
+		new_mission_item->acceptance_radius = _acceptance_radius;
+		new_mission_item->time_inside = 0.0f;
+		new_mission_item->pitch_min = 0.0f;
+		new_mission_item->autocontinue = true;
+		new_mission_item->origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %d meters above home",
+			(int)(climb_alt - _home_position.alt));
+		break;
+	}
+	case RTL_STATE_RETURN: {
+
+		new_mission_item->lat = _home_position.lat;
+		new_mission_item->lon = _home_position.lon;
+
+		/* TODO: add this again */
+		// don't change altitude
+		// if (_pos_sp_triplet.previous.valid) {
+		// 	/* if previous setpoint is valid then use it to calculate heading to home */
+		// 	new_mission_item->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, new_mission_item->lat, new_mission_item->lon);
+
+		// } else {
+		// 	/* else use current position */
+		// 	new_mission_item->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, new_mission_item->lat, new_mission_item->lon);
+		// }
+		new_mission_item->loiter_radius = _loiter_radius;
+		new_mission_item->loiter_direction = 1;
+		new_mission_item->nav_cmd = NAV_CMD_WAYPOINT;
+		new_mission_item->acceptance_radius = _acceptance_radius;
+		new_mission_item->time_inside = 0.0f;
+		new_mission_item->pitch_min = 0.0f;
+		new_mission_item->autocontinue = true;
+		new_mission_item->origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %d meters above home",
+			(int)(new_mission_item->altitude - _home_position.alt));
+		break;
+	}
+
+	case RTL_STATE_DESCEND: {
+
+		new_mission_item->lat = _home_position.lat;
+		new_mission_item->lon = _home_position.lon;
+		new_mission_item->altitude_is_relative = false;
+		new_mission_item->altitude = _home_position.alt + _param_descend_alt.get();
+		new_mission_item->yaw = NAN;
+		new_mission_item->loiter_radius = _loiter_radius;
+		new_mission_item->loiter_direction = 1;
+		new_mission_item->nav_cmd = NAV_CMD_WAYPOINT;
+		new_mission_item->acceptance_radius = _acceptance_radius;
+		new_mission_item->time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
+		new_mission_item->pitch_min = 0.0f;
+		new_mission_item->autocontinue = _param_land_delay.get() > -0.001f;
+		new_mission_item->origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %d meters above home",
+			(int)(new_mission_item->altitude - _home_position.alt));
+		break;
+	}
+
+	case RTL_STATE_LAND: {
+
+		new_mission_item->lat = _home_position.lat;
+		new_mission_item->lon = _home_position.lon;
+		new_mission_item->altitude_is_relative = false;
+		new_mission_item->altitude = _home_position.alt;
+		new_mission_item->yaw = NAN;
+		new_mission_item->loiter_radius = _loiter_radius;
+		new_mission_item->loiter_direction = 1;
+		new_mission_item->nav_cmd = NAV_CMD_LAND;
+		new_mission_item->acceptance_radius = _acceptance_radius;
+		new_mission_item->time_inside = 0.0f;
+		new_mission_item->pitch_min = 0.0f;
+		new_mission_item->autocontinue = true;
+		new_mission_item->origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_mavlink_fd, "#audio: RTL: land at home");
+		break;
+	}
+
+	case RTL_STATE_FINISHED: {
+		/* nothing to do, report fail */
+		return false;
+	}
+
+	default:
+		return false;	
+	}
+
+	return true;
+}
+
+bool
+RTL::get_next_rtl_item(mission_item_s *new_mission_item)
+{
+	/* TODO implement */
+	return false;
+}
+
+void
+RTL::move_to_next()
+{
+	switch (_rtl_state) {
+	case RTL_STATE_CLIMB:
+		_rtl_state = RTL_STATE_RETURN;
+		break;
+	
+	case RTL_STATE_RETURN:
+		_rtl_state = RTL_STATE_DESCEND;
+		break;
+
+	case RTL_STATE_DESCEND:
+		/* only go to land if autoland is enabled */
+		if (_param_land_delay.get() < 0) {
+			_rtl_state = RTL_STATE_FINISHED;
+		} else {
+			_rtl_state = RTL_STATE_LAND;
+		}
+		break;
+
+	case RTL_STATE_LAND:
+		_rtl_state = RTL_STATE_FINISHED;
+		break;
+
+	case RTL_STATE_FINISHED:
+		break;
+
+	default:
+		break;
+	}
+}

src/modules/navigator/rtl.h

@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file navigator_mission.h
+ * Helper class to access missions
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef NAVIGATOR_RTL_H
+#define NAVIGATOR_RTL_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/topics/mission.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+
+class RTL : public control::SuperBlock
+{
+public:
+	/**
+	 * Constructor
+	 */
+	RTL();
+
+	/**
+	 * Destructor
+	 */
+	~RTL();
+
+	void		set_home_position(const home_position_s *home_position);
+
+	bool		get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item);
+	bool		get_next_rtl_item(mission_item_s *mission_item);
+
+	void		move_to_next();
+
+private:
+	int 		_mavlink_fd;
+
+	enum RTLState {
+		RTL_STATE_NONE = 0,
+		RTL_STATE_CLIMB,
+		RTL_STATE_RETURN,
+		RTL_STATE_DESCEND,
+		RTL_STATE_LAND,
+		RTL_STATE_FINISHED,
+	}		_rtl_state;
+
+	home_position_s _home_position;
+	float		_loiter_radius;
+	float		_acceptance_radius;
+
+
+	control::BlockParamFloat _param_return_alt;
+	control::BlockParamFloat _param_descend_alt;
+	control::BlockParamFloat _param_land_delay;
+};
+
+#endif

src/modules/navigator/rtl_params.c

@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rtl_params.c
+ *
+ * Parameters for RTL
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * RTL parameters, accessible via MAVLink
+ */
+
+/**
+ * RTL altitude
+ *
+ * Altitude to fly back in RTL in meters
+ *
+ * @unit meters
+ * @min 0
+ * @max 1
+ * @group RTL
+ */
+PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100);
+
+
+/**
+ * RTL loiter altitude
+ *
+ * Stay at this altitude above home position after RTL descending.
+ * Land (i.e. slowly descend) from this altitude if autolanding allowed.
+ *
+ * @unit meters
+ * @min 0
+ * @max 100
+ * @group RTL
+ */
+PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
+
+/**
+ * RTL delay
+ *
+ * Delay after descend before landing in RTL mode.
+ * If set to -1 the system will not land but loiter at NAV_LAND_ALT.
+ *
+ * @unit seconds
+ * @min -1
+ * @max
+ * @group RTL
+ */
+PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);