diff --git a/src/modules/mc_pos_control/Utility/ControlMath.cpp b/src/modules/mc_pos_control/Utility/ControlMath.cpp
index fda1de5cb6..f5d28d9dbe 100644
--- a/src/modules/mc_pos_control/Utility/ControlMath.cpp
+++ b/src/modules/mc_pos_control/Utility/ControlMath.cpp
@@ -140,7 +140,7 @@ Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)
 		// 				   		   v0(0/1/2) -> v0/1/2
 		// 				   		   u1(0/1/2) -> u0/1/2
 		//
-		// ||v + s * u||^2 = (v0+s*u0)^2+(v1+s*u1)^2+(v1+s*u1)^2 = max^2
+		// ||v + s * u||^2 = (v0+s*u0)^2+(v1+s*u1)^2+(v2+s*u2)^2 = max^2
 		// v0^2+2*s*u0*v0+s^2*u0^2 + v1^2+2*s*u1*v1+s^2*u1^2 + v2^2+2*s*u2*v2+s^2*u2^2 = max^2
 		// s^2*(u0^2+u1^2+u2^2) + s*2*(u0*v0+u1*v1+u2*v2) + (v0^2+v1^2+v2^2-max^2) = 0
 		//