From 316e85f7ef0cea2a57f0b424c30b9e99e73f191a Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Tue, 26 Sep 2017 13:45:30 +0200
Subject: [PATCH] FlightTaskManual: Smooth flight integration: Copy over
 set_manual_acceleration_z without any changes

---
 .../FlightTasks/tasks/FlightTaskManual.hpp    | 59 +++++++++++++++++++
 1 file changed, 59 insertions(+)

diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp
index 645a1822d6..22bb7e9f90 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp
@@ -407,4 +407,63 @@ private:
 		}
 	}
 
+	void set_manual_acceleration_z(float &max_acceleration, const float stick_z, const float dt)
+	{
+
+		/* in manual altitude control apply acceleration limit based on stick input
+		 * we consider two states
+		 * 1.) brake
+		 * 2.) accelerate */
+
+		/* check if zero input stick */
+		const bool is_current_zero = (fabsf(stick_z) <= FLT_EPSILON);
+
+		/* default is acceleration */
+		manual_stick_input intention = acceleration;
+
+		/* check zero input stick */
+		if (is_current_zero) {
+			intention = brake;
+		}
+
+		/* get max and min acceleration where min acceleration is just 1/5 of max acceleration */
+		max_acceleration = (stick_z <= 0.0f) ? _acceleration_z_max_up.get() : _acceleration_z_max_down.get();
+
+		/*
+		 * update user input
+		 */
+		if ((_user_intention_z != brake) && (intention  == brake)) {
+
+			/* we start with lowest acceleration */
+			_acceleration_state_dependent_z = _acceleration_z_max_down.get();
+
+			/* reset slew rate */
+			_vel_sp_prev(2) = _vel(2);
+			_user_intention_z = brake;
+		}
+
+		_user_intention_z = intention;
+
+		/*
+		 * apply acceleration depending on state
+		 */
+		if (_user_intention_z == brake) {
+
+			/* limit jerk when braking to zero */
+			float jerk = (_acceleration_z_max_up.get() - _acceleration_state_dependent_z) / dt;
+
+			if (jerk > _manual_jerk_limit_z) {
+				_acceleration_state_dependent_z = _manual_jerk_limit_z * dt + _acceleration_state_dependent_z;
+
+			} else {
+				_acceleration_state_dependent_z = _acceleration_z_max_up.get();
+			}
+		}
+
+		if (_user_intention_z == acceleration) {
+			_acceleration_state_dependent_z = (max_acceleration - _acceleration_z_max_down.get()) * fabsf(
+					stick_z) + _acceleration_z_max_down.get();
+		}
+	}
+
 };