Fixes for main board used as expansion

2 files changed, 54 insertions, 31 deletions

diff --git a/src/CAN/CanInterface.cpp b/src/CAN/CanInterface.cpp
index 4c3a6739..31c43bd2 100644
--- a/src/CAN/CanInterface.cpp
+++ b/src/CAN/CanInterface.cpp
@@ -25,13 +25,13 @@
 #include <GCodes/GCodeBuffer/GCodeBuffer.h>
 #include <ClosedLoop/ClosedLoop.h>
 
-
 #if HAS_SBC_INTERFACE
 # include "SBC/SbcInterface.h"
 #endif
 
 #if SUPPORT_REMOTE_COMMANDS
 # include <Version.h>
+# include <InputMonitors/InputMonitor.h>
 #endif
 
 #include <memory>
@@ -420,7 +420,7 @@ uint16_t CanInterface::GetTimeStampPeriod() noexcept
 
 #endif
 
-// Send a message on the CAN FD channel and reord any errors
+// Send a message on the CAN FD channel and record any errors
 static void SendCanMessage(CanDevice::TxBufferNumber whichBuffer, uint32_t timeout, CanMessageBuffer *buffer) noexcept
 {
 	const uint32_t cancelledId = can0dev->SendMessage(whichBuffer, timeout, buffer);
@@ -437,46 +437,69 @@ extern "C" [[noreturn]] void CanSenderLoop(void *) noexcept
 {
 	for (;;)
 	{
-		TaskBase::Take(Mutex::TimeoutUnlimited);
-		for (;;)
+#if SUPPORT_REMOTE_COMMANDS
+		if (inExpansionMode)
 		{
-			CanMessageBuffer * const urgentMessage = CanMotion::GetUrgentMessage();
-			if (urgentMessage != nullptr)
+			// In expansion mode this task just send notifications when the states of input handles change
+			CanMessageBuffer buf(nullptr);
+			auto msg = buf.SetupStatusMessage<CanMessageInputChanged>(CanInterface::GetCanAddress(), CanInterface::GetCurrentMasterAddress());
+			msg->states = 0;
+			msg->zero = 0;
+			msg->numHandles = 0;
+
+			const uint32_t timeToWait = InputMonitor::AddStateChanges(msg);
+			if (msg->numHandles != 0)
 			{
-				SendCanMessage(TxBufferIndexUrgent, MaxUrgentSendWait, urgentMessage);
+				buf.dataLength = msg->GetActualDataLength();
+				SendCanMessage(TxBufferIndexUrgent, MaxUrgentSendWait, &buf);
 			}
-			else if (pendingMotionBuffers != nullptr)
+			TaskBase::Take(timeToWait);							// wait until we are woken up because a message is available, or we time out
+		}
+		else
+#endif
+		{
+			// In main board mode this task sends urgent messages concerning motion
+			for (;;)
 			{
-				CanMessageBuffer *buf;
+				CanMessageBuffer * const urgentMessage = CanMotion::GetUrgentMessage();
+				if (urgentMessage != nullptr)
+				{
+					SendCanMessage(TxBufferIndexUrgent, MaxUrgentSendWait, urgentMessage);
+				}
+				else if (pendingMotionBuffers != nullptr)
 				{
-					TaskCriticalSectionLocker lock;
-					buf = pendingMotionBuffers;
-					pendingMotionBuffers = buf->next;
+					CanMessageBuffer *buf;
+					{
+						TaskCriticalSectionLocker lock;
+						buf = pendingMotionBuffers;
+						pendingMotionBuffers = buf->next;
 #if 0	//unused
-					--numPendingMotionBuffers;
+						--numPendingMotionBuffers;
 #endif
-				}
+					}
 
-				// Send the message
-				SendCanMessage(TxBufferIndexMotion, MaxMotionSendWait, buf);
-				reprap.GetPlatform().OnProcessingCanMessage();
+					// Send the message
+					SendCanMessage(TxBufferIndexMotion, MaxMotionSendWait, buf);
+					reprap.GetPlatform().OnProcessingCanMessage();
 
 #ifdef CAN_DEBUG
-				// Display a debug message too
-				debugPrintf("CCCR %08" PRIx32 ", PSR %08" PRIx32 ", ECR %08" PRIx32 ", TXBRP %08" PRIx32 ", TXBTO %08" PRIx32 ", st %08" PRIx32 "\n",
-							MCAN1->MCAN_CCCR, MCAN1->MCAN_PSR, MCAN1->MCAN_ECR, MCAN1->MCAN_TXBRP, MCAN1->MCAN_TXBTO, GetAndClearStatusBits());
-				buf->msg.DebugPrint();
-				delay(50);
-				debugPrintf("CCCR %08" PRIx32 ", PSR %08" PRIx32 ", ECR %08" PRIx32 ", TXBRP %08" PRIx32 ", TXBTO %08" PRIx32 ", st %08" PRIx32 "\n",
-							MCAN1->MCAN_CCCR, MCAN1->MCAN_PSR, MCAN1->MCAN_ECR, MCAN1->MCAN_TXBRP, MCAN1->MCAN_TXBTO, GetAndClearStatusBits());
+					// Display a debug message too
+					debugPrintf("CCCR %08" PRIx32 ", PSR %08" PRIx32 ", ECR %08" PRIx32 ", TXBRP %08" PRIx32 ", TXBTO %08" PRIx32 ", st %08" PRIx32 "\n",
+								MCAN1->MCAN_CCCR, MCAN1->MCAN_PSR, MCAN1->MCAN_ECR, MCAN1->MCAN_TXBRP, MCAN1->MCAN_TXBTO, GetAndClearStatusBits());
+					buf->msg.DebugPrint();
+					delay(50);
+					debugPrintf("CCCR %08" PRIx32 ", PSR %08" PRIx32 ", ECR %08" PRIx32 ", TXBRP %08" PRIx32 ", TXBTO %08" PRIx32 ", st %08" PRIx32 "\n",
+								MCAN1->MCAN_CCCR, MCAN1->MCAN_PSR, MCAN1->MCAN_ECR, MCAN1->MCAN_TXBRP, MCAN1->MCAN_TXBTO, GetAndClearStatusBits());
 #endif
-				// Free the message buffer.
-				CanMessageBuffer::Free(buf);
-			}
-			else
-			{
-				break;
+					// Free the message buffer.
+					CanMessageBuffer::Free(buf);
+				}
+				else
+				{
+					break;
+				}
 			}
+			TaskBase::Take(Mutex::TimeoutUnlimited);
 		}
 	}
 }
diff --git a/src/InputMonitors/InputMonitor.cpp b/src/InputMonitors/InputMonitor.cpp
index 5f797606..d449e679 100644
--- a/src/InputMonitors/InputMonitor.cpp
+++ b/src/InputMonitors/InputMonitor.cpp
@@ -39,7 +39,7 @@ bool InputMonitor::Activate(bool useInterrupt) noexcept
 #if SAME5x
 				!useInterrupt || port.SetAnalogCallback(CommonAnalogPortInterrupt, CallbackParameter(this), 1);
 #else
-				true;			// SAME70 doesn't support SetAnalogCallback yet
+				false;			// SAME70 doesn't support SetAnalogCallback yet
 #endif
 		}
 		active = true;