Moved input shaper into a separate class

5 files changed, 182 insertions, 97 deletions

diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp
index eccaa907..a12552fb 100644
--- a/src/Movement/DDA.cpp
+++ b/src/Movement/DDA.cpp
@@ -1124,7 +1124,7 @@ pre(disableDeltaMapping || drive < MaxAxes)
 inline void DDA::AdjustAcceleration() noexcept
 {
 	// Try to reduce the acceleration/deceleration of the move to cancel ringing
-	const float idealPeriod = (float)((uint32_t)reprap.GetMove().GetShapingHalfPeriod() * 2)/65536;
+	const float idealPeriod = (float)((uint32_t)reprap.GetMove().GetShaper().GetHalfPeriod() * 2)/65536;
 
 	float proposedAcceleration = acceleration, proposedAccelDistance = beforePrepare.accelDistance;
 	bool adjustAcceleration = false;
@@ -1170,7 +1170,7 @@ inline void DDA::AdjustAcceleration() noexcept
 
 	if (adjustAcceleration || adjustDeceleration)
 	{
-		const float drcMinimumAcceleration = reprap.GetMove().GetShapingMinimumAcceleration();
+		const float drcMinimumAcceleration = reprap.GetMove().GetShaper().GetMinimumAcceleration();
 		if (proposedAccelDistance + proposedDecelDistance <= totalDistance)
 		{
 			if (proposedAcceleration < drcMinimumAcceleration || proposedDeceleration < drcMinimumAcceleration)
@@ -1253,7 +1253,7 @@ inline void DDA::AdjustAcceleration() noexcept
 void DDA::Prepare(uint8_t simMode, float extrusionPending[]) noexcept
 {
 	if (   flags.xyMoving
-		&& reprap.GetMove().GetShapingType() == InputShaping::DAA
+		&& reprap.GetMove().GetShaper().GetType() == InputShaperType::DAA
 		&& topSpeed > startSpeed && topSpeed > endSpeed
 		&& (fabsf(directionVector[X_AXIS]) > 0.5 || fabsf(directionVector[Y_AXIS]) > 0.5)
 	   )
diff --git a/src/Movement/InputShaper.cpp b/src/Movement/InputShaper.cpp
new file mode 100644
index 00000000..958bd156
--- /dev/null
+++ b/src/Movement/InputShaper.cpp
@@ -0,0 +1,103 @@
+/*
+ * InputShaper.cpp
+ *
+ *  Created on: 20 Feb 2021
+ *      Author: David
+ */
+
+#include "InputShaper.h"
+
+#include <GCodes/GCodeBuffer/GCodeBuffer.h>
+#include <RepRap.h>
+#include "StepTimer.h"
+
+// Object model table and functions
+// Note: if using GCC version 7.3.1 20180622 and lambda functions are used in this table, you must compile this file with option -std=gnu++17.
+// Otherwise the table will be allocated in RAM instead of flash, which wastes too much RAM.
+
+// Macro to build a standard lambda function that includes the necessary type conversions
+#define OBJECT_MODEL_FUNC(...) OBJECT_MODEL_FUNC_BODY(InputShaper, __VA_ARGS__)
+#define OBJECT_MODEL_FUNC_IF(...) OBJECT_MODEL_FUNC_IF_BODY(InputShaper, __VA_ARGS__)
+
+constexpr ObjectModelTableEntry InputShaper::objectModelTable[] =
+{
+	// Within each group, these entries must be in alphabetical order
+	// 0. InputShaper members
+	{ "damping",				OBJECT_MODEL_FUNC(self->GetFloatDamping(), 2), 							ObjectModelEntryFlags::none },
+	{ "frequency",				OBJECT_MODEL_FUNC(self->GetFrequency(), 2), 							ObjectModelEntryFlags::none },
+	{ "minimumAcceleration",	OBJECT_MODEL_FUNC(self->minimumAcceleration, 1),						ObjectModelEntryFlags::none },
+	{ "type", 					OBJECT_MODEL_FUNC(self->type.ToString()), 								ObjectModelEntryFlags::none },
+};
+
+constexpr uint8_t InputShaper::objectModelTableDescriptor[] = { 1, 4 };
+
+DEFINE_GET_OBJECT_MODEL_TABLE(InputShaper)
+
+InputShaper::InputShaper() noexcept
+	: halfPeriod((uint16_t)lrintf(StepTimer::StepClockRate/(2 * DefaultFrequency))),
+	  damping(lrintf(DefaultDamping * 65536)),
+	  minimumAcceleration(DefaultMinimumAcceleration),
+	  type(InputShaperType::none)
+{
+}
+
+// Process M593
+GCodeResult InputShaper::Configure(GCodeBuffer& gb, const StringRef& reply) noexcept
+{
+	const float MinimumInputShapingFrequency = (float)StepTimer::StepClockRate/(2 * 65535);			// we use a 16-bit number of step clocks to represent half the input shaping period
+	const float MaximumInputShapingFrequency = 1000.0;
+	bool seen = false;
+	if (gb.Seen('F'))
+	{
+		seen = true;
+		halfPeriod = (float)StepTimer::StepClockRate/(2 * gb.GetLimitedFValue('F', MinimumInputShapingFrequency, MaximumInputShapingFrequency));
+	}
+	if (gb.Seen('L'))
+	{
+		seen = true;
+		minimumAcceleration = max<float>(gb.GetFValue(), 1.0);		// very low accelerations cause problems with the maths
+	}
+	if (gb.Seen('S'))
+	{
+		seen = true;
+		damping = (uint16_t)lrintf(63336 * gb.GetLimitedFValue('S', 0.0, 0.99));
+	}
+
+	if (gb.Seen('P'))
+	{
+		seen = true;
+		type = (InputShaperType)gb.GetLimitedUIValue('S', InputShaperType::NumValues);
+	}
+	else if (seen && type == InputShaperType::none)
+	{
+		// For backwards compatibility, if we have set input shaping parameters but not defined shaping type, default to DAA for now. Change this when we support better types of input shaping.
+		type = InputShaperType::DAA;
+	}
+
+	if (seen)
+	{
+		reprap.MoveUpdated();
+	}
+	else if (type != InputShaperType::none)
+	{
+		reply.printf("%s input shaping at %.1fHz damping factor %.2f, min. acceleration %.1f",
+						type.ToString(), (double)GetFrequency(), (double)GetFloatDamping(), (double)minimumAcceleration);
+	}
+	else
+	{
+		reply.copy("Input shaping is disabled");
+	}
+	return GCodeResult::ok;
+}
+
+float InputShaper::GetFrequency() const noexcept
+{
+	return (float)StepTimer::StepClockRate/(2.0 * (float)halfPeriod);
+}
+
+float InputShaper::GetFloatDamping() const noexcept
+{
+	return ((float)damping)/65536;
+}
+
+// End
diff --git a/src/Movement/InputShaper.h b/src/Movement/InputShaper.h
new file mode 100644
index 00000000..f91d43d7
--- /dev/null
+++ b/src/Movement/InputShaper.h
@@ -0,0 +1,51 @@
+/*
+ * InputShaper.h
+ *
+ *  Created on: 20 Feb 2021
+ *      Author: David
+ */
+
+#ifndef SRC_MOVEMENT_INPUTSHAPER_H_
+#define SRC_MOVEMENT_INPUTSHAPER_H_
+
+#include <RepRapFirmware.h>
+#include <GCodes/GCodeResult.h>
+#include <General/NamedEnum.h>
+#include <ObjectModel/ObjectModel.h>
+
+NamedEnum(InputShaperType, uint8_t,
+	none,
+	ZVD,
+	ZVDD,
+	EI2,
+	DAA
+);
+
+class InputShaper INHERIT_OBJECT_MODEL
+{
+public:
+	InputShaper() noexcept;
+
+	uint16_t GetHalfPeriod() const noexcept { return halfPeriod; }
+	float GetFrequency() const noexcept;
+	float GetFloatDamping() const noexcept;
+	float GetMinimumAcceleration() const noexcept { return minimumAcceleration; }
+	InputShaperType GetType() const noexcept { return type; }
+
+	GCodeResult Configure(GCodeBuffer& gb, const StringRef& reply) noexcept;	// process M593
+
+protected:
+	DECLARE_OBJECT_MODEL
+
+private:
+	static constexpr float DefaultFrequency = 40.0;
+	static constexpr float DefaultDamping = 0.2;
+	static constexpr float DefaultMinimumAcceleration = 10.0;
+
+	uint16_t halfPeriod;							// half the period of ringing that we don't want to excite
+	uint16_t damping;								// damping factor of the ringing as a 16-bit fractional number
+	float minimumAcceleration;						// the minimum value that we reduce acceleration to
+	InputShaperType type;
+};
+
+#endif /* SRC_MOVEMENT_INPUTSHAPER_H_ */
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index aeafb33e..3d03f5ce 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -49,8 +49,6 @@
 # include <CAN/CanMotion.h>
 #endif
 
-#if SUPPORT_OBJECT_MODEL
-
 // Object model table and functions
 // Note: if using GCC version 7.3.1 20180622 and lambda functions are used in this table, you must compile this file with option -std=gnu++17.
 // Otherwise the table will be allocated in RAM instead of flash, which wastes too much RAM.
@@ -78,31 +76,25 @@ constexpr ObjectModelTableEntry Move::objectModelTable[] =
 	// Within each group, these entries must be in alphabetical order
 	// 0. Move members
 	{ "axes",					OBJECT_MODEL_FUNC_NOSELF(&axesArrayDescriptor), 										ObjectModelEntryFlags::live },
-	{ "calibration",			OBJECT_MODEL_FUNC(self, 4),																ObjectModelEntryFlags::none },
-	{ "compensation",			OBJECT_MODEL_FUNC(self, 7),																ObjectModelEntryFlags::none },
-	{ "currentMove",			OBJECT_MODEL_FUNC(self, 3),																ObjectModelEntryFlags::live },
+	{ "calibration",			OBJECT_MODEL_FUNC(self, 3),																ObjectModelEntryFlags::none },
+	{ "compensation",			OBJECT_MODEL_FUNC(self, 6),																ObjectModelEntryFlags::none },
+	{ "currentMove",			OBJECT_MODEL_FUNC(self, 2),																ObjectModelEntryFlags::live },
 	{ "extruders",				OBJECT_MODEL_FUNC_NOSELF(&extrudersArrayDescriptor),									ObjectModelEntryFlags::live },
-	{ "idle",					OBJECT_MODEL_FUNC(self, 2),																ObjectModelEntryFlags::none },
+	{ "idle",					OBJECT_MODEL_FUNC(self, 1),																ObjectModelEntryFlags::none },
 	{ "kinematics",				OBJECT_MODEL_FUNC(self->kinematics),													ObjectModelEntryFlags::none },
 	{ "printingAcceleration",	OBJECT_MODEL_FUNC(self->maxPrintingAcceleration, 1),									ObjectModelEntryFlags::none },
-	{ "shaping",				OBJECT_MODEL_FUNC(self, 1),																ObjectModelEntryFlags::none },
+	{ "shaping",				OBJECT_MODEL_FUNC(&self->shaper, 0),													ObjectModelEntryFlags::none },
 	{ "speedFactor",			OBJECT_MODEL_FUNC_NOSELF(reprap.GetGCodes().GetSpeedFactor(), 2),						ObjectModelEntryFlags::none },
 	{ "travelAcceleration",		OBJECT_MODEL_FUNC(self->maxTravelAcceleration, 1),										ObjectModelEntryFlags::none },
 	{ "virtualEPos",			OBJECT_MODEL_FUNC_NOSELF(reprap.GetGCodes().GetVirtualExtruderPosition(), 5),			ObjectModelEntryFlags::live },
 	{ "workplaceNumber",		OBJECT_MODEL_FUNC_NOSELF((int32_t)reprap.GetGCodes().GetWorkplaceCoordinateSystemNumber() - 1),	ObjectModelEntryFlags::none },
 	{ "workspaceNumber",		OBJECT_MODEL_FUNC_NOSELF((int32_t)reprap.GetGCodes().GetWorkplaceCoordinateSystemNumber()),	ObjectModelEntryFlags::none },
 
-	// 1. Move.shaping members
-	{ "damping",				OBJECT_MODEL_FUNC((float)self->drcDamping/65536, 2), 									ObjectModelEntryFlags::none },
-	{ "minimumAcceleration",	OBJECT_MODEL_FUNC(self->drcMinimumAcceleration, 1),										ObjectModelEntryFlags::none },
-	{ "period",					OBJECT_MODEL_FUNC(self->drcHalfPeriod * 2, 1), 											ObjectModelEntryFlags::none },
-	{ "type", 					OBJECT_MODEL_FUNC(self->shapingType.ToString()), 										ObjectModelEntryFlags::none },
-
-	// 2. Move.Idle members
+	// 1. Move.Idle members
 	{ "factor",					OBJECT_MODEL_FUNC_NOSELF(reprap.GetPlatform().GetIdleCurrentFactor(), 1),				ObjectModelEntryFlags::none },
 	{ "timeout",				OBJECT_MODEL_FUNC(0.001f * (float)self->idleTimeout, 1),								ObjectModelEntryFlags::none },
 
-	// 3. move.currentMove members
+	// 2. move.currentMove members
 	{ "acceleration",			OBJECT_MODEL_FUNC(self->GetAcceleration(), 1),											ObjectModelEntryFlags::live },
 	{ "deceleration",			OBJECT_MODEL_FUNC(self->GetDeceleration(), 1),											ObjectModelEntryFlags::live },
 # if SUPPORT_LASER
@@ -112,56 +104,51 @@ constexpr ObjectModelTableEntry Move::objectModelTable[] =
 	{ "requestedSpeed",			OBJECT_MODEL_FUNC(self->GetRequestedSpeed(), 1),										ObjectModelEntryFlags::live },
 	{ "topSpeed",				OBJECT_MODEL_FUNC(self->GetTopSpeed(), 1),												ObjectModelEntryFlags::live },
 
-	// 4. move.calibration members
-	{ "final",					OBJECT_MODEL_FUNC(self, 6),																ObjectModelEntryFlags::none },
-	{ "initial",				OBJECT_MODEL_FUNC(self, 5),																ObjectModelEntryFlags::none },
+	// 3. move.calibration members
+	{ "final",					OBJECT_MODEL_FUNC(self, 5),																ObjectModelEntryFlags::none },
+	{ "initial",				OBJECT_MODEL_FUNC(self, 4),																ObjectModelEntryFlags::none },
 	{ "numFactors",				OBJECT_MODEL_FUNC((int32_t)self->numCalibratedFactors),									ObjectModelEntryFlags::none },
 
-	// 5. move.calibration.initialDeviation members
+	// 4. move.calibration.initialDeviation members
 	{ "deviation",				OBJECT_MODEL_FUNC(self->initialCalibrationDeviation.GetDeviationFromMean(), 3),			ObjectModelEntryFlags::none },
 	{ "mean",					OBJECT_MODEL_FUNC(self->initialCalibrationDeviation.GetMean(), 3),						ObjectModelEntryFlags::none },
 
-	// 6. move.calibration.finalDeviation members
+	// 5. move.calibration.finalDeviation members
 	{ "deviation",				OBJECT_MODEL_FUNC(self->latestCalibrationDeviation.GetDeviationFromMean(), 3),			ObjectModelEntryFlags::none },
 	{ "mean",					OBJECT_MODEL_FUNC(self->latestCalibrationDeviation.GetMean(), 3),						ObjectModelEntryFlags::none },
 
-	// 7. move.compensation members
+	// 6. move.compensation members
 	{ "fadeHeight",				OBJECT_MODEL_FUNC((self->useTaper) ? self->taperHeight : std::numeric_limits<float>::quiet_NaN(), 1),	ObjectModelEntryFlags::none },
 #if HAS_MASS_STORAGE || HAS_LINUX_INTERFACE
 	{ "file",					OBJECT_MODEL_FUNC_IF(self->usingMesh, self->heightMap.GetFileName()),					ObjectModelEntryFlags::none },
 #endif
-	{ "meshDeviation",			OBJECT_MODEL_FUNC_IF(self->usingMesh, self, 8),											ObjectModelEntryFlags::none },
+	{ "meshDeviation",			OBJECT_MODEL_FUNC_IF(self->usingMesh, self, 7),											ObjectModelEntryFlags::none },
 	{ "probeGrid",				OBJECT_MODEL_FUNC((const GridDefinition *)&self->GetGrid()),							ObjectModelEntryFlags::none },
-	{ "skew",					OBJECT_MODEL_FUNC(self, 9),																ObjectModelEntryFlags::none },
+	{ "skew",					OBJECT_MODEL_FUNC(self, 8),																ObjectModelEntryFlags::none },
 	{ "type",					OBJECT_MODEL_FUNC(self->GetCompensationTypeString()),									ObjectModelEntryFlags::none },
 
-	// 8. move.compensation.meshDeviation members
+	// 7. move.compensation.meshDeviation members
 	{ "deviation",				OBJECT_MODEL_FUNC(self->latestMeshDeviation.GetDeviationFromMean(), 3),					ObjectModelEntryFlags::none },
 	{ "mean",					OBJECT_MODEL_FUNC(self->latestMeshDeviation.GetMean(), 3),								ObjectModelEntryFlags::none },
 
-	// 9. move.compensation.skew members
+	// 8. move.compensation.skew members
 	{ "compensateXY",			OBJECT_MODEL_FUNC(self->compensateXY),													ObjectModelEntryFlags::none },
 	{ "tanXY",					OBJECT_MODEL_FUNC(self->tanXY, 4),														ObjectModelEntryFlags::none },
 	{ "tanXZ",					OBJECT_MODEL_FUNC(self->tanXZ, 4),														ObjectModelEntryFlags::none },
 	{ "tanYZ",					OBJECT_MODEL_FUNC(self->tanYZ, 4),														ObjectModelEntryFlags::none },
 };
 
-constexpr uint8_t Move::objectModelTableDescriptor[] = { 10, 14, 4, 2, 4 + SUPPORT_LASER, 3, 2, 2, 5 + (HAS_MASS_STORAGE || HAS_LINUX_INTERFACE), 2, 4 };
+constexpr uint8_t Move::objectModelTableDescriptor[] = { 9, 14, 2, 4 + SUPPORT_LASER, 3, 2, 2, 5 + (HAS_MASS_STORAGE || HAS_LINUX_INTERFACE), 2, 4 };
 
 DEFINE_GET_OBJECT_MODEL_TABLE(Move)
 
-#endif
-
 Move::Move() noexcept
 	:
 #if SUPPORT_ASYNC_MOVES
 	  heightController(nullptr),
 #endif
 	  active(false),
-	  shapingType(InputShaping::none),								// no input shaping
 	  maxPrintingAcceleration(10000.0), maxTravelAcceleration(10000.0),
-	  drcHalfPeriod(StepTimer::StepClockRate/(2 * 40)),				// 40Hz
-	  drcMinimumAcceleration(10.0),
 	  jerkPolicy(0),
 	  numCalibratedFactors(0)
 {
@@ -990,49 +977,6 @@ GCodeResult Move::ConfigureAccelerations(GCodeBuffer&gb, const StringRef& reply)
 	return GCodeResult::ok;
 }
 
-// Process M593
-GCodeResult Move::ConfigureInputShaping(GCodeBuffer& gb, const StringRef& reply) noexcept
-{
-	const float MinimumInputShapingFrequency = (float)StepTimer::StepClockRate/(2 * 65535);			// we use a 16-bit number of step clocks to represent half the input shaping period
-	const float MaximumInputShapingFrequency = 1000.0;
-	bool seen = false;
-	if (gb.Seen('F'))
-	{
-		drcHalfPeriod = (float)StepTimer::StepClockRate/(2 * gb.GetLimitedFValue('F', MinimumInputShapingFrequency, MaximumInputShapingFrequency));
-	}
-	if (gb.Seen('L'))
-	{
-		seen = true;
-		drcMinimumAcceleration = max<float>(gb.GetFValue(), 1.0);		// very low accelerations cause problems with the maths
-	}
-	if (gb.Seen('S'))
-	{
-		drcDamping = (uint16_t)lrintf(63336 * gb.GetLimitedFValue('S', 0.0, 0.99));
-	}
-	if (gb.Seen('P'))
-	{
-		shapingType = (InputShaping)gb.GetLimitedUIValue('S', InputShaping::NumValues);
-	}
-
-	if (seen)
-	{
-		reprap.MoveUpdated();
-	}
-	else if (shapingType != InputShaping::none)
-	{
-		reply.printf("%s input shaping at %.1fHz damping factor %.2f, min. acceleration %.1f",
-						shapingType.ToString(),
-						(double)((float)StepTimer::StepClockRate/(2.0 * (float)drcHalfPeriod)),
-						(double)((float)drcDamping/65536),
-						(double)drcMinimumAcceleration);
-	}
-	else
-	{
-		reply.copy("Input shaping is disabled");
-	}
-	return GCodeResult::ok;
-}
-
 // Process M595
 GCodeResult Move::ConfigureMovementQueue(GCodeBuffer& gb, const StringRef& reply) noexcept
 {
diff --git a/src/Movement/Move.h b/src/Movement/Move.h
index 5d8ce2f7..730e81f0 100644
--- a/src/Movement/Move.h
+++ b/src/Movement/Move.h
@@ -8,15 +8,16 @@
 #ifndef MOVE_H_
 #define MOVE_H_
 
-#include "RepRapFirmware.h"
-#include <Movement/StraightProbeSettings.h>
-#include "MessageType.h"
+#include <RepRapFirmware.h>
+#include <MessageType.h>
+#include "InputShaper.h"
+#include "StraightProbeSettings.h"
 #include "DDARing.h"
 #include "DDA.h"								// needed because of our inline functions
 #include "BedProbing/RandomProbePointSet.h"
 #include "BedProbing/Grid.h"
 #include "Kinematics/Kinematics.h"
-#include "GCodes/RestorePoint.h"
+#include <GCodes/RestorePoint.h>
 #include <Math/Deviation.h>
 
 #if SUPPORT_ASYNC_MOVES
@@ -49,14 +50,6 @@ const unsigned int NumDms = 20 * 5;												// suitable for e.g. a delta + 2-
 
 constexpr uint32_t MovementStartDelayClocks = StepTimer::StepClockRate/100;		// 10ms delay between preparing the first move and starting it
 
-NamedEnum(InputShaping, uint8_t,
-	none,
-	ZVD,
-	ZVDD,
-	EI2,
-	DAA
-);
-
 // This is the master movement class.  It controls all movement in the machine.
 class Move INHERIT_OBJECT_MODEL
 {
@@ -102,14 +95,11 @@ public:
 	float PushBabyStepping(size_t axis, float amount) noexcept;				// Try to push some babystepping through the lookahead queue
 
 	GCodeResult ConfigureAccelerations(GCodeBuffer&gb, const StringRef& reply) noexcept;		// process M204
-	GCodeResult ConfigureInputShaping(GCodeBuffer& gb, const StringRef& reply) noexcept;	// process M593
 	GCodeResult ConfigureMovementQueue(GCodeBuffer& gb, const StringRef& reply) noexcept;		// process M595
 
 	float GetMaxPrintingAcceleration() const noexcept { return maxPrintingAcceleration; }
 	float GetMaxTravelAcceleration() const noexcept { return maxTravelAcceleration; }
-	float GetShapingHalfPeriod() const noexcept { return drcHalfPeriod; }
-	float GetShapingMinimumAcceleration() const noexcept { return drcMinimumAcceleration; }
-	InputShaping GetShapingType() const noexcept { return shapingType; }
+	InputShaper& GetShaper() noexcept { return shaper; }
 
 	void Diagnostics(MessageType mtype) noexcept;							// Report useful stuff
 
@@ -244,13 +234,9 @@ private:
 	bool active;										// Are we live and running?
 	uint8_t simulationMode;								// Are we simulating, or really printing?
 	MoveState moveState;								// whether the idle timer is active
-	InputShaping shapingType;
 
 	float maxPrintingAcceleration;
 	float maxTravelAcceleration;
-	uint16_t drcHalfPeriod;								// half the period of ringing that we don't want to excite
-	uint16_t drcDamping;								// damping factor of the ringing as a 16-bit fractional number
-	float drcMinimumAcceleration;						// the minimum value that we reduce acceleration to
 
 	unsigned int jerkPolicy;							// When we allow jerk
 	unsigned int idleCount;								// The number of times Spin was called and had no new moves to process
@@ -278,6 +264,7 @@ private:
 
 	Kinematics *kinematics;								// What kinematics we are using
 
+	InputShaper shaper;
 	StraightProbeSettings straightProbeSettings;		// G38 straight probe settings
 
 	float latestLiveCoordinates[MaxAxesPlusExtruders];