More work on multicast

2 files changed, 52 insertions, 10 deletions

diff --git a/src/CAN/CanInterface.cpp b/src/CAN/CanInterface.cpp
index f3d1de15..a5ad7a81 100644
--- a/src/CAN/CanInterface.cpp
+++ b/src/CAN/CanInterface.cpp
@@ -203,6 +203,50 @@ extern "C" [[noreturn]] void CanSenderLoop(void *) noexcept;
 extern "C" [[noreturn]] void CanClockLoop(void *) noexcept;
 extern "C" [[noreturn]] void CanReceiverLoop(void *) noexcept;
 
+// Status LED handling
+
+#if SUPPORT_MULTICAST_DISCOVERY
+
+// The STATUS LED is also used to identify one board among several visible to the user
+static volatile uint32_t identInitialClocks = 0;		// when we started identifying
+static volatile uint32_t identTotalClocks = 0;			// how many step clocks to identify for, zero means until cancelled
+static volatile bool identifying = false;
+
+void CanInterface::SetStatusLedIdentify(uint32_t seconds) noexcept
+{
+	identTotalClocks = seconds * StepClockRate;
+	identInitialClocks = StepTimer::GetTimerTicks();
+	identifying = true;
+}
+
+void CanInterface::SetStatusLedNormal() noexcept
+{
+	identifying = false;
+}
+
+#endif
+
+// This is called only from the CAN clock loop, so inline
+static inline void UpdateLed(uint32_t stepClocks) noexcept
+{
+	// Blink the LED at about 2Hz. Duet 3 expansion boards will blink in sync when they have established clock sync with us.
+	bool turnLedOn = (stepClocks & (1u << 19)) != 0;
+#if SUPPORT_MULTICAST_DISCOVERY
+	if (identifying)
+	{
+		if (identTotalClocks != 0 && stepClocks - identInitialClocks >= identTotalClocks)
+		{
+			identifying = 0;							// stop identifying
+		}
+		else if ((stepClocks & (1u << 17)) == 0)		// double flash instead of single
+		{
+			turnLedOn = false;
+		}
+	}
+#endif
+	digitalWrite(DiagPin, XNor(DiagOnPolarity, turnLedOn));
+}
+
 static void InitReceiveFilters() noexcept
 {
 	// Set up a filter to receive all request messages addressed to us in FIFO 0
@@ -570,8 +614,7 @@ extern "C" [[noreturn]] void CanClockLoop(void *) noexcept
 		SendCanMessage(TxBufferIndexTimeSync, 0, &buf);
 		++timeSyncMessagesSent;
 
-		// Blink the LED at about 2Hz. Duet 3 expansion boards will blink in sync when they have established clock sync with us.
-		digitalWrite(DiagPin, XNor(DiagOnPolarity, lastTimeSent & (1u << 19)) != 0);
+		UpdateLed(lastTimeSent);
 
 		// Delay until it is time again
 		vTaskDelayUntil(&lastWakeTime, CanClockIntervalMillis);
@@ -1490,15 +1533,14 @@ GCodeResult CanInterface::StartClosedLoopDataCollection(DriverId device, uint16_
 }
 
 #if DUAL_CAN
+
 CanId CanInterface::ODrive::ArbitrationId(DriverId const driver, uint8_t const cmd) noexcept {
 	const auto arbitration_id = (driver.boardAddress << 5) + cmd;
 	CanId canId;
 	canId.SetReceivedId(arbitration_id);
 	return canId;
 }
-#endif
 
-#if DUAL_CAN
 CanMessageBuffer * CanInterface::ODrive::PrepareSimpleMessage(DriverId const driver, const StringRef& reply) noexcept
 {
 	// Detect any early return conditions
@@ -1523,17 +1565,12 @@ CanMessageBuffer * CanInterface::ODrive::PrepareSimpleMessage(DriverId const dri
 
 	return buf;
 }
-#endif
 
-#if DUAL_CAN
 void CanInterface::ODrive::FlushCanReceiveHardware() noexcept
 {
 	while (CanInterface::ReceivePlainMessage(nullptr, 0)) { }
 }
-#endif
 
-
-#if DUAL_CAN
 bool CanInterface::ODrive::GetExpectedSimpleMessage(CanMessageBuffer *buf, DriverId const driver, uint8_t const cmd, const StringRef& reply) noexcept
 {
 	CanId const expectedId = ArbitrationId(driver, cmd);
@@ -1554,7 +1591,7 @@ bool CanInterface::ODrive::GetExpectedSimpleMessage(CanMessageBuffer *buf, Drive
 
 	return ok;
 }
-#endif
+#endif	// DUAL_CAN
 
 #endif
 
diff --git a/src/CAN/CanInterface.h b/src/CAN/CanInterface.h
index ef322205..14b9faca 100644
--- a/src/CAN/CanInterface.h
+++ b/src/CAN/CanInterface.h
@@ -109,6 +109,11 @@ namespace CanInterface
 #endif
 	GCodeResult StartClosedLoopDataCollection(DriverId device, uint16_t filter, uint16_t numSamples, uint16_t rateRequested, uint8_t movementRequested, uint8_t mode, const GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);
 
+#if SUPPORT_MULTICAST_DISCOVERY
+	void SetStatusLedIdentify(uint32_t seconds) noexcept;
+	void SetStatusLedNormal() noexcept;
+#endif
+
 #if DUAL_CAN
 namespace ODrive {
 	CanId ArbitrationId(DriverId driver, uint8_t cmd) noexcept;