diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index ec9528fce8..a34e2656f2 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -319,11 +319,13 @@ Navigator::task_main()
 	params_update();
 
 	/* wakeup source(s) */
-	px4_pollfd_struct_t fds[1] = {};
+	px4_pollfd_struct_t fds[2] = {};
 
 	/* Setup of loop */
 	fds[0].fd = _global_pos_sub;
 	fds[0].events = POLLIN;
+	fds[1].fd = _vehicle_command_sub;
+	fds[1].events = POLLIN;
 
 	bool global_pos_available_once = false;
 
diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp
index 95d3849f55..de13ae6480 100644
--- a/src/modules/navigator/takeoff.cpp
+++ b/src/modules/navigator/takeoff.cpp
@@ -73,14 +73,14 @@ Takeoff::on_inactive()
 void
 Takeoff::on_activation()
 {
-	/* set current mission item to Takeoff */
+	// set current mission item to takeoff
 	set_takeoff_item(&_mission_item, _param_min_alt.get());
 	_navigator->get_mission_result()->reached = false;
 	_navigator->get_mission_result()->finished = false;
 	_navigator->set_mission_result_updated();
 	reset_mission_item_reached();
 
-	/* convert mission item to current setpoint */
+	// convert mission item to current setpoint
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 	pos_sp_triplet->previous.valid = false;
 	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
@@ -88,6 +88,14 @@ Takeoff::on_activation()
 	pos_sp_triplet->current.yaw_valid = true;
 	pos_sp_triplet->next.valid = false;
 
+	// check if a specific target altitude has been set
+	struct position_setpoint_triplet_s *rep = _navigator->get_reposition_triplet();
+	if (rep->current.valid && PX4_ISFINITE(rep->current.alt)) {
+		pos_sp_triplet->current.alt = rep->current.alt;
+		// mark this as done
+		memset(rep, 0, sizeof(*rep));
+	}
+
 	_navigator->set_can_loiter_at_sp(true);
 
 	_navigator->set_position_setpoint_triplet_updated();