From dd1821b02e2985da8e42b1b5df9dcd13b880a978 Mon Sep 17 00:00:00 2001
From: devbharat <bharat.tak8@gmail.com>
Date: Tue, 15 Nov 2016 14:25:31 +0100
Subject: [PATCH] Reset alt/position when entring position control from auto

---
 Firmware.sublime-project                      | 154 +++++-------------
 .../mc_pos_control/mc_pos_control_main.cpp    |   9 +-
 2 files changed, 49 insertions(+), 114 deletions(-)

diff --git a/Firmware.sublime-project b/Firmware.sublime-project
index 2ab2b47c0b..bfdddda2b9 100644
--- a/Firmware.sublime-project
+++ b/Firmware.sublime-project
@@ -1,50 +1,8 @@
 {
-	"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",
@@ -68,87 +26,57 @@
 				".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":
 			{
-				"additional_options_file": "${project_path}/Tools/astylerc",
-				"use_only_additional_options": true
+				"use_only_additional_options": true,
+				"additional_options_file": "${project_path}/Tools/astylerc"
 			},
 			"options_c++":
 			{
-				"additional_options_file": "${project_path}/Tools/astylerc",
-				"use_only_additional_options": true
+				"use_only_additional_options": true,
+				"additional_options_file": "${project_path}/Tools/astylerc"
 			}
+		}
+	},
+	"build_systems":
+	[
+		{
+			"name": "PX4: make all",
+			"working_dir": "${project_path}",
+			"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
+			"cmd": ["make"],
+			"shell": true
 		},
-		"highlight_line": true,
-		"tab_size": 8,
-		"translate_tabs_to_spaces": false
-	}
+		{
+			"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
+		}
+	]
 }
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 c1cf2146b5..cebdf23531 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -865,6 +865,14 @@ MulticopterPositionControl::limit_pos_sp_offset()
 void
 MulticopterPositionControl::control_manual(float dt)
 {
+	/* Entering manual control from non-manual control mode, reset alt/pos setpoints */
+	if (_mode_auto) {
+		_mode_auto = false;
+
+		_reset_pos_sp = true;
+		_reset_alt_sp = true;
+	}
+
 	math::Vector<3> req_vel_sp; // X,Y in local frame and Z in global (D), in [-1,1] normalized range
 	req_vel_sp.zero();
 
@@ -1433,7 +1441,6 @@ MulticopterPositionControl::task_main()
 			if (_control_mode.flag_control_manual_enabled) {
 				/* manual control */
 				control_manual(dt);
-				_mode_auto = false;
 
 			} else if (_control_mode.flag_control_offboard_enabled) {
 				/* offboard control */