From 60a9b45c88949dfc595da89c72e01e2da4f99ac5 Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis@yuneecresearch.com>
Date: Fri, 23 Mar 2018 16:45:01 +0100
Subject: [PATCH] Positioncontrol: limit velocity upwards during smooth takeoff

---
 src/modules/mc_pos_control/PositionControl.cpp | 2 +-
 src/modules/mc_pos_control/PositionControl.hpp | 3 +++
 2 files changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp
index c7cc1a1ae0..74b42ef5d2 100644
--- a/src/modules/mc_pos_control/PositionControl.cpp
+++ b/src/modules/mc_pos_control/PositionControl.cpp
@@ -188,7 +188,7 @@ void PositionControl::_positionController()
 			     _VelMaxXY);
 	_vel_sp(0) = vel_sp_xy(0);
 	_vel_sp(1) = vel_sp_xy(1);
-	_vel_sp(2) = math::constrain(_vel_sp(2), -_VelMaxZ.up, _VelMaxZ.down);
+	_vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.vel_max_z_up, _VelMaxZ.down);
 }
 
 void PositionControl::_velocityController(const float &dt)
diff --git a/src/modules/mc_pos_control/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl.hpp
index 7d40f9dbe9..b5f55f6571 100644
--- a/src/modules/mc_pos_control/PositionControl.hpp
+++ b/src/modules/mc_pos_control/PositionControl.hpp
@@ -54,6 +54,9 @@ namespace Controller
 {
 struct Constraints {
 	float tilt_max;
+	float vel_max_z_up;
+	float vel_max_z_down;
+	float vel_max_xy;
 };
 }