From 1b858f5e5656fe1f34678a9c23b50f74d86c98b1 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Tue, 26 Sep 2017 14:11:53 +0200
Subject: [PATCH] FlightTaskManual: Smooth flight integration: Copy over
 vel_sp_slewrate without any changes

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

diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp
index a5789732d1..310f35b4ea 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp
@@ -202,6 +202,37 @@ private:
 	math::LowPassFilter2p _filter_manual_pitch;
 	math::LowPassFilter2p _filter_manual_roll;
 
+	void vel_sp_slewrate(float dt)
+	{
+		matrix::Vector2f vel_sp_xy(_vel_sp(0), _vel_sp(1));
+		matrix::Vector2f vel_sp_prev_xy(_vel_sp_prev(0), _vel_sp_prev(1));
+		matrix::Vector2f vel_xy(_vel(0), _vel(1));
+		matrix::Vector2f acc_xy = (vel_sp_xy - vel_sp_prev_xy) / dt;
+
+		/* limit total horizontal acceleration */
+		if (acc_xy.length() > _acceleration_state_dependent_xy) {
+			vel_sp_xy = _acceleration_state_dependent_xy * acc_xy.normalized() * dt + vel_sp_prev_xy;
+			_vel_sp(0) = vel_sp_xy(0);
+			_vel_sp(1) = vel_sp_xy(1);
+		}
+
+		/* limit vertical acceleration */
+		float acc_z = (_vel_sp(2) - _vel_sp_prev(2)) / dt;
+		float max_acc_z;
+
+		if (_control_mode.flag_control_manual_enabled) {
+			max_acc_z = (acc_z < 0.0f) ? -_acceleration_state_dependent_z : _acceleration_state_dependent_z;
+
+		} else {
+			max_acc_z = (acc_z < 0.0f) ? -_acceleration_z_max_up.get() : _acceleration_z_max_down.get();
+		}
+
+		if (fabsf(acc_z) > fabsf(max_acc_z)) {
+			_vel_sp(2) = max_acc_z * dt + _vel_sp_prev(2);
+		}
+
+	}
+
 	void set_manual_acceleration_xy(matrix::Vector2f &stick_xy, const float dt)
 	{