mission require valid landing after DO_LAND_START

msg/geofence_result.msg

@@ -5,4 +5,6 @@ uint8 GF_ACTION_RTL = 3                     # switch to AUTO|RTL
 uint8 GF_ACTION_TERMINATE = 4               # flight termination
 
 bool geofence_violated		# true if the geofence is violated
-uint8 geofence_action       # action to take when geofence is violated
+uint8 geofence_action       	# action to take when geofence is violated
+
+bool home_required		# true if the geofence requires a valid home position

msg/mission_result.msg

@@ -1,14 +1,18 @@
-uint32 instance_count # Instance count of this mission. Increments monotonically whenever the mission is modified
+uint32 instance_count		# Instance count of this mission. Increments monotonically whenever the mission is modified
+
 uint32 seq_reached		# Sequence of the mission item which has been reached
 uint32 seq_current		# Sequence of the current mission item
 uint32 seq_total		# Total number of mission items
+
 bool valid			# true if mission is valid
 bool warning			# true if mission is valid, but has potentially problematic items leading to safety warnings
 bool reached			# true if mission has been reached
 bool finished			# true if mission has been completed
+bool failure			# true if the mission cannot continue or be completed for some reason
+
 bool stay_in_failsafe		# true if the commander should not switch out of the failsafe mode
 bool flight_termination		# true if the navigator demands a flight termination from the commander app
+
 bool item_do_jump_changed	# true if the number of do jumps remaining has changed
 uint32 item_changed_index	# indicate which item has changed
 uint32 item_do_jump_remaining	# set to the number of do jumps remaining for that item
-bool mission_failure		# true if the mission cannot continue or be completed for some reason

src/modules/commander/commander.cpp

@@ -2477,8 +2477,8 @@ int commander_thread_main(int argc, char *argv[])
 
 			status_flags.condition_auto_mission_available = _mission_result.valid && !_mission_result.finished;
 
-			if (status.mission_failure != _mission_result.mission_failure) {
-				status.mission_failure = _mission_result.mission_failure;
+			if (status.mission_failure != _mission_result.failure) {
+				status.mission_failure = _mission_result.failure;
 				status_changed = true;
 
 				if (status.mission_failure) {
@@ -2625,6 +2625,7 @@ int commander_thread_main(int argc, char *argv[])
 		if (status_flags.condition_home_position_valid &&
 			(hrt_elapsed_time(&_home.timestamp) > 2000000) &&
 			_last_mission_instance != _mission_result.instance_count) {
+
 			if (!_mission_result.valid) {
 				/* the mission is invalid */
 				tune_mission_fail(true);

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -1525,8 +1525,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 
 			// continuously reset launch detection and runway takeoff until armed
 			if (!_control_mode.flag_armed) {
-				_runway_takeoff.reset();
-
 				_launchDetector.reset();
 				_launch_detection_state = LAUNCHDETECTION_RES_NONE;
 				_launch_detection_notify = 0;

src/modules/navigator/geofence.cpp

@@ -38,54 +38,30 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 #include "geofence.h"
-
-#include <uORB/topics/home_position.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <string.h>
-#include <dataman/dataman.h>
-#include <systemlib/err.h>
-#include <systemlib/mavlink_log.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <ctype.h>
-#include <px4_config.h>
-#include <px4_defines.h>
-#include <unistd.h>
-#include <geo/geo.h>
-#include <drivers/drv_hrt.h>
 #include "navigator.h"
 
+#include <ctype.h>
+
+#include <dataman/dataman.h>
+#include <drivers/drv_hrt.h>
+#include <geo/geo.h>
+#include <systemlib/mavlink_log.h>
+
 #define GEOFENCE_RANGE_WARNING_LIMIT 5000000
 
 Geofence::Geofence(Navigator *navigator) :
 	SuperBlock(navigator, "GF"),
 	_navigator(navigator),
-	_fence_pub(nullptr),
-	_home_pos{},
-	_home_pos_set(false),
-	_last_horizontal_range_warning(0),
-	_last_vertical_range_warning(0),
-	_altitude_min(0),
-	_altitude_max(0),
-	_vertices_count(0),
 	_param_action(this, "GF_ACTION", false),
 	_param_altitude_mode(this, "GF_ALTMODE", false),
 	_param_source(this, "GF_SOURCE", false),
 	_param_counter_threshold(this, "GF_COUNT", false),
 	_param_max_hor_distance(this, "GF_MAX_HOR_DIST", false),
-	_param_max_ver_distance(this, "GF_MAX_VER_DIST", false),
-	_outside_counter(0)
+	_param_max_ver_distance(this, "GF_MAX_VER_DIST", false)
 {
-	/* Load initial params */
 	updateParams();
 }
 
-Geofence::~Geofence()
-{
-
-}
-
-
 bool Geofence::inside(const struct vehicle_global_position_s &global_position)
 {
 	return inside(global_position.lat, global_position.lon, global_position.alt);
@@ -96,14 +72,9 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f
 	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,
-		      const struct home_position_s home_pos, bool home_position_set)
+		      const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl)
 {
-	_home_pos = home_pos;
-	_home_pos_set = home_position_set;
-
 	if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) {
 		if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
 			return inside(global_position);
@@ -133,38 +104,38 @@ bool Geofence::inside(double lat, double lon, float altitude)
 {
 	bool inside_fence = true;
 
-	float max_horizontal_distance = _param_max_hor_distance.get();
-	float max_vertical_distance = _param_max_ver_distance.get();
+	if (isHomeRequired() && _navigator->home_position_valid()) {
 
-	if (max_horizontal_distance > 1.0f || max_vertical_distance > 1.0f) {
-		if (_home_pos_set) {
-			float dist_xy = -1.0f;
-			float dist_z = -1.0f;
-			get_distance_to_point_global_wgs84(lat, lon, altitude,
-							   _home_pos.lat, _home_pos.lon, _home_pos.alt,
-							   &dist_xy, &dist_z);
+		const float max_horizontal_distance = _param_max_hor_distance.get();
+		const float max_vertical_distance = _param_max_ver_distance.get();
 
-			if (max_vertical_distance > 1.0f && (dist_z > max_vertical_distance)) {
-				if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
-					mavlink_log_critical(_navigator->get_mavlink_log_pub(),
-							     "Maximum altitude above home exceeded by %.1f m",
-							     (double)(dist_z - max_vertical_distance));
-					_last_vertical_range_warning = hrt_absolute_time();
-				}
+		const double home_lat = _navigator->get_home_position()->lat;
+		const double home_lon = _navigator->get_home_position()->lon;
+		const double home_alt = _navigator->get_home_position()->alt;
 
-				inside_fence = false;
+		float dist_xy = -1.0f;
+		float dist_z = -1.0f;
+
+		get_distance_to_point_global_wgs84(lat, lon, altitude, home_lat, home_lon, home_alt, &dist_xy, &dist_z);
+
+		if (max_vertical_distance > FLT_EPSILON && (dist_z > max_vertical_distance)) {
+			if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum altitude above home exceeded by %.1f m",
+						     (double)(dist_z - max_vertical_distance));
+				_last_vertical_range_warning = hrt_absolute_time();
 			}
 
-			if (max_horizontal_distance > 1.0f && (dist_xy > max_horizontal_distance)) {
-				if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
-					mavlink_log_critical(_navigator->get_mavlink_log_pub(),
-							     "Maximum distance from home exceeded by %.1f m",
-							     (double)(dist_xy - max_horizontal_distance));
-					_last_horizontal_range_warning = hrt_absolute_time();
-				}
+			inside_fence = false;
+		}
 
-				inside_fence = false;
+		if (max_horizontal_distance > FLT_EPSILON && (dist_xy > max_horizontal_distance)) {
+			if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum distance from home exceeded by %.1f m",
+						     (double)(dist_xy - max_horizontal_distance));
+				_last_horizontal_range_warning = hrt_absolute_time();
 			}
+
+			inside_fence = false;
 		}
 	}
 
@@ -188,11 +159,9 @@ bool Geofence::inside(double lat, double lon, float altitude)
 	}
 }
 
-
 bool Geofence::inside_polygon(double lat, double lon, float altitude)
 {
 	if (valid()) {
-
 		if (!isEmpty()) {
 			/* Vertical check */
 			if (altitude > _altitude_max || altitude < _altitude_min) {
@@ -426,3 +395,12 @@ int Geofence::clearDm()
 	dm_clear(DM_KEY_FENCE_POINTS);
 	return PX4_OK;
 }
+
+bool Geofence::isHomeRequired()
+{
+	bool max_horizontal_enabled = (_param_max_hor_distance.get() > FLT_EPSILON);
+	bool max_vertical_enabled = (_param_max_ver_distance.get() > FLT_EPSILON);
+	bool geofence_action_rtl = (getGeofenceAction() == geofence_result_s::GF_ACTION_RTL);
+
+	return max_horizontal_enabled || max_vertical_enabled || geofence_action_rtl;
+}

src/modules/navigator/geofence.h

@@ -41,15 +41,16 @@
 #ifndef GEOFENCE_H_
 #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 <uORB/topics/home_position.h>
-#include <controllib/blocks.hpp>
+#include <cfloat>
+
 #include <controllib/block/BlockParam.hpp>
+#include <controllib/blocks.hpp>
 #include <drivers/drv_hrt.h>
 #include <px4_defines.h>
+#include <uORB/topics/fence.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
 
 #define GEOFENCE_FILENAME PX4_ROOTFSDIR"/fs/microsd/etc/geofence.txt"
 
@@ -59,11 +60,9 @@ class Geofence : public control::SuperBlock
 {
 public:
 	Geofence(Navigator *navigator);
-
 	Geofence(const Geofence &) = delete;
 	Geofence &operator=(const Geofence &) = delete;
-
-	~Geofence();
+	~Geofence() = default;
 
 	/* Altitude mode, corresponding to the param GF_ALTMODE */
 	enum {
@@ -85,8 +84,7 @@ public:
 	 * @return true: system is inside fence, false: system is outside fence
 	 */
 	bool inside(const struct vehicle_global_position_s &global_position,
-		    const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl,
-		    const struct home_position_s home_pos, bool home_position_set);
+		    const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl);
 
 	bool inside(const struct mission_item_s &mission_item);
 
@@ -108,26 +106,23 @@ public:
 	bool isEmpty() {return _vertices_count == 0;}
 
 	int getAltitudeMode() { return _param_altitude_mode.get(); }
-
 	int getSource() { return _param_source.get(); }
-
 	int getGeofenceAction() { return _param_action.get(); }
 
+	bool isHomeRequired();
+
 private:
-	Navigator	*_navigator;
+	Navigator	*_navigator{nullptr};
 
-	orb_advert_t	_fence_pub;			/**< publish fence topic */
+	orb_advert_t	_fence_pub{nullptr};			/**< publish fence topic */
 
-	home_position_s _home_pos;
-	bool _home_pos_set;
+	hrt_abstime _last_horizontal_range_warning{0};
+	hrt_abstime _last_vertical_range_warning{0};
 
-	hrt_abstime _last_horizontal_range_warning;
-	hrt_abstime _last_vertical_range_warning;
+	float _altitude_min{0.0f};
+	float _altitude_max{0.0f};
 
-	float _altitude_min;
-	float _altitude_max;
-
-	unsigned _vertices_count;
+	unsigned _vertices_count{0};
 
 	/* Params */
 	control::BlockParamInt _param_action;
@@ -137,12 +132,11 @@ private:
 	control::BlockParamFloat _param_max_hor_distance;
 	control::BlockParamFloat _param_max_ver_distance;
 
-	int _outside_counter;
+	int _outside_counter{0};
 
 	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);
 };
 
-
 #endif /* GEOFENCE_H_ */

src/modules/navigator/mission.cpp

@@ -67,7 +67,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_param_yawmode(this, "MIS_YAWMODE", false),
 	_param_force_vtol(this, "NAV_FORCE_VT", false),
 	_param_fw_climbout_diff(this, "FW_CLMBOUT_DIFF", false),
-	_missionFeasibilityChecker()
+	_missionFeasibilityChecker(navigator)
 {
 	updateParams();
 }
@@ -304,7 +304,7 @@ Mission::update_onboard_mission()
 		// XXX check validity here as well
 		_navigator->get_mission_result()->valid = true;
 		/* reset mission failure if we have an updated valid mission */
-		_navigator->get_mission_result()->mission_failure = false;
+		_navigator->get_mission_result()->failure = false;
 
 		/* reset reached info as well */
 		_navigator->get_mission_result()->reached = false;
@@ -355,7 +355,7 @@ Mission::update_offboard_mission()
 
 		if (!failed) {
 			/* reset mission failure if we have an updated valid mission */
-			_navigator->get_mission_result()->mission_failure = false;
+			_navigator->get_mission_result()->failure = false;
 
 			/* reset reached info as well */
 			_navigator->get_mission_result()->reached = false;
@@ -1374,21 +1374,8 @@ Mission::check_mission_valid(bool force)
 {
 	if ((!_home_inited && _navigator->home_position_valid()) || force) {
 
-		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
-
 		_navigator->get_mission_result()->valid =
-			_missionFeasibilityChecker.checkMissionFeasible(
-				_navigator->get_mavlink_log_pub(),
-				(_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
-				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
-				_navigator->get_home_position()->alt,
-				_navigator->home_position_valid(),
-				_navigator->get_global_position()->lat,
-				_navigator->get_global_position()->lon,
-				_param_dist_1wp.get(),
-				_navigator->get_mission_result()->warning,
-				_navigator->get_default_acceptance_radius(),
-				_navigator->get_land_detected()->landed);
+			_missionFeasibilityChecker.checkMissionFeasible(_offboard_mission, _param_dist_1wp.get(), false);
 
 		_navigator->get_mission_result()->seq_total = _offboard_mission.count;
 		_navigator->increment_mission_instance_count();

src/modules/navigator/mission_block.cpp

@@ -474,29 +474,20 @@ MissionBlock::get_time_inside(const struct mission_item_s &item)
 bool
 MissionBlock::item_contains_position(const struct mission_item_s *item)
 {
-	// XXX: maybe extend that check onto item properties
-	if (item->nav_cmd == NAV_CMD_DO_JUMP ||
-	    item->nav_cmd == NAV_CMD_DO_CHANGE_SPEED ||
-	    item->nav_cmd == NAV_CMD_DO_SET_SERVO ||
-	    item->nav_cmd == NAV_CMD_DO_LAND_START ||
-	    item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL ||
-	    item->nav_cmd == NAV_CMD_IMAGE_START_CAPTURE ||
-	    item->nav_cmd == NAV_CMD_IMAGE_STOP_CAPTURE ||
-	    item->nav_cmd == NAV_CMD_VIDEO_START_CAPTURE ||
-	    item->nav_cmd == NAV_CMD_VIDEO_STOP_CAPTURE ||
-	    item->nav_cmd == NAV_CMD_DO_MOUNT_CONFIGURE ||
-	    item->nav_cmd == NAV_CMD_DO_MOUNT_CONTROL ||
-	    item->nav_cmd == NAV_CMD_DO_SET_ROI ||
-	    item->nav_cmd == NAV_CMD_ROI ||
-	    item->nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST ||
-	    item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION ||
-	    item->nav_cmd == NAV_CMD_DELAY ||
-	    item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
+	if (item->nav_cmd == NAV_CMD_WAYPOINT ||
+	    item->nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
+	    item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+	    item->nav_cmd == NAV_CMD_LAND ||
+	    item->nav_cmd == NAV_CMD_TAKEOFF ||
+	    item->nav_cmd == NAV_CMD_LOITER_TO_ALT ||
+	    item->nav_cmd == NAV_CMD_VTOL_TAKEOFF ||
+	    item->nav_cmd == NAV_CMD_VTOL_LAND) {
+
+		return true;
 
-		return false;
 	}
 
-	return true;
+	return false;
 }
 
 void

src/modules/navigator/mission_block.h

@@ -66,7 +66,6 @@ public:
 	MissionBlock(const MissionBlock &) = delete;
 	MissionBlock &operator=(const MissionBlock &) = delete;
 
-	/* TODO: move this to a helper class in navigator */
 	static bool item_contains_position(const struct mission_item_s *item);
 
 protected:

src/modules/navigator/mission_feasibility_checker.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2017 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
@@ -42,61 +42,54 @@
 #ifndef MISSION_FEASIBILITY_CHECKER_H_
 #define MISSION_FEASIBILITY_CHECKER_H_
 
-#include <unistd.h>
+#include <dataman/dataman.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/fw_pos_ctrl_status.h>
-#include <dataman/dataman.h>
-#include "geofence.h"
 
+class Geofence;
+class Navigator;
 
 class MissionFeasibilityChecker
 {
 private:
-	orb_advert_t		*_mavlink_log_pub;
-
-	int _fw_pos_ctrl_status_sub;
-	struct fw_pos_ctrl_status_s _fw_pos_ctrl_status;
-
-	bool _initDone;
-	bool _dist_1wp_ok;
-	void init();
+	Navigator *_navigator{nullptr};
 
 	/* Checks for all airframes */
-	bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
+	bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);
+
 	bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid,
 				       bool &warning_issued, bool throw_error = false);
+
 	bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed);
-	bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp,
-			    bool &warning_issued);
-	bool isPositionCommand(unsigned cmd);
+
+	bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon,
+			    float dist_first_wp, bool &warning_issued);
 
 	/* Checks specific to fixedwing airframes */
-	bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt,
-					   bool home_valid);
-	bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems);
-	void updateNavigationCapabilities();
+	bool checkFixedwing(dm_item_t dm_current, size_t nMissionItems, fw_pos_ctrl_status_s *fw_pos_ctrl_status,
+			    float home_alt, bool home_valid, float default_acceptance_rad, bool land_start_req);
+
+	bool checkFixedWingTakeoff(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid,
+				   float default_acceptance_rad);
+	bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems, fw_pos_ctrl_status_s *fw_pos_ctrl_status,
+				   bool land_start_req);
 
 	/* Checks specific to rotarywing airframes */
-	bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt,
-					    bool home_valid, float default_acceptance_rad);
-public:
+	bool checkRotarywing(dm_item_t dm_current, size_t nMissionItems,
+			     float home_alt, bool home_valid, float default_acceptance_rad);
 
-	MissionFeasibilityChecker();
+public:
+	MissionFeasibilityChecker(Navigator *navigator) : _navigator(navigator) {};
+	~MissionFeasibilityChecker() = default;
 
 	MissionFeasibilityChecker(const MissionFeasibilityChecker &) = delete;
 	MissionFeasibilityChecker &operator=(const MissionFeasibilityChecker &) = delete;
 
-	~MissionFeasibilityChecker() {}
-
 	/*
 	 * Returns true if mission is feasible and false otherwise
 	 */
-	bool checkMissionFeasible(orb_advert_t *mavlink_log_pub, bool isRotarywing, dm_item_t dm_current,
-				  size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid,
-				  double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, float default_acceptance_rad,
-				  bool condition_landed);
+	bool checkMissionFeasible(const mission_s &mission, float max_waypoint_distance, bool land_start_req);
 
 };
 
-
 #endif /* MISSION_FEASIBILITY_CHECKER_H_ */

src/modules/navigator/mission_params.c

@@ -87,7 +87,7 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1);
  *
  * Failsafe check to prevent running mission stored from previous flight at a new takeoff location.
  * Set a value of zero or less to disable. The mission will not be started if the current
- * waypoint is more distant than MIS_DIS_1WP from the current position.
+ * waypoint is more distant than MIS_DIS_1WP from the home position.
  *
  * @unit m
  * @min 0

src/modules/navigator/navigator.h

@@ -304,7 +304,7 @@ private:
 	float _mission_throttle{-1.0f};
 
 	// update subscriptions
-	void		fw_pos_ctrl_status_update();
+	void		fw_pos_ctrl_status_update(bool force = false);
 	void		global_position_update();
 	void		gps_position_update();
 	void		home_position_update(bool force = false);

src/modules/navigator/navigator_main.cpp

@@ -180,12 +180,12 @@ Navigator::home_position_update(bool force)
 }
 
 void
-Navigator::fw_pos_ctrl_status_update()
+Navigator::fw_pos_ctrl_status_update(bool force)
 {
 	bool updated = false;
 	orb_check(_fw_pos_ctrl_status_sub, &updated);
 
-	if (updated) {
+	if (updated || force) {
 		orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status);
 	}
 }
@@ -265,7 +265,7 @@ Navigator::task_main()
 	gps_position_update();
 	sensor_combined_update();
 	home_position_update(true);
-	fw_pos_ctrl_status_update();
+	fw_pos_ctrl_status_update(true);
 	params_update();
 
 	/* wakeup source(s) */
@@ -531,13 +531,13 @@ Navigator::task_main()
 		    (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
 		    (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {
 
-			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos,
-						       home_position_valid());
+			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter);
 			last_geofence_check = hrt_absolute_time();
 			have_geofence_position_data = false;
 
 			_geofence_result.timestamp = hrt_absolute_time();
 			_geofence_result.geofence_action = _geofence.getGeofenceAction();
+			_geofence_result.home_required = _geofence.isHomeRequired();
 
 			if (!inside) {
 				/* inform other apps via the mission result */
@@ -969,8 +969,8 @@ Navigator::publish_vehicle_cmd(const struct vehicle_command_s &vcmd)
 void
 Navigator::set_mission_failure(const char *reason)
 {
-	if (!_mission_result.mission_failure) {
-		_mission_result.mission_failure = true;
+	if (!_mission_result.failure) {
+		_mission_result.failure = true;
 		set_mission_result_updated();
 		mavlink_log_critical(&_mavlink_log_pub, "%s", reason);
 	}