Merge branch 'v3-chrishamm' into 3.4-dev

13 files changed, 17 insertions, 248 deletions

diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index bf3d76bb..07827293 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -2141,7 +2141,6 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated, const char *& e
 		}
 		if (reprap.GetMove().IsUsingMesh() && (moveState.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>(
@@ -3083,18 +3082,14 @@ void GCodes::DoManualBedProbe(GCodeBuffer& gb)
 // Prior to calling this the movement system must be locked.
 GCodeResult GCodes::ProbeGrid(GCodeBuffer& gb, const StringRef& reply)
 {
-	reprap.GetMove().heightMapLock.LockForWriting();
-
 	if (!defaultGrid.IsValid())
 	{
-		reprap.GetMove().heightMapLock.ReleaseWriter();
 		reply.copy("No valid grid defined for bed probing");
 		return GCodeResult::error;
 	}
 
 	if (!AllAxesAreHomed())
 	{
-		reprap.GetMove().heightMapLock.ReleaseWriter();
 		reply.copy("Must home printer before bed probing");
 		return GCodeResult::error;
 	}
@@ -3113,19 +3108,10 @@ GCodeResult GCodes::ProbeGrid(GCodeBuffer& gb, const StringRef& reply)
 	return GCodeResult::ok;
 }
 
-#if HAS_MASS_STORAGE
+#if HAS_MASS_STORAGE || HAS_SBC_INTERFACE
 
 GCodeResult GCodes::LoadHeightMap(GCodeBuffer& gb, const StringRef& reply)
 {
-#if HAS_SBC_INTERFACE
-	// If we have an SBC interface and we're using it, the SBC service will take care of file I/O and this should not be called
-	if (reprap.UsingSbcInterface())
-	{
-		reply.copy("Cannot use height map on local SD card when SBC interface is used");
-		return GCodeResult::error;
-	}
-#endif
-
 	ClearBedMapping();
 
 	String<MaxFilenameLength> heightMapFileName;
@@ -3146,7 +3132,6 @@ GCodeResult GCodes::LoadHeightMap(GCodeBuffer& gb, const StringRef& reply)
 	}
 	reply.printf("Failed to load height map from file %s: ", fullName.c_str());	// set up error message to append to
 
-	WriteLocker locker(reprap.GetMove().heightMapLock);
 	const bool err = reprap.GetMove().LoadHeightMapFromFile(f, fullName.c_str(), reply);
 	f->Close();
 
@@ -3169,15 +3154,6 @@ GCodeResult GCodes::LoadHeightMap(GCodeBuffer& gb, const StringRef& reply)
 // Save the height map and append the success or error message to 'reply', returning true if an error occurred
 bool GCodes::TrySaveHeightMap(const char *filename, const StringRef& reply) const noexcept
 {
-#if HAS_SBC_INTERFACE
-	// If we have an SBC connected, the SBC service will take care of heightmap-related file I/O
-	if (reprap.UsingSbcInterface())
-	{
-		reply.copy("Cannot use height map on local SD card when SBC interface is used");
-		return true;
-	}
-#endif
-
 	String<MaxFilenameLength> fullName;
 	platform.MakeSysFileName(fullName.GetRef(), filename);
 	FileStore * const f = MassStorage::OpenFile(fullName.c_str(), OpenMode::write, 0);
@@ -3207,8 +3183,6 @@ bool GCodes::TrySaveHeightMap(const char *filename, const StringRef& reply) cons
 // Save the height map to the file specified by P parameter
 GCodeResult GCodes::SaveHeightMap(GCodeBuffer& gb, const StringRef& reply) const
 {
-	ReadLocker locker(reprap.GetMove().heightMapLock);
-
 	// No need to check if we're using the SBC interface here, because TrySaveHeightMap does that already
 	if (gb.Seen('P'))
 	{
@@ -4975,17 +4949,6 @@ Pwm_t GCodes::ConvertLaserPwm(float reqVal) const noexcept
 	return (uint16_t)constrain<long>(lrintf((reqVal * 65535)/laserMaxPower), 0, 65535);
 }
 
-// Assign the heightmap using the given parameters, returning true if they were valid
-bool GCodes::AssignGrid(const char axesLetters[2], const float axis0Range[2], const float axis1Range[2], float radius, float spacing[2]) noexcept
-{
-	const bool ok = defaultGrid.Set(axesLetters, axis0Range, axis1Range, radius, spacing);
-	if (ok)
-	{
-		reprap.GetMove().AccessHeightMap().SetGrid(defaultGrid);
-	}
-	return ok;
-}
-
 void GCodes::ActivateHeightmap(bool activate) noexcept
 {
 	reprap.GetMove().UseMesh(activate);
@@ -4994,15 +4957,6 @@ void GCodes::ActivateHeightmap(bool activate) noexcept
 		// Update the current position to allow for any bed compensation at the current XY coordinates
 		reprap.GetMove().GetCurrentUserPosition(moveState.coords, 0, reprap.GetCurrentTool());
 		ToolOffsetInverseTransform(moveState.coords, moveState.currentUserPosition);	// update user coordinates to reflect any height map offset at the current position
-
-#if HAS_SBC_INTERFACE
-		// Set a dummy heightmap filename
-		if (reprap.UsingSbcInterface())
-		{
-			HeightMap& map = reprap.GetMove().AccessHeightMap();
-			map.SetFileName(DefaultHeightMapFile);
-		}
-#endif
 	}
 }
 
diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h
index 0b56edbc..49b24058 100644
--- a/src/GCodes/GCodes.h
+++ b/src/GCodes/GCodes.h
@@ -222,7 +222,6 @@ public:
 	void EmergencyStop() noexcept;												// Cancel everything
 
 	const GridDefinition& GetDefaultGrid() const { return defaultGrid; };		// Get the default grid definition
-	bool AssignGrid(const char axesLetters[2], const float axis0Range[2], const float axis1Range[2], float radius, float spacing[2]) noexcept;	// Assign the heightmap using the given parameters
 	void ActivateHeightmap(bool activate) noexcept;								// (De-)Activate the height map
 
 	int GetNewToolNumber() const noexcept { return newToolNumber; }
@@ -466,7 +465,7 @@ private:
 	bool IsMappedFan(unsigned int fanNumber) noexcept;							// Return true if this fan number is currently being used as a print cooling fan
 
 	GCodeResult DefineGrid(GCodeBuffer& gb, const StringRef &reply) THROWS(GCodeException);	// Define the probing grid, returning true if error
-#if HAS_MASS_STORAGE
+#if HAS_MASS_STORAGE || HAS_SBC_INTERFACE
 	GCodeResult LoadHeightMap(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);	// Load the height map from file
 	bool TrySaveHeightMap(const char *filename, const StringRef& reply) const noexcept;	// Save the height map to the specified file
 	GCodeResult SaveHeightMap(GCodeBuffer& gb, const StringRef& reply) const;	// Save the height map to the file specified by P parameter
diff --git a/src/GCodes/GCodes2.cpp b/src/GCodes/GCodes2.cpp
index 8a41bb53..8797c342 100644
--- a/src/GCodes/GCodes2.cpp
+++ b/src/GCodes/GCodes2.cpp
@@ -280,16 +280,6 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 			break;
 
 		case 29: // Grid-based bed probing
-
-#if HAS_SBC_INTERFACE
-			// Pass file- and system-related commands to the SBC service if they came from anywhere else.
-			// They will be passed back to us via a binary buffer or separate SPI message if necessary.
-			if (reprap.UsingSbcInterface() && reprap.GetSbcInterface().IsConnected() && !gb.IsBinary())
-			{
-				gb.SendToSbc();
-				return false;
-			}
-#endif
 			if (!LockMovementAndWaitForStandstill(gb))			// do this first to make sure that a new grid isn't being defined
 			{
 				return false;
@@ -316,7 +306,7 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 					break;
 
 				case 1:		// load height map file
-#if HAS_MASS_STORAGE
+#if HAS_MASS_STORAGE || HAS_SBC_INTERFACE
 					result = LoadHeightMap(gb, reply);
 #else
 					result = GCodeResult::errorNotSupported;
@@ -328,7 +318,7 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 					break;
 
 				case 3:		// save height map to names file
-#if HAS_MASS_STORAGE
+#if HAS_MASS_STORAGE || HAS_SBC_INTERFACE
 					result = SaveHeightMap(gb, reply);
 #else
 					result = GCodeResult::errorNotSupported;
@@ -476,10 +466,9 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 	// They will be passed back to us via a binary buffer or separate SPI message if necessary.
 	if (   reprap.UsingSbcInterface() && reprap.GetSbcInterface().IsConnected() && !gb.IsBinary()
 		&& (   code == 0 || code == 1
-			|| code == 20 || code == 21 || code == 22 || code == 23 || code == 24 || code == 26 || code == 27 || code == 28 || code == 29
+			|| code == 20 || code == 21 || code == 22 || code == 23 || code == 24 || code == 26 || code == 27 || code == 28
 			|| code == 30 || code == 32 || code == 36 || code == 37 || code == 38 || code == 39
 			|| code == 112
-			|| code == 374 || code == 375
 			|| code == 470 || code == 471
 			|| code == 503 || code == 505
 			|| code == 540 || code == 550 || code == 552 || code == 586 || (code >= 587 && code <= 589)
@@ -2759,7 +2748,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 				}
 				break;
 
-#if HAS_MASS_STORAGE
+#if HAS_MASS_STORAGE || HAS_SBC_INTERFACE
 			case 374: // Save grid and height map to file
 				result = SaveHeightMap(gb, reply);
 				break;
diff --git a/src/GCodes/GCodes3.cpp b/src/GCodes/GCodes3.cpp
index b3910549..4b7eaec1 100644
--- a/src/GCodes/GCodes3.cpp
+++ b/src/GCodes/GCodes3.cpp
@@ -304,8 +304,6 @@ GCodeResult GCodes::DefineGrid(GCodeBuffer& gb, const StringRef &reply) THROWS(G
 
 	if (!axesSeen && !seenR && !seenS && !seenP)
 	{
-		ReadLocker rlocker(reprap.GetMove().heightMapLock);
-
 		// Just print the existing grid parameters
 		if (defaultGrid.IsValid())
 		{
@@ -389,7 +387,6 @@ GCodeResult GCodes::DefineGrid(GCodeBuffer& gb, const StringRef &reply) THROWS(G
 		}
 	}
 
-	WriteLocker locker(reprap.GetMove().heightMapLock);
 	const bool ok = defaultGrid.Set(axesLetters, axis0Values, axis1Values, radius, spacings);
 	reprap.MoveUpdated();
 	if (ok)
diff --git a/src/GCodes/GCodes4.cpp b/src/GCodes/GCodes4.cpp
index bb8292b3..663d1fb5 100644
--- a/src/GCodes/GCodes4.cpp
+++ b/src/GCodes/GCodes4.cpp
@@ -727,7 +727,6 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 				}
 				else if (zp->Stopped())
 				{
-					reprap.GetMove().heightMapLock.ReleaseWriter();
 					reprap.GetHeat().SuspendHeaters(false);
 					gb.LatestMachineState().SetError("Probe already triggered before probing move started");
 					gb.SetState(GCodeState::checkError);
@@ -740,7 +739,6 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 					SetMoveBufferDefaults();
 					if (!platform.GetEndstops().EnableZProbe(currentZProbeNumber) || !zp->SetProbing(true))
 					{
-						reprap.GetMove().heightMapLock.ReleaseWriter();
 						gb.LatestMachineState().SetError("Failed to enable probe");
 						gb.SetState(GCodeState::checkError);
 						RetractZProbe(gb);
@@ -774,7 +772,6 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 				zp->SetProbing(false);
 				if (!zProbeTriggered)
 				{
-					reprap.GetMove().heightMapLock.ReleaseWriter();
 					gb.LatestMachineState().SetError("Probe was not triggered during probing move");
 					gb.SetState(GCodeState::checkError);
 					RetractZProbe(gb);
@@ -848,7 +845,6 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 			}
 			else
 			{
-				reprap.GetMove().heightMapLock.ReleaseWriter();
 				gb.LatestMachineState().SetError("Z probe readings not consistent");
 				gb.SetState(GCodeState::checkError);
 				RetractZProbe(gb);
@@ -909,14 +905,9 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 				reply.printf("%" PRIu32 " points probed, min error %.3f, max error %.3f, mean %.3f, deviation %.3f\n",
 								numPointsProbed, (double)minError, (double)maxError, (double)deviation.GetMean(), (double)deviation.GetDeviationFromMean());
 #if HAS_MASS_STORAGE
-# if HAS_SBC_INTERFACE
-				if (!reprap.UsingSbcInterface())
-# endif
+				if (TrySaveHeightMap(DefaultHeightMapFile, reply))
 				{
-					if (TrySaveHeightMap(DefaultHeightMapFile, reply))
-					{
-						stateMachineResult = GCodeResult::error;
-					}
+					stateMachineResult = GCodeResult::error;
 				}
 #endif
 				reprap.GetMove().AccessHeightMap().ExtrapolateMissing();
@@ -931,7 +922,6 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 			{
 				gb.LatestMachineState().SetError("Too few points probed");
 			}
-			reprap.GetMove().heightMapLock.ReleaseWriter();
 		}
 		if (stateMachineResult == GCodeResult::ok)
 		{
diff --git a/src/Movement/BedProbing/Grid.cpp b/src/Movement/BedProbing/Grid.cpp
index 45b81a92..81211fa7 100644
--- a/src/Movement/BedProbing/Grid.cpp
+++ b/src/Movement/BedProbing/Grid.cpp
@@ -372,7 +372,7 @@ unsigned int HeightMap::GetMinimumSegments(float deltaAxis0, float deltaAxis1) c
 	return max<unsigned int>(axis0Segments, axis1Segments);
 }
 
-#if HAS_MASS_STORAGE
+#if HAS_MASS_STORAGE || HAS_SBC_INTERFACE
 
 // Save the grid to file returning true if an error occurred
 bool HeightMap::SaveToFile(FileStore *f, const char *fname, float zOffset) noexcept
@@ -527,30 +527,6 @@ bool HeightMap::LoadFromFile(FileStore *f, const char *fname, const StringRef& r
 
 #endif
 
-#if HAS_SBC_INTERFACE
-
-// Update the filename
-void HeightMap::SetFileName(const char *name) noexcept
-{
-	fileName.copy(name);
-}
-
-// Save the grid to a sequential array in the same way as to a regular CSV file
-void HeightMap::SaveToArray(float *arr, float zOffset) const noexcept
-{
-	size_t index = 0;
-	for (size_t i = 0; i < def.nums[1]; ++i)
-	{
-		for (size_t j = 0; j < def.nums[0]; ++j)
-		{
-			arr[index] = gridHeightSet.IsBitSet(index) ? (gridHeights[index] + zOffset) : std::numeric_limits<float>::quiet_NaN();
-			index++;
-		}
-	}
-}
-
-#endif
-
 // Return number of points probed, mean and RMS deviation, min and max error
 unsigned int HeightMap::GetStatistics(Deviation& deviation, float& minError, float& maxError) const noexcept
 {
diff --git a/src/Movement/BedProbing/Grid.h b/src/Movement/BedProbing/Grid.h
index b7366d32..cd8bd857 100644
--- a/src/Movement/BedProbing/Grid.h
+++ b/src/Movement/BedProbing/Grid.h
@@ -79,24 +79,15 @@ public:
 	float GetInterpolatedHeightError(float axis0, float axis1) const noexcept;			// Compute the interpolated height error at the specified point
 	void ClearGridHeights() noexcept;													// Clear all grid height corrections
 	void SetGridHeight(size_t axis0Index, size_t axis1Index, float height) noexcept;	// Set the height of a grid point
-	void SetGridHeight(size_t index, float height) noexcept;							// Set the height of a grid point
 
-#if HAS_MASS_STORAGE
+#if HAS_MASS_STORAGE || HAS_SBC_INTERFACE
 	bool SaveToFile(FileStore *f, const char *fname, float zOffset) noexcept	// Save the grid to file returning true if an error occurred
 	pre(IsValid());
 	bool LoadFromFile(FileStore *f, const char *fname, const StringRef& r) noexcept;	// Load the grid from file returning true if an error occurred
-#endif
 
-#if HAS_MASS_STORAGE || HAS_SBC_INTERFACE
 	const char *GetFileName() const noexcept { return fileName.c_str(); }
 #endif
 
-#if HAS_SBC_INTERFACE
-	void SetFileName(const char *name) noexcept;								// Update the filename
-	void SaveToArray(float *array, float zOffset) const noexcept				// Save the grid Z coordinates to an array
-	pre(IsValid());
-#endif
-
 	unsigned int GetMinimumSegments(float deltaAxis0, float deltaAxis1) const noexcept;	// Return the minimum number of segments for a move by this X or Y amount
 
 	bool UseHeightMap(bool b) noexcept;
@@ -118,6 +109,7 @@ private:
 	bool useMap;													// True to do bed compensation
 
 	uint32_t GetMapIndex(uint32_t axis0Index, uint32_t axis1Index) const noexcept { return (axis1Index * def.NumAxisPoints(0)) + axis0Index; }
+	void SetGridHeight(size_t index, float height) noexcept;							// Set the height of a grid point
 
 	float InterpolateAxis0Axis1(uint32_t axis0Index, uint32_t axis1Index, float axis0Frac, float axis1Frac) const noexcept;
 };
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index f3bbc97e..cceaf23e 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -747,7 +747,7 @@ void Move::SetIdentityTransform() noexcept
 	reprap.MoveUpdated();
 }
 
-#if HAS_MASS_STORAGE
+#if HAS_MASS_STORAGE || HAS_SBC_INTERFACE
 
 // Load the height map from file, returning true if an error occurred with the error reason appended to the buffer
 bool Move::LoadHeightMapFromFile(FileStore *f, const char *fname, const StringRef& r) noexcept
@@ -773,16 +773,6 @@ bool Move::SaveHeightMapToFile(FileStore *f, const char *fname) noexcept
 
 #endif
 
-#if HAS_SBC_INTERFACE
-
-// Save the height map Z coordinates to an array
-void Move::SaveHeightMapToArray(float *arr) const noexcept
-{
-	return heightMap.SaveToArray(arr, zShift);
-}
-
-#endif
-
 void Move::SetTaperHeight(float h) noexcept
 {
 	useTaper = (h > 1.0);
diff --git a/src/Movement/Move.h b/src/Movement/Move.h
index 8d4e560e..d4f3b971 100644
--- a/src/Movement/Move.h
+++ b/src/Movement/Move.h
@@ -143,19 +143,14 @@ public:
 	uint32_t GetCompletedMoves() const noexcept { return mainDDARing.GetCompletedMoves(); }	// How many moves have been completed?
 	void ResetMoveCounters() noexcept { mainDDARing.ResetMoveCounters(); }
 
-	ReadWriteLock heightMapLock;
 	HeightMap& AccessHeightMap() noexcept { return heightMap; }								// Access the bed probing grid
 	const GridDefinition& GetGrid() const noexcept { return heightMap.GetGrid(); }			// Get the grid definition
 
-#if HAS_MASS_STORAGE
+#if HAS_MASS_STORAGE || HAS_SBC_INTERFACE
 	bool LoadHeightMapFromFile(FileStore *f, const char *fname, const StringRef& r) noexcept;	// Load the height map from a file returning true if an error occurred
 	bool SaveHeightMapToFile(FileStore *f, const char *fname) noexcept;						// Save the height map to a file returning true if an error occurred
 #endif
 
-#if HAS_SBC_INTERFACE
-	void SaveHeightMapToArray(float *arr) const noexcept;									// Save the height map Z coordinates to an array
-#endif
-
 	const RandomProbePointSet& GetProbePoints() const noexcept { return probePoints; }		// Return the probe point set constructed from G30 commands
 
 	DDARing& GetMainDDARing() noexcept { return mainDDARing; }
diff --git a/src/SBC/DataTransfer.cpp b/src/SBC/DataTransfer.cpp
index 2902da8b..967134f4 100644
--- a/src/SBC/DataTransfer.cpp
+++ b/src/SBC/DataTransfer.cpp
@@ -601,48 +601,6 @@ GCodeChannel DataTransfer::ReadMacroCompleteInfo(bool &error) noexcept
 	return GCodeChannel(header->channel);
 }
 
-bool DataTransfer::ReadHeightMap() noexcept
-{
-	// Read height map header
-	const HeightMapHeader * const header = ReadDataHeader<HeightMapHeader>();
-	char axesLetter[2] 		= { 'X', 'Y' };
-	float axis0Range[2]		= { header->xMin, header->xMax };
-	float axis1Range[2]		= { header->yMin, header->yMax };
-	float spacing[2]		= { header->xSpacing, header->ySpacing };
-	const bool ok = reprap.GetGCodes().AssignGrid(axesLetter, axis0Range, axis1Range, header->radius, spacing);
-	if (ok)
-	{
-		// Read Z coordinates
-		const size_t numPoints = header->numX * header->numY;
-		const float *points = reinterpret_cast<const float *>(ReadData(sizeof(float) * numPoints));
-
-		HeightMap& map = reprap.GetMove().AccessHeightMap();
-		map.ClearGridHeights();
-		for (size_t i = 0; i < numPoints; i++)
-		{
-			if (!std::isnan(points[i]))
-			{
-				map.SetGridHeight(i, points[i]);
-			}
-		}
-
-		map.ExtrapolateMissing();
-
-		// Activate it
-		reprap.GetGCodes().ActivateHeightmap(true);
-
-		// Recalculate the deviations
-		float minError, maxError;
-		Deviation deviation;
-		const uint32_t numPointsProbed = reprap.GetMove().AccessHeightMap().GetStatistics(deviation, minError, maxError);
-		if (numPointsProbed >= 4)
-		{
-			reprap.GetMove().SetLatestMeshDeviation(deviation);
-		}
-	}
-	return ok;
-}
-
 GCodeChannel DataTransfer::ReadCodeChannel() noexcept
 {
 	const CodeChannelHeader *header = ReadDataHeader<CodeChannelHeader>();
@@ -1192,42 +1150,6 @@ bool DataTransfer::WritePrintPaused(FilePosition position, PrintPausedReason rea
 	return true;
 }
 
-bool DataTransfer::WriteHeightMap() noexcept
-{
-	const GridDefinition& grid = reprap.GetMove().GetGrid();
-
-	size_t numPoints = reprap.GetMove().AccessHeightMap().UsingHeightMap() ? grid.NumPoints() : 0;
-	size_t bytesToWrite = sizeof(HeightMapHeader) + numPoints * sizeof(float);
-	if (!CanWritePacket(bytesToWrite))
-	{
-		return false;
-	}
-
-	// Write packet header
-	(void)WritePacketHeader(FirmwareRequest::HeightMap, bytesToWrite);
-
-	// Write heightmap header
-	HeightMapHeader *header = WriteDataHeader<HeightMapHeader>();
-	header->xMin = grid.GetMin(0);
-	header->xMax = grid.GetMax(0);
-	header->xSpacing = grid.GetSpacing(0);
-	header->yMin = grid.GetMin(1);
-	header->yMax = grid.GetMax(1);
-	header->ySpacing = grid.GetSpacing(1);
-	header->radius = grid.radius;
-	header->numX = grid.NumAxisPoints(0);
-	header->numY = grid.NumAxisPoints(1);
-
-	// Write Z points
-	if (numPoints != 0)
-	{
-		float *zPoints = reinterpret_cast<float*>(txBuffer + txPointer);
-		reprap.GetMove().SaveHeightMapToArray(zPoints);
-		txPointer += numPoints * sizeof(float);
-	}
-	return true;
-}
-
 bool DataTransfer::WriteLocked(GCodeChannel channel) noexcept
 {
 	if (!CanWritePacket(sizeof(CodeChannelHeader)))
diff --git a/src/SBC/DataTransfer.h b/src/SBC/DataTransfer.h
index 4882b703..bdc87bc9 100644
--- a/src/SBC/DataTransfer.h
+++ b/src/SBC/DataTransfer.h
@@ -55,7 +55,6 @@ public:
 	void ReadPrintStartedInfo(size_t packetLength, const StringRef& filename, GCodeFileInfo &info) noexcept;	// Read info about the started file print
 	PrintStoppedReason ReadPrintStoppedInfo() noexcept;										// Read info about why the print has been stopped
 	GCodeChannel ReadMacroCompleteInfo(bool &error) noexcept;								// Read info about a completed macro file
-	bool ReadHeightMap() noexcept;															// Read heightmap parameters
 	GCodeChannel ReadCodeChannel() noexcept;												// Read a code channel
 	void ReadFileChunk(char *buffer, int32_t& dataLength, uint32_t& fileLength) noexcept;	// Read another chunk of a file
 	GCodeChannel ReadEvaluateExpression(size_t packetLength, const StringRef& expression) noexcept;	// Read an expression request
@@ -73,7 +72,6 @@ public:
 	bool WriteAbortFileRequest(GCodeChannel channel, bool abortAll) noexcept;
 	bool WriteMacroFileClosed(GCodeChannel channel) noexcept;
 	bool WritePrintPaused(FilePosition position, PrintPausedReason reason) noexcept;
-	bool WriteHeightMap() noexcept;
 	bool WriteLocked(GCodeChannel channel) noexcept;
 	bool WriteFileChunkRequest(const char *filename, uint32_t offset, uint32_t maxLength) noexcept;
 	bool WriteEvaluationResult(const char *expression, const ExpressionValue& value) noexcept;
diff --git a/src/SBC/SbcInterface.cpp b/src/SBC/SbcInterface.cpp
index d4bbb5a5..2230d2ec 100644
--- a/src/SBC/SbcInterface.cpp
+++ b/src/SBC/SbcInterface.cpp
@@ -431,39 +431,6 @@ void SbcInterface::ExchangeData() noexcept
 			break;
 		}
 
-		// Return heightmap as generated by G29 S0
-		case SbcRequest::GetHeightMap:
-		{
-			ConditionalReadLocker locker(reprap.GetMove().heightMapLock);
-			if (locker.IsLocked())
-			{
-				packetAcknowledged = transfer.WriteHeightMap();
-			}
-			else
-			{
-				packetAcknowledged = false;
-			}
-			break;
-		}
-
-		// Set heightmap via G29 S1
-		case SbcRequest::SetHeightMap:
-		{
-			ConditionalWriteLocker locker(reprap.GetMove().heightMapLock);
-			if (locker.IsLocked())
-			{
-				if (!transfer.ReadHeightMap())
-				{
-					reprap.GetPlatform().Message(ErrorMessage, "Failed to set height map - bad data?\n");
-				}
-			}
-			else
-			{
-				packetAcknowledged = false;
-			}
-			break;
-		}
-
 		// Lock movement and wait for standstill
 		case SbcRequest::LockMovementAndWaitForStandstill:
 		{
@@ -887,7 +854,7 @@ void SbcInterface::ExchangeData() noexcept
 			}
 			break;
 
-			// Invalid request
+		// Invalid request
 		default:
 			REPORT_INTERNAL_ERROR;
 			break;
diff --git a/src/SBC/SbcMessageFormats.h b/src/SBC/SbcMessageFormats.h
index 11991fb7..c536f881 100644
--- a/src/SBC/SbcMessageFormats.h
+++ b/src/SBC/SbcMessageFormats.h
@@ -207,7 +207,7 @@ enum class FirmwareRequest : uint16_t
 	AbortFile = 5,						// Request the current file to be closed
 	StackEvent_Obsolete = 6,			// Stack has been changed (unused)
 	PrintPaused = 7,					// Print has been paused
-	HeightMap = 8,						// Response to a heightmap request
+	HeightMap_Obsolete = 8,				// Response to a heightmap request (no longer used)
 	Locked = 9,							// Movement has been locked and machine is in standstill
 	FileChunk = 10,            			// Request another chunk of a file (only used by CAN expansion board updates)
 	EvaluationResult = 11,				// Response to an expression evaluation request
@@ -257,8 +257,8 @@ enum class SbcRequest : uint16_t
 	SetPrintFileInfo = 5,						// Print is about to be started, set file print information
     PrintStopped = 6,							// Print has been stopped, reset file print information
     MacroCompleted = 7,							// Notification that a macro file has been fully processed
-    GetHeightMap = 8,							// Request the heightmap coordinates as generated by G29 S0
-    SetHeightMap = 9,							// Set the heightmap coordinates via G29 S1
+    GetHeightMap_deprecated = 8,				// Request the heightmap coordinates as generated by G29 S0 (no longer used)
+    SetHeightMap_deprecated = 9,				// Set the heightmap coordinates via G29 S1 (no longer used)
     LockMovementAndWaitForStandstill = 10,		// Lock movement and wait for standstill
 	Unlock = 11,								// Unlock occupied resources
 	WriteIap = 12,								// Write another chunk of the IAP binary