From 6d7e503670463924f44c7de2a65b9ba7271905ea Mon Sep 17 00:00:00 2001
From: David Crocker <dcrocker@eschertech.com>
Date: Tue, 13 Sep 2022 10:35:30 +0100
Subject: Support M915 on MB6XD for CAN-connected drivers

---
 src/GCodes/GCodes2.cpp    |  2 +-
 src/Platform/Platform.cpp | 75 ++++++++++++++++++++++++++++++++---------------
 src/Platform/Platform.h   |  2 +-
 3 files changed, 53 insertions(+), 26 deletions(-)

diff --git a/src/GCodes/GCodes2.cpp b/src/GCodes/GCodes2.cpp
index 0ece485b..ddff52d8 100644
--- a/src/GCodes/GCodes2.cpp
+++ b/src/GCodes/GCodes2.cpp
@@ -4453,7 +4453,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 				break;
 #endif
 
-#if HAS_STALL_DETECT
+#if HAS_STALL_DETECT || SUPPORT_CAN_EXPANSION
 			case 915:
 				result = platform.ConfigureStallDetection(gb, reply, outBuf);
 				break;
diff --git a/src/Platform/Platform.cpp b/src/Platform/Platform.cpp
index f84808c9..0eb3bd59 100644
--- a/src/Platform/Platform.cpp
+++ b/src/Platform/Platform.cpp
@@ -4275,7 +4275,7 @@ float Platform::GetTmcDriversTemperature(unsigned int boardNumber) const noexcep
 
 #endif
 
-#if HAS_STALL_DETECT
+#if HAS_STALL_DETECT || SUPPORT_CAN_EXPANSION
 
 // Configure the motor stall detection, returning true if an error was encountered
 GCodeResult Platform::ConfigureStallDetection(GCodeBuffer& gb, const StringRef& reply, OutputBuffer *& buf) THROWS(GCodeException)
@@ -4283,9 +4283,9 @@ GCodeResult Platform::ConfigureStallDetection(GCodeBuffer& gb, const StringRef&
 	// Build a bitmap of all the drivers referenced
 	// First looks for explicit driver numbers
 	DriversBitmap drivers;
-#if SUPPORT_CAN_EXPANSION
+# if SUPPORT_CAN_EXPANSION
 	CanDriversList canDrivers;
-#endif
+# endif
 	if (gb.Seen('P'))
 	{
 		DriverId drives[NumDirectDrivers];
@@ -4295,19 +4295,21 @@ GCodeResult Platform::ConfigureStallDetection(GCodeBuffer& gb, const StringRef&
 		{
 			if (drives[i].IsLocal())
 			{
+# if HAS_SMART_DRIVERS
 				if (drives[i].localDriver >= numSmartDrivers)
 				{
 					reply.printf("Invalid local drive number '%u'", drives[i].localDriver);
 					return GCodeResult::error;
 				}
+# endif
 				drivers.SetBit(drives[i].localDriver);
 			}
-#if SUPPORT_CAN_EXPANSION
+# if SUPPORT_CAN_EXPANSION
 			else
 			{
 				canDrivers.AddEntry(drives[i]);
 			}
-#endif
+# endif
 		}
 	}
 
@@ -4318,9 +4320,9 @@ GCodeResult Platform::ConfigureStallDetection(GCodeBuffer& gb, const StringRef&
 		{
 			IterateDrivers(axis,
 							[&drivers](uint8_t localDriver){ drivers.SetBit(localDriver); }
-#if SUPPORT_CAN_EXPANSION
+# if SUPPORT_CAN_EXPANSION
 						  , [&canDrivers](DriverId driver){ canDrivers.AddEntry(driver); }
-#endif
+# endif
 						  );
 		}
 	}
@@ -4340,16 +4342,17 @@ GCodeResult Platform::ConfigureStallDetection(GCodeBuffer& gb, const StringRef&
 				{
 					drivers.SetBit(driver.localDriver);
 				}
-#if SUPPORT_CAN_EXPANSION
+# if SUPPORT_CAN_EXPANSION
 				else
 				{
 					canDrivers.AddEntry(driver);
 				}
-#endif
+# endif
 			}
 		}
 	}
 
+# if HAS_STALL_DETECT
 	// Now check for values to change
 	bool seen = false;
 	if (gb.Seen('S'))
@@ -4400,39 +4403,52 @@ GCodeResult Platform::ConfigureStallDetection(GCodeBuffer& gb, const StringRef&
 			break;
 		}
 	}
+#else
+	// Board does not have any local drivers with stall detection but may have CAN-connected drivers
+	const bool seen = gb.SeenAny("SFHTR");
+#endif
 
 	if (seen)
 	{
-#if SUPPORT_CAN_EXPANSION
-		return CanInterface::GetSetRemoteDriverStallParameters(canDrivers, gb, reply, buf);
-#else
+# if SUPPORT_CAN_EXPANSION
+		const GCodeResult rslt = CanInterface::GetSetRemoteDriverStallParameters(canDrivers, gb, reply, buf);
+#  if !HAS_SMART_DRIVERS
+		if (drivers.IsNonEmpty())
+		{
+			reply.lcatf("Stall detection not available for external drivers");
+			return max(rslt, GCodeResult::warning);
+		}
+#  endif
+		return rslt;
+# else
 		return GCodeResult::ok;
-#endif
+# endif
 	}
 
 	// Print the stall status
-	if (!OutputBuffer::Allocate(buf))
-	{
-		return GCodeResult::notFinished;
-	}
-
+# if HAS_SMART_DRIVERS
 	if (drivers.IsEmpty()
-#if SUPPORT_CAN_EXPANSION
+#  if SUPPORT_CAN_EXPANSION
 		&& canDrivers.IsEmpty()
-#endif
+#  endif
 	   )
 	{
 		drivers = DriversBitmap::MakeLowestNBits(numSmartDrivers);
 	}
 
+	if (!OutputBuffer::Allocate(buf))
+	{
+		return GCodeResult::notFinished;
+	}
+
 	drivers.Iterate
 		([buf, this, &reply](unsigned int drive, unsigned int) noexcept
 			{
-#if SUPPORT_CAN_EXPANSION
+#  if SUPPORT_CAN_EXPANSION
 				buf->lcatf("Driver 0.%u: ", drive);
-#else
+#  else
 				buf->lcatf("Driver %u: ", drive);
-#endif
+#  endif
 				reply.Clear();										// we use 'reply' as a temporary buffer
 				SmartDrivers::AppendStallConfig(drive, reply);
 				buf->cat(reply.c_str());
@@ -4443,12 +4459,23 @@ GCodeResult Platform::ConfigureStallDetection(GCodeBuffer& gb, const StringRef&
 						  );
 			}
 		);
+# else
+	if (canDrivers.IsEmpty())
+	{
+		reply.copy("No local drivers have stall detection");
+		return GCodeResult::ok;
+	}
 
+	if (!OutputBuffer::Allocate(buf))
+	{
+		return GCodeResult::notFinished;
+	}
+# endif
 # if SUPPORT_CAN_EXPANSION
 	return CanInterface::GetSetRemoteDriverStallParameters(canDrivers, gb, reply, buf);
 # else
 	return GCodeResult::ok;
-#endif
+# endif
 }
 
 #endif
diff --git a/src/Platform/Platform.h b/src/Platform/Platform.h
index f92ee9e1..1ed5186b 100644
--- a/src/Platform/Platform.h
+++ b/src/Platform/Platform.h
@@ -609,7 +609,7 @@ public:
 	bool HasVinPower() const noexcept { return true; }
 #endif
 
-#if HAS_STALL_DETECT
+#if HAS_STALL_DETECT || SUPPORT_CAN_EXPANSION
 	GCodeResult ConfigureStallDetection(GCodeBuffer& gb, const StringRef& reply, OutputBuffer *& buf) THROWS(GCodeException);
 #endif
 
-- 
cgit v1.2.3