diff --git a/posix-configs/SITL/init/rcS_gazebo_plane b/posix-configs/SITL/init/rcS_gazebo_plane
index bcd62ca582f..a431c1ed288 100644
--- a/posix-configs/SITL/init/rcS_gazebo_plane
+++ b/posix-configs/SITL/init/rcS_gazebo_plane
@@ -37,11 +37,11 @@ pwm_out_sim mode_pwm
 sleep 1
 sensors start
 commander start
-land_detector start fixedwing
 navigator start
 ekf2 start
 fw_pos_control_l1 start
 fw_att_control start
+land_detector start fixedwing
 mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/plane_sitl.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 3752f56bf02..e45cb9d15c3 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -97,8 +97,6 @@
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/vtol_vehicle_status.h>
-#include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index a55a43c813e..0788056104c 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -1184,9 +1184,8 @@ bool FixedwingPositionControl::update_desired_altitude(float dt)
 bool FixedwingPositionControl::in_takeoff_situation()
 {
 	const hrt_abstime delta_takeoff = 10000000;
-	const float throttle_threshold = 0.1f;
 
-	if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold
+	if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff
 	    && _global_pos.alt <= _takeoff_ground_alt + _parameters.climbout_diff) {
 		return true;
 	}
@@ -1362,10 +1361,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				alt_sp = pos_sp_triplet.current.alt + 2.0f * _parameters.climbout_diff;
 
 			} else {
-				// use altitude given by wapoint
+				// use altitude given by waypoint
 				alt_sp = pos_sp_triplet.current.alt;
 			}
 
+			if (in_takeoff_situation()) {
+				alt_sp = math::max(alt_sp, _takeoff_ground_alt + _parameters.climbout_diff);
+			}
+
 			if (in_takeoff_situation() ||
 			    ((_global_pos.alt < pos_sp_triplet.current.alt + _parameters.climbout_diff)
 			     && _fw_pos_ctrl_status.abort_landing == true)) {