diff --git a/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp b/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp
index ab7f87fa63..89a367175a 100644
--- a/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp
@@ -73,7 +73,7 @@ void StraightLine::generateSetpoints(matrix::Vector3f &position_setpoint, matrix
 	float speed_sp_prev = math::max(velocity_setpoint * u_orig_to_target, 0.0f);
 
 	// Calculate accelerating/decelerating distance depending on speed, speed at target and acceleration/deceleration
-	float acc_dec_distance = fabs(_desired_speed * _desired_speed - _desired_speed_at_target * _desired_speed_at_target) / 2.0f;
+	float acc_dec_distance = fabs((_desired_speed * _desired_speed) - (_desired_speed_at_target * _desired_speed_at_target)) / 2.0f;
 	acc_dec_distance /= _desired_speed > _desired_speed_at_target ? _desired_deceleration : _desired_acceleration;
 
 	float dist_to_target = (_target - _pos).length(); // distance to target
@@ -95,7 +95,6 @@ void StraightLine::generateSetpoints(matrix::Vector3f &position_setpoint, matrix
 	// set the position and velocity setpoints
 	position_setpoint = closest_pt_on_line;
 	velocity_setpoint = u_orig_to_target * speed_sp;
-
 }
 
 float StraightLine::getMaxAcc()
@@ -109,7 +108,7 @@ float StraightLine::getMaxAcc()
 	Vector3f u_orig_to_target = (_target - _origin).unit_or_zero();
 
 	// calculate the maximal horizontal acceleration
-	float divider = (sqrt(u_orig_to_target(0) * u_orig_to_target(0) + u_orig_to_target(1) * u_orig_to_target(1)));
+	float divider = Vector2f(u_orig_to_target.data()).length();
 	float max_acc_hor = MPC_ACC_HOR_MAX.get();
 
 	if (divider > FLT_EPSILON) {
@@ -144,7 +143,7 @@ float StraightLine::getMaxVel()
 	Vector3f u_orig_to_target = (_target - _origin).unit_or_zero();
 
 	// calculate the maximal horizontal velocity
-	float divider = (sqrt(u_orig_to_target(0) * u_orig_to_target(0) + u_orig_to_target(1) * u_orig_to_target(1)));
+	float divider = Vector2f(u_orig_to_target.data()).length();
 	float max_vel_hor = MPC_XY_VEL_MAX.get();
 
 	if (divider > FLT_EPSILON) {