Added support for magnetic filament monitor with V2 firmware

5 files changed, 212 insertions, 76 deletions

diff --git a/Maths/closestPointOfApproach3d.wxmx b/Maths/closestPointOfApproach3d.wxmx
new file mode 100644
index 00000000..af011508
--- /dev/null
+++ b/Maths/closestPointOfApproach3d.wxmx
diff --git a/src/FilamentMonitors/LaserFilamentMonitor.cpp b/src/FilamentMonitors/LaserFilamentMonitor.cpp
index 84cf4f94..a7f0572c 100644
--- a/src/FilamentMonitors/LaserFilamentMonitor.cpp
+++ b/src/FilamentMonitors/LaserFilamentMonitor.cpp
@@ -27,6 +27,7 @@ LaserFilamentMonitor::LaserFilamentMonitor(unsigned int extruder, int type)
 
 void LaserFilamentMonitor::Init()
 {
+	dataReceived = false;
 	sensorValue = 0;
 	parityErrorCount = framingErrorCount = overrunErrorCount = polarityErrorCount = overdueCount = 0;
 	lastMeasurementTime = 0;
diff --git a/src/FilamentMonitors/LaserFilamentMonitor.h b/src/FilamentMonitors/LaserFilamentMonitor.h
index 4a441c2f..4e9d328d 100644
--- a/src/FilamentMonitors/LaserFilamentMonitor.h
+++ b/src/FilamentMonitors/LaserFilamentMonitor.h
@@ -107,7 +107,6 @@ private:
 	float totalExtrusionCommanded;
 	float totalMovementMeasured;
 
-	uint8_t samplesReceived;
 	bool dataReceived;
 	bool backwards;
 
diff --git a/src/FilamentMonitors/RotatingMagnetFilamentMonitor.cpp b/src/FilamentMonitors/RotatingMagnetFilamentMonitor.cpp
index 2883bf7c..489047a4 100644
--- a/src/FilamentMonitors/RotatingMagnetFilamentMonitor.cpp
+++ b/src/FilamentMonitors/RotatingMagnetFilamentMonitor.cpp
@@ -22,16 +22,21 @@ RotatingMagnetFilamentMonitor::RotatingMagnetFilamentMonitor(unsigned int extrud
 	  minMovementAllowed(DefaultMinMovementAllowed), maxMovementAllowed(DefaultMaxMovementAllowed),
 	  minimumExtrusionCheckLength(DefaultMinimumExtrusionCheckLength), comparisonEnabled(false), checkNonPrintingMoves(false)
 {
-	switchOpenMask = (type == 4) ? TypeMagnetSwitchOpenMask : 0;
+	switchOpenMask = (type == 4) ? TypeMagnetV1SwitchOpenMask : 0;
 	Init();
 }
 
 void RotatingMagnetFilamentMonitor::Init()
 {
+	dataReceived = false;
 	sensorValue = 0;
-	framingErrorCount = overrunErrorCount = polarityErrorCount = overdueCount = 0;
-	calibrationStarted = dataReceived = false;
+	parityErrorCount = framingErrorCount = overrunErrorCount = polarityErrorCount = overdueCount = 0;
+	dataReceived = false;
 	lastMeasurementTime = 0;
+	lastErrorCode = 0;
+	version = 1;
+	backwards = false;
+	sensorError = false;
 	InitReceiveBuffer();
 	Reset();
 }
@@ -39,9 +44,9 @@ void RotatingMagnetFilamentMonitor::Init()
 void RotatingMagnetFilamentMonitor::Reset()
 {
 	extrusionCommandedThisSegment = extrusionCommandedSinceLastSync = movementMeasuredThisSegment = movementMeasuredSinceLastSync = 0.0;
-	comparisonStarted = false;
+	magneticMonitorState = MagneticMonitorState::idle;
 	haveStartBitData = false;
-	wasPrintingAtStartBit = false;			// force a resync
+	synced = false;							// force a resync
 }
 
 // Configure this sensor, returning true if error and setting 'seen' if we processed any configuration parameters
@@ -89,7 +94,8 @@ bool RotatingMagnetFilamentMonitor::Configure(GCodeBuffer& gb, const StringRef&
 	}
 	else
 	{
-		reply.printf("Duet3D rotating magnet filament monitor%s on input %u, %s, sensitivity %.2fmm/rev, allow %ld%% to %ld%%, check every %.1fmm, ",
+		reply.printf("Duet3D rotating magnet filament monitor v%u%s on input %u, %s, sensitivity %.2fmm/rev, allow %ld%% to %ld%%, check every %.1fmm, ",
+						version,
 						(switchOpenMask != 0) ? " with switch" : "",
 						GetEndstopNumber(),
 						(comparisonEnabled) ? "enabled" : "disabled",
@@ -97,24 +103,29 @@ bool RotatingMagnetFilamentMonitor::Configure(GCodeBuffer& gb, const StringRef&
 						lrintf(minMovementAllowed * 100.0),
 						lrintf(maxMovementAllowed * 100.0),
 						(double)minimumExtrusionCheckLength);
+
 		if (!dataReceived)
 		{
 			reply.cat("no data received");
 		}
-		else if ((sensorValue & TypeMagnetErrorMask) != 0)
+		else if (sensorError)
 		{
 			reply.cat("error");
+			if (lastErrorCode != 0)
+			{
+				reply.catf(" %u", lastErrorCode);
+			}
 		}
 		else
 		{
 			reply.catf("current pos %.1f, ", (double)GetCurrentPosition());
-			if (calibrationStarted && fabsf(totalMovementMeasured) > 1.0 && totalExtrusionCommanded > 20.0)
+			if (magneticMonitorState != MagneticMonitorState::calibrating && totalExtrusionCommanded > 10.0)
 			{
 				const float measuredMmPerRev = totalExtrusionCommanded/totalMovementMeasured;
 				reply.catf("measured sensitivity %.2fmm/rev, min %ld%% max %ld%% over %.1fmm\n",
 					(double)measuredMmPerRev,
-					lrintf(100 * minMovementRatio),
-					lrintf(100 * maxMovementRatio),
+					lrintf(100 * minMovementRatio * measuredMmPerRev),
+					lrintf(100 * maxMovementRatio * measuredMmPerRev),
 					(double)totalExtrusionCommanded);
 			}
 			else
@@ -135,10 +146,86 @@ void RotatingMagnetFilamentMonitor::HandleIncomingData()
 	while ((res = PollReceiveBuffer(val)) != PollResult::incomplete)
 	{
 		// We have either received a report or there has been a receive error
+		bool receivedPositionReport = false;
 		if (res == PollResult::complete)
 		{
 			// We have a completed a position report
-			dataReceived = true;
+			// Check the parity
+			uint8_t data8 = (uint8_t)((val >> 8) ^ val);
+			data8 ^= (data8 >> 4);
+			data8 ^= (data8 >> 2);
+			data8 ^= (data8 >> 1);
+
+			// Version 1 sensor:
+			//  Data word:			0S00 00pp pppppppp		S = switch open, pppppppppp = 10-bit filament position
+			//  Error word:			1000 0000 00000000
+			//
+			// Version 2 sensor (this firmware):
+			//  Data word:			P00S 10pp pppppppp		S = switch open, ppppppppppp = 10-bit filament position
+			//  Error word:			P010 0000 0000eeee		eeee = error code
+			//	Version word:		P110 0000 vvvvvvvv		vvvvvvvv = sensor/firmware version, at least 2
+			if (version == 1)
+			{
+				if ((data8 & 1) == 0 && (val & 0x7F00) == 0x6000 && (val & 0x00FF) >= 2)
+				{
+					// Received a version word with the correct parity, so must be version 2 or later
+					version = val & 0x00FF;
+					if (switchOpenMask != 0)
+					{
+						switchOpenMask = TypeMagnetV2SwitchOpenMask;
+					}
+				}
+				else if (val == TypeMagnetV1ErrorMask)
+				{
+					sensorError = true;
+					lastErrorCode = 0;
+				}
+				else if ((val & 0xBC00) == 0)
+				{
+					receivedPositionReport = true;
+					dataReceived = true;
+					sensorError = false;
+				}
+			}
+			else if ((data8 & 1) != 0)
+			{
+				++parityErrorCount;
+			}
+			else
+			{
+				switch (val & TypeMagnetV2MessageTypeMask)
+				{
+				case TypeMagnetV2MessageTypePosition:
+					receivedPositionReport = true;
+					dataReceived = true;
+					sensorError = false;
+					break;
+
+				case TypeMagnetV2MessageTypeError:
+					lastErrorCode = val & 0x00FF;
+					sensorError = true;
+					break;
+
+				case TypeMagnetV2MessageTypeInfo:
+					if ((val & TypeMagnetV2InfoTypeMask) == TypeMagnetV2InfoTypeVersion)
+					{
+						version = val & 0x00FF;
+					}
+					break;
+
+				default:
+					break;
+				}
+			}
+		}
+		else
+		{
+			// A receive error occurred. Any start bit data we stored is wrong.
+			++framingErrorCount;
+		}
+
+		if (receivedPositionReport)
+		{
 			lastMeasurementTime = millis();
 			const uint16_t angleChange = (val - sensorValue) & TypeMagnetAngleMask;			// angle change in range 0..1023
 			const int32_t movement = (angleChange <= 512) ? (int32_t)angleChange : (int32_t)angleChange - 1024;
@@ -147,24 +234,23 @@ void RotatingMagnetFilamentMonitor::HandleIncomingData()
 
 			if (haveStartBitData)					// if we have a synchronised value for the amount of extrusion commanded
 			{
-				if (   checkNonPrintingMoves
-					|| (wasPrintingAtStartBit && (int32_t)(lastSyncTime - reprap.GetMove().ExtruderPrintingSince()) > SyncDelayMillis)
-				   )
+				if (synced)
 				{
-					// We can use this measurement
-					extrusionCommandedThisSegment += extrusionCommandedAtCandidateStartBit;
-					movementMeasuredThisSegment += movementMeasuredSinceLastSync;
+					if (   checkNonPrintingMoves
+						|| (wasPrintingAtStartBit && (int32_t)(lastSyncTime - reprap.GetMove().ExtruderPrintingSince()) > SyncDelayMillis)
+					   )
+					{
+						// We can use this measurement
+						extrusionCommandedThisSegment += extrusionCommandedAtCandidateStartBit;
+						movementMeasuredThisSegment += movementMeasuredSinceLastSync;
+					}
 				}
 				lastSyncTime = candidateStartBitTime;
 				extrusionCommandedSinceLastSync -= extrusionCommandedAtCandidateStartBit;
 				movementMeasuredSinceLastSync = 0.0;
+				synced = checkNonPrintingMoves || wasPrintingAtStartBit;
 			}
 		}
-		else
-		{
-			// A receive error occurred. Any start bit data we stored is wrong.
-			++framingErrorCount;
-		}
 		haveStartBitData = false;
 	}
 }
@@ -198,7 +284,7 @@ FilamentSensorStatus RotatingMagnetFilamentMonitor::Check(bool isPrinting, bool
 
 	// 4. Decide whether it is time to do a comparison, and return the status
 	FilamentSensorStatus ret = FilamentSensorStatus::ok;
-	if ((sensorValue & TypeMagnetErrorMask) != 0)
+	if (sensorError)
 	{
 		ret = FilamentSensorStatus::sensorError;
 	}
@@ -233,56 +319,76 @@ FilamentSensorStatus RotatingMagnetFilamentMonitor::CheckFilament(float amountCo
 	}
 
 	FilamentSensorStatus ret = FilamentSensorStatus::ok;
-	const float extrusionMeasured = amountMeasured * mmPerRev;
 
-	if (!comparisonStarted)
-	{
-		// The first measurement after we start extruding is often a long way out, so discard it
-		comparisonStarted = true;
-		calibrationStarted = false;
-	}
-	else if (comparisonEnabled)
+	switch (magneticMonitorState)
 	{
-		const float minExtrusionExpected = (amountCommanded >= 0.0)
-											 ? amountCommanded * minMovementAllowed
-												: amountCommanded * maxMovementAllowed;
-		if (extrusionMeasured < minExtrusionExpected)
-		{
-			ret = FilamentSensorStatus::tooLittleMovement;
-		}
-		else
+	case MagneticMonitorState::idle:
+		magneticMonitorState = MagneticMonitorState::calibrating;
+		totalExtrusionCommanded = amountCommanded;
+		totalMovementMeasured = amountMeasured;
+		break;
+
+	case MagneticMonitorState::calibrating:
+		totalExtrusionCommanded += amountCommanded;
+		totalMovementMeasured += amountMeasured;
+		if (totalExtrusionCommanded >= 10.0)
 		{
-			const float maxExtrusionExpected = (amountCommanded >= 0.0)
-												 ? amountCommanded * maxMovementAllowed
-													: amountCommanded * minMovementAllowed;
-			if (extrusionMeasured > maxExtrusionExpected)
+			backwards = (totalMovementMeasured < 0.0);
+			if (backwards)
 			{
-				ret = FilamentSensorStatus::tooMuchMovement;
+				totalMovementMeasured = -totalMovementMeasured;
 			}
-		}
-	}
+			float ratio = totalMovementMeasured/totalExtrusionCommanded;
+			minMovementRatio = maxMovementRatio = ratio;
 
-	// Update the calibration accumulators, even if the user hasn't asked to do calibration
-	const float ratio = extrusionMeasured/amountCommanded;
-	if (calibrationStarted)
-	{
-		if (ratio < minMovementRatio)
-		{
-			minMovementRatio = ratio;
+			if (comparisonEnabled)
+			{
+				ratio *= mmPerRev;
+				if (ratio < minMovementAllowed)
+				{
+					ret = FilamentSensorStatus::tooLittleMovement;
+				}
+				else if (ratio > maxMovementAllowed)
+				{
+					ret = FilamentSensorStatus::tooMuchMovement;
+				}
+			}
+			magneticMonitorState = MagneticMonitorState::comparing;
 		}
-		if (ratio > maxMovementRatio)
+		break;
+
+	case MagneticMonitorState::comparing:
 		{
-			maxMovementRatio = ratio;
+			totalExtrusionCommanded += amountCommanded;
+			if (backwards)
+			{
+				amountMeasured = -amountMeasured;
+			}
+			totalMovementMeasured += amountMeasured;
+			float ratio = amountMeasured/amountCommanded;
+			if (ratio > maxMovementRatio)
+			{
+				maxMovementRatio = ratio;
+			}
+			else if (ratio < minMovementRatio)
+			{
+				minMovementRatio = ratio;
+			}
+
+			if (comparisonEnabled)
+			{
+				ratio *= mmPerRev;
+				if (ratio < minMovementAllowed)
+				{
+					ret = FilamentSensorStatus::tooLittleMovement;
+				}
+				else if (ratio > maxMovementAllowed)
+				{
+					ret = FilamentSensorStatus::tooMuchMovement;
+				}
+			}
 		}
-		totalExtrusionCommanded += amountCommanded;
-		totalMovementMeasured += amountMeasured;
-	}
-	else
-	{
-		minMovementRatio = maxMovementRatio = ratio;
-		totalExtrusionCommanded = amountCommanded;
-		totalMovementMeasured = amountMeasured;
-		calibrationStarted = true;
+		break;
 	}
 
 	return ret;
@@ -294,7 +400,7 @@ FilamentSensorStatus RotatingMagnetFilamentMonitor::Clear()
 	Reset();
 	HandleIncomingData();								// to keep the diagnostics up to date
 
-	return ((sensorValue & TypeMagnetErrorMask) != 0) ? FilamentSensorStatus::sensorError
+	return (sensorError) ? FilamentSensorStatus::sensorError
 			: ((sensorValue & switchOpenMask) != 0) ? FilamentSensorStatus::noFilament
 				: FilamentSensorStatus::ok;
 }
@@ -303,17 +409,17 @@ FilamentSensorStatus RotatingMagnetFilamentMonitor::Clear()
 void RotatingMagnetFilamentMonitor::Diagnostics(MessageType mtype, unsigned int extruder)
 {
 	const char* const statusText = (!dataReceived) ? "no data received"
-									: ((sensorValue & TypeMagnetErrorMask) != 0) ? "error"
+									: (sensorError) ? "error"
 										: ((sensorValue & switchOpenMask) != 0) ? "no filament"
 											: "ok";
 	reprap.GetPlatform().MessageF(mtype, "Extruder %u: pos %.2f, %s, ", extruder, (double)GetCurrentPosition(), statusText);
-	if (calibrationStarted && fabsf(totalMovementMeasured) > 1.0 && totalExtrusionCommanded > 20.0)
+	if (magneticMonitorState != MagneticMonitorState::calibrating && totalExtrusionCommanded > 20.0)
 	{
 		const float measuredMmPerRev = totalExtrusionCommanded/totalMovementMeasured;
 		reprap.GetPlatform().MessageF(mtype, "measured sens %.2fmm/rev min %ld%% max %ld%% over %.1fmm",
 			(double)measuredMmPerRev,
-			lrintf(100 * minMovementRatio),
-			lrintf(100 * maxMovementRatio),
+			lrintf(100 * minMovementRatio * measuredMmPerRev),
+			lrintf(100 * maxMovementRatio * measuredMmPerRev),
 			(double)totalExtrusionCommanded);
 	}
 	else
@@ -322,8 +428,8 @@ void RotatingMagnetFilamentMonitor::Diagnostics(MessageType mtype, unsigned int
 	}
 	if (dataReceived)
 	{
-		reprap.GetPlatform().MessageF(mtype, ", errs: frame %" PRIu32 " ovrun %" PRIu32 " pol %" PRIu32  " ovdue %" PRIu32 "\n",
-			framingErrorCount, overrunErrorCount, polarityErrorCount, overdueCount);
+		reprap.GetPlatform().MessageF(mtype, ", errs: frame %" PRIu32 " parity %" PRIu32 " ovrun %" PRIu32 " pol %" PRIu32  " ovdue %" PRIu32 "\n",
+			framingErrorCount, parityErrorCount, overrunErrorCount, polarityErrorCount, overdueCount);
 	}
 }
 
diff --git a/src/FilamentMonitors/RotatingMagnetFilamentMonitor.h b/src/FilamentMonitors/RotatingMagnetFilamentMonitor.h
index 72acbbdb..84f98bbd 100644
--- a/src/FilamentMonitors/RotatingMagnetFilamentMonitor.h
+++ b/src/FilamentMonitors/RotatingMagnetFilamentMonitor.h
@@ -26,8 +26,26 @@ private:
 	static constexpr float DefaultMaxMovementAllowed = 1.6;
 	static constexpr float DefaultMinimumExtrusionCheckLength = 3.0;
 
-	static constexpr uint16_t TypeMagnetErrorMask = 0x8000u;
-	static constexpr uint16_t TypeMagnetSwitchOpenMask = 0x4000u;
+	// Version 1 message definitions
+	static constexpr uint16_t TypeMagnetV1ErrorMask = 0x8000u;
+	static constexpr uint16_t TypeMagnetV1SwitchOpenMask = 0x4000;
+
+	// Version 2 message definitions
+	static constexpr uint16_t TypeMagnetV2ParityMask = 0x8000;
+
+	// Definitions for identifying the top level type of a message
+	static constexpr uint16_t TypeMagnetV2MessageTypeMask = 0x6000;
+	static constexpr uint16_t TypeMagnetV2MessageTypePosition = 0x0000;
+	static constexpr uint16_t TypeMagnetV2MessageTypeError = 0x2000;
+	static constexpr uint16_t TypeMagnetV2MessageTypeInfo = 0x6000;
+
+	// Definitions for position data messages
+	static constexpr uint16_t TypeMagnetV2SwitchOpenMask = 0x1000;
+
+	// Definitions for info message types
+	static constexpr uint16_t TypeMagnetV2InfoTypeMask = 0x1F00;
+	static constexpr uint16_t TypeMagnetV2InfoTypeVersion = 0x0000;
+
 	static constexpr uint16_t TypeMagnetAngleMask = 0x03FF;			// we use a 10-bit sensor angle
 
 	void Init();
@@ -45,6 +63,7 @@ private:
 
 	// Other data
 	uint32_t framingErrorCount;								// the number of framing errors we received
+	uint32_t parityErrorCount;								// the number of words with bad parity we received
 	uint32_t overdueCount;									// the number of times a position report was overdue
 
 	uint32_t candidateStartBitTime;							// the time that we received a possible start bit
@@ -57,9 +76,13 @@ private:
 	uint16_t sensorValue;									// last known filament position (10 bits)
 	uint32_t lastMeasurementTime;							// the last time we received a value
 	uint16_t switchOpenMask;								// mask to isolate the switch open bit(s) from the sensor value
+	uint8_t version;										// sensor/firmware version
+	uint8_t lastErrorCode;									// the last error code received
+	bool sensorError;										// true if received an error report (cleared by a position report)
 
 	bool wasPrintingAtStartBit;
 	bool haveStartBitData;
+	bool synced;
 
 	float extrusionCommandedThisSegment;					// the amount of extrusion commanded (mm) since we last did a comparison
 	float movementMeasuredThisSegment;						// the accumulated movement in complete rotations since the previous comparison
@@ -70,8 +93,15 @@ private:
 	float totalMovementMeasured;
 
 	bool dataReceived;
-	bool comparisonStarted;
-	bool calibrationStarted;
+	bool backwards;
+
+	enum class MagneticMonitorState : uint8_t
+	{
+		idle,
+		calibrating,
+		comparing
+	};
+	MagneticMonitorState magneticMonitorState;
 };
 
 #endif /* SRC_FILAMENTSENSORS_ROTATINGMAGNETFILAMENTMONITOR_H_ */