diff --git a/gp2rml/gp2rml.c b/gp2rml/gp2rml.c
index 72563ee..a64f45f 100644
--- a/gp2rml/gp2rml.c
+++ b/gp2rml/gp2rml.c
@@ -133,7 +133,7 @@ static void output_paths(double z_clear, double xy_speed, double z_speed)
 	const struct segment *seg;
 	double x = 0, y = 0, z = 0;
 	double d, s = 0, t = 0;
-	double txy, tz;
+	double speed;
 
 	printf("IN;!MC1;PA\n");
 	printf("!ZO%d;!PZ0,%d;PU\n", units(z_max), units(z_clear));
@@ -156,13 +156,19 @@ static void output_paths(double z_clear, double xy_speed, double z_speed)
 		z = seg->z;
 		seg = seg->next;
 		while (seg) {
-			printf("!ZZ%d,%d,%d\n", units(seg->x), units(seg->y),
-			    units((seg->z - z_max) * z_scale));
+			if (path->segments->z == z && z == seg->z) {
+				printf("PA%d,%d\n",
+				    units(seg->x), units(seg->y));
+				speed = xy_speed;
+			} else {
+				printf("!ZZ%d,%d,%d\n",
+				    units(seg->x), units(seg->y),
+				    units((seg->z - z_max) * z_scale));
+				speed = z_speed;
+			}
 			d = hypot(x - seg->x, y - seg->y);
-			txy = d / xy_speed;
-			tz = fabs(z - seg->z) / z_speed;
-			t += txy > tz ? txy : tz;
 			d = hypot(d, z - seg->z);
+			t += d / speed;
 			s += d;
 			x = seg->x;
 			y = seg->y;