Use CAN time stamps to reduce clock sync jitter

3 files changed, 33 insertions, 18 deletions

diff --git a/src/CAN/CanInterface.cpp b/src/CAN/CanInterface.cpp
index 7092cad6..2212cc93 100644
--- a/src/CAN/CanInterface.cpp
+++ b/src/CAN/CanInterface.cpp
@@ -493,6 +493,20 @@ void CanInterface::CheckCanAddress(uint32_t address, const GCodeBuffer& gb) THRO
 	}
 }
 
+#if !SAME70
+
+uint16_t CanInterface::GetTimeStampCounter() noexcept
+{
+	return can0dev->ReadTimeStampCounter();
+}
+
+uint16_t CanInterface::GetTimeStampPeriod() noexcept
+{
+	return can0dev->GetTimeStampPeriod();
+}
+
+#endif
+
 //TODO can we get rid of the CanSender task if we send movement messages via the Tx FIFO?
 // This task picks up motion messages and sends them
 extern "C" [[noreturn]] void CanSenderLoop(void *) noexcept
diff --git a/src/CAN/CanInterface.h b/src/CAN/CanInterface.h
index a9148275..bdc581a8 100644
--- a/src/CAN/CanInterface.h
+++ b/src/CAN/CanInterface.h
@@ -84,6 +84,11 @@ namespace CanInterface
 	CanMessageBuffer *AllocateBuffer(const GCodeBuffer* gb) THROWS(GCodeException);
 	void CheckCanAddress(uint32_t address, const GCodeBuffer& gb) THROWS(GCodeException);
 
+#if !SAME70
+	uint16_t GetTimeStampCounter() noexcept;
+	uint16_t GetTimeStampPeriod() noexcept;
+#endif
+
 	// Info functions
 	GCodeResult GetRemoteFirmwareDetails(uint32_t boardAddress, GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);
 	GCodeResult RemoteDiagnostics(MessageType mt, uint32_t boardAddress, unsigned int type, GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);
diff --git a/src/CAN/CommandProcessor.cpp b/src/CAN/CommandProcessor.cpp
index 23ef0162..86936b56 100644
--- a/src/CAN/CommandProcessor.cpp
+++ b/src/CAN/CommandProcessor.cpp
@@ -282,13 +282,13 @@ static void HandleInputStateChanged(const CanMessageInputChanged& msg, CanAddres
 
 static GCodeResult EutGetInfo(const CanMessageReturnInfo& msg, const StringRef& reply, uint8_t& extra)
 {
-	static constexpr uint8_t LastDiagnosticsPart = 8;				// the last diagnostics part is typeDiagnosticsPart0 + 8
+	static constexpr uint8_t LastDiagnosticsPart = 9;				// the last diagnostics part is typeDiagnosticsPart0 + 9
 
 	switch (msg.type)
 	{
 	case CanMessageReturnInfo::typeFirmwareVersion:
 	default:
-		reply.printf("%s (%s)", VERSION, DATE);
+		reply.printf("%s (%s%s)", VERSION, DATE, TIME_SUFFIX);
 		break;
 
 	case CanMessageReturnInfo::typeBoardName:
@@ -322,7 +322,7 @@ static GCodeResult EutGetInfo(const CanMessageReturnInfo& msg, const StringRef&
 
 	case CanMessageReturnInfo::typeDiagnosticsPart0:
 		extra = LastDiagnosticsPart;
-		reply.lcatf("%s (%s)", VERSION, DATE);
+		reply.lcatf("%s (%s%s)", VERSION, DATE, TIME_SUFFIX);
 		break;
 
 	case CanMessageReturnInfo::typeDiagnosticsPart0 + 1:
@@ -351,19 +351,24 @@ static GCodeResult EutGetInfo(const CanMessageReturnInfo& msg, const StringRef&
 		extra = LastDiagnosticsPart;
 		{
 #if HAS_VOLTAGE_MONITOR && HAS_12V_MONITOR
-			reply.catf("VIN: %.1fV, V12: %.1fV\n", (double)reprap.GetPlatform().GetCurrentPowerVoltage(), (double)reprap.GetPlatform().GetCurrentV12Voltage());
+			reply.catf("VIN: %.1fV, V12: %.1fV", (double)reprap.GetPlatform().GetCurrentPowerVoltage(), (double)reprap.GetPlatform().GetCurrentV12Voltage());
 #elif HAS_VOLTAGE_MONITOR
-			reply.catf("VIN: %.1fV\n", (double)reprap.GetPlatform().GetCurrentPowerVoltage());
+			reply.catf("VIN: %.1fV", (double)reprap.GetPlatform().GetCurrentPowerVoltage());
 #elif HAS_12V_MONITOR
-			reply.catf("V12: %.1fV\n", (double)reprap.GetPlatform().GetCurrentV12Voltage());
+			reply.catf("V12: %.1fn", (double)reprap.GetPlatform().GetCurrentV12Voltage());
 #endif
 #if HAS_CPU_TEMP_SENSOR
 			const MinMaxCurrent temps = reprap.GetPlatform().GetMcuTemperatures();
-			reply.catf("MCU temperature: min %.1fC, current %.1fC, max %.1fC\n", (double)temps.min, (double)temps.current, (double)temps.max);
+			reply.catf("MCU temperature: min %.1fC, current %.1fC, max %.1fC", (double)temps.min, (double)temps.current, (double)temps.max);
 #endif
 		}
 		break;
-	}
+
+	case CanMessageReturnInfo::typeDiagnosticsPart0 + 9:
+		extra = LastDiagnosticsPart;
+		StepTimer::Diagnostics(reply);
+		break;
+}
 	return GCodeResult::ok;
 }
 
@@ -387,16 +392,7 @@ void CommandProcessor::ProcessReceivedMessage(CanMessageBuffer *buf) noexcept
 			switch (id)
 			{
 			case CanMessageType::timeSync:
-				//TODO re-implement this as a PLL and use the CAN time stamps for greater accuracy
-				StepTimer::SetLocalTimeOffset(StepTimer::GetTimerTicks() - buf->msg.sync.timeSent);
-#if 0
-				//TODO implement this when we want to support using a main board as an expansion board
-				reprap.GetPlatform().SetPrinting(buf->msg.sync.isPrinting);
-#endif
-				if (buf->dataLength >= 16)				// if real time is included
-				{
-					reprap.GetPlatform().SetDateTime((time_t)buf->msg.sync.realTime);
-				}
+				StepTimer::ProcessTimeSyncMessage(buf->msg.sync, buf->dataLength, buf->timeStamp);
 				return;							// no reply needed
 
 			case CanMessageType::movementLinear: