mc_pos_control: replacement for multirotor_pos_control, rewritten to C++ and new mathlib

1 files changed, 24 insertions, 0 deletions

diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index b7840170c..c7323c215 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -262,6 +262,30 @@ public:
 	}
 
 	/**
+	 * element by element multiplication
+	 */
+	const Vector<N> emult(const Vector<N> &v) const {
+		Vector<N> res;
+
+		for (unsigned int i = 0; i < N; i++)
+			res.data[i] = data[i] * v.data[i];
+
+		return res;
+	}
+
+	/**
+	 * element by element division
+	 */
+	const Vector<N> edivide(const Vector<N> &v) const {
+		Vector<N> res;
+
+		for (unsigned int i = 0; i < N; i++)
+			res.data[i] = data[i] / v.data[i];
+
+		return res;
+	}
+
+	/**
 	 * gets the length of this vector squared
 	 */
 	float length_squared() const {