From 132c9180ead0c64c2edcc36dec2bf8896679a040 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Mon, 1 Sep 2014 18:47:56 +0200
Subject: [PATCH] mc_pos_control: move position offset limiting to separate
 method

---
 .../mc_pos_control/mc_pos_control_main.cpp    | 28 +++++++++++++++++++
 1 file changed, 28 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 01fdb4c653..ce26f7dc25 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -221,6 +221,11 @@ private:
 	 */
 	void		reset_alt_sp();
 
+	/**
+	 * Check if position setpoint is too far from current position and adjust it if needed.
+	 */
+	void		limit_pos_sp_offset();
+
 	/**
 	 * Set position setpoint using manual control
 	 */
@@ -544,6 +549,29 @@ MulticopterPositionControl::reset_alt_sp()
 	}
 }
 
+void
+MulticopterPositionControl::limit_pos_sp_offset()
+{
+	math::Vector<3> pos_sp_offs;
+	pos_sp_offs.zero();
+
+	if (_control_mode.flag_control_position_enabled) {
+		pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
+		pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
+	}
+
+	if (_control_mode.flag_control_altitude_enabled) {
+		pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
+	}
+
+	float pos_sp_offs_norm = pos_sp_offs.length();
+
+	if (pos_sp_offs_norm > 1.0f) {
+		pos_sp_offs /= pos_sp_offs_norm;
+		_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
+	}
+}
+
 void
 MulticopterPositionControl::control_manual(float dt)
 {