PositionSmoothing: refactor _getMaxZSpeed()

src/lib/motion_planning/PositionSmoothing.cpp

@@ -106,27 +106,22 @@ float PositionSmoothing::_getMaxXYSpeed(const Vector3f(&waypoints)[3]) const
 
 float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const
 {
-	const auto &start_position = waypoints[0];
-	const auto &target = waypoints[1];
-	const auto &next_target = waypoints[2];
+	const Vector3f &start_position = waypoints[0];
+	const Vector3f &target = waypoints[1];
+	const Vector3f &next_target = waypoints[2];
+
+	const Vector2f start_position_xy_z = {start_position.xy().norm(), start_position(2)};
+	const Vector2f target_xy_z = {target.xy().norm(), target(2)};
+	const Vector2f next_target_xy_z = {next_target.xy().norm(), next_target(2)};
 
 	Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
 			  _trajectory[1].getCurrentPosition(),
 			  _trajectory[2].getCurrentPosition());
 
-	const float distance_start_target = fabs(target(2) - pos_traj(2));
 	float arrival_z_speed = 0.0f;
+	const bool target_next_different = fabsf(target(2) - next_target(2)) > 0.001f;
 
-	const float distance_target_next = fabsf(target(2) - next_target(2));
-
-	const bool target_next_different = distance_target_next  > 0.001f;
-
-	Vector2f target_xy_z = {target.xy().norm(), target(2)};
-	Vector2f next_target_xy_z = {next_target.xy().norm(), next_target(2)};
-	Vector2f start_position_xy_z = {start_position.xy().norm(), start_position(2)};
-
-	if (target_next_different
-	   ) {
+	if (target_next_different) {
 		const float alpha = acosf(Vector2f((target_xy_z - start_position_xy_z)).unit_or_zero().dot(
 						  Vector2f((target_xy_z - next_target_xy_z)).unit_or_zero()));
 
@@ -137,6 +132,7 @@ float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const
 		arrival_z_speed = math::min(max_speed_in_turn, _trajectory[2].getMaxVel());
 	}
 
+	const float distance_start_target = fabs(target(2) - pos_traj(2));
 	float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance(
 					    _trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(),
 					    distance_start_target, arrival_z_speed));