Merge remote-tracking branch 'upstream/master' into takeoff_fix

Conflicts:
	src/modules/navigator/mission.cpp

23 files changed, 1799 insertions, 77 deletions

diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
new file mode 100644
index 000000000..66f1c8c73
--- /dev/null
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -0,0 +1,227 @@
+/****************************************************************************
+ *
+ *   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 datalinkloss.cpp
+ * Helper class for Data Link Loss Mode according to the OBC rules
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/home_position.h>
+
+#include "navigator.h"
+#include "datalinkloss.h"
+
+#define DELAY_SIGMA	0.01f
+
+DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) :
+	MissionBlock(navigator, name),
+	_param_commsholdwaittime(this, "CH_T"),
+	_param_commsholdlat(this, "CH_LAT"),
+	_param_commsholdlon(this, "CH_LON"),
+	_param_commsholdalt(this, "CH_ALT"),
+	_param_airfieldhomelat(this, "NAV_AH_LAT", false),
+	_param_airfieldhomelon(this, "NAV_AH_LON", false),
+	_param_airfieldhomealt(this, "NAV_AH_ALT", false),
+	_param_airfieldhomewaittime(this, "AH_T"),
+	_param_numberdatalinklosses(this, "N"),
+	_param_skipcommshold(this, "CHSK"),
+	_dll_state(DLL_STATE_NONE)
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	on_inactive();
+}
+
+DataLinkLoss::~DataLinkLoss()
+{
+}
+
+void
+DataLinkLoss::on_inactive()
+{
+	/* reset DLL state only if setpoint moved */
+	if (!_navigator->get_can_loiter_at_sp()) {
+		_dll_state = DLL_STATE_NONE;
+	}
+}
+
+void
+DataLinkLoss::on_activation()
+{
+	_dll_state = DLL_STATE_NONE;
+	updateParams();
+	advance_dll();
+	set_dll_item();
+}
+
+void
+DataLinkLoss::on_active()
+{
+	if (is_mission_item_reached()) {
+		updateParams();
+		advance_dll();
+		set_dll_item();
+	}
+}
+
+void
+DataLinkLoss::set_dll_item()
+{
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	set_previous_pos_setpoint();
+	_navigator->set_can_loiter_at_sp(false);
+
+	switch (_dll_state) {
+	case DLL_STATE_FLYTOCOMMSHOLDWP: {
+		_mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7;
+		_mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _param_commsholdalt.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 = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get();
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_can_loiter_at_sp(true);
+		break;
+	}
+	case DLL_STATE_FLYTOAIRFIELDHOMEWP: {
+		_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7;
+		_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _param_airfieldhomealt.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.time_inside = _param_airfieldhomewaittime.get() < 0.0f ? 0.0f : _param_airfieldhomewaittime.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_can_loiter_at_sp(true);
+		break;
+	}
+	case DLL_STATE_TERMINATE: {
+		/* Request flight termination from the commander */
+		_navigator->get_mission_result()->flight_termination = true;
+		_navigator->publish_mission_result();
+		warnx("not switched to manual: request flight termination");
+		pos_sp_triplet->previous.valid = false;
+		pos_sp_triplet->current.valid = false;
+		pos_sp_triplet->next.valid = false;
+		break;
+	}
+	default:
+		break;
+	}
+
+	reset_mission_item_reached();
+
+	/* 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;
+
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+void
+DataLinkLoss::advance_dll()
+{
+	switch (_dll_state) {
+	case DLL_STATE_NONE:
+		/* Check the number of data link losses. If above home fly home directly */
+		/* if number of data link losses limit is not reached fly to comms hold wp */
+		if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) {
+			warnx("%d data link losses, limit is %d, fly to airfield home",
+					_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
+			_navigator->get_mission_result()->stay_in_failsafe = true;
+			_navigator->publish_mission_result();
+			_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
+		} else {
+			if (!_param_skipcommshold.get()) {
+				warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter);
+				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold");
+				_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
+			} else {
+				/* comms hold wp not active, fly to airfield home directly */
+				warnx("Skipping comms hold wp. Flying directly to airfield home");
+				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped");
+				_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
+			}
+		}
+		break;
+	case DLL_STATE_FLYTOCOMMSHOLDWP:
+		warnx("fly to airfield home");
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home");
+		_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
+		_navigator->get_mission_result()->stay_in_failsafe = true;
+		_navigator->publish_mission_result();
+		break;
+	case DLL_STATE_FLYTOAIRFIELDHOMEWP:
+		_dll_state = DLL_STATE_TERMINATE;
+		warnx("time is up, state should have been changed manually by now");
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
+		_navigator->get_mission_result()->stay_in_failsafe = true;
+		_navigator->publish_mission_result();
+		break;
+	case DLL_STATE_TERMINATE:
+		warnx("dll end");
+		_dll_state = DLL_STATE_END;
+		break;
+
+	default:
+		break;
+	}
+}
diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h
new file mode 100644
index 000000000..31e0e3907
--- /dev/null
+++ b/src/modules/navigator/datalinkloss.h
@@ -0,0 +1,98 @@
+/***************************************************************************
+ *
+ *   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 datalinkloss.h
+ * Helper class for Data Link Loss Mode acording to the OBC rules
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef NAVIGATOR_DATALINKLOSS_H
+#define NAVIGATOR_DATALINKLOSS_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/Subscription.hpp>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class Navigator;
+
+class DataLinkLoss : public MissionBlock
+{
+public:
+	DataLinkLoss(Navigator *navigator, const char *name);
+
+	~DataLinkLoss();
+
+	virtual void on_inactive();
+
+	virtual void on_activation();
+
+	virtual void on_active();
+
+private:
+	/* Params */
+	control::BlockParamFloat _param_commsholdwaittime;
+	control::BlockParamInt _param_commsholdlat; // * 1e7
+	control::BlockParamInt _param_commsholdlon; // * 1e7
+	control::BlockParamFloat _param_commsholdalt;
+	control::BlockParamInt _param_airfieldhomelat; // * 1e7
+	control::BlockParamInt _param_airfieldhomelon; // * 1e7
+	control::BlockParamFloat _param_airfieldhomealt;
+	control::BlockParamFloat _param_airfieldhomewaittime;
+	control::BlockParamInt _param_numberdatalinklosses;
+	control::BlockParamInt _param_skipcommshold;
+
+	enum DLLState {
+		DLL_STATE_NONE = 0,
+		DLL_STATE_FLYTOCOMMSHOLDWP = 1,
+		DLL_STATE_FLYTOAIRFIELDHOMEWP = 2,
+		DLL_STATE_TERMINATE = 3,
+		DLL_STATE_END = 4
+	} _dll_state;
+
+	/**
+	 * Set the DLL item
+	 */
+	void		set_dll_item();
+
+	/**
+	 * Move to next DLL item
+	 */
+	void		advance_dll();
+
+};
+#endif
diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c
new file mode 100644
index 000000000..6780c0c88
--- /dev/null
+++ b/src/modules/navigator/datalinkloss_params.c
@@ -0,0 +1,126 @@
+/****************************************************************************
+ *
+ *   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 datalinkloss_params.c
+ *
+ * Parameters for DLL
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * Data Link Loss parameters, accessible via MAVLink
+ */
+
+/**
+ * Comms hold wait time
+ *
+ * The amount of time in seconds the system should wait at the comms hold waypoint
+ *
+ * @unit seconds
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f);
+
+/**
+ * Comms hold Lat
+ *
+ * Latitude of comms hold waypoint
+ *
+ * @unit degrees * 1e7
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120);
+
+/**
+ * Comms hold Lon
+ *
+ * Longitude of comms hold waypoint
+ *
+ * @unit degrees * 1e7
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890);
+
+/**
+ * Comms hold alt
+ *
+ * Altitude of comms hold waypoint
+ *
+ * @unit m
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f);
+
+/**
+ * Aifield hole wait time
+ *
+ * The amount of time in seconds the system should wait at the airfield home waypoint
+ *
+ * @unit seconds
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f);
+
+/**
+ * Number of allowed Datalink timeouts
+ *
+ * After more than this number of data link timeouts the aircraft returns home directly
+ *
+ * @group DLL
+ * @min 0
+ * @max 1000
+ */
+PARAM_DEFINE_INT32(NAV_DLL_N, 2);
+
+/**
+ * Skip comms hold wp
+ *
+ * If set to 1 the system will skip the comms hold wp on data link loss and will directly fly to
+ * airfield home
+ *
+ * @group DLL
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_INT32(NAV_DLL_CHSK, 0);
diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp
new file mode 100644
index 000000000..e007b208b
--- /dev/null
+++ b/src/modules/navigator/enginefailure.cpp
@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ *   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 enginefailure.cpp
+ * Helper class for a fixedwing engine failure mode
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+
+#include "navigator.h"
+#include "enginefailure.h"
+
+#define DELAY_SIGMA	0.01f
+
+EngineFailure::EngineFailure(Navigator *navigator, const char *name) :
+	MissionBlock(navigator, name),
+	_ef_state(EF_STATE_NONE)
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	on_inactive();
+}
+
+EngineFailure::~EngineFailure()
+{
+}
+
+void
+EngineFailure::on_inactive()
+{
+	_ef_state = EF_STATE_NONE;
+}
+
+void
+EngineFailure::on_activation()
+{
+	_ef_state = EF_STATE_NONE;
+	advance_ef();
+	set_ef_item();
+}
+
+void
+EngineFailure::on_active()
+{
+	if (is_mission_item_reached()) {
+		advance_ef();
+		set_ef_item();
+	}
+}
+
+void
+EngineFailure::set_ef_item()
+{
+	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 (_ef_state) {
+	case EF_STATE_LOITERDOWN: {
+		//XXX create mission item at ground (below?) here
+
+		_mission_item.lat = _navigator->get_global_position()->lat;
+		_mission_item.lon = _navigator->get_global_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		 //XXX setting altitude to a very low value, evaluate other options
+		_mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f;
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_can_loiter_at_sp(true);
+		break;
+	}
+	default:
+		break;
+	}
+
+	reset_mission_item_reached();
+
+	/* 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;
+
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+void
+EngineFailure::advance_ef()
+{
+	switch (_ef_state) {
+	case EF_STATE_NONE:
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: Engine failure. Loitering down");
+		_ef_state = EF_STATE_LOITERDOWN;
+		break;
+	default:
+		break;
+	}
+}
diff --git a/src/modules/navigator/enginefailure.h b/src/modules/navigator/enginefailure.h
new file mode 100644
index 000000000..2c48c2fce
--- /dev/null
+++ b/src/modules/navigator/enginefailure.h
@@ -0,0 +1,83 @@
+/***************************************************************************
+ *
+ *   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 enginefailure.h
+ * Helper class for a fixedwing engine failure mode
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef NAVIGATOR_ENGINEFAILURE_H
+#define NAVIGATOR_ENGINEFAILURE_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/Subscription.hpp>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class Navigator;
+
+class EngineFailure : public MissionBlock
+{
+public:
+	EngineFailure(Navigator *navigator, const char *name);
+
+	~EngineFailure();
+
+	virtual void on_inactive();
+
+	virtual void on_activation();
+
+	virtual void on_active();
+
+private:
+	enum EFState {
+		EF_STATE_NONE = 0,
+		EF_STATE_LOITERDOWN = 1,
+	} _ef_state;
+
+	/**
+	 * Set the DLL item
+	 */
+	void		set_ef_item();
+
+	/**
+	 * Move to next EF item
+	 */
+	void		advance_ef();
+
+};
+#endif
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index 49aec21e0..0f431ded2 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -48,6 +48,7 @@
 #include <ctype.h>
 #include <nuttx/config.h>
 #include <unistd.h>
+#include <mavlink/mavlink_log.h>
 
 
 /* Oddly, ERROR is not defined for C++ */
@@ -62,7 +63,12 @@ Geofence::Geofence() :
 		_altitude_min(0),
 		_altitude_max(0),
 		_verticesCount(0),
-		param_geofence_on(this, "ON")
+		_param_geofence_on(this, "ON"),
+		_param_altitude_mode(this, "ALTMODE"),
+		_param_source(this, "SOURCE"),
+		_param_counter_threshold(this, "COUNT"),
+		_outside_counter(0),
+		_mavlinkFd(-1)
 {
 	/* Load initial params */
 	updateParams();
@@ -74,23 +80,69 @@ Geofence::~Geofence()
 }
 
 
-bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
+bool Geofence::inside(const struct vehicle_global_position_s &global_position)
 {
-	return inside(vehicle->lat, vehicle->lon, vehicle->alt);
+	return inside(global_position.lat, global_position.lon, global_position.alt);
+}
+
+bool Geofence::inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl)
+{
+	return inside(global_position.lat, global_position.lon, baro_altitude_amsl);
+}
+
+
+bool Geofence::inside(const struct vehicle_global_position_s &global_position,
+			const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl) {
+	updateParams();
+
+	if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) {
+		if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
+			return inside(global_position);
+		} else {
+			return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
+					(double)gps_position.alt * 1.0e-3);
+		}
+	} else {
+		if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
+			return inside(global_position, baro_altitude_amsl);
+		} else {
+			return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
+					baro_altitude_amsl);
+		}
+	}
 }
 
 bool Geofence::inside(double lat, double lon, float altitude)
 {
+	bool inside_fence = inside_polygon(lat, lon, altitude);
+
+	if (inside_fence) {
+		_outside_counter = 0;
+		return inside_fence;
+	} {
+		_outside_counter++;
+		if(_outside_counter > _param_counter_threshold.get()) {
+			return inside_fence;
+		} else {
+			return true;
+		}
+	}
+}
+
+
+bool Geofence::inside_polygon(double lat, double lon, float altitude)
+{
 	/* Return true if geofence is disabled */
-	if (param_geofence_on.get() != 1)
+	if (_param_geofence_on.get() != 1)
 		return true;
 
 	if (valid()) {
 
 		if (!isEmpty()) {
 			/* Vertical check */
-			if (altitude > _altitude_max || altitude < _altitude_min)
+			if (altitude > _altitude_max || altitude < _altitude_min) {
 				return false;
+			}
 
 			/*Horizontal check */
 			/* Adaptation of algorithm originally presented as
@@ -280,8 +332,10 @@ Geofence::loadFromFile(const char *filename)
 	{
 		_verticesCount = pointCounter;
 		warnx("Geofence: imported successfully");
+		mavlink_log_info(_mavlinkFd, "Geofence imported");
 	} else {
 		warnx("Geofence: import error");
+		mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error");
 	}
 
 	return ERROR;
diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h
index 2eb126ab5..9d647cb68 100644
--- a/src/modules/navigator/geofence.h
+++ b/src/modules/navigator/geofence.h
@@ -42,6 +42,9 @@
 #define GEOFENCE_H_
 
 #include <uORB/topics/fence.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/sensor_combined.h>
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
 
@@ -49,30 +52,32 @@
 
 class Geofence : public control::SuperBlock
 {
-private:
-	orb_advert_t	_fence_pub;			/**< publish fence topic */
-
-	float			_altitude_min;
-	float			_altitude_max;
-
-	unsigned 			_verticesCount;
-
-	/* Params */
-	control::BlockParamInt param_geofence_on;
 public:
 	Geofence();
 	~Geofence();
 
+	/* Altitude mode, corresponding to the param GF_ALTMODE */
+	enum {
+		GF_ALT_MODE_WGS84 = 0,
+		GF_ALT_MODE_AMSL = 1
+	};
+
+	/* Source, corresponding to the param GF_SOURCE */
+	enum {
+		GF_SOURCE_GLOBALPOS = 0,
+		GF_SOURCE_GPS = 1
+	};
+
 	/**
-	 * Return whether craft is inside geofence.
+	 * Return whether system is inside geofence.
 	 *
 	 * Calculate whether point is inside arbitrary polygon
 	 * @param craft pointer craft coordinates
-	 * @param fence pointer to array of coordinates, one per vertex. First and last vertex are assumed connected
-	 * @return true: craft is inside fence, false:craft is outside fence
+	 * @return true: system is inside fence, false: system is outside fence
 	 */
-	bool inside(const struct vehicle_global_position_s *craft);
-	bool inside(double lat, double lon, float altitude);
+	bool inside(const struct vehicle_global_position_s &global_position,
+			const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl);
+	bool inside_polygon(double lat, double lon, float altitude);
 
 	int clearDm();
 
@@ -88,6 +93,34 @@ public:
 	int loadFromFile(const char *filename);
 
 	bool isEmpty() {return _verticesCount == 0;}
+
+	int getAltitudeMode() { return _param_altitude_mode.get(); }
+
+	int getSource() { return _param_source.get(); }
+
+	void setMavlinkFd(int value) { _mavlinkFd = value; }
+
+private:
+	orb_advert_t	_fence_pub;			/**< publish fence topic */
+
+	float			_altitude_min;
+	float			_altitude_max;
+
+	unsigned 			_verticesCount;
+
+	/* Params */
+	control::BlockParamInt _param_geofence_on;
+	control::BlockParamInt _param_altitude_mode;
+	control::BlockParamInt _param_source;
+	control::BlockParamInt _param_counter_threshold;
+
+	uint8_t			_outside_counter;
+
+	int _mavlinkFd;
+
+	bool inside(double lat, double lon, float altitude);
+	bool inside(const struct vehicle_global_position_s &global_position);
+	bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl);
 };
 
 
diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c
index 653b1ad84..fca3918e1 100644
--- a/src/modules/navigator/geofence_params.c
+++ b/src/modules/navigator/geofence_params.c
@@ -58,3 +58,39 @@
  * @group Geofence
  */
 PARAM_DEFINE_INT32(GF_ON, 1);
+
+/**
+ * Geofence altitude mode
+ *
+ * Select which altitude reference should be used
+ * 0 = WGS84, 1 = AMSL
+ *
+ * @min 0
+ * @max 1
+ * @group Geofence
+ */
+PARAM_DEFINE_INT32(GF_ALTMODE, 0);
+
+/**
+ * Geofence source
+ *
+ * Select which position source should be used. Selecting GPS instead of global position makes sure that there is
+ * no dependence on the position estimator
+ * 0 = global position, 1 = GPS
+ *
+ * @min 0
+ * @max 1
+ * @group Geofence
+ */
+PARAM_DEFINE_INT32(GF_SOURCE, 0);
+
+/**
+ * Geofence counter limit
+ *
+ * Set how many subsequent position measurements outside of the fence are needed before geofence violation is triggered
+ *
+ * @min -1
+ * @max 10
+ * @group Geofence
+ */
+PARAM_DEFINE_INT32(GF_COUNT, -1);
diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp
new file mode 100644
index 000000000..cd55f60b0
--- /dev/null
+++ b/src/modules/navigator/gpsfailure.cpp
@@ -0,0 +1,178 @@
+/****************************************************************************
+ *
+ *   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 gpsfailure.cpp
+ * Helper class for gpsfailure mode according to the OBC rules
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/home_position.h>
+
+#include "navigator.h"
+#include "gpsfailure.h"
+
+#define DELAY_SIGMA	0.01f
+
+GpsFailure::GpsFailure(Navigator *navigator, const char *name) :
+	MissionBlock(navigator, name),
+	_param_loitertime(this, "LT"),
+	_param_openlooploiter_roll(this, "R"),
+	_param_openlooploiter_pitch(this, "P"),
+	_param_openlooploiter_thrust(this, "TR"),
+	_gpsf_state(GPSF_STATE_NONE),
+	_timestamp_activation(0)
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	on_inactive();
+}
+
+GpsFailure::~GpsFailure()
+{
+}
+
+void
+GpsFailure::on_inactive()
+{
+	/* reset GPSF state only if setpoint moved */
+	if (!_navigator->get_can_loiter_at_sp()) {
+		_gpsf_state = GPSF_STATE_NONE;
+	}
+}
+
+void
+GpsFailure::on_activation()
+{
+	_gpsf_state = GPSF_STATE_NONE;
+	_timestamp_activation = hrt_absolute_time();
+	updateParams();
+	advance_gpsf();
+	set_gpsf_item();
+}
+
+void
+GpsFailure::on_active()
+{
+
+	switch (_gpsf_state) {
+	case GPSF_STATE_LOITER: {
+		/* Position controller does not run in this mode:
+		 * navigator has to publish an attitude setpoint */
+		_navigator->get_att_sp()->roll_body = M_DEG_TO_RAD_F * _param_openlooploiter_roll.get();
+		_navigator->get_att_sp()->pitch_body = M_DEG_TO_RAD_F * _param_openlooploiter_pitch.get();
+		_navigator->get_att_sp()->thrust = _param_openlooploiter_thrust.get();
+		_navigator->publish_att_sp();
+
+		/* Measure time */
+		hrt_abstime elapsed = hrt_elapsed_time(&_timestamp_activation);
+
+		//warnx("open loop loiter, posctl enabled %u, elapsed %.1fs, thrust %.2f",
+				//_navigator->get_control_mode()->flag_control_position_enabled, elapsed * 1e-6, (double)_param_openlooploiter_thrust.get());
+		if (elapsed > _param_loitertime.get() * 1e6f) {
+			/* no recovery, adavance the state machine */
+			warnx("gps not recovered, switch to next state");
+			advance_gpsf();
+		}
+		break;
+	}
+	case GPSF_STATE_TERMINATE:
+		set_gpsf_item();
+		advance_gpsf();
+		break;
+	default:
+		break;
+	}
+}
+
+void
+GpsFailure::set_gpsf_item()
+{
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	/* Set pos sp triplet to invalid to stop pos controller */
+	pos_sp_triplet->previous.valid = false;
+	pos_sp_triplet->current.valid = false;
+	pos_sp_triplet->next.valid = false;
+
+	switch (_gpsf_state) {
+	case GPSF_STATE_TERMINATE: {
+		/* Request flight termination from the commander */
+		_navigator->get_mission_result()->flight_termination = true;
+		_navigator->publish_mission_result();
+		warnx("gps fail: request flight termination");
+	}
+	default:
+		break;
+	}
+
+	reset_mission_item_reached();
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+void
+GpsFailure::advance_gpsf()
+{
+	updateParams();
+
+	switch (_gpsf_state) {
+	case GPSF_STATE_NONE:
+		_gpsf_state = GPSF_STATE_LOITER;
+		warnx("gpsf loiter");
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: open loop loiter");
+		break;
+	case GPSF_STATE_LOITER:
+		_gpsf_state = GPSF_STATE_TERMINATE;
+		warnx("gpsf terminate");
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no gps recovery, termination");
+		warnx("mavlink sent");
+		break;
+	case GPSF_STATE_TERMINATE:
+		warnx("gpsf end");
+		_gpsf_state = GPSF_STATE_END;
+	default:
+		break;
+	}
+}
diff --git a/src/modules/navigator/gpsfailure.h b/src/modules/navigator/gpsfailure.h
new file mode 100644
index 000000000..e9e72babf
--- /dev/null
+++ b/src/modules/navigator/gpsfailure.h
@@ -0,0 +1,97 @@
+/***************************************************************************
+ *
+ *   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 gpsfailure.h
+ * Helper class for Data Link Loss Mode according to the OBC rules
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef NAVIGATOR_GPSFAILURE_H
+#define NAVIGATOR_GPSFAILURE_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/uORB.h>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class Navigator;
+
+class GpsFailure : public MissionBlock
+{
+public:
+	GpsFailure(Navigator *navigator, const char *name);
+
+	~GpsFailure();
+
+	virtual void on_inactive();
+
+	virtual void on_activation();
+
+	virtual void on_active();
+
+private:
+	/* Params */
+	control::BlockParamFloat _param_loitertime;
+	control::BlockParamFloat _param_openlooploiter_roll;
+	control::BlockParamFloat _param_openlooploiter_pitch;
+	control::BlockParamFloat _param_openlooploiter_thrust;
+
+	enum GPSFState {
+		GPSF_STATE_NONE = 0,
+		GPSF_STATE_LOITER = 1,
+		GPSF_STATE_TERMINATE = 2,
+		GPSF_STATE_END = 3,
+	} _gpsf_state;
+
+	hrt_abstime _timestamp_activation; //*< timestamp when this mode was activated */
+
+	/**
+	 * Set the GPSF item
+	 */
+	void		set_gpsf_item();
+
+	/**
+	 * Move to next GPSF item
+	 */
+	void		advance_gpsf();
+
+};
+#endif
diff --git a/src/modules/navigator/gpsfailure_params.c b/src/modules/navigator/gpsfailure_params.c
new file mode 100644
index 000000000..39d179eed
--- /dev/null
+++ b/src/modules/navigator/gpsfailure_params.c
@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   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 gpsfailure_params.c
+ *
+ * Parameters for GPSF navigation mode
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * GPS Failure Navigation Mode parameters, accessible via MAVLink
+ */
+
+/**
+ * Loiter time
+ *
+ * The amount of time in seconds the system should do open loop loiter and wait for gps recovery
+ * before it goes into flight termination.
+ *
+ * @unit seconds
+ * @min 0.0
+ * @group GPSF
+ */
+PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f);
+
+/**
+ * Open loop loiter roll
+ *
+ * Roll in degrees during the open loop loiter
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 30.0
+ * @group GPSF
+ */
+PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f);
+
+/**
+ * Open loop loiter pitch
+ *
+ * Pitch in degrees during the open loop loiter
+ *
+ * @unit deg
+ * @min -30.0
+ * @max 30.0
+ * @group GPSF
+ */
+PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f);
+
+/**
+ * Open loop loiter thrust
+ *
+ * Thrust value which is set during the open loop loiter
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group GPSF
+ */
+PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.7f);
+
+
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 8bacaa425..87d16f1e6 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -62,18 +62,16 @@
 
 Mission::Mission(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
-	_param_onboard_enabled(this, "ONBOARD_EN"),
-	_param_takeoff_alt(this, "TAKEOFF_ALT"),
-	_param_dist_1wp(this, "DIST_1WP"),
-	_param_altmode(this, "ALTMODE"),
+	_param_onboard_enabled(this, "MIS_ONBOARD_EN", false),
+	_param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false),
+	_param_dist_1wp(this, "MIS_DIST_1WP", false),
+	_param_altmode(this, "MIS_ALTMODE", false),
 	_onboard_mission({0}),
 	_offboard_mission({0}),
 	_current_onboard_mission_index(-1),
 	_current_offboard_mission_index(-1),
 	_need_takeoff(true),
 	_takeoff(false),
-	_mission_result_pub(-1),
-	_mission_result({0}),
 	_mission_type(MISSION_TYPE_NONE),
 	_inited(false),
 	_dist_1wp_ok(false),
@@ -158,7 +156,7 @@ Mission::on_active()
 		} else {
 			/* else just report that item reached */
 			if (_mission_type == MISSION_TYPE_OFFBOARD) {
-				if (!(_mission_result.seq_reached == _current_offboard_mission_index && _mission_result.reached)) {
+				if (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) {
 					report_mission_item_reached();
 				}
 			}
@@ -705,19 +703,19 @@ Mission::save_offboard_mission_state()
 void
 Mission::report_mission_item_reached()
 {
-	_mission_result.reached = true;
-	_mission_result.seq_reached = _current_offboard_mission_index;
-	publish_mission_result();
+	_navigator->get_mission_result()->reached = true;
+	_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
+	_navigator->publish_mission_result();
 }
 
 void
 Mission::report_current_offboard_mission_item()
 {
 	warnx("current offboard mission index: %d", _current_offboard_mission_index);
-	_mission_result.reached = false;
-	_mission_result.finished = false;
-	_mission_result.seq_current = _current_offboard_mission_index;
-	publish_mission_result();
+	_navigator->get_mission_result()->reached = false;
+	_navigator->get_mission_result()->finished = false;
+	_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
+	_navigator->publish_mission_result();
 
 	save_offboard_mission_state();
 }
@@ -725,20 +723,6 @@ Mission::report_current_offboard_mission_item()
 void
 Mission::report_mission_finished()
 {
-	_mission_result.finished = true;
-	publish_mission_result();
-}
-
-void
-Mission::publish_mission_result()
-{
-	/* lazily publish the mission result only once available */
-	if (_mission_result_pub > 0) {
-		/* publish mission result */
-		orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
-
-	} else {
-		/* advertise and publish */
-		_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
-	}
+	_navigator->get_mission_result()->finished = true;
+	_navigator->publish_mission_result();
 }
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 4737be391..5164737f3 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -144,11 +144,6 @@ private:
 	 */
 	void report_mission_finished();
 
-	/**
-	 * Publish the mission result so commander and mavlink know what is going on
-	 */
-	void publish_mission_result();
-
 	control::BlockParamInt _param_onboard_enabled;
 	control::BlockParamFloat _param_takeoff_alt;
 	control::BlockParamFloat _param_dist_1wp;
@@ -162,9 +157,6 @@ private:
 	bool _need_takeoff;					/**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
 	bool _takeoff;						/**< takeoff state flag */
 
-	orb_advert_t _mission_result_pub;
-	struct mission_result_s _mission_result;
-
 	enum {
 		MISSION_TYPE_NONE,
 		MISSION_TYPE_ONBOARD,
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index 7ae7f60cb..389cdd0d2 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -120,7 +120,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
 				return false;
 			}
 
-			if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) {
+			if (!geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) {
 				mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i);
 				return false;
 			}
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index b50198996..c075903b7 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -49,7 +49,14 @@ SRCS		= navigator_main.cpp \
 		  offboard.cpp \
 		  mission_feasibility_checker.cpp \
 		  geofence.cpp \
-		  geofence_params.c
+		  geofence_params.c \
+		  datalinkloss.cpp \
+		  datalinkloss_params.c \
+		  rcloss.cpp \
+		  rcloss_params.c \
+		  enginefailure.cpp \
+		  gpsfailure.cpp \
+		  gpsfailure_params.c
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 8edbb63b3..bf42acff9 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -35,6 +35,7 @@
  * Helper class to access missions
  * @author Julian Oes <julian@oes.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #ifndef NAVIGATOR_H
@@ -50,20 +51,26 @@
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/mission_result.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
 
 #include "navigator_mode.h"
 #include "mission.h"
 #include "loiter.h"
 #include "rtl.h"
 #include "offboard.h"
+#include "datalinkloss.h"
+#include "enginefailure.h"
+#include "gpsfailure.h"
+#include "rcloss.h"
 #include "geofence.h"
 
 /**
  * Number of navigation modes that need on_active/on_inactive calls
- * Currently: mission, loiter, and rtl
  */
-#define NAVIGATOR_MODE_ARRAY_SIZE 4
+#define NAVIGATOR_MODE_ARRAY_SIZE 8
 
 class Navigator : public control::SuperBlock
 {
@@ -101,6 +108,17 @@ public:
 	void		load_fence_from_file(const char *filename);
 
 	/**
+	 * Publish the mission result so commander and mavlink know what is going on
+	 */
+	void publish_mission_result();
+
+	/**
+	 * Publish the attitude sp, only to be used in very special modes when position control is deactivated
+	 * Example: mode that is triggered on gps failure
+	 */
+	void publish_att_sp();
+
+	/**
 	 * Setters
 	 */
 	void		set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
@@ -112,8 +130,13 @@ public:
 	struct vehicle_status_s*	    get_vstatus() { return &_vstatus; }
 	struct vehicle_control_mode_s*	    get_control_mode() { return &_control_mode; }
 	struct vehicle_global_position_s*   get_global_position() { return &_global_pos; }
+	struct vehicle_gps_position_s*	    get_gps_position() { return &_gps_pos; }
+	struct sensor_combined_s*	    get_sensor_combined() { return &_sensor_combined; }
 	struct home_position_s*		    get_home_position() { return &_home_pos; }
-	struct position_setpoint_triplet_s*		    get_position_setpoint_triplet() { return &_pos_sp_triplet; }
+	struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
+	struct mission_result_s*	    get_mission_result() { return &_mission_result; }
+	struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; }
+
 	int		get_onboard_mission_sub() { return _onboard_mission_sub; }
 	int		get_offboard_mission_sub() { return _offboard_mission_sub; }
 	int		get_offboard_control_sp_sub() { return _offboard_control_sp_sub; }
@@ -131,6 +154,8 @@ private:
 	int		_mavlink_fd;			/**< the file descriptor to send messages over mavlink */
 
 	int		_global_pos_sub;		/**< global position subscription */
+	int		_gps_pos_sub;		/**< gps position subscription */
+	int		_sensor_combined_sub;		/**< sensor combined subscription */
 	int		_home_pos_sub;			/**< home position subscription */
 	int		_vstatus_sub;			/**< vehicle status subscription */
 	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
@@ -141,15 +166,24 @@ private:
 	int		_param_update_sub;		/**< param update 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 */
 
 	vehicle_status_s				_vstatus;		/**< vehicle status */
 	vehicle_control_mode_s				_control_mode;		/**< vehicle control mode */
 	vehicle_global_position_s			_global_pos;		/**< global vehicle position */
+	vehicle_gps_position_s				_gps_pos;		/**< gps position */
+	sensor_combined_s				_sensor_combined;	/**< sensor values */
 	home_position_s					_home_pos;		/**< home position for RTL */
 	mission_item_s 					_mission_item;		/**< current mission item */
 	navigation_capabilities_s			_nav_caps;		/**< navigation capabilities */
 	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
 
+	mission_result_s				_mission_result;
+	vehicle_attitude_setpoint_s					_att_sp;
+
 	bool 		_mission_item_valid;		/**< flags if the current mission item is valid */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
@@ -157,14 +191,19 @@ private:
 	Geofence	_geofence;			/**< class that handles the geofence */
 	bool		_geofence_violation_warning_sent; /**< prevents spaming to mavlink */
 
-	bool		_fence_valid;			/**< flag if fence is valid */
 	bool		_inside_fence;			/**< vehicle is inside fence */
 
 	NavigatorMode	*_navigation_mode;		/**< abstract pointer to current navigation mode class */
 	Mission		_mission;			/**< class that handles the missions */
 	Loiter		_loiter;			/**< class that handles loiter */
 	RTL 		_rtl;				/**< class that handles RTL */
+	RCLoss 		_rcLoss;				/**< class that handles RTL according to
+							  OBC rules (rc loss mode) */
 	Offboard	_offboard;			/**< class that handles offboard */
+	DataLinkLoss	_dataLinkLoss;			/**< class that handles the OBC datalink loss mode */
+	EngineFailure	_engineFailure;			/**< class that handles the engine failure mode
+							  (FW only!) */
+	GpsFailure	_gpsFailure;			/**< class that handles the OBC gpsfailure loss mode */
 
 	NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE];	/**< array of navigation modes */
 
@@ -173,12 +212,24 @@ private:
 
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
 	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
+	control::BlockParamInt _param_datalinkloss_obc;	/**< if true: obc mode on data link loss enabled */
+	control::BlockParamInt _param_rcloss_obc;	/**< if true: obc mode on rc loss enabled */
 	/**
 	 * Retrieve global position
 	 */
 	void		global_position_update();
 
 	/**
+	 * Retrieve gps position
+	 */
+	void		gps_position_update();
+
+	/**
+	 * Retrieve sensor values
+	 */
+	void		sensor_combined_update();
+
+	/**
 	 * Retrieve home position
 	 */
 	void		home_position_update();
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 042c46afd..bf5e36d39 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -40,6 +40,7 @@
  * @author Jean Cyr <jean.m.cyr@gmail.com>
  * @author Julian Oes <julian@oes.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #include <nuttx/config.h>
@@ -68,6 +69,7 @@
 #include <uORB/topics/fence.h>
 #include <uORB/topics/navigation_capabilities.h>
 #include <uORB/topics/offboard_control_setpoint.h>
+#include <drivers/drv_baro.h>
 
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
@@ -85,6 +87,7 @@
  */
 extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
 
+#define GEOFENCE_CHECK_INTERVAL 200000
 
 namespace navigator
 {
@@ -98,6 +101,7 @@ Navigator::Navigator() :
 	_navigator_task(-1),
 	_mavlink_fd(-1),
 	_global_pos_sub(-1),
+	_gps_pos_sub(-1),
 	_home_pos_sub(-1),
 	_vstatus_sub(-1),
 	_capabilities_sub(-1),
@@ -107,34 +111,49 @@ Navigator::Navigator() :
 	_offboard_mission_sub(-1),
 	_param_update_sub(-1),
 	_pos_sp_triplet_pub(-1),
+	_mission_result_pub(-1),
+	_att_sp_pub(-1),
 	_vstatus{},
 	_control_mode{},
 	_global_pos{},
+	_gps_pos{},
+	_sensor_combined{},
 	_home_pos{},
 	_mission_item{},
 	_nav_caps{},
 	_pos_sp_triplet{},
+	_mission_result{},
+	_att_sp{},
 	_mission_item_valid(false),
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
 	_geofence{},
 	_geofence_violation_warning_sent(false),
-	_fence_valid(false),
 	_inside_fence(true),
 	_navigation_mode(nullptr),
 	_mission(this, "MIS"),
 	_loiter(this, "LOI"),
 	_rtl(this, "RTL"),
+	_rcLoss(this, "RCL"),
 	_offboard(this, "OFF"),
+	_dataLinkLoss(this, "DLL"),
+	_engineFailure(this, "EF"),
+	_gpsFailure(this, "GPSF"),
 	_can_loiter_at_sp(false),
 	_pos_sp_triplet_updated(false),
 	_param_loiter_radius(this, "LOITER_RAD"),
-	_param_acceptance_radius(this, "ACC_RAD")
+	_param_acceptance_radius(this, "ACC_RAD"),
+	_param_datalinkloss_obc(this, "DLL_OBC"),
+	_param_rcloss_obc(this, "RCL_OBC")
 {
 	/* Create a list of our possible navigation types */
 	_navigation_mode_array[0] = &_mission;
 	_navigation_mode_array[1] = &_loiter;
 	_navigation_mode_array[2] = &_rtl;
 	_navigation_mode_array[3] = &_offboard;
+	_navigation_mode_array[4] = &_dataLinkLoss;
+	_navigation_mode_array[5] = &_engineFailure;
+	_navigation_mode_array[6] = &_gpsFailure;
+	_navigation_mode_array[7] = &_rcLoss;
 
 	updateParams();
 }
@@ -171,6 +190,18 @@ Navigator::global_position_update()
 }
 
 void
+Navigator::gps_position_update()
+{
+	orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &_gps_pos);
+}
+
+void
+Navigator::sensor_combined_update()
+{
+	orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
+}
+
+void
 Navigator::home_position_update()
 {
 	orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
@@ -221,6 +252,7 @@ Navigator::task_main()
 	warnx("Initializing..");
 
 	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	_geofence.setMavlinkFd(_mavlink_fd);
 
 	/* Try to load the geofence:
 	 * if /fs/microsd/etc/geofence.txt load from this file
@@ -232,6 +264,7 @@ Navigator::task_main()
 		_geofence.loadFromFile(GEOFENCE_FILENAME);
 
 	} else {
+		mavlink_log_critical(_mavlink_fd, "#audio: No geofence file");
 		if (_geofence.clearDm() > 0)
 			warnx("Geofence cleared");
 		else
@@ -240,6 +273,8 @@ Navigator::task_main()
 
 	/* do subscriptions */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	_gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
@@ -253,6 +288,8 @@ Navigator::task_main()
 	vehicle_status_update();
 	vehicle_control_mode_update();
 	global_position_update();
+	gps_position_update();
+	sensor_combined_update();
 	home_position_update();
 	navigation_capabilities_update();
 	params_update();
@@ -264,7 +301,7 @@ Navigator::task_main()
 	const hrt_abstime mavlink_open_interval = 500000;
 
 	/* wakeup source(s) */
-	struct pollfd fds[6];
+	struct pollfd fds[8];
 
 	/* Setup of loop */
 	fds[0].fd = _global_pos_sub;
@@ -279,6 +316,10 @@ Navigator::task_main()
 	fds[4].events = POLLIN;
 	fds[5].fd = _param_update_sub;
 	fds[5].events = POLLIN;
+	fds[6].fd = _sensor_combined_sub;
+	fds[6].events = POLLIN;
+	fds[7].fd = _gps_pos_sub;
+	fds[7].events = POLLIN;
 
 	while (!_task_should_exit) {
 
@@ -303,6 +344,21 @@ Navigator::task_main()
 			_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 		}
 
+		static bool have_geofence_position_data = false;
+
+		/* gps updated */
+		if (fds[7].revents & POLLIN) {
+			gps_position_update();
+			if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) {
+				have_geofence_position_data = true;
+			}
+		}
+
+		/* sensors combined updated */
+		if (fds[6].revents & POLLIN) {
+			sensor_combined_update();
+		}
+
 		/* parameters updated */
 		if (fds[5].revents & POLLIN) {
 			params_update();
@@ -332,9 +388,23 @@ Navigator::task_main()
 		/* global position updated */
 		if (fds[0].revents & POLLIN) {
 			global_position_update();
+			static int gposcounter = 0;
+			if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
+				have_geofence_position_data = true;
+			}
+			gposcounter++;
+		}
 
-			/* Check geofence violation */
-			if (!_geofence.inside(&_global_pos)) {
+		/* Check geofence violation */
+		static hrt_abstime last_geofence_check = 0;
+		if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) {
+			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter);
+			last_geofence_check = hrt_absolute_time();
+			have_geofence_position_data = false;
+			if (!inside) {
+				/* inform other apps via the mission result */
+				_mission_result.geofence_violated = true;
+				publish_mission_result();
 
 				/* Issue a warning about the geofence violation once */
 				if (!_geofence_violation_warning_sent) {
@@ -342,6 +412,9 @@ Navigator::task_main()
 					_geofence_violation_warning_sent = true;
 				}
 			} else {
+				/* inform other apps via the mission result */
+				_mission_result.geofence_violated = false;
+				publish_mission_result();
 				/* Reset the _geofence_violation_warning_sent field */
 				_geofence_violation_warning_sent = false;
 			}
@@ -364,11 +437,30 @@ Navigator::task_main()
 			case NAVIGATION_STATE_AUTO_LOITER:
 				_navigation_mode = &_loiter;
 				break;
+			case NAVIGATION_STATE_AUTO_RCRECOVER:
+				if (_param_rcloss_obc.get() != 0) {
+					_navigation_mode = &_rcLoss;
+				} else {
+					_navigation_mode = &_rtl;
+				}
+				break;
 			case NAVIGATION_STATE_AUTO_RTL:
-				_navigation_mode = &_rtl;
+					_navigation_mode = &_rtl;
 				break;
 			case NAVIGATION_STATE_AUTO_RTGS:
-				_navigation_mode = &_rtl; /* TODO: change this to something else */
+				/* Use complex data link loss mode only when enabled via param
+				* otherwise use rtl */
+				if (_param_datalinkloss_obc.get() != 0) {
+					_navigation_mode = &_dataLinkLoss;
+				} else {
+					_navigation_mode = &_rtl;
+				}
+				break;
+			case NAVIGATION_STATE_AUTO_LANDENGFAIL:
+				_navigation_mode = &_engineFailure;
+				break;
+			case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
+				_navigation_mode = &_gpsFailure;
 				break;
 			case NAVIGATION_STATE_OFFBOARD:
 				_navigation_mode = &_offboard;
@@ -442,7 +534,7 @@ Navigator::status()
 	// 	warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F));
 	// }
 
-	if (_fence_valid) {
+	if (_geofence.valid()) {
 		warnx("Geofence is valid");
 		/* TODO: needed? */
 //		warnx("Vertex longitude latitude");
@@ -450,7 +542,7 @@ Navigator::status()
 //		warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat);
 
 	} else {
-		warnx("Geofence not set");
+		warnx("Geofence not set (no /etc/geofence.txt on microsd) or not valid");
 	}
 }
 
@@ -534,3 +626,34 @@ int navigator_main(int argc, char *argv[])
 
 	return 0;
 }
+
+void
+Navigator::publish_mission_result()
+{
+	/* lazily publish the mission result only once available */
+	if (_mission_result_pub > 0) {
+		/* publish mission result */
+		orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
+
+	} else {
+		/* advertise and publish */
+		_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
+	}
+	/* reset reached bool */
+	_mission_result.reached = false;
+	_mission_result.finished = false;
+}
+
+void
+Navigator::publish_att_sp()
+{
+	/* lazily publish the attitude sp only once available */
+	if (_att_sp_pub > 0) {
+		/* publish att sp*/
+		orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
+
+	} else {
+		/* advertise and publish */
+		_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
+	}
+}
diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp
index f43215665..3807c5ea8 100644
--- a/src/modules/navigator/navigator_mode.cpp
+++ b/src/modules/navigator/navigator_mode.cpp
@@ -43,7 +43,7 @@
 #include "navigator.h"
 
 NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
-	SuperBlock(NULL, name),
+	SuperBlock(navigator, name),
 	_navigator(navigator),
 	_first_run(true)
 {
@@ -63,6 +63,9 @@ NavigatorMode::run(bool active) {
 		if (_first_run) {
 			/* first run */
 			_first_run = false;
+			/* Reset stay in failsafe flag */
+			_navigator->get_mission_result()->stay_in_failsafe = false;
+			_navigator->publish_mission_result();
 			on_activation();
 
 		} else {
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 084afe340..1f40e634e 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -37,6 +37,7 @@
  * Parameters for navigator in general
  *
  * @author Julian Oes <julian@oes.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #include <nuttx/config.h>
@@ -64,3 +65,56 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f);
+
+/**
+ * Set OBC mode for data link loss
+ *
+ * If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules
+ *
+ * @min 0
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(NAV_DLL_OBC, 0);
+
+/**
+ * Set OBC mode for rc loss
+ *
+ * If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules
+ *
+ * @min 0
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(NAV_RCL_OBC, 0);
+
+/**
+ * Airfield home Lat
+ *
+ * Latitude of airfield home waypoint
+ *
+ * @unit degrees * 1e7
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810);
+
+/**
+ * Airfield home Lon
+ *
+ * Longitude of airfield home waypoint
+ *
+ * @unit degrees * 1e7
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250);
+
+/**
+ * Airfield home alt
+ *
+ * Altitude of airfield home waypoint
+ *
+ * @unit m
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f);
diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp
new file mode 100644
index 000000000..5564a1c42
--- /dev/null
+++ b/src/modules/navigator/rcloss.cpp
@@ -0,0 +1,182 @@
+/****************************************************************************
+ *
+ *   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 rcloss.cpp
+ * Helper class for RC Loss Mode according to the OBC rules
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/home_position.h>
+
+#include "navigator.h"
+#include "datalinkloss.h"
+
+#define DELAY_SIGMA	0.01f
+
+RCLoss::RCLoss(Navigator *navigator, const char *name) :
+	MissionBlock(navigator, name),
+	_param_loitertime(this, "LT"),
+	_rcl_state(RCL_STATE_NONE)
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	on_inactive();
+}
+
+RCLoss::~RCLoss()
+{
+}
+
+void
+RCLoss::on_inactive()
+{
+	/* reset RCL state only if setpoint moved */
+	if (!_navigator->get_can_loiter_at_sp()) {
+		_rcl_state = RCL_STATE_NONE;
+	}
+}
+
+void
+RCLoss::on_activation()
+{
+	_rcl_state = RCL_STATE_NONE;
+	updateParams();
+	advance_rcl();
+	set_rcl_item();
+}
+
+void
+RCLoss::on_active()
+{
+	if (is_mission_item_reached()) {
+		updateParams();
+		advance_rcl();
+		set_rcl_item();
+	}
+}
+
+void
+RCLoss::set_rcl_item()
+{
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	set_previous_pos_setpoint();
+	_navigator->set_can_loiter_at_sp(false);
+
+	switch (_rcl_state) {
+	case RCL_STATE_LOITER: {
+		_mission_item.lat = _navigator->get_global_position()->lat;
+		_mission_item.lon = _navigator->get_global_position()->lon;
+		_mission_item.altitude = _navigator->get_global_position()->alt;
+		_mission_item.altitude_is_relative = false;
+		_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 = _param_loitertime.get() < 0.0f ? 0.0f : _param_loitertime.get();
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_can_loiter_at_sp(true);
+		break;
+	}
+	case RCL_STATE_TERMINATE: {
+		/* Request flight termination from the commander */
+		_navigator->get_mission_result()->flight_termination = true;
+		_navigator->publish_mission_result();
+		warnx("rc not recovered: request flight termination");
+		pos_sp_triplet->previous.valid = false;
+		pos_sp_triplet->current.valid = false;
+		pos_sp_triplet->next.valid = false;
+		break;
+	}
+	default:
+		break;
+	}
+
+	reset_mission_item_reached();
+
+	/* 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;
+
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+void
+RCLoss::advance_rcl()
+{
+	switch (_rcl_state) {
+	case RCL_STATE_NONE:
+		if (_param_loitertime.get() > 0.0f) {
+			warnx("RC loss, OBC mode, loiter");
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering");
+			_rcl_state = RCL_STATE_LOITER;
+		} else {
+			warnx("RC loss, OBC mode, slip loiter, terminate");
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating");
+			_rcl_state = RCL_STATE_TERMINATE;
+			_navigator->get_mission_result()->stay_in_failsafe = true;
+			_navigator->publish_mission_result();
+		}
+		break;
+	case RCL_STATE_LOITER:
+		_rcl_state = RCL_STATE_TERMINATE;
+		warnx("time is up, no RC regain, terminating");
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating");
+		_navigator->get_mission_result()->stay_in_failsafe = true;
+		_navigator->publish_mission_result();
+		break;
+	case RCL_STATE_TERMINATE:
+		warnx("rcl end");
+		_rcl_state = RCL_STATE_END;
+		break;
+	default:
+		break;
+	}
+}
diff --git a/src/modules/navigator/rcloss.h b/src/modules/navigator/rcloss.h
new file mode 100644
index 000000000..bcb74d877
--- /dev/null
+++ b/src/modules/navigator/rcloss.h
@@ -0,0 +1,88 @@
+/***************************************************************************
+ *
+ *   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 rcloss.h
+ * Helper class for RC Loss Mode acording to the OBC rules
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef NAVIGATOR_RCLOSS_H
+#define NAVIGATOR_RCLOSS_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/Subscription.hpp>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class Navigator;
+
+class RCLoss : public MissionBlock
+{
+public:
+	RCLoss(Navigator *navigator, const char *name);
+
+	~RCLoss();
+
+	virtual void on_inactive();
+
+	virtual void on_activation();
+
+	virtual void on_active();
+
+private:
+	/* Params */
+	control::BlockParamFloat _param_loitertime;
+
+	enum RCLState {
+		RCL_STATE_NONE = 0,
+		RCL_STATE_LOITER = 1,
+		RCL_STATE_TERMINATE = 2,
+		RCL_STATE_END = 3
+	} _rcl_state;
+
+	/**
+	 * Set the RCL item
+	 */
+	void		set_rcl_item();
+
+	/**
+	 * Move to next RCL item
+	 */
+	void		advance_rcl();
+
+};
+#endif
diff --git a/src/modules/navigator/rcloss_params.c b/src/modules/navigator/rcloss_params.c
new file mode 100644
index 000000000..f1a01c73b
--- /dev/null
+++ b/src/modules/navigator/rcloss_params.c
@@ -0,0 +1,60 @@
+/****************************************************************************
+ *
+ *   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 rcloss_params.c
+ *
+ * Parameters for RC Loss (OBC)
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * OBC RC Loss mode parameters, accessible via MAVLink
+ */
+
+/**
+ * Loiter Time
+ *
+ * The amount of time in seconds the system should loiter at current position before termination
+ * Set to -1 to make the system skip loitering
+ *
+ * @unit seconds
+ * @min -1.0
+ * @group RCL
+ */
+PARAM_DEFINE_FLOAT(NAV_RCL_LT, 120.0f);
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 142a73409..b6c4b8fdf 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -58,9 +58,9 @@
 RTL::RTL(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
 	_rtl_state(RTL_STATE_NONE),
-	_param_return_alt(this, "RETURN_ALT"),
-	_param_descend_alt(this, "DESCEND_ALT"),
-	_param_land_delay(this, "LAND_DELAY")
+	_param_return_alt(this, "RTL_RETURN_ALT", false),
+	_param_descend_alt(this, "RTL_DESCEND_ALT", false),
+	_param_land_delay(this, "RTL_LAND_DELAY", false)
 {
 	/* load initial params */
 	updateParams();