Navigator resurrect FW GPS failure navigation (#7762)

src/modules/commander/state_machine_helper.cpp

@@ -72,7 +72,6 @@ static const char reason_no_rc[] = "no RC";
 static const char reason_no_offboard[] = "no offboard";
 static const char reason_no_rc_and_no_offboard[] = "no RC and no offboard";
 static const char reason_no_gps[] = "no gps";
-static const char reason_no_gps_cmd[] = "no gps cmd";
 static const char reason_no_local_position[] = "no local position";
 static const char reason_no_global_position[] = "no global position";
 static const char reason_no_datalink[] = "no datalink";
@@ -518,6 +517,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 
 				if (hitl_on) {
 					mavlink_log_info(mavlink_log_pub, "Enabled Hardware-in-the-loop simulation.");
+
 				} else {
 					mavlink_log_critical(mavlink_log_pub, "Set parameter SYS_HITL to 1 before starting HITL.");
 					ret = TRANSITION_DENIED;
@@ -648,6 +648,7 @@ bool set_nav_state(struct vehicle_status_s *status,
 
 			} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, !(posctl_nav_loss_act == 1), !status->is_rotary_wing)) {
 				// nothing to do - everything done in check_invalid_pos_nav_state
+
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
 			}
@@ -669,20 +670,21 @@ bool set_nav_state(struct vehicle_status_s *status,
 		} else if (status_flags->data_link_lost_cmd) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
 
-		} else if (status_flags->gps_failure_cmd) {
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
-			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps_cmd);
-
 		} else if (status_flags->rc_signal_lost_cmd) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
 
 		} else if (status_flags->vtol_transition_failure_cmd) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
 
-			/* finished handling commands which have priority, now handle failures */
+		} else if (status_flags->gps_failure || status_flags->gps_failure_cmd) {
+			if (status->is_rotary_wing) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+			} else {
+				// TODO: FW position controller doesn't run without condition_global_position_valid
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+			}
 
-		} else if (status_flags->gps_failure) {
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps);
 
 		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
@@ -696,28 +698,24 @@ bool set_nav_state(struct vehicle_status_s *status,
 		} else if (status->mission_failure) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
 
+		} else if (data_link_loss_act_configured && status->data_link_lost) {
 			/* datalink loss enabled:
 			 * check for datalink lost: this should always trigger RTGS */
-
-		} else if (data_link_loss_act_configured && status->data_link_lost) {
 			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
 
 			set_data_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act);
 
+		} else if (!data_link_loss_act_configured && status->rc_signal_lost && status->data_link_lost && !landed
+			   && mission_finished) {
 			/* datalink loss DISABLED:
 			 * check if both, RC and datalink are lost during the mission
 			 * or all links are lost after the mission finishes in air: this should always trigger RCRECOVER */
-
-		} else if (!data_link_loss_act_configured && status->rc_signal_lost && status->data_link_lost && !landed
-			   && mission_finished) {
-
 			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
 
 			set_rc_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act);
 
-			/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
-
 		} else if (!stay_in_failsafe) {
+			/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
 		}
 
@@ -730,28 +728,31 @@ bool set_nav_state(struct vehicle_status_s *status,
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
 
 		} else if (status_flags->gps_failure) {
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+			if (status->is_rotary_wing) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+			} else {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+			}
+
 			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps);
 
-			/* also go into failsafe if just datalink is lost, and we're actually in air */
-
-			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
 		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
 		} else if (status->data_link_lost && data_link_loss_act_configured && !landed) {
-
+			/* also go into failsafe if just datalink is lost, and we're actually in air */
 			set_data_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act);
 
-			/* go into failsafe if RC is lost and datalink loss is not set up and rc loss is not DISABLED */
+			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
 
 		} else if (rc_lost && !data_link_loss_act_configured) {
+			/* go into failsafe if RC is lost and datalink loss is not set up and rc loss is not DISABLED */
 			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
 
 			set_rc_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act);
 
-			/* don't bother if RC is lost if datalink is connected */
-
 		} else if (status->rc_signal_lost) {
+			/* don't bother if RC is lost if datalink is connected */
 
 			/* this mode is ok, we don't need RC for LOITERing */
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
@@ -771,7 +772,14 @@ bool set_nav_state(struct vehicle_status_s *status,
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
 
 		} else if (status_flags->gps_failure) {
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+			if (status->is_rotary_wing) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+			} else {
+				// TODO: FW position controller doesn't run without condition_global_position_valid
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+			}
+
 			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps);
 
 		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
@@ -857,7 +865,13 @@ bool set_nav_state(struct vehicle_status_s *status,
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
 
 				} else if (status_flags->condition_local_altitude_valid) {
-					status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+					if (status->is_rotary_wing) {
+						status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+					} else {
+						// TODO: FW position controller doesn't run without condition_global_position_valid
+						status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+					}
 
 				} else {
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -890,7 +904,13 @@ bool set_nav_state(struct vehicle_status_s *status,
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
 
 				} else if (status_flags->condition_local_altitude_valid) {
-					status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+					if (status->is_rotary_wing) {
+						status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+					} else {
+						// TODO: FW position controller doesn't run without condition_global_position_valid
+						status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+					}
 
 				} else {
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -901,7 +921,13 @@ bool set_nav_state(struct vehicle_status_s *status,
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
 
 				} else if (status_flags->condition_local_altitude_valid) {
-					status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+					if (status->is_rotary_wing) {
+						status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+					} else {
+						// TODO: FW position controller doesn't run without condition_global_position_valid
+						status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+					}
 
 				} else {
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -948,17 +974,27 @@ bool check_invalid_pos_nav_state(struct vehicle_status_s *status,
 			// fallback to a mode that gives the operator stick control
 			if (status->is_rotary_wing && status_flags->condition_local_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+
 			} else if (status_flags->condition_local_altitude_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
+
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
 			}
+
 		} else {
 			// go into a descent that does not require stick control
 			if (status_flags->condition_local_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+
 			} else  if (status_flags->condition_local_altitude_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+				if (status->is_rotary_wing) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+				} else {
+					// TODO: FW position controller doesn't run without condition_global_position_valid
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+				}
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
@@ -966,6 +1002,7 @@ bool check_invalid_pos_nav_state(struct vehicle_status_s *status,
 
 		if (using_global_pos) {
 			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_global_position);
+
 		} else {
 			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_local_position);
 		}
@@ -1026,7 +1063,13 @@ void set_link_loss_nav_state(vehicle_status_s *status,
 		status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
 
 	} else if (status_flags->condition_local_altitude_valid) {
-		status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+		if (status->is_rotary_wing) {
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+		} else {
+			// TODO: FW position controller doesn't run without condition_global_position_valid
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+		}
 
 	} else {
 		status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;

src/modules/navigator/gpsfailure.cpp

@@ -37,24 +37,21 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include <string.h>
-#include <stdlib.h>
-#include <math.h>
-#include <fcntl.h>
+#include "gpsfailure.h"
+#include "navigator.h"
 
 #include <systemlib/mavlink_log.h>
-#include <systemlib/err.h>
 #include <geo/geo.h>
 #include <navigator/navigation.h>
-
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/home_position.h>
+#include <mathlib/mathlib.h>
 
-#include "navigator.h"
-#include "gpsfailure.h"
+using matrix::Eulerf;
+using matrix::Quatf;
 
-#define DELAY_SIGMA	0.01f
+static constexpr float DELAY_SIGMA = 0.01f;
 
 GpsFailure::GpsFailure(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
@@ -67,14 +64,11 @@ GpsFailure::GpsFailure(Navigator *navigator, const char *name) :
 {
 	/* load initial params */
 	updateParams();
+
 	/* initial reset */
 	on_inactive();
 }
 
-GpsFailure::~GpsFailure()
-{
-}
-
 void
 GpsFailure::on_inactive()
 {
@@ -96,24 +90,34 @@ GpsFailure::on_activation()
 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();
+			vehicle_attitude_setpoint_s att_sp = {};
+			att_sp.timestamp = hrt_absolute_time();
+			att_sp.roll_body = math::radians(_param_openlooploiter_roll.get());
+			att_sp.pitch_body = math::radians(_param_openlooploiter_pitch.get());
+			att_sp.thrust = _param_openlooploiter_thrust.get();
+
+			Quatf q(Eulerf(att_sp.roll_body, att_sp.pitch_body, 0.0f));
+			q.copyTo(att_sp.q_d);
+			att_sp.q_d_valid = true;
+
+			if (_att_sp_pub != nullptr) {
+				/* 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);
+			}
 
 			/* 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");
+			if ((_param_loitertime.get() > FLT_EPSILON) &&
+			    (hrt_elapsed_time(&_timestamp_activation) > _param_loitertime.get() * 1e6f)) {
+				/* no recovery, advance the state machine */
+				PX4_WARN("GPS not recovered, switching to next failure state");
 				advance_gpsf();
 			}
 
@@ -142,17 +146,17 @@ GpsFailure::set_gpsf_item()
 
 	switch (_gpsf_state) {
 	case GPSF_STATE_TERMINATE: {
-			/* Request flight termination from the commander */
+			/* Request flight termination from commander */
 			_navigator->get_mission_result()->flight_termination = true;
 			_navigator->set_mission_result_updated();
-			warnx("gps fail: request flight termination");
+			PX4_WARN("GPS failure: request flight termination");
 		}
+		break;
 
 	default:
 		break;
 	}
 
-	reset_mission_item_reached();
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
@@ -162,20 +166,18 @@ GpsFailure::advance_gpsf()
 	switch (_gpsf_state) {
 	case GPSF_STATE_NONE:
 		_gpsf_state = GPSF_STATE_LOITER;
-		warnx("gpsf loiter");
-		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "GPS failed: open loop loiter");
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Global position failure: fixed bank loiter");
 		break;
 
 	case GPSF_STATE_LOITER:
 		_gpsf_state = GPSF_STATE_TERMINATE;
-		warnx("gpsf terminate");
-		mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "no gps recovery, termination");
-		warnx("mavlink sent");
+		mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "no GPS recovery, terminating flight");
 		break;
 
 	case GPSF_STATE_TERMINATE:
-		warnx("gpsf end");
+		PX4_WARN("terminate");
 		_gpsf_state = GPSF_STATE_END;
+		break;
 
 	default:
 		break;

src/modules/navigator/gpsfailure.h

@@ -47,9 +47,6 @@
 #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;
@@ -58,14 +55,11 @@ class GpsFailure : public MissionBlock
 {
 public:
 	GpsFailure(Navigator *navigator, const char *name);
+	~GpsFailure() = default;
 
-	~GpsFailure();
-
-	virtual void on_inactive();
-
-	virtual void on_activation();
-
-	virtual void on_active();
+	void on_inactive() override;
+	void on_activation() override;
+	void on_active() override;
 
 private:
 	/* Params */
@@ -79,9 +73,11 @@ private:
 		GPSF_STATE_LOITER = 1,
 		GPSF_STATE_TERMINATE = 2,
 		GPSF_STATE_END = 3,
-	} _gpsf_state;
+	} _gpsf_state{GPSF_STATE_NONE};
 
-	hrt_abstime _timestamp_activation; //*< timestamp when this mode was activated */
+	hrt_abstime _timestamp_activation{0}; //*< timestamp when this mode was activated */
+
+	orb_advert_t	_att_sp_pub{nullptr};
 
 	/**
 	 * Set the GPSF item

src/modules/navigator/gpsfailure_params.c

@@ -36,18 +36,13 @@
  *
  * Parameters for GPSF navigation mode
  *
- * @author Thomas Gubler <thomasgubler@gmail.com>
- */
-
-/*
- * 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.
+ * The time in seconds the system should do open loop loiter and wait for GPS recovery
+ * before it goes into flight termination. Set to 0 to disable.
  *
  * @unit s
  * @min 0.0
@@ -56,12 +51,12 @@
  * @increment 1
  * @group GPS Failure Navigation
  */
-PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f);
+PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 0.0f);
 
 /**
- * Open loop loiter roll
+ * Fixed bank angle
  *
- * Roll in degrees during the open loop loiter
+ * Roll in degrees during the loiter
  *
  * @unit deg
  * @min 0.0
@@ -73,7 +68,7 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f);
 PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f);
 
 /**
- * Open loop loiter pitch
+ * Fixed pitch angle
  *
  * Pitch in degrees during the open loop loiter
  *
@@ -87,7 +82,7 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f);
 PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f);
 
 /**
- * Open loop loiter thrust
+ * Thrust
  *
  * Thrust value which is set during the open loop loiter
  *
@@ -98,4 +93,4 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f);
  * @increment 0.05
  * @group GPS Failure Navigation
  */
-PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.7f);
+PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.0f);

src/modules/navigator/navigator.h

@@ -130,7 +130,6 @@ public:
 	struct position_setpoint_triplet_s *get_position_setpoint_triplet() { return &_pos_sp_triplet; }
 	struct position_setpoint_triplet_s *get_reposition_triplet() { return &_reposition_triplet; }
 	struct position_setpoint_triplet_s *get_takeoff_triplet() { return &_takeoff_triplet; }
-	struct vehicle_attitude_setpoint_s *get_att_sp() { return &_att_sp; }
 	struct vehicle_global_position_s *get_global_position() { return &_global_pos; }
 	struct vehicle_gps_position_s *get_gps_position() { return &_gps_pos; }
 	struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; }
@@ -239,7 +238,6 @@ private:
 	int		_vehicle_command_sub{-1};	/**< vehicle commands (onboard and offboard) */
 	int		_vstatus_sub{-1};		/**< vehicle status subscription */
 
-	orb_advert_t	_att_sp_pub{nullptr};
 	orb_advert_t	_geofence_result_pub{nullptr};
 	orb_advert_t	_mission_result_pub{nullptr};
 	orb_advert_t	_pos_sp_triplet_pub{nullptr};

src/modules/navigator/navigator_main.cpp

@@ -57,14 +57,9 @@
 #include <px4_tasks.h>
 #include <sys/ioctl.h>
 #include <sys/stat.h>
-
 #include <sys/types.h>
 #include <systemlib/mavlink_log.h>
 #include <systemlib/systemlib.h>
-#include <drivers/device/device.h>
-#include <arch/board/board.h>
-
-#include <uORB/uORB.h>
 #include <uORB/topics/fw_pos_ctrl_status.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/mission.h>
@@ -267,13 +262,11 @@ Navigator::task_main()
 	px4_pollfd_struct_t fds[1] = {};
 
 	/* Setup of loop */
-	fds[0].fd = _global_pos_sub;
+	fds[0].fd = _local_pos_sub;
 	fds[0].events = POLLIN;
 
-	bool global_pos_available_once = false;
-
-	/* rate-limit global pos subscription to 20 Hz / 50 ms */
-	orb_set_interval(_global_pos_sub, 49);
+	/* rate-limit position subscription to 20 Hz / 50 ms */
+	orb_set_interval(_local_pos_sub, 50);
 
 	while (!_task_should_exit) {
 
@@ -281,11 +274,6 @@ Navigator::task_main()
 		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
 
 		if (pret == 0) {
-			/* timed out - periodic check for _task_should_exit, etc. */
-			if (global_pos_available_once) {
-				global_pos_available_once = false;
-			}
-
 			/* Let the loop run anyway, don't do `continue` here. */
 
 		} else if (pret < 0) {
@@ -295,16 +283,9 @@ Navigator::task_main()
 			continue;
 
 		} else {
-
 			if (fds[0].revents & POLLIN) {
-				/* success, global pos is available */
-				global_position_update();
-
-				if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
-					have_geofence_position_data = true;
-				}
-
-				global_pos_available_once = true;
+				/* success, local pos is available */
+				local_position_update();
 			}
 		}
 
@@ -323,11 +304,15 @@ Navigator::task_main()
 			}
 		}
 
-		/* local position updated */
-		orb_check(_local_pos_sub, &updated);
+		/* global position updated */
+		orb_check(_global_pos_sub, &updated);
 
 		if (updated) {
-			local_position_update();
+			global_position_update();
+
+			if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
+				have_geofence_position_data = true;
+			}
 		}
 
 		/* sensors combined updated */
@@ -372,6 +357,7 @@ Navigator::task_main()
 			home_position_update();
 		}
 
+		/* vehicle_command updated */
 		orb_check(_vehicle_command_sub, &updated);
 
 		if (updated) {
@@ -509,9 +495,6 @@ Navigator::task_main()
 					_mission.set_current_offboard_mission_index(cmd.param1);
 				}
 
-			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) {
-				warnx("navigator: got pause/continue command");
-
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) {
 				if (cmd.param2 > FLT_EPSILON) {
 					// XXX not differentiating ground and airspeed yet
@@ -680,29 +663,17 @@ Navigator::task_main()
 	}
 
 	orb_unsubscribe(_global_pos_sub);
-	_global_pos_sub = -1;
 	orb_unsubscribe(_local_pos_sub);
-	_local_pos_sub = -1;
 	orb_unsubscribe(_gps_pos_sub);
-	_gps_pos_sub = -1;
 	orb_unsubscribe(_sensor_combined_sub);
-	_sensor_combined_sub = -1;
 	orb_unsubscribe(_fw_pos_ctrl_status_sub);
-	_fw_pos_ctrl_status_sub = -1;
 	orb_unsubscribe(_vstatus_sub);
-	_vstatus_sub = -1;
 	orb_unsubscribe(_land_detected_sub);
-	_land_detected_sub = -1;
 	orb_unsubscribe(_home_pos_sub);
-	_home_pos_sub = -1;
 	orb_unsubscribe(_onboard_mission_sub);
-	_onboard_mission_sub = -1;
 	orb_unsubscribe(_offboard_mission_sub);
-	_offboard_mission_sub = -1;
 	orb_unsubscribe(_param_update_sub);
-	_param_update_sub = -1;
 	orb_unsubscribe(_vehicle_command_sub);
-	_vehicle_command_sub = -1;
 
 	PX4_INFO("exiting");
 
@@ -985,20 +956,6 @@ Navigator::publish_geofence_result()
 	}
 }
 
-void
-Navigator::publish_att_sp()
-{
-	/* lazily publish the attitude sp only once available */
-	if (_att_sp_pub != nullptr) {
-		/* 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);
-	}
-}
-
 void
 Navigator::publish_vehicle_cmd(const struct vehicle_command_s &vcmd)
 {