Merge branch '3.3-dev' into 3.3-input-shaping

2 files changed, 46 insertions, 14 deletions

diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index 577f5f4b..7768688b 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -1868,8 +1868,10 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated, const char *& e
 	// Set up the initial coordinates
 	memcpyf(moveBuffer.initialCoords, moveBuffer.coords, numVisibleAxes);
 
-	// Deal with axis movement
-	const float initialXY[2] = { currentUserPosition[X_AXIS], currentUserPosition[Y_AXIS] };
+	// Save the current position, we need it possibly later
+	float initialUserPosition[MaxAxes];
+	memcpyf(initialUserPosition, currentUserPosition, numVisibleAxes);
+
 	AxesBitmap axesMentioned;
 	for (size_t axis = 0; axis < numVisibleAxes; axis++)
 	{
@@ -2074,20 +2076,31 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated, const char *& e
 		{
 			// This kinematics approximates linear motion by means of segmentation.
 			// We assume that the segments will be smaller than the mesh spacing.
-			const float xyLength = sqrtf(fsquare(currentUserPosition[X_AXIS] - initialXY[0]) + fsquare(currentUserPosition[Y_AXIS] - initialXY[1]));
+			const float xyLength = sqrtf(fsquare(currentUserPosition[X_AXIS] - initialUserPosition[X_AXIS]) + fsquare(currentUserPosition[Y_AXIS] - initialUserPosition[Y_AXIS]));
 			const float moveTime = xyLength/moveBuffer.feedRate;			// this is a best-case time, often the move will take longer
-			moveBuffer.totalSegments = (unsigned int)max<long>(1, min<long>(lrintf(xyLength/kin.GetMinSegmentLength()), lrintf(moveTime * kin.GetSegmentsPerSecond())));
-		}
-		else if (reprap.GetMove().IsUsingMesh() && (moveBuffer.isCoordinated || machineType == MachineType::fff))
-		{
-			ReadLocker locker(reprap.GetMove().heightMapLock);
-			const HeightMap& heightMap = reprap.GetMove().AccessHeightMap();
-			moveBuffer.totalSegments = max<unsigned int>(1, heightMap.GetMinimumSegments(currentUserPosition[X_AXIS] - initialXY[0], currentUserPosition[Y_AXIS] - initialXY[1]));
+			moveBuffer.totalSegments = (unsigned int)max<long>(1, lrintf(min<float>(xyLength * kin.GetReciprocalMinSegmentLength(), moveTime * kin.GetSegmentsPerSecond())));
 		}
 		else
 		{
 			moveBuffer.totalSegments = 1;
 		}
+		if (reprap.GetMove().IsUsingMesh() && (moveBuffer.isCoordinated || machineType == MachineType::fff))
+		{
+			ReadLocker locker(reprap.GetMove().heightMapLock);
+			const HeightMap& heightMap = reprap.GetMove().AccessHeightMap();
+			const GridDefinition& grid = heightMap.GetGrid();
+			const unsigned int minMeshSegments = max<unsigned int>(
+					1,
+					heightMap.GetMinimumSegments(
+							currentUserPosition[grid.GetNumber0()] - initialUserPosition[grid.GetNumber0()],
+							currentUserPosition[grid.GetNumber1()] - initialUserPosition[grid.GetNumber1()]
+					)
+			);
+			if (minMeshSegments > moveBuffer.totalSegments)
+			{
+				moveBuffer.totalSegments = minMeshSegments;
+			}
+		}
 	}
 
 	moveBuffer.doingArcMove = false;
diff --git a/src/GCodes/ObjectTracker.cpp b/src/GCodes/ObjectTracker.cpp
index c4f45551..6c19b12a 100644
--- a/src/GCodes/ObjectTracker.cpp
+++ b/src/GCodes/ObjectTracker.cpp
@@ -153,8 +153,20 @@ GCodeResult ObjectTracker::HandleM486(GCodeBuffer &gb, const StringRef &reply, O
 	{
 		// Cancel an object
 		seen = true;
-		const int objectToCancel = (seenC) ? currentObjectNumber : gb.GetIValue();
-		if (objectToCancel >= 0 && objectToCancel < (int)objectsCancelled.MaxBits() && !objectsCancelled.IsBitSet(objectToCancel))
+		const int objectToCancel = (seenC) ? currentObjectNumber : (int)gb.GetUIValue();
+		if (objectToCancel < 0)
+		{
+			reply.copy("No current object");
+			return GCodeResult::error;
+		}
+		if (objectToCancel >= (int)objectsCancelled.MaxBits())
+		{
+			reply.copy("Object number out of range");
+			return GCodeResult::error;
+		}
+
+		// We don't flag an error if you try to cancel the same object twice
+		if (!objectsCancelled.IsBitSet(objectToCancel))
 		{
 			objectsCancelled.SetBit(objectToCancel);
 			if (objectToCancel == currentObjectNumber)
@@ -170,8 +182,15 @@ GCodeResult ObjectTracker::HandleM486(GCodeBuffer &gb, const StringRef &reply, O
 	{
 		// Resume an object
 		seen = true;
-		const int objectToResume = gb.GetIValue();
-		if (objectToResume >= 0 && objectToResume < (int)objectsCancelled.MaxBits() && objectsCancelled.IsBitSet(objectToResume))
+		const int objectToResume = gb.GetUIValue();
+		if (objectToResume >= (int)objectsCancelled.MaxBits())
+		{
+			reply.copy("Object number out of range");
+			return GCodeResult::error;
+		}
+
+		// We don't flag an error if you try to resume an object that is not cancelled
+		if (objectsCancelled.IsBitSet(objectToResume))
 		{
 			objectsCancelled.ClearBit(objectToResume);
 			if (objectToResume == currentObjectNumber)