Started implementing new motion system without using FPU

3 files changed, 124 insertions, 27 deletions

diff --git a/src/Movement/DriveMovement.h b/src/Movement/DriveMovement.h
index 75ad8346..fe3afcdf 100644
--- a/src/Movement/DriveMovement.h
+++ b/src/Movement/DriveMovement.h
@@ -99,11 +99,12 @@ private:
 	uint32_t nextStepTime;								// how many clocks after the start of this move the next step is due
 	uint32_t stepInterval;								// how many clocks between steps
 
-	float distanceSoFar;
 #if DM_USE_FPU
+	float distanceSoFar;
 	float timeSoFar;
 	float pA, pB, pC;
 #else
+	uint32_t distanceSoFar;
 	uint32_t iTimeSoFar;
 	int64_t ipA;
 	int32_t ipB, ipC;
@@ -127,24 +128,27 @@ private:
 			int64_t dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared;
 			int32_t hmz0sK;								// the starting step position less the starting Z height, multiplied by the Z movement fraction and K (can go negative)
 			int32_t minusAaPlusBbTimesKs;
+			int32_t reverseStartDistance;				// the overall move distance at which movement reversal occurs
 #endif
 		} delta;
 
 		struct CartesianParameters
 		{
+#if DM_USE_FPU
 			float pressureAdvanceK;						// how much pressure advance is applied to this move
 			float effectiveStepsPerMm;					// the steps/mm multiplied by the movement fraction
 			float effectiveMmPerStep;					// reciprocal of [the steps/mm multiplied by the movement fraction]
 			float extraExtrusionDistance;				// the extra extrusion distance in the acceleration phase
 			float extrusionBroughtForwards;				// the amount of extrusion brought forwards from previous moves. Only needed for debug output.
+#else
+			uint32_t iPressureAdvanceK;					// how much pressure advance is applied to this move
+			uint32_t iEffectiveStepsPerMm;				// the steps/mm multiplied by the movement fraction
+			uint32_t iEffectiveMmPerStep;				// reciprocal of [the steps/mm multiplied by the movement fraction]
+			uint32_t iExtraExtrusionDistance;			// the extra extrusion distance in the acceleration phase
+			uint32_t iExtrusionBroughtForwards;			// the amount of extrusion brought forwards from previous moves. Only needed for debug output.
+#endif
 		} cart;
 	} mp;
-
-#if !DM_USE_FPU
-	static constexpr uint32_t K1 = 1024;				// a power of 2 used to multiply the value mmPerStepTimesCdivtopSpeed to reduce rounding errors
-	static constexpr uint32_t K2 = 512;					// a power of 2 used in delta calculations to reduce rounding errors (but too large makes things worse)
-	static constexpr int32_t Kc = 1024 * 1024;			// a power of 2 for scaling the Z movement fraction
-#endif
 };
 
 // Calculate and store the time since the start of the move when the next step for the specified DriveMovement is due.
diff --git a/src/Movement/MoveSegment.cpp b/src/Movement/MoveSegment.cpp
index 666af6b8..9dd3e788 100644
--- a/src/Movement/MoveSegment.cpp
+++ b/src/Movement/MoveSegment.cpp
@@ -50,6 +50,7 @@ void MoveSegment::AddToTail(MoveSegment *tail) noexcept
 
 void MoveSegment::DebugPrint(char ch) const noexcept
 {
+#if MS_USE_FPU
 	debugPrintf("%c d=%.4e t=%.1f ", ch, (double)segmentLength, (double)segTime);
 	if (IsLinear())
 	{
@@ -59,6 +60,17 @@ void MoveSegment::DebugPrint(char ch) const noexcept
 	{
 		debugPrintf("b=%.4e c=%.4e\n", (double)b, (double)c);
 	}
+#else
+	debugPrintf("%c d=%" PRIu32 " t=%" PRIu32 " ", ch, segmentLength, segTime);
+	if (IsLinear())
+	{
+		debugPrintf("c=%" PRIi32 "\n", c);
+	}
+	else
+	{
+		debugPrintf("b=%" PRIi32 " c=%" PRIi32 "\n", b, c);
+	}
+#endif
 }
 
 // End
diff --git a/src/Movement/MoveSegment.h b/src/Movement/MoveSegment.h
index 10917f74..3385f163 100644
--- a/src/Movement/MoveSegment.h
+++ b/src/Movement/MoveSegment.h
@@ -134,6 +134,8 @@
 #include <RepRapFirmware.h>
 #include <Platform/Tasks.h>
 
+#define MS_USE_FPU			(__FPU_USED)
+
 class MoveSegment
 {
 public:
@@ -144,6 +146,7 @@ public:
 
 	MoveSegment(MoveSegment *p_next) noexcept;
 
+#if MS_USE_FPU
 	float GetSegmentLength() const noexcept { return segmentLength; }
 	float GetSegmentTime() const noexcept { return segTime; }
 	float CalcNonlinearA(float startDistance) const noexcept;
@@ -154,18 +157,31 @@ public:
 	float CalcC(float mmPerStep) const noexcept;
 	float GetC() const noexcept { return c; }
 
+	void SetLinear(float pSegmentLength, float p_segTime, float p_c) noexcept;
+	void SetNonLinear(float pSegmentLength, float p_segTime, float p_b, float p_c) noexcept;
+#else
+	uint32_t GetSegmentLength() const noexcept { return segmentLength; }
+	uint32_t GetSegmentTime() const noexcept { return segTime; }
+	int64_t CalcNonlinearA(uint32_t startDistance) const noexcept;
+	int64_t CalcNonlinearA(uint32_t startDistance, uint32_t pressureAdvanceK) const noexcept;
+	int32_t CalcNonlinearB(uint32_t startTime) const noexcept;
+	int32_t CalcNonlinearB(uint32_t startTime, uint32_t pressureAdvanceK) const noexcept;
+	int32_t CalcLinearB(uint32_t startDistance, uint32_t startTime) const noexcept;
+	int32_t CalcC(uint32_t mmPerStep) const noexcept;
+	int32_t GetC() const noexcept { return c; }
+
+	void SetLinear(uint32_t pSegmentLength, uint32_t p_segTime, int32_t p_c) noexcept;
+	void SetNonLinear(uint32_t pSegmentLength, uint32_t p_segTime, int32_t p_b, int32_t p_c) noexcept;
+#endif
+	void SetReverse() noexcept;
+
 	MoveSegment *GetNext() const noexcept;
 	bool IsLinear() const noexcept;
 	bool IsAccelerating() const noexcept pre(!IsLinear());
 	bool IsLast() const noexcept;
 
 	void SetNext(MoveSegment *p_next) noexcept;
-	void SetLinear(float pSegmentLength, float p_segTime, float p_c) noexcept;
-	void SetNonLinear(float pSegmentLength, float p_segTime, float p_b, float p_c) noexcept;
-	void SetReverse() noexcept;
-
 	void AddToTail(MoveSegment *tail) noexcept;
-
 	void DebugPrint(char ch) const noexcept;
 
 	// Allocate a MoveSegment, clearing the Linear and Last flags
@@ -187,10 +203,21 @@ private:
 	static_assert(sizeof(MoveSegment*) == sizeof(uint32_t));
 
 	// The 'next' field is a MoveSegment pointer with two flag bits in the bottom two bits
-	uint32_t nextAndFlags;			// pointer to the next segment, plus flag bits
-	float segmentLength;			// the length of this segment before applying the movement fraction
-	float segTime;					// the time in step clocks at which this move ends
-	float b, c;						// the move parameters (b is not needed for linear moves)
+	uint32_t nextAndFlags;								// pointer to the next segment, plus flag bits
+#if MS_USE_FPU
+	float segmentLength;								// the length of this segment before applying the movement fraction
+	float segTime;										// the time in step clocks at which this move ends
+	float b, c;											// the move parameters (b is not needed for linear moves)
+#else
+	uint32_t segmentLength;								// the length of this segment before applying the movement fraction
+	uint32_t segTime;									// the time in step clocks at which this move ends
+	int32_t b, c;										// the move parameters (b is not needed for linear moves)
+
+	static constexpr uint32_t Kdistance = 1u << 10;		// a power of 2 used to multiply distances by so we can store them as integers
+	static constexpr uint32_t KstepsPerMm = 1u << 16;	// a power of 2 used to multiply steps/mm by so we can store them as integers
+	static constexpr uint32_t KmmPerStep = 1u << 31;	// a power of 2 for scaling the Z movement fraction
+#endif
+
 };
 
 // Create a new one, leaving the flags clear
@@ -221,6 +248,38 @@ inline bool MoveSegment::IsLast() const noexcept
 	return GetNext() == nullptr;
 }
 
+#if MS_USE_FPU
+
+inline float MoveSegment::CalcNonlinearA(float startDistance) const noexcept
+{
+	return fsquare(b) - startDistance * c;
+}
+
+inline float MoveSegment::CalcNonlinearA(float startDistance, float pressureAdvanceK) const noexcept
+{
+	return fsquare(b - pressureAdvanceK) - startDistance * c;
+}
+
+inline float MoveSegment::CalcNonlinearB(float startTime) const noexcept
+{
+	return b + startTime;
+}
+
+inline float MoveSegment::CalcNonlinearB(float startTime, float pressureAdvanceK) const noexcept
+{
+	return (b - pressureAdvanceK) + startTime;
+}
+
+inline float MoveSegment::CalcLinearB(float startDistance, float startTime) const noexcept
+{
+	return startTime - (startDistance * c);
+}
+
+inline float MoveSegment::CalcC(float mmPerStep) const noexcept
+{
+	return c * mmPerStep;
+}
+
 inline void MoveSegment::SetLinear(float pSegmentLength, float p_segTime, float p_c) noexcept
 {
 	segmentLength = pSegmentLength;
@@ -239,36 +298,58 @@ inline void MoveSegment::SetNonLinear(float pSegmentLength, float p_segTime, flo
 	c = p_c;
 }
 
-inline float MoveSegment::CalcNonlinearA(float startDistance) const noexcept
+#else
+
+inline int64_t MoveSegment::CalcNonlinearA(uint32_t startDistance) const noexcept
 {
-	return fsquare(b) - startDistance * c;
+	return isquare64(b) - (int64_t)startDistance * c;
 }
 
-inline float MoveSegment::CalcNonlinearA(float startDistance, float pressureAdvanceK) const noexcept
+inline int64_t MoveSegment::CalcNonlinearA(uint32_t startDistance, uint32_t pressureAdvanceK) const noexcept
 {
-	return fsquare(b - pressureAdvanceK) - startDistance * c;
+	return isquare64(b - pressureAdvanceK) - (int64_t)startDistance * c;
 }
 
-inline float MoveSegment::CalcNonlinearB(float startTime) const noexcept
+inline int32_t MoveSegment::CalcNonlinearB(uint32_t startTime) const noexcept
 {
-	return b + startTime;
+	return b + (int32_t)startTime;
 }
 
-inline float MoveSegment::CalcNonlinearB(float startTime, float pressureAdvanceK) const noexcept
+inline int32_t MoveSegment::CalcNonlinearB(uint32_t startTime, uint32_t pressureAdvanceK) const noexcept
 {
-	return (b - pressureAdvanceK) + startTime;
+	return (b - (int32_t)pressureAdvanceK) + (int32_t)startTime;
 }
 
-inline float MoveSegment::CalcLinearB(float startDistance, float startTime) const noexcept
+inline int32_t MoveSegment::CalcLinearB(uint32_t startDistance, uint32_t startTime) const noexcept
 {
-	return startTime - (startDistance * c);
+	return (int32_t)startTime - ((int32_t)startDistance * c);
 }
 
-inline float MoveSegment::CalcC(float mmPerStep) const noexcept
+inline int32_t MoveSegment::CalcC(uint32_t mmPerStep) const noexcept
 {
 	return c * mmPerStep;
 }
 
+inline void MoveSegment::SetLinear(uint32_t pSegmentLength, uint32_t p_segTime, int32_t p_c) noexcept
+{
+	segmentLength = pSegmentLength;
+	segTime = p_segTime;
+	b = 0;
+	c = p_c;
+	nextAndFlags |= LinearFlag;
+}
+
+// Set up an accelerating or decelerating move. We assume that the 'linear' flag is already clear.
+inline void MoveSegment::SetNonLinear(uint32_t pSegmentLength, uint32_t p_segTime, int32_t p_b, int32_t p_c) noexcept
+{
+	segmentLength = pSegmentLength;
+	segTime = p_segTime;
+	b = p_b;
+	c = p_c;
+}
+
+#endif
+
 // Given that this is an accelerating or decelerating move, return true if it is accelerating
 inline bool MoveSegment::IsAccelerating() const noexcept
 {