diff --git a/Firmware.sublime-project b/Firmware.sublime-project
index bfdddda2b9..2ab2b47c0b 100644
--- a/Firmware.sublime-project
+++ b/Firmware.sublime-project
@@ -1,8 +1,50 @@
 {
+	"build_systems":
+	[
+		{
+			"cmd":
+			[
+				"make"
+			],
+			"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
+			"name": "PX4: make all",
+			"shell": true,
+			"working_dir": "${project_path}"
+		},
+		{
+			"cmd":
+			[
+				"make upload px4fmu-v2_default -j8"
+			],
+			"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
+			"name": "PX4: make and upload",
+			"shell": true,
+			"working_dir": "${project_path}"
+		},
+		{
+			"cmd":
+			[
+				"make posix"
+			],
+			"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
+			"name": "PX4: make posix",
+			"shell": true,
+			"working_dir": "${project_path}"
+		},
+		{
+			"cmd":
+			[
+				"make upload mindpx-v2_default -j8"
+			],
+			"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
+			"name": "MindPX_V2: make and upload",
+			"shell": true,
+			"working_dir": "${project_path}"
+		}
+	],
 	"folders":
 	[
 		{
-			"path": ".",
 			"file_exclude_patterns":
 			[
 				"*.o",
@@ -26,57 +68,87 @@
 				".settings",
 				"nuttx/arch/arm/src/board",
 				"nuttx/arch/arm/src/chip"
-			]
+			],
+			"path": "."
+		},
+		{
+			"path": "build_posix_sitl_default"
+		},
+		{
+			"path": "build_px4fmu-v2_default"
+		},
+		{
+			"path": "build_px4fmu-v4_default"
+		},
+		{
+			"path": "cmake"
+		},
+		{
+			"path": "Debug"
+		},
+		{
+			"path": "Documentation"
+		},
+		{
+			"path": "Images"
+		},
+		{
+			"path": "integrationtests"
+		},
+		{
+			"path": "launch"
+		},
+		{
+			"path": "mavlink"
+		},
+		{
+			"path": "misc"
+		},
+		{
+			"path": "msg"
+		},
+		{
+			"path": "NuttX"
+		},
+		{
+			"path": "nuttx-configs"
+		},
+		{
+			"path": "posix-configs"
+		},
+		{
+			"path": "ROMFS"
+		},
+		{
+			"path": "src"
+		},
+		{
+			"path": "test_data"
+		},
+		{
+			"path": "Tools"
+		},
+		{
+			"path": "unittests"
 		}
 	],
 	"settings":
 	{
-		"tab_size": 8,
-		"translate_tabs_to_spaces": false,
-		"highlight_line": true,
 		"AStyleFormatter":
 		{
 			"options_c":
 			{
-				"use_only_additional_options": true,
-				"additional_options_file": "${project_path}/Tools/astylerc"
+				"additional_options_file": "${project_path}/Tools/astylerc",
+				"use_only_additional_options": true
 			},
 			"options_c++":
 			{
-				"use_only_additional_options": true,
-				"additional_options_file": "${project_path}/Tools/astylerc"
+				"additional_options_file": "${project_path}/Tools/astylerc",
+				"use_only_additional_options": true
 			}
-		}
-	},
-	"build_systems":
-	[
-		{
-			"name": "PX4: make all",
-			"working_dir": "${project_path}",
-			"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
-			"cmd": ["make"],
-			"shell": true
 		},
-		{
-			"name": "PX4: make and upload",
-			"working_dir": "${project_path}",
-			"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
-			"cmd": ["make upload px4fmu-v2_default -j8"],
-			"shell": true
-		},
-		{
-			"name": "PX4: make posix",
-			"working_dir": "${project_path}",
-			"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
-			"cmd": ["make posix"],
-			"shell": true
-		},
-		{
-			"name": "MindPX_V2: make and upload",
-			"working_dir": "${project_path}",
-			"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
-			"cmd": ["make upload mindpx-v2_default -j8"],
-			"shell": true
-		}
-	]
+		"highlight_line": true,
+		"tab_size": 8,
+		"translate_tabs_to_spaces": false
+	}
 }
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 1ea176ad70..c1cf2146b5 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1076,11 +1076,12 @@ void MulticopterPositionControl::control_auto(float dt)
 
 		_reset_pos_sp = true;
 		_reset_alt_sp = true;
-
-		reset_pos_sp();
-		reset_alt_sp();
 	}
 
+	// Always check reset state of altitude and position control flags in auto
+	reset_pos_sp();
+	reset_alt_sp();
+
 	//Poll position setpoint
 	bool updated;
 	orb_check(_pos_sp_triplet_sub, &updated);
@@ -1239,25 +1240,6 @@ void MulticopterPositionControl::control_auto(float dt)
 			_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
 		}
 
-		/*
-		 * if we're already near the current takeoff setpoint don't reset in case we switch back to posctl.
-		 * this makes the takeoff finish smoothly.
-		 */
-		if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
-		     || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)
-		    && _pos_sp_triplet.current.acceptance_radius > 0.0f
-		    /* need to detect we're close a bit before the navigator switches from takeoff to next waypoint */
-		    && (_pos - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius * 1.2f) {
-			_reset_pos_sp = false;
-			_reset_alt_sp = false;
-
-			/* otherwise: in case of interrupted mission don't go to waypoint but stay at current position */
-
-		} else {
-			_reset_pos_sp = true;
-			_reset_alt_sp = true;
-		}
-
 		// During a mission or in loiter it's safe to retract the landing gear.
 		if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION ||
 		     _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) &&
@@ -1660,7 +1642,7 @@ MulticopterPositionControl::task_main()
 				acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt;
 				acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt;
 
-				if (acc_hor.length() > _params.acc_hor_max) {
+				if ((acc_hor.length() > _params.acc_hor_max) & !_reset_pos_sp) {
 					acc_hor.normalize();
 					acc_hor *= _params.acc_hor_max;
 					math::Vector<2> vel_sp_hor_prev(_vel_sp_prev(0), _vel_sp_prev(1));
@@ -1670,9 +1652,10 @@ MulticopterPositionControl::task_main()
 				}
 
 				// limit vertical acceleration
+
 				float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt;
 
-				if (fabsf(acc_v) > 2 * _params.acc_hor_max) {
+				if ((fabsf(acc_v) > 2 * _params.acc_hor_max) & !_reset_alt_sp) {
 					acc_v /= fabsf(acc_v);
 					_vel_sp(2) = acc_v * 2 * _params.acc_hor_max * dt + _vel_sp_prev(2);
 				}