From 90cee2d5eae2ec098d86c17e0b61e6c58b98f6b1 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Tue, 30 Oct 2018 10:04:50 +0100
Subject: [PATCH] Manual Smoth Vel - Override reActivate to not reset Z axis
 (prevented takeoff)

---
 .../FlightTaskManualPositionSmoothVel.cpp     | 32 +++++++++++++++++--
 .../FlightTaskManualPositionSmoothVel.hpp     |  3 ++
 2 files changed, 32 insertions(+), 3 deletions(-)

diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
index 9399e78cc2..fc4ffa7027 100644
--- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
@@ -42,16 +42,42 @@ bool FlightTaskManualPositionSmoothVel::activate()
 {
 	bool ret = FlightTaskManualPosition::activate();
 
+	reset(Axes::XYZ);
+
+	return ret;
+}
+
+void FlightTaskManualPositionSmoothVel::reActivate()
+{
+	reset(Axes::XY);
+}
+
+void FlightTaskManualPositionSmoothVel::reset(Axes axes)
+{
+	int count;
+
+	switch (axes) {
+	case Axes::XY:
+		count = 2;
+		break;
+
+	case Axes::XYZ:
+		count = 3;
+		break;
+
+	default:
+		count = 0;
+		break;
+	}
+
 	// TODO: get current accel
-	for (int i = 0; i < 3; ++i) {
+	for (int i = 0; i < count; ++i) {
 		_smoothing[i].reset(0.f, _velocity(i), _position(i));
 	}
 
 	_position_lock_xy_active = false;
 	_position_setpoint_xy_locked(0) = NAN;
 	_position_setpoint_xy_locked(1) = NAN;
-
-	return ret;
 }
 
 void FlightTaskManualPositionSmoothVel::_updateSetpoints()
diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
index b224ea65c5..9892f59cda 100644
--- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
+++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
@@ -50,6 +50,7 @@ public:
 	virtual ~FlightTaskManualPositionSmoothVel() = default;
 
 	bool activate() override;
+	void reActivate() override;
 
 protected:
 
@@ -63,6 +64,8 @@ protected:
 				       )
 private:
 
+	enum class Axes {XY, XYZ};
+	void reset(Axes axes);
 	VelocitySmoothing _smoothing[3]; ///< Smoothing in x, y and z directions
 	matrix::Vector3f _vel_sp_smooth;
 	bool _position_lock_xy_active{false};