diff --git a/gp2rml/gp2rml.c b/gp2rml/gp2rml.c
index 8297b6a..c6bf5af 100644
--- a/gp2rml/gp2rml.c
+++ b/gp2rml/gp2rml.c
@@ -144,9 +144,11 @@ static void output_paths(double z_clear, double xy_speed, double z_speed)
 		printf("!PZ%d,%d;PA%d,%d;PD\n",
 		    units(seg->z-z_max), units(z_clear),
 		    units(seg->x), units(seg->y));
-		d = hypot(x-seg->x, y-seg->y)+(z-z_max);
+		d = hypot(x-seg->x, y-seg->y);
 		s += d;
+		s += z-z_max;
 		t += d/V_MAX;
+		t += (z-z_max)/z_speed;
 		x = seg->x;
 		y = seg->y;
 		z = seg->z;