From adbccfaa1cd100609490b61f2081a0619b0a36c8 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Wed, 3 Jun 2015 09:32:02 -0400
Subject: [PATCH] Saturate velocity command for mc_pos_control.

---
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 7 +++++++
 1 file changed, 7 insertions(+)

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 7a3a5a679b..2509f2b8c4 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1032,7 +1032,14 @@ MulticopterPositionControl::task_main()
 				/* run position & altitude controllers, calculate velocity setpoint */
 				math::Vector<3> pos_err = _pos_sp - _pos;
 
+				/* make sure velocity setpoint is saturated */
 				_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
+				for (int i=0; i<3; i++) {
+					if (_vel_sp(i) > _params.vel_max(i)) {
+						_vel_sp(i) = _params.vel_max(i);
+					} else if (_vel_sp(i) < -_params.vel_max(i))
+						_vel_sp(i) = -_params.vel_max(i);
+				}
 
 				if (!_control_mode.flag_control_altitude_enabled) {
 					_reset_alt_sp = true;