math: support max of three values additional to min and use it everywhere

src/lib/mathlib/math/Limits.hpp

@@ -68,6 +68,12 @@ constexpr _Tp max(_Tp a, _Tp b)
 	return (a > b) ? a : b;
 }
 
+template<typename _Tp>
+constexpr _Tp max(_Tp a, _Tp b, _Tp c)
+{
+	return max(max(a, b), c);
+}
+
 template<typename _Tp>
 constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
 {

src/lib/motion_planning/TrajectoryConstraints.hpp

@@ -89,7 +89,7 @@ inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, co
 		const float safe_alpha = constrain(alpha, 0.f, M_PI_F - FLT_EPSILON);
 		float accel_tmp = config.max_acc_xy_radius_scale * config.max_acc_xy;
 		float max_speed_in_turn = computeMaxSpeedInWaypoint(safe_alpha, accel_tmp, config.xy_accept_rad);
-		speed_at_target = min(min(max_speed_in_turn, exit_speed), config.max_speed_xy);
+		speed_at_target = min(max_speed_in_turn, exit_speed, config.max_speed_xy);
 	}
 
 	float start_to_target = (start_position - target).xy().norm();