Changes in navigator module

16 files changed, 1578 insertions, 305 deletions

diff --git a/src/modules/navigator/abs_follow.cpp b/src/modules/navigator/abs_follow.cpp
new file mode 100644
index 000000000..cce2b712a
--- /dev/null
+++ b/src/modules/navigator/abs_follow.cpp
@@ -0,0 +1,257 @@
+/****************************************************************************
+ *
+ *   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 abs_follow.cpp
+ *
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Martins Frolovs <martins.f@airdog.com>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+
+#include "navigator.h"
+#include "abs_follow.h"
+
+AbsFollow::AbsFollow(Navigator *navigator, const char *name) :
+	MissionBlock(navigator, name)
+{
+    updateParameters();
+}
+
+AbsFollow::~AbsFollow()
+{
+}
+
+void
+AbsFollow::on_inactive()
+{
+}
+
+void
+AbsFollow::on_activation()
+{
+	updateParameters();
+
+	_afollow_offset.zero();
+	global_pos = _navigator->get_global_position();
+	target_pos = _navigator->get_target_position();
+	home_pos = _navigator->get_home_position();
+	pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	_init_alt = global_pos->alt;
+
+	float target_alt = target_pos->alt;
+
+	get_vector_to_next_waypoint_fast(target_pos->lat, target_pos->lon, global_pos->lat, global_pos->lon, &_afollow_offset.data[0], &_afollow_offset.data[1]);
+	_afollow_offset.data[2] = -(global_pos->alt - target_alt);
+
+	mavlink_log_info(_navigator->get_mavlink_fd(), "[abs_fol] Reset abs_follow offset: %.2f, %.2f, %.2f", (double)_afollow_offset(0), (double)_afollow_offset(1), (double)_afollow_offset(2));
+
+	point_camera_to_target(&(pos_sp_triplet->current));
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+void
+AbsFollow::on_active()
+{
+
+	/* Update position data pointer */
+	global_pos = _navigator->get_global_position();
+	home_pos = _navigator->get_home_position();
+	target_pos = _navigator->get_target_position();
+	pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	// Execute command if received
+	if ( update_vehicle_command() )
+			execute_vehicle_command();
+
+	_target_lat = target_pos->lat;
+	_target_lon = target_pos->lon;
+
+	double lat_new;
+	double lon_new;
+
+	/* add offset to target position */
+	add_vector_to_global_position(
+			_target_lat, _target_lon,
+			_afollow_offset(0), _afollow_offset(1),
+			&lat_new, &lon_new
+	);
+
+	pos_sp_triplet->current.valid = true;
+	pos_sp_triplet->current.type = SETPOINT_TYPE_MOVING;
+
+	pos_sp_triplet->current.lat = lat_new;
+	pos_sp_triplet->current.lon = lon_new;
+
+	if (_parameters.afol_rep_target_alt)
+		pos_sp_triplet->current.alt = target_pos->alt - _afollow_offset(2);
+	else
+		pos_sp_triplet->current.alt = _init_alt;
+
+
+	/* calculate direction to target */
+	pos_sp_triplet->current.yaw = get_bearing_to_next_waypoint(global_pos->lat, global_pos->lon, target_pos->lat, target_pos->lon);
+	pos_sp_triplet->current.pitch_min = 0.0f;
+
+	_navigator->set_position_setpoint_triplet_updated();
+
+}
+
+void
+AbsFollow::execute_vehicle_command() {
+
+    // Update _parameter values with the latest navigator_mode parameters
+    memcpy(&_parameters, &(NavigatorMode::_parameters), sizeof(_parameters));
+
+	vehicle_command_s cmd = _vcommand;
+
+	if (cmd.command == VEHICLE_CMD_NAV_REMOTE_CMD) {
+
+		REMOTE_CMD remote_cmd = (REMOTE_CMD)cmd.param1;
+
+		math::Vector<3> offset =_afollow_offset;
+
+		switch(remote_cmd){
+			case REMOTE_CMD_PLAY_PAUSE: {
+				commander_request_s *commander_request = _navigator->get_commander_request();
+				commander_request->request_type = V_MAIN_STATE_CHANGE;
+				commander_request->main_state = MAIN_STATE_AUTO_LOITER;
+				_navigator->set_commander_request_updated();
+				break;
+			}
+
+			case REMOTE_CMD_UP: {
+
+				_afollow_offset.data[2] -= _parameters.loi_step_len;
+				break;
+			}
+			case REMOTE_CMD_DOWN: {
+
+				_afollow_offset.data[2] += _parameters.loi_step_len;
+				break;
+			}
+			case REMOTE_CMD_LEFT: {
+
+				math::Matrix<3, 3> R_phi;
+
+				double radius = sqrt(offset(0) * offset(0) + offset(1) * offset(1));
+
+				// derived from formula: ( step / ( sqrt(x^2 + y^2)*2PI ) ) *  2PI
+				// radius: (sqrt(x^2 + y^2)
+				// circumference C: (radius * 2* PI)
+				// step length fraction of C: step/C
+				// angle of step fraction in radians: step/C * 2PI
+				double alpha = (double)_parameters.loi_step_len / radius;
+
+				// vector yaw rotation +alpha or -alpha depending on left or right
+				R_phi.from_euler(0.0f, 0.0f, alpha);
+				math::Vector<3> offset_new  = R_phi * offset;
+
+				_afollow_offset = offset_new;
+
+				break;
+			}
+			case REMOTE_CMD_RIGHT: {
+
+				math::Matrix<3, 3> R_phi;
+
+				double radius = sqrt(offset(0) * offset(0) + offset(1) * offset(1));
+
+				// derived from formula: ( step / ( sqrt(x^2 + y^2)*2PI ) ) *  2PI
+				// radius: (sqrt(x^2 + y^2)
+				// circumference C: (radius * 2* PI)
+				// step length fraction of C: step/C
+				// angle of step fraction in radians: step/C * 2PI
+				double alpha = (double)_parameters.loi_step_len / radius;
+
+				// vector yaw rotation +alpha or -alpha depending on left or right
+				R_phi.from_euler(0.0f, 0.0f, -alpha);
+				math::Vector<3> offset_new  = R_phi * offset;
+
+				_afollow_offset = offset_new;
+
+				break;
+			}
+			case REMOTE_CMD_CLOSER: {
+
+				// Calculate vector angle from target to device with atan2(y, x)
+				float alpha = atan2f(offset(1), offset(0));
+
+				// Create vector in the same direction, with loiter_step length
+				math::Vector<3> offset_delta(
+						cosf(alpha) * _parameters.loi_step_len,
+						sinf(alpha) * _parameters.loi_step_len,
+						0);
+
+				math::Vector<3> offset_new = offset - offset_delta;
+
+				_afollow_offset = offset_new;
+
+				break;
+
+			}
+
+			case REMOTE_CMD_FURTHER: {
+
+				// Calculate vector angle from target to device with atan2(y, x)
+				float alpha = atan2(offset(1), offset(0));
+
+				// Create vector in the same direction, with loiter_step length
+				math::Vector<3> offset_delta(
+						cosf(alpha) * _parameters.loi_step_len,
+						sinf(alpha) * _parameters.loi_step_len,
+						0);
+
+				math::Vector<3> offset_new = offset + offset_delta;
+
+				_afollow_offset = offset_new;
+
+				break;
+			}
+
+		}
+	}
+
+}
diff --git a/src/modules/navigator/abs_follow.h b/src/modules/navigator/abs_follow.h
new file mode 100644
index 000000000..47d4383f0
--- /dev/null
+++ b/src/modules/navigator/abs_follow.h
@@ -0,0 +1,78 @@
+/***************************************************************************
+ *
+ *   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 abs_follow.h
+ *
+ * @author Edgars Simanovskis <es@robotics.lv>
+ * @author Martins Frolovs <martins.f@airdog.com>
+ */
+
+#ifndef NAVIGATOR_ABS_FOLLOW_H
+#define NAVIGATOR_ABS_FOLLOW_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/uORB.h>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class AbsFollow : public MissionBlock
+{
+public:
+	AbsFollow(Navigator *navigator, const char *name);
+
+	~AbsFollow();
+
+	virtual void on_inactive();
+
+	virtual void on_activation();
+
+	virtual void on_active();
+
+	virtual void execute_vehicle_command();
+
+private:
+
+	math::Vector<3>  _afollow_offset;			/**< offset from target for AFOLLOW mode */
+	double	_target_lat;		/**< prediction for target latitude */
+	double	_target_lon;		/**< prediction for target longitude */
+	float	_target_alt;		/**< prediction for target altitude */
+	float 	_init_alt; 			/**< initial altitude on abs follow start > */
+	math::Vector<3>	_target_vel;	/**< target velocity vector */
+	hrt_abstime _t_prev;
+
+};
+
+#endif
diff --git a/src/modules/navigator/abs_follow_params.c b/src/modules/navigator/abs_follow_params.c
new file mode 100644
index 000000000..575df858e
--- /dev/null
+++ b/src/modules/navigator/abs_follow_params.c
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   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 abs_follow_params.c
+ *
+ * Parameter for absolute follow mode.
+ *
+ * @author Martins Frolovs <martins.f@airdog.com>
+ */
+
+#include <nuttx/config.h>
+#include <systemlib/param/param.h>
+
+
+/**
+ * 	Use camera pitch
+ *
+ * 	Used in Abs_follow mode
+ *
+ *	0: Camera pitch is still
+ *	1: Camera pitch follows target
+ *	@group Navigator
+ */
+PARAM_DEFINE_INT32(AFOL_USE_CAM_P, 1);
+
+
+/**
+ * 	Drone repeat target altitude changes
+ *
+ *	0: Drone don't repeat target's altitude change
+ *	1: Drone repeat target's altitude changes
+ *	@group Navigator
+ */
+PARAM_DEFINE_INT32(AFOL_REP_TALT, 1);
diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
index f827e70c9..46c81523f 100644
--- a/src/modules/navigator/loiter.cpp
+++ b/src/modules/navigator/loiter.cpp
@@ -37,6 +37,7 @@
  *
  * @author Julian Oes <julian@oes.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Martins Frolovs <martins.f@airdog.vom>
  */
 
 #include <string.h>
@@ -44,6 +45,7 @@
 #include <stdbool.h>
 #include <math.h>
 #include <fcntl.h>
+#include <stdio.h>
 
 #include <mavlink/mavlink_log.h>
 #include <systemlib/err.h>
@@ -57,8 +59,7 @@
 Loiter::Loiter(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name)
 {
-	/* load initial params */
-	updateParams();
+    updateParameters();
 }
 
 Loiter::~Loiter()
@@ -73,21 +74,471 @@ Loiter::on_inactive()
 void
 Loiter::on_activation()
 {
-	/* set current mission item to loiter */
-	set_loiter_item(&_mission_item);
+	updateParameters();
 
-	/* convert mission item to current setpoint */
-	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	// Determine current loiter sub mode
+	struct vehicle_status_s *vstatus = _navigator->get_vstatus();
+
+    pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	_mavlink_fd = _navigator->get_mavlink_fd();
+
+	if (vstatus->condition_landed) {
+		set_sub_mode(LOITER_SUB_MODE_LANDED);
+	}
+	else {
+		set_sub_mode(LOITER_SUB_MODE_AIM_AND_SHOOT);
+	}
+}
+
+void
+Loiter::on_active()
+{
+	target_pos = _navigator->get_target_position();
+	global_pos = _navigator->get_global_position();
+	pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+
+	if (loiter_sub_mode == LOITER_SUB_MODE_TAKING_OFF && check_current_pos_sp_reached()) {
+		set_sub_mode(LOITER_SUB_MODE_AIM_AND_SHOOT);
+	}
+
+	if (loiter_sub_mode == LOITER_SUB_MODE_LANDING && check_current_pos_sp_reached()) {
+		set_sub_mode(LOITER_SUB_MODE_LANDED);
+
+		disarm();
+	}
+
+	if (loiter_sub_mode == LOITER_SUB_MODE_COME_TO_ME && check_current_pos_sp_reached()) {
+		set_sub_mode(LOITER_SUB_MODE_AIM_AND_SHOOT);
+	}
+
+
+	if ( (loiter_sub_mode == LOITER_SUB_MODE_AIM_AND_SHOOT) && (target_pos->alt + _parameters.loi_min_alt > pos_sp_triplet->current.alt)){
+		pos_sp_triplet->current.alt = target_pos->alt + _parameters.loi_min_alt;
+	}
+
+	if ( update_vehicle_command() )
+			execute_vehicle_command();
+
+	if (loiter_sub_mode == LOITER_SUB_MODE_AIM_AND_SHOOT || loiter_sub_mode == LOITER_SUB_MODE_COME_TO_ME)
+		point_camera_to_target(&(pos_sp_triplet->current));
+
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+void
+Loiter::execute_vehicle_command()
+{
+    updateParams();
+
+	vehicle_command_s cmd = _vcommand;
+
+	switch (loiter_sub_mode){
+		case LOITER_SUB_MODE_LANDED:
+			execute_command_in_landed(cmd);
+			break;
+		case LOITER_SUB_MODE_AIM_AND_SHOOT:
+			execute_command_in_aim_and_shoot(cmd);
+			break;
+		case LOITER_SUB_MODE_LOOK_DOWN:
+			execute_command_in_look_down(cmd);
+			break;
+		case LOITER_SUB_MODE_COME_TO_ME:
+			execute_command_in_come_to_me(cmd);
+			break;
+		case LOITER_SUB_MODE_LANDING:
+			execute_command_in_landing(cmd);
+			break;
+		case LOITER_SUB_MODE_TAKING_OFF:
+			execute_command_in_taking_off(cmd);
+			break;
+	}
+
+}
+
+void
+Loiter::execute_command_in_landed(vehicle_command_s cmd){
+
+	if (cmd.command == VEHICLE_CMD_NAV_REMOTE_CMD) {
+
+		int remote_cmd = cmd.param1;
+
+		if (remote_cmd == REMOTE_CMD_TAKEOFF) {
+			set_sub_mode(LOITER_SUB_MODE_TAKING_OFF);
+			takeoff();
+		} else if (remote_cmd == REMOTE_CMD_LAND_DISARM) {
+			disarm();
+		}
+	}
+}
+
+void
+Loiter::execute_command_in_aim_and_shoot(vehicle_command_s cmd){
+
+
+
+	// Calculate offset
+	float offset_x;
+	float offset_y;
+	float offset_z = target_pos->alt - global_pos->alt;
+
+	get_vector_to_next_waypoint(
+			target_pos->lat,
+			target_pos->lon,
+			global_pos->lat,
+			global_pos->lon,
+			&offset_x,
+			&offset_y
+	);
+
+
+
+	math::Vector<3> offset(offset_x, offset_y, offset_z);
+
+
+	if (cmd.command == VEHICLE_CMD_DO_SET_MODE){
+
+		//uint8_t base_mode = (uint8_t)cmd.param1;
+		uint8_t main_mode = (uint8_t)cmd.param2;
+
+		if (main_mode == PX4_CUSTOM_SUB_MODE_AUTO_RTL) {
+			// Make request to COMMANDER do change state to RTL
+			commander_request_s *commander_request = _navigator->get_commander_request();
+			commander_request->request_type = V_MAIN_STATE_CHANGE;
+			commander_request->main_state = MAIN_STATE_AUTO_RTL;
+			_navigator->set_commander_request_updated();
+
+		}
+	}
+
+
+	if (cmd.command == VEHICLE_CMD_NAV_REMOTE_CMD) {
+
+		REMOTE_CMD remote_cmd = (REMOTE_CMD)cmd.param1;
+
+		pos_sp_triplet->previous.valid = false;
+		pos_sp_triplet->current.valid = true;
+		pos_sp_triplet->next.valid = false;
+
+		switch(remote_cmd){
+			case  REMOTE_CMD_LAND_DISARM: {
+				land();
+				set_sub_mode(LOITER_SUB_MODE_LANDING);
+				break;
+			}
+			case REMOTE_CMD_UP: {
+
+				pos_sp_triplet->current.alt = global_pos->alt + _parameters.loi_step_len;
+				pos_sp_triplet->current.lat = global_pos->lat;
+				pos_sp_triplet->current.lon = global_pos->lon;
+
+				pos_sp_triplet->current.type = SETPOINT_TYPE_POSITION;
+
+				break;
+			}
+			case REMOTE_CMD_DOWN: {
+
+				pos_sp_triplet->current.alt = global_pos->alt - _parameters.loi_step_len;
+				pos_sp_triplet->current.lat = global_pos->lat;
+				pos_sp_triplet->current.lon = global_pos->lon;
+
+				pos_sp_triplet->current.type = SETPOINT_TYPE_POSITION;
+				break;
+			}
+			case REMOTE_CMD_LEFT: {
+
+				math::Matrix<3, 3> R_phi;
+
+				double radius = sqrt(offset(0) * offset(0) + offset(1) * offset(1));
+
+				// derived from formula: ( step / ( sqrt(x^2 + y^2)*2PI ) ) *  2PI
+				// radius: (sqrt(x^2 + y^2)
+				// circumference C: (radius * 2* PI)
+				// step length fraction of C: step/C
+				// angle of step fraction in radians: step/C * 2PI
+				double alpha = (double)_parameters.loi_step_len / radius;
+
+				// vector yaw rotation +alpha or -alpha depending on left or right
+				R_phi.from_euler(0.0f, 0.0f, -alpha);
+				math::Vector<3> offset_new  = R_phi * offset;
+
+				double lat_new, lon_new;
+				add_vector_to_global_position(
+						(*target_pos).lat,
+						(*target_pos).lon,
+						offset_new(0),
+						offset_new(1),
+						&lat_new,
+						&lon_new
+				);
+
+				pos_sp_triplet->current.lat = lat_new;
+				pos_sp_triplet->current.lon = lon_new;
+				pos_sp_triplet->current.type = SETPOINT_TYPE_POSITION;
+
+				break;
+			}
+			case REMOTE_CMD_RIGHT: {
+
+				math::Matrix<3, 3> R_phi;
+
+				double radius = sqrt(offset(0) * offset(0) + offset(1) * offset(1));
+
+				// derived from formula: ( step / ( sqrt(x^2 + y^2)*2PI ) ) *  2PI
+				// radius: (sqrt(x^2 + y^2)
+				// circumference C: (radius * 2* PI)
+				// step length fraction of C: step/C
+				// angle of step fraction in radians: step/C * 2PI
+				double alpha = (double)_parameters.loi_step_len / radius;
+
+				// vector yaw rotation +alpha or -alpha depending on left or right
+				R_phi.from_euler(0.0f, 0.0f, +alpha);
+				math::Vector<3> offset_new  = R_phi * offset;
+
+				double lat_new, lon_new;
+				add_vector_to_global_position(
+						(*target_pos).lat,
+						(*target_pos).lon,
+						offset_new(0),
+						offset_new(1),
+						&lat_new,
+						&lon_new
+				);
+
+				pos_sp_triplet->current.lat = lat_new;
+				pos_sp_triplet->current.lon = lon_new;
+				pos_sp_triplet->current.type = SETPOINT_TYPE_POSITION;
+
+				break;
+			}
+			case REMOTE_CMD_CLOSER: {
+
+				// Calculate vector angle from target to device with atan2(y, x)
+				float alpha = atan2f(offset(1), offset(0));
+
+				// Create vector in the same direction, with loiter_step length
+				math::Vector<3> offset_delta(
+						cosf(alpha) * _parameters.loi_step_len,
+						sinf(alpha) * _parameters.loi_step_len,
+						0);
+
+				math::Vector<3> offset_new = offset - offset_delta;
+
+				double lat_new, lon_new;
+				add_vector_to_global_position(
+						(*target_pos).lat,
+						(*target_pos).lon,
+						offset_new(0),
+						offset_new(1),
+						&lat_new,
+						&lon_new
+				);
+
+				pos_sp_triplet->current.lat = lat_new;
+				pos_sp_triplet->current.lon = lon_new;
+				pos_sp_triplet->current.type = SETPOINT_TYPE_POSITION;
+
+				break;
+
+			}
+
+			case REMOTE_CMD_FURTHER: {
+
+				// Calculate vector angle from target to device with atan2(y, x)
+				float alpha = atan2(offset(1), offset(0));
+
+				// Create vector in the same direction, with loiter_step length
+				math::Vector<3> offset_delta(
+						cosf(alpha) * _parameters.loi_step_len,
+						sinf(alpha) * _parameters.loi_step_len,
+						0);
+
+				math::Vector<3> offset_new = offset + offset_delta;
+
+				double lat_new, lon_new;
+				add_vector_to_global_position(
+						target_pos->lat,
+						target_pos->lon,
+						offset_new(0),
+						offset_new(1),
+						&lat_new,
+						&lon_new
+				);
+
+				pos_sp_triplet->current.lat = lat_new;
+				pos_sp_triplet->current.lon = lon_new;
+				pos_sp_triplet->current.type = SETPOINT_TYPE_POSITION;
+
+				break;
+			}
+			case REMOTE_CMD_COME_TO_ME: {
+
+				pos_sp_triplet->current.lat = target_pos->lat;
+				pos_sp_triplet->current.lon = target_pos->lon;
+				pos_sp_triplet->current.type = SETPOINT_TYPE_POSITION;
+
+				set_sub_mode(LOITER_SUB_MODE_COME_TO_ME);
+
+				break;
+			}
+			case REMOTE_CMD_LOOK_DOWN: {
+
+				pos_sp_triplet->current.camera_pitch = -1.57f;
+				set_sub_mode(LOITER_SUB_MODE_LOOK_DOWN);
+				break;
+
+			}
+			case REMOTE_CMD_PLAY_PAUSE: {
+
+				commander_request_s *commander_request = _navigator->get_commander_request();
+				commander_request->request_type = V_MAIN_STATE_CHANGE;
+				commander_request->main_state = MAIN_STATE_AUTO_ABS_FOLLOW;
+				_navigator->set_commander_request_updated();
+
+				break;
+			}
+
+		}
+
+	}
+
+	_navigator->set_position_setpoint_triplet_updated();
+
+}
+
+void
+Loiter::execute_command_in_look_down(vehicle_command_s cmd){
+
+	if (cmd.command == VEHICLE_CMD_NAV_REMOTE_CMD) {
+		int remote_cmd = cmd.param1;
+		if (remote_cmd == REMOTE_CMD_PLAY_PAUSE) {
+			set_sub_mode(LOITER_SUB_MODE_AIM_AND_SHOOT);
+		}
+	}
+}
+
+void
+Loiter::execute_command_in_come_to_me(vehicle_command_s cmd){
+
+	if (cmd.command == VEHICLE_CMD_NAV_REMOTE_CMD) {
+		int remote_cmd = cmd.param1;
+		if (remote_cmd == REMOTE_CMD_PLAY_PAUSE) {
+			set_sub_mode(LOITER_SUB_MODE_AIM_AND_SHOOT);
+		}
+	}
+
+}
+
+void
+Loiter::execute_command_in_landing(vehicle_command_s cmd){
+
+	if (cmd.command == VEHICLE_CMD_NAV_REMOTE_CMD) {
+		int remote_cmd = cmd.param1;
+		if (remote_cmd == REMOTE_CMD_PLAY_PAUSE) {
+			set_sub_mode(LOITER_SUB_MODE_AIM_AND_SHOOT);
+			loiter_sub_mode = LOITER_SUB_MODE_AIM_AND_SHOOT;
+		}
+	}
+}
+
+void
+Loiter::set_sub_mode(LOITER_SUB_MODE new_sub_mode){
+
+	if (new_sub_mode == LOITER_SUB_MODE_AIM_AND_SHOOT) {
+
+		// Reset setpoint position to current global position
+		global_pos = _navigator->get_global_position();
+
+		pos_sp_triplet->previous.valid = false;
+		pos_sp_triplet->current.valid = true;
+		pos_sp_triplet->next.valid = false;
+
+		pos_sp_triplet->current.type = SETPOINT_TYPE_POSITION;
+		pos_sp_triplet->current.alt = global_pos->alt;
+		pos_sp_triplet->current.lon = global_pos->lon;
+		pos_sp_triplet->current.lat = global_pos->lat;
+
+		_navigator->set_position_setpoint_triplet_updated();
+
+	}
+
+	loiter_sub_mode = new_sub_mode;
+
+	char * sub_mode_str;
+
+	switch(new_sub_mode){
+		case LOITER_SUB_MODE_AIM_AND_SHOOT:
+			sub_mode_str = "Aim-and-shoot";
+			break;
+		case LOITER_SUB_MODE_LOOK_DOWN:
+			sub_mode_str = "Look down";
+			break;
+		case LOITER_SUB_MODE_COME_TO_ME:
+			sub_mode_str = "Come-to-me";
+			break;
+		case LOITER_SUB_MODE_LANDING:
+			sub_mode_str = "Landing";
+			break;
+		case LOITER_SUB_MODE_TAKING_OFF:
+			sub_mode_str = "Taking-off";
+			break;
+		case LOITER_SUB_MODE_LANDED:
+			sub_mode_str = "Landed";
+			break;
+	}
+
+	mavlink_log_info(_mavlink_fd, "[loiter] Loiter sub mode set to %s ! ", sub_mode_str);
+
+}
+
+void
+Loiter::execute_command_in_taking_off(vehicle_command_s cmd) {
+}
+
+void
+Loiter::takeoff()
+{
 	pos_sp_triplet->previous.valid = false;
-	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	pos_sp_triplet->current.valid = true;
 	pos_sp_triplet->next.valid = false;
 
-	_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
+	pos_sp_triplet->current.lat = global_pos->lat;
+	pos_sp_triplet->current.lon = global_pos->lon;
+	pos_sp_triplet->current.alt = global_pos->alt + _parameters.takeoff_alt;
+
+    int toa = (int)_parameters.takeoff_alt;
+
+    mavlink_log_info(_navigator->get_mavlink_fd(), "Use takeoff alt: %d", toa);
+
+	pos_sp_triplet->current.yaw = NAN;
+	pos_sp_triplet->current.type = SETPOINT_TYPE_TAKEOFF;
 
 	_navigator->set_position_setpoint_triplet_updated();
+
 }
 
 void
-Loiter::on_active()
+Loiter::land()
+{
+
+	pos_sp_triplet->previous.valid = false;
+	pos_sp_triplet->current.valid = true;
+	pos_sp_triplet->next.valid = false;
+
+	pos_sp_triplet->current.lat = global_pos->lat;
+	pos_sp_triplet->current.lon = global_pos->lon;
+	pos_sp_triplet->current.alt = global_pos->alt;
+	pos_sp_triplet->current.yaw = NAN;
+	pos_sp_triplet->current.type = SETPOINT_TYPE_LAND;
+
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+void
+Loiter::disarm()
 {
+	commander_request_s *commander_request = _navigator->get_commander_request();
+	commander_request->request_type = V_DISARM;
+	_navigator->set_commander_request_updated();
 }
diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h
index 37ab57a07..2dcb4c39b 100644
--- a/src/modules/navigator/loiter.h
+++ b/src/modules/navigator/loiter.h
@@ -41,8 +41,11 @@
 #ifndef NAVIGATOR_LOITER_H
 #define NAVIGATOR_LOITER_H
 
+#include <mathlib/mathlib.h>
+#include <geo/geo.h>
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
+#include <uORB/topics/vehicle_command.h>
 
 #include "navigator_mode.h"
 #include "mission_block.h"
@@ -59,6 +62,35 @@ public:
 	virtual void on_activation();
 
 	virtual void on_active();
+
+	virtual void execute_vehicle_command();
+
+private:
+
+	void		takeoff();
+	void 		land();
+	void		disarm();
+
+	enum LOITER_SUB_MODE {
+		LOITER_SUB_MODE_LANDED = 0, 		// vehicle on ground
+		LOITER_SUB_MODE_AIM_AND_SHOOT,		// aim and shoot
+		LOITER_SUB_MODE_LOOK_DOWN, 			// look down
+		LOITER_SUB_MODE_COME_TO_ME, 		// come to me
+		LOITER_SUB_MODE_LANDING, 			// vehicle is landing
+		LOITER_SUB_MODE_TAKING_OFF,			// vehicle is taking off
+
+	} loiter_sub_mode;
+
+	void execute_command_in_landed(vehicle_command_s cmd);
+	void execute_command_in_aim_and_shoot(vehicle_command_s cmd);
+	void execute_command_in_look_down(vehicle_command_s cmd);
+	void execute_command_in_come_to_me(vehicle_command_s cmd);
+	void execute_command_in_landing(vehicle_command_s cmd);
+	void execute_command_in_taking_off(vehicle_command_s cmd);
+
+	void set_sub_mode(LOITER_SUB_MODE new_sub_mode);
+
+	bool flag_sub_mode_goal_reached;
 };
 
 #endif
diff --git a/src/modules/navigator/loiter_params.c b/src/modules/navigator/loiter_params.c
new file mode 100644
index 000000000..5c53c69d0
--- /dev/null
+++ b/src/modules/navigator/loiter_params.c
@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   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 loter_params.c
+ *
+ * Parameter for loter.
+ *
+ * @author Martins Frolovs <martins.f@airdog.com>
+ */
+
+#include <nuttx/config.h>
+#include <systemlib/param/param.h>
+
+/**
+ * Step length for one movement command
+ *
+ * @unit meters
+ * @min 1
+ * @group Navigator
+ */
+PARAM_DEFINE_FLOAT(LOI_STEP_LEN, 5.00f);
+
+
+/**
+ * Minimum altitude above target
+ *
+ * @unit meters
+ * @group Navigator
+ */
+PARAM_DEFINE_FLOAT(LOI_MIN_ALT, 2.00f);
+
+
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index c44d4c35e..429f7cd22 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -40,10 +40,12 @@ MODULE_COMMAND	= navigator
 SRCS		= navigator_main.cpp \
 		  navigator_params.c \
 		  navigator_mode.cpp \
+		  navigator_mode_params.c \
 		  mission_block.cpp \
 		  mission.cpp \
 		  mission_params.c \
 		  loiter.cpp \
+		  loiter_params.c \
 		  rtl.cpp \
 		  rtl_params.c \
 		  mission_feasibility_checker.cpp \
@@ -55,7 +57,9 @@ SRCS		= navigator_main.cpp \
 		  rcloss_params.c \
 		  enginefailure.cpp \
 		  gpsfailure.cpp \
-		  gpsfailure_params.c
+		  gpsfailure_params.c \
+		  abs_follow.cpp \
+		  abs_follow_params.c 
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index d550dcc4c..f1c2d7caf 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -55,6 +55,10 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/target_global_position.h>
+#include <uORB/topics/commander_request.h>
+
+#include <commander/px4_custom_mode.h>
 
 #include "navigator_mode.h"
 #include "mission.h"
@@ -65,9 +69,11 @@
 #include "gpsfailure.h"
 #include "rcloss.h"
 #include "geofence.h"
+#include "abs_follow.h"
 
 /**
  * Number of navigation modes that need on_active/on_inactive calls
+ * Currently: mission, loiter, rtl, offboard, abs_follow
  */
 #define NAVIGATOR_MODE_ARRAY_SIZE 7
 
@@ -122,6 +128,7 @@ public:
 	 */
 	void		set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
 	void		set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; }
+	void		set_commander_request_updated() { _commander_request_updated = true; }
 
 	/**
 	 * Getters
@@ -136,8 +143,11 @@ public:
 	struct mission_result_s*	    get_mission_result() { return &_mission_result; }
 	struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; }
 
+	struct commander_request_s*	get_commander_request() { return &_commander_request;};
+	struct target_global_position_s*    get_target_position() {return &_target_pos; }
 	int		get_onboard_mission_sub() { return _onboard_mission_sub; }
 	int		get_offboard_mission_sub() { return _offboard_mission_sub; }
+	int		get_vehicle_command_sub() { return _vcommand_sub; }
 	Geofence&	get_geofence() { return _geofence; }
 	bool		get_can_loiter_at_sp() { return _can_loiter_at_sp; }
 	float		get_loiter_radius() { return _param_loiter_radius.get(); }
@@ -161,12 +171,16 @@ private:
 	int		_onboard_mission_sub;		/**< onboard mission subscription */
 	int		_offboard_mission_sub;		/**< offboard mission subscription */
 	int		_param_update_sub;		/**< param update subscription */
+	int 	_vcommand_sub;			/**< vehicle control subscription */
+	int 	_target_pos_sub; 		/**< target position subscription */
+
 
 	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
 	orb_advert_t	_mission_result_pub;
 	orb_advert_t	_att_sp_pub;			/**< publish att sp
 							  used only in very special failsafe modes
 							  when pos control is deactivated */
+	orb_advert_t 	_commander_request_pub; 		/**< publish commander requests */
 
 	vehicle_status_s				_vstatus;		/**< vehicle status */
 	vehicle_control_mode_s				_control_mode;		/**< vehicle control mode */
@@ -180,6 +194,8 @@ private:
 
 	mission_result_s				_mission_result;
 	vehicle_attitude_setpoint_s					_att_sp;
+	target_global_position_s 			_target_pos;		/**< global target position */
+	commander_request_s					_commander_request;
 
 	bool 		_mission_item_valid;		/**< flags if the current mission item is valid */
 
@@ -200,11 +216,13 @@ private:
 	EngineFailure	_engineFailure;			/**< class that handles the engine failure mode
 							  (FW only!) */
 	GpsFailure	_gpsFailure;			/**< class that handles the OBC gpsfailure loss mode */
+	AbsFollow 	_abs_follow;			/**< class that handles AFollow */
 
 	NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE];	/**< array of navigation modes */
 
 	bool		_can_loiter_at_sp;			/**< flags if current position SP can be used to loiter */
 	bool		_pos_sp_triplet_updated;		/**< flags if position SP triplet needs to be published */
+	bool		_commander_request_updated;		/**< flags if commander request needs to be published */
 
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
 	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
@@ -249,6 +267,11 @@ private:
 	 * Update parameters
 	 */
 	void		params_update();
+	
+	/**
+	 * Retrieve target global position
+	 */
+	void		target_position_update();
 
 	/**
 	 * Shim for calling task_main from task_create.
@@ -270,6 +293,11 @@ private:
 	 */
 	void		publish_position_setpoint_triplet();
 
+	/**
+	 * Publish requests for commander
+	 */
+	void 		publish_commander_request();
+
 	/* this class has ptr data members, so it should not be copied,
 	 * consequently the copy constructors are private.
 	 */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index def2af424..7fc4d3a1d 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -108,9 +108,12 @@ Navigator::Navigator() :
 	_onboard_mission_sub(-1),
 	_offboard_mission_sub(-1),
 	_param_update_sub(-1),
+	_vcommand_sub(-1),
+	_target_pos_sub(-1),
 	_pos_sp_triplet_pub(-1),
 	_mission_result_pub(-1),
 	_att_sp_pub(-1),
+	_commander_request_pub(-1),
 	_vstatus{},
 	_control_mode{},
 	_global_pos{},
@@ -135,8 +138,10 @@ Navigator::Navigator() :
 	_dataLinkLoss(this, "DLL"),
 	_engineFailure(this, "EF"),
 	_gpsFailure(this, "GPSF"),
+	_abs_follow(this, "FOL"),
 	_can_loiter_at_sp(false),
 	_pos_sp_triplet_updated(false),
+	_commander_request_updated(false),
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_param_acceptance_radius(this, "ACC_RAD"),
 	_param_datalinkloss_obc(this, "DLL_OBC"),
@@ -150,6 +155,7 @@ Navigator::Navigator() :
 	_navigation_mode_array[4] = &_engineFailure;
 	_navigation_mode_array[5] = &_gpsFailure;
 	_navigation_mode_array[6] = &_rcLoss;
+	_navigation_mode_array[4] = &_abs_follow;
 
 	updateParams();
 }
@@ -236,6 +242,12 @@ Navigator::params_update()
 }
 
 void
+Navigator::target_position_update()
+{
+	orb_copy(ORB_ID(target_global_position), _target_pos_sub, &_target_pos);
+}
+
+void
 Navigator::task_main_trampoline(int argc, char *argv[])
 {
 	navigator::g_navigator->task_main();
@@ -278,6 +290,8 @@ Navigator::task_main()
 	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
 	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
 	_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
+	_vcommand_sub = orb_subscribe(ORB_ID(vehicle_command));
+	_target_pos_sub = orb_subscribe(ORB_ID(target_global_position));
 
 	/* copy all topics first time */
 	vehicle_status_update();
@@ -288,6 +302,7 @@ Navigator::task_main()
 	home_position_update();
 	navigation_capabilities_update();
 	params_update();
+	target_position_update();
 
 	/* rate limit position updates to 50 Hz */
 	orb_set_interval(_global_pos_sub, 20);
@@ -296,7 +311,7 @@ Navigator::task_main()
 	const hrt_abstime mavlink_open_interval = 500000;
 
 	/* wakeup source(s) */
-	struct pollfd fds[8];
+	struct pollfd fds[9];
 
 	/* Setup of loop */
 	fds[0].fd = _global_pos_sub;
@@ -315,6 +330,8 @@ Navigator::task_main()
 	fds[6].events = POLLIN;
 	fds[7].fd = _gps_pos_sub;
 	fds[7].events = POLLIN;
+	fds[8].fd = _target_pos_sub;
+	fds[8].events = POLLIN;
 
 	while (!_task_should_exit) {
 
@@ -339,6 +356,11 @@ Navigator::task_main()
 			_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 		}
 
+		/* target position updated */
+		if (fds[8].revents & POLLIN) {
+			target_position_update();
+		}
+
 		static bool have_geofence_position_data = false;
 
 		/* gps updated */
@@ -354,10 +376,13 @@ Navigator::task_main()
 			sensor_combined_update();
 		}
 
+		bool parameters_updated = false;
+
 		/* parameters updated */
 		if (fds[5].revents & POLLIN) {
 			params_update();
 			updateParams();
+			parameters_updated = true;
 		}
 
 		/* vehicle control mode updated */
@@ -454,6 +479,9 @@ Navigator::task_main()
 			case NAVIGATION_STATE_AUTO_LANDENGFAIL:
 				_navigation_mode = &_engineFailure;
 				break;
+			case NAVIGATION_STATE_AUTO_ABS_FOLLOW:
+				_navigation_mode = &_abs_follow; /* TODO: change this to something else */
+				break;
 			case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
 				_navigation_mode = &_gpsFailure;
 				break;
@@ -465,7 +493,7 @@ Navigator::task_main()
 
 		/* iterate through navigation modes and set active/inactive for each */
 		for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
-			_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
+			_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i], parameters_updated);
 		}
 
 		/* if nothing is running, set position setpoint triplet invalid */
@@ -482,6 +510,11 @@ Navigator::task_main()
 			_pos_sp_triplet_updated = false;
 		}
 
+		if (_commander_request_updated) {
+			publish_commander_request();
+			_commander_request_updated = false;
+		}
+
 		perf_end(_loop_perf);
 	}
 	warnx("exiting.");
@@ -553,6 +586,18 @@ Navigator::publish_position_setpoint_triplet()
 	}
 }
 
+void
+Navigator::publish_commander_request()
+{
+	/* publish commander request only once available */
+	if (_commander_request_pub > 0) {
+		orb_publish(ORB_ID(commander_request), _commander_request_pub, &_commander_request);
+
+	} else {
+		_commander_request_pub = orb_advertise(ORB_ID(commander_request), &_commander_request);
+	}
+}
+
 void Navigator::add_fence_point(int argc, char *argv[])
 {
 	_geofence.addPoint(argc, argv);
@@ -563,7 +608,6 @@ void Navigator::load_fence_from_file(const char *filename)
 	_geofence.loadFromFile(filename);
 }
 
-
 static void usage()
 {
 	errx(1, "usage: navigator {start|stop|status|fence|fencefile}");
diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp
index 3807c5ea8..b86e03894 100644
--- a/src/modules/navigator/navigator_mode.cpp
+++ b/src/modules/navigator/navigator_mode.cpp
@@ -37,20 +37,53 @@
  *
  * @author Julian Oes <julian@oes.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Martins Frolovs <martins.f@airdog.com>
  */
 
 #include "navigator_mode.h"
 #include "navigator.h"
 
+#include <uORB/uORB.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/parameter_update.h>
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+
+#include <drivers/device/device.h>
+#include <drivers/drv_hrt.h>
+#include <arch/board/board.h>
+
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+#include <geo/geo.h>
+#include <dataman/dataman.h>
+#include <mathlib/mathlib.h>
+#include <mavlink/mavlink_log.h>
+
+
 NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
 	SuperBlock(navigator, name),
 	_navigator(navigator),
 	_first_run(true)
 {
-	/* load initial params */
 	updateParams();
-	/* set initial mission items */
 	on_inactive();
+
+	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
 }
 
 NavigatorMode::~NavigatorMode()
@@ -58,7 +91,60 @@ NavigatorMode::~NavigatorMode()
 }
 
 void
-NavigatorMode::run(bool active) {
+NavigatorMode::updateParameters() {
+
+	updateParamHandles();
+	updateParamValues();
+
+}
+
+void
+NavigatorMode::updateParamHandles() {
+
+	_parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT");
+	_parameter_handles.takeoff_acceptance_radius = param_find("NAV_TAKEOFF_ACR");
+	_parameter_handles.acceptance_radius = param_find("NAV_ACC_RAD");
+	_parameter_handles.velocity_lpf = param_find("NAV_VEL_LPF");
+
+	_parameter_handles.afol_use_cam_pitch = param_find("AFOL_USE_CAM_P");
+	_parameter_handles.afol_rep_target_alt = param_find("AFOL_REP_TALT");
+
+	_parameter_handles.loi_min_alt = param_find("LOI_MIN_ALT");
+	_parameter_handles.loi_step_len = param_find("LOI_STEP_LEN");
+
+	_parameter_handles.rtl_ret_alt = param_find("RTL_RET_ALT");
+}
+
+void
+NavigatorMode::updateParamValues() {
+
+	param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt));
+	param_get(_parameter_handles.takeoff_acceptance_radius, &(_parameters.takeoff_acceptance_radius));
+	param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius));
+	param_get(_parameter_handles.velocity_lpf, &(_parameters.velocity_lpf));
+
+	param_get(_parameter_handles.afol_use_cam_pitch, &(_parameters.afol_use_cam_pitch));
+	param_get(_parameter_handles.afol_rep_target_alt, &(_parameters.afol_rep_target_alt));
+
+	param_get(_parameter_handles.loi_min_alt, &(_parameters.loi_min_alt));
+	param_get(_parameter_handles.loi_step_len, &(_parameters.loi_step_len));
+
+	param_get(_parameter_handles.rtl_ret_alt, &(_parameters.rtl_ret_alt));
+
+    int toa = (int)_parameters.takeoff_alt;
+    mavlink_log_info(_navigator->get_mavlink_fd(), "Takeoff alt now is %d", toa);
+
+}
+
+
+void
+NavigatorMode::run(bool active, bool parameters_updated) {
+
+
+    if (parameters_updated) {
+        updateParameters();    
+    }
+
 	if (active) {
 		if (_first_run) {
 			/* first run */
@@ -96,3 +182,102 @@ void
 NavigatorMode::on_active()
 {
 }
+
+bool
+NavigatorMode::update_vehicle_command()
+{
+	bool vcommand_updated = false;
+	orb_check(_navigator->get_vehicle_command_sub(), &vcommand_updated);
+
+	if (vcommand_updated) {
+
+		if (orb_copy(ORB_ID(vehicle_command), _navigator->get_vehicle_command_sub(), &_vcommand) == OK) {
+			return true;
+		}
+		else
+			return false;
+	}
+
+	return false;
+}
+
+void
+NavigatorMode::execute_vehicle_command()
+{
+}
+
+void
+NavigatorMode::point_camera_to_target(position_setpoint_s *sp)
+{
+	target_pos = _navigator->get_target_position();
+	global_pos = _navigator->get_global_position();
+
+	// Calculate offset values for later use.
+	float offset_x;
+	float offset_y;
+	float offset_z = target_pos->alt - global_pos->alt;
+
+	get_vector_to_next_waypoint(
+			target_pos->lat,
+			target_pos->lon,
+			global_pos->lat,
+			global_pos->lon,
+			&offset_x,
+			&offset_y
+	);
+
+	math::Vector<3> offset(offset_x, offset_y, offset_z);
+	math::Vector<2> offset_xy(offset_x, offset_y);
+
+	float offset_xy_len = offset_xy.length();
+
+	if (offset_xy_len > 2.0f)
+		sp->yaw = _wrap_pi(atan2f(-offset_xy(1), -offset_xy(0)));
+
+	sp->camera_pitch = atan2f(offset(2), offset_xy_len);
+}
+
+bool
+NavigatorMode::check_current_pos_sp_reached()
+{
+	struct vehicle_status_s *vstatus = _navigator->get_vstatus();
+	struct position_setpoint_triplet_s 	*pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	struct vehicle_global_position_s 	*global_pos = _navigator->get_global_position();;
+
+	switch (pos_sp_triplet->current.type)
+	{
+	case SETPOINT_TYPE_IDLE:
+		return true;
+		break;
+
+	case SETPOINT_TYPE_LAND:
+		return vstatus->condition_landed;
+		break;
+
+	case SETPOINT_TYPE_TAKEOFF:
+	{
+		float alt_diff = fabs(pos_sp_triplet->current.alt - global_pos->alt);
+		return _parameters.takeoff_acceptance_radius >= alt_diff;
+		break;
+	}
+	case SETPOINT_TYPE_POSITION:
+	{
+		float dist_xy = -1;
+		float dist_z = -1;
+
+		float distance = get_distance_to_point_global_wgs84(
+			global_pos->lat, global_pos->lon, global_pos->alt,
+			pos_sp_triplet->current.lat, pos_sp_triplet->current.lon, pos_sp_triplet->current.alt,
+			&dist_xy, &dist_z
+		);
+
+		return _parameters.acceptance_radius >= distance;
+
+		break;
+	}
+	default:
+		return false;
+		break;
+
+	}
+}
diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h
index de5545dcb..c9fa738db 100644
--- a/src/modules/navigator/navigator_mode.h
+++ b/src/modules/navigator/navigator_mode.h
@@ -50,6 +50,7 @@
 #include <dataman/dataman.h>
 
 #include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_command.h>
 
 class Navigator;
 
@@ -66,7 +67,7 @@ public:
 	 */
 	virtual ~NavigatorMode();
 
-	void run(bool active);
+	void run(bool active, bool parameters_updated);
 
 	/**
 	 * This function is called while the mode is inactive
@@ -83,17 +84,88 @@ public:
 	 */
 	virtual void on_active();
 
+	/*
+	 * This function defines how vehicle reacts on commands in
+	 * current navigator mode
+	 */
+	virtual void execute_vehicle_command();
+
+	/*
+	 * Check if vehicle command subscription has been updated
+	 * if it has it updates _vcommand structure and returns true
+	 */
+	bool update_vehicle_command();
+
+	void point_camera_to_target(position_setpoint_s *sp);
+
+	void updateParameters();
+	void updateParamValues();
+	void updateParamHandles();
+
+	struct {
+		float takeoff_alt;
+		float takeoff_acceptance_radius;
+		float acceptance_radius;
+		float loiter_step;
+		float velocity_lpf;
+
+		int afol_rep_target_alt;
+		int afol_use_cam_pitch;
+
+		float loi_step_len;
+		float loi_min_alt;
+
+		float rtl_ret_alt;
+
+	} _parameters;		
+
+
+	struct {
+		param_t takeoff_alt;
+		param_t takeoff_acceptance_radius;
+		param_t acceptance_radius;
+		param_t velocity_lpf;
+
+		param_t afol_rep_target_alt;
+		param_t afol_use_cam_pitch;
+
+		param_t loi_step_len;
+		param_t loi_min_alt;
+
+		param_t rtl_ret_alt;
+
+	} _parameter_handles;
+
+
+
 protected:
 	Navigator *_navigator;
 
+	struct position_setpoint_triplet_s 	*pos_sp_triplet;
+
+	struct target_global_position_s 	*target_pos;
+	struct vehicle_global_position_s 	*global_pos;
+	struct home_position_s 				*home_pos;
+
+	struct vehicle_command_s _vcommand;
+
+	int		_mavlink_fd;			/**< the file descriptor to send messages over mavlink */
+
+	bool check_current_pos_sp_reached();
+
+
+
 private:
+
 	bool _first_run;
 
-	/* this class has ptr data members, so it should not be copied,
+	/*
+	 * This class has ptr data members, so it should not be copied,
 	 * consequently the copy constructors are private.
 	 */
 	NavigatorMode(const NavigatorMode&);
 	NavigatorMode operator=(const NavigatorMode&);
+
 };
 
 #endif
diff --git a/src/modules/navigator/navigator_mode_params.c b/src/modules/navigator/navigator_mode_params.c
new file mode 100644
index 000000000..46e1a22ce
--- /dev/null
+++ b/src/modules/navigator/navigator_mode_params.c
@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   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 navigator_mode_params.c
+ *
+ * Parameter for all navigator modes.
+ *
+ * @author Martins Frolovs <martins.f@airdog.com>
+ */
+
+#include <nuttx/config.h>
+#include <systemlib/param/param.h>
+
+/*
+ * Navigator mode parameters
+ */
+
+/**
+ * Take-off altitude
+ *
+ *	Altitude to which vehicle will take-off.
+ *
+ * @unit meters
+ * @min 0
+ * @group Navigator
+ */
+PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f);
+
+
+/**
+ * Step size for one commend in loiter navigator mode
+ *
+ * Altitude to fly back in RTL in meters
+ *
+ * @unit meters
+ * @min 1
+ * @group Navigator
+ */
+PARAM_DEFINE_FLOAT(LOI_STEP_LEN, 5.00f);
+
+/**
+ *	Velocity LPF.
+ *
+ *	Low pass filter. Time in seconds to average target movements.
+ *
+ *	@unit seconds
+ *	@group Navigator
+ */
+PARAM_DEFINE_FLOAT(NAV_VEL_LPF, 0.3f);
+
+/**
+ * Acceptance radius to determine if setpoint have been reached
+ *
+ * @unit meters
+ * @min 1
+ * @max 50
+ * @group Navigator
+ */
+PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 2.00f);
+
+/**
+ * Minimum altitude above target in loiter.
+ *
+ * @unit meters
+ * @group Navigator
+ */
+PARAM_DEFINE_FLOAT(LOI_MIN_ALT, 2.00f);
+
+/**
+ * Acceptance radius for takeoff setpoint
+ *
+ * @unit meters
+ * @group Navigator
+ */
+PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ACR, 2.00f);
+
+
+
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 1f40e634e..025fcbb0d 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -55,16 +55,6 @@
  */
 PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
 
-/**
- * Acceptance Radius
- *
- * Default acceptance radius, overridden by acceptance radius of waypoint if set.
- *
- * @unit meters
- * @min 1.0
- * @group Mission
- */
-PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f);
 
 /**
  * Set OBC mode for data link loss
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index b6c4b8fdf..1e7689200 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -35,6 +35,7 @@
  * Helper class to access RTL
  * @author Julian Oes <julian@oes.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Martins Frolovs <martins.f@airdog.com>
  */
 
 #include <string.h>
@@ -49,23 +50,17 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/home_position.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 
 #include "navigator.h"
 #include "rtl.h"
 
-#define DELAY_SIGMA	0.01f
 
 RTL::RTL(Navigator *navigator, const char *name) :
-	MissionBlock(navigator, name),
-	_rtl_state(RTL_STATE_NONE),
-	_param_return_alt(this, "RTL_RETURN_ALT", false),
-	_param_descend_alt(this, "RTL_DESCEND_ALT", false),
-	_param_land_delay(this, "RTL_LAND_DELAY", false)
+	MissionBlock(navigator, name)
 {
-	/* load initial params */
-	updateParams();
-	/* initial reset */
-	on_inactive();
+    updateParameters();
+	rtl_state = RTL_STATE_NONE;
 }
 
 RTL::~RTL()
@@ -75,243 +70,186 @@ RTL::~RTL()
 void
 RTL::on_inactive()
 {
-	/* reset RTL state only if setpoint moved */
-	if (!_navigator->get_can_loiter_at_sp()) {
-		_rtl_state = RTL_STATE_NONE;
-	}
 }
 
 void
 RTL::on_activation()
 {
-	/* decide where to enter the RTL procedure when we switch into it */
-	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_LANDED;
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
-
-		/* if lower than return altitude, climb up first */
-		} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
-			   + _param_return_alt.get()) {
-			_rtl_state = RTL_STATE_CLIMB;
-
-		/* otherwise go straight to return */
-		} else {
-			/* set altitude setpoint to current altitude */
-			_rtl_state = RTL_STATE_RETURN;
-			_mission_item.altitude_is_relative = false;
-			_mission_item.altitude = _navigator->get_global_position()->alt;
-		}
+	updateParameters();
+
+	global_pos =  _navigator->get_global_position();
+	home_pos = _navigator->get_home_position();
+	pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	first_rtl_setpoint_set = false;
+
+	float accaptance_radius = _parameters.acceptance_radius;
+
+	float xy_distance = get_distance_to_next_waypoint(
+			global_pos->lat, global_pos->lon,
+			home_pos->lat, home_pos->lon
+	);
+
+
+	/* Determine and set current RTL state */
+
+	/* Vehicle have already landed */
+	if (_navigator->get_vstatus()->condition_landed) {
+		rtl_state = RTL_STATE_LANDED;
+	/* Already home we only need to land */
+	} else if (xy_distance <= accaptance_radius) {
+		rtl_state = RTL_STATE_LAND;
+		/* No need climb */
+	} else if ( global_pos->alt >= home_pos->alt + _parameters.rtl_ret_alt) {
+		rtl_state = RTL_STATE_RETURN;
+	} else {
+		rtl_state = RTL_STATE_CLIMB;
 	}
 
-	set_rtl_item();
+	set_rtl_setpoint();
+
 }
 
 void
 RTL::on_active()
 {
-	if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
-		advance_rtl();
-		set_rtl_item();
+
+	mv_fd = _navigator->get_mavlink_fd();
+	global_pos =  _navigator->get_global_position();
+	home_pos = _navigator->get_home_position();
+	pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	if ( update_vehicle_command() )
+			execute_vehicle_command();
+
+	if (rtl_state != RTL_STATE_LANDED ) {
+
+		if (!first_rtl_setpoint_set) {
+			set_rtl_setpoint();
+			first_rtl_setpoint_set = true;
+		} else if (check_current_pos_sp_reached()) {
+			set_next_rtl_state();
+			set_rtl_setpoint();
+		}
 	}
+
 }
 
 void
-RTL::set_rtl_item()
+RTL::execute_vehicle_command()
 {
-	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-
-	/* make sure we have the latest params */
-	updateParams();
-
-	set_previous_pos_setpoint();
-	_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;
-		_mission_item.lon = _navigator->get_global_position()->lon;
-		_mission_item.altitude_is_relative = false;
-		_mission_item.altitude = climb_alt;
-		_mission_item.yaw = NAN;
-		_mission_item.loiter_radius = _navigator->get_loiter_radius();
-		_mission_item.loiter_direction = 1;
-		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-		_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: climb to %d meters above home",
-			(int)(climb_alt - _navigator->get_home_position()->alt));
-		break;
-	}
+    // Update _parameter values with the latest navigator_mode parameters
 
-	case RTL_STATE_RETURN: {
-		_mission_item.lat = _navigator->get_home_position()->lat;
-		_mission_item.lon = _navigator->get_home_position()->lon;
-		 // don't change altitude
-
-		 if (pos_sp_triplet->previous.valid) {
-		 	/* if previous setpoint is valid then use it to calculate heading to home */
-		 	_mission_item.yaw = get_bearing_to_next_waypoint(
-		 	        pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
-		 	        _mission_item.lat, _mission_item.lon);
-
-		 } else {
-		 	/* else use current position */
-		 	_mission_item.yaw = get_bearing_to_next_waypoint(
-		 	        _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
-		 	        _mission_item.lat, _mission_item.lon);
-		 }
-		_mission_item.loiter_radius = _navigator->get_loiter_radius();
-		_mission_item.loiter_direction = 1;
-		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-		_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: return at %d meters above home",
-			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
-		break;
-	}
+	vehicle_command_s cmd = _vcommand;
 
-	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;
-		_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 = NAV_CMD_LOITER_TIME_LIMIT;
-		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-		_mission_item.time_inside = 0.0f;
-		_mission_item.pitch_min = 0.0f;
-		_mission_item.autocontinue = false;
-		_mission_item.origin = ORIGIN_ONBOARD;
-
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
-			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
-		break;
+	if (cmd.command == VEHICLE_CMD_NAV_REMOTE_CMD) {
+		int remote_cmd = cmd.param1;
+		if (remote_cmd == REMOTE_CMD_PLAY_PAUSE) {
+			commander_request_s *commander_request = _navigator->get_commander_request();
+			commander_request->request_type = V_MAIN_STATE_CHANGE;
+			commander_request->main_state = MAIN_STATE_AUTO_LOITER;
+			_navigator->set_commander_request_updated();
+		}
 	}
 
-	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 = _navigator->get_acceptance_radius();
-		_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", (double)_mission_item.time_inside);
-
-		} else {
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter");
+}
+
+void
+RTL::set_rtl_setpoint()
+{
+	switch (rtl_state) {
+		case RTL_STATE_CLIMB: {
+			float climb_alt = _navigator->get_home_position()->alt + _parameters.rtl_ret_alt;
+
+			pos_sp_triplet->current.lat = global_pos->lat;
+			pos_sp_triplet->current.lon = global_pos->lon;
+			pos_sp_triplet->current.alt = climb_alt;
+			pos_sp_triplet->current.type = SETPOINT_TYPE_POSITION;
+
+			break;
 		}
-		break;
-	}
+		case RTL_STATE_RETURN: {
 
-	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;
-		_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_LAND;
-		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-		_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: land at home");
-		break;
-	}
+            global_pos = _navigator->get_global_position();
+            home_pos = _navigator->get_home_position();
 
-	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 = _navigator->get_acceptance_radius();
-		_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;
-	}
+            // Calculate offset values for later use.
+            float offset_x;
+            float offset_y;
+            float offset_z = target_pos->alt - global_pos->alt;
 
-	default:
-		break;
-	}
+            get_vector_to_next_waypoint(
+                    global_pos->lat,
+                    global_pos->lon,
+                    home_pos->lat,
+                    home_pos->lon,
+                    &offset_x,
+                    &offset_y
+            );
 
-	reset_mission_item_reached();
+            math::Vector<3> offset(offset_x, offset_y, offset_z);
+            math::Vector<2> offset_xy(offset_x, offset_y);
 
-	/* convert mission item to current position setpoint and make it valid */
-	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
-	pos_sp_triplet->next.valid = false;
+            float offset_xy_len = offset_xy.length();
+
+            if (offset_xy_len > 2.0f)
+                pos_sp_triplet->current.yaw = _wrap_pi(atan2f(-offset_xy(1), -offset_xy(0)));
+
+			pos_sp_triplet->current.lat = home_pos->lat;
+			pos_sp_triplet->current.lon = home_pos->lon;
+			pos_sp_triplet->current.alt = global_pos->alt;
+			pos_sp_triplet->current.type = SETPOINT_TYPE_POSITION;
+
+			break;
+		}
+		case RTL_STATE_LAND: {
+			pos_sp_triplet->current.lat = home_pos->lat;
+			pos_sp_triplet->current.lon = home_pos->lon;
+			pos_sp_triplet->current.alt = global_pos->alt;
+			pos_sp_triplet->current.type = SETPOINT_TYPE_LAND;
+
+			break;
+		}
+		case RTL_STATE_LANDED: {
+
+			disarm();
+
+			break;
+		}
+		default:
+			break;
+	}
 
 	_navigator->set_position_setpoint_triplet_updated();
+
 }
 
 void
-RTL::advance_rtl()
+RTL::set_next_rtl_state()
 {
-	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() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
-			_rtl_state = RTL_STATE_LOITER;
-
-		} else {
-			_rtl_state = RTL_STATE_LAND;
-		}
-		break;
 
-	case RTL_STATE_LOITER:
-		_rtl_state = RTL_STATE_LAND;
-		break;
+	switch (rtl_state) {
+		case RTL_STATE_CLIMB:
+			rtl_state = RTL_STATE_RETURN;
+			break;
 
-	case RTL_STATE_LAND:
-		_rtl_state = RTL_STATE_LANDED;
-		break;
+		case RTL_STATE_RETURN:
+			rtl_state = RTL_STATE_LAND;
+			break;
 
-	default:
-		break;
+		case RTL_STATE_LAND:
+			rtl_state = RTL_STATE_LANDED;
+			break;
+
+		default:
+			break;
 	}
 }
+
+void
+RTL::disarm()
+{
+	commander_request_s *commander_request = _navigator->get_commander_request();
+	commander_request->request_type = V_DISARM;
+	_navigator->set_commander_request_updated();
+}
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index 5928f1f07..84ab155dc 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -44,6 +44,8 @@
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
 
+#include <uORB/uORB.h>
+
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/home_position.h>
@@ -52,6 +54,7 @@
 #include "navigator_mode.h"
 #include "mission_block.h"
 
+
 class Navigator;
 
 class RTL : public MissionBlock
@@ -67,30 +70,28 @@ public:
 
 	virtual void on_active();
 
+	virtual void execute_vehicle_command();
+
 private:
-	/**
-	 * Set the RTL item
-	 */
-	void		set_rtl_item();
 
-	/**
-	 * Move to next RTL item
-	 */
-	void		advance_rtl();
+	void		set_rtl_setpoint();
 
-	enum RTLState {
+	void		set_next_rtl_state();
+
+	void		disarm();
+
+	enum RTL_state {
 		RTL_STATE_NONE = 0,
 		RTL_STATE_CLIMB,
 		RTL_STATE_RETURN,
-		RTL_STATE_DESCEND,
-		RTL_STATE_LOITER,
 		RTL_STATE_LAND,
 		RTL_STATE_LANDED,
-	} _rtl_state;
+	} rtl_state;
+
+	float return_alt;
+	int mv_fd;
+	bool first_rtl_setpoint_set;
 
-	control::BlockParamFloat _param_return_alt;
-	control::BlockParamFloat _param_descend_alt;
-	control::BlockParamFloat _param_land_delay;
 };
 
 #endif
diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c
index bfe6ce7e1..6a54aee95 100644
--- a/src/modules/navigator/rtl_params.c
+++ b/src/modules/navigator/rtl_params.c
@@ -17,7 +17,7 @@
  *    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
+ * "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,
@@ -34,65 +34,21 @@
 /**
  * @file rtl_params.c
  *
- * Parameters for RTL
+ * Parameter for RTL.
  *
- * @author Julian Oes <julian@oes.ch>
+ * @author Martins Frolovs <martins.f@airdog.com>
  */
 
 #include <nuttx/config.h>
-
 #include <systemlib/param/param.h>
 
-/*
- * RTL parameters, accessible via MAVLink
- */
-
-/**
- * Loiter radius after RTL (FW only)
- *
- * Default value of loiter radius after RTL (fixedwing only).
- *
- * @unit meters
- * @min 0.0
- * @group RTL
- */
-PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
-
 /**
  * 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
+ * @min 1
+ * @group Navigator
  */
-PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
+PARAM_DEFINE_FLOAT(RTL_RET_ALT, 30.00f);