Prepared TMC biquad filters

Author: Ultrawipf

Firmware/FFBoard/Inc/Axis.h

@@ -177,15 +177,7 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 
 	const Error outOfBoundsError = Error(ErrorCode::axisOutOfRange,ErrorType::warning,"Axis out of bounds");
 
-	const TMC4671PIDConf tmcpids = TMC4671PIDConf({.fluxI = 400,
-											 .fluxP = 400,
-											 .torqueI = 400,
-											 .torqueP = 300,
-											 .velocityI = 0,
-											 .velocityP = 128,
-											 .positionI = 0,
-											 .positionP = 64,
-											 .sequentialPI = true});
+
 	TMC4671Limits tmclimits = TMC4671Limits({.pid_torque_flux_ddt = 32767,
 											 .pid_uq_ud = 30000,
 											 .pid_torque_flux = 30000,
@@ -194,13 +186,24 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 											 .pid_pos_low = -2147483647,
 											 .pid_pos_high = 2147483647});
 
-	// Lowpass 1KHZ
-	const TMC4671Biquad fluxbq = TMC4671Biquad({.a1 = -134913,
-										  .a2 = 536735999,
-										  .b0 = 67457,
-										  .b1 = 134913,
-										  .b2 = 67457,
-										  .enable = true});
+	// Lowpass 500Hz Q 0.7 @ 25khz
+	const TMC4671Biquad tmcbq_500hz_07q_25k = TMC4671Biquad(
+			{ .a1 = 979476766,
+			  .a2 = -450370144,
+			  .b0 = 1941073,
+			  .b1 = 3882145,
+			  .b2 = 1941073,
+			  .enable = true});
+
+	// Lowpass 1000Hz Q 0.7 @ 25khz
+	const TMC4671Biquad tmcbq_1000hz_07q_25k = TMC4671Biquad(
+			{ .a1 = 886773302,
+			  .a2 = -378358476,
+			  .b0 = 7114021,
+			  .b1 = 14228043,
+			  .b2 = 7114021,
+			  .enable = true});
+
 
 
 	float encoderOffset = 0; // Offset for absolute encoders
@@ -247,7 +250,7 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	const biquad_constant_t filterSpeedCst[4] = {{ 30, 55 }, { 60, 55 }, { 120, 55 }, {120, 55}};
 	const biquad_constant_t filterAccelCst[4] = {{ 40, 30 }, { 55, 30 }, { 70, 30 }, {120, 55}};
 	const biquad_constant_t filterDamperCst = {60, 55};
-	uint8_t filterProfileId = 0;
+	uint8_t filterProfileId = 1; // Default medium (1) as this is the most common encoder resolution and users can go lower or higher if required.
 	const float filter_f = 1000; // 1khz
 	const int32_t damperClip = 20000;
 	uint8_t damperIntensity = 30;

Firmware/FFBoard/Inc/Filters.h

@@ -26,7 +26,9 @@ enum class BiquadType : uint8_t {
     highshelf
 };
 
+class TMC4671Biquad;
 class Biquad{
+	friend TMC4671Biquad;
 public:
 	Biquad();
     Biquad(BiquadType type, float Fc, float Q, float peakGainDB);

Firmware/FFBoard/Src/Axis.cpp

@@ -329,7 +329,7 @@ void Axis::setupTMC4671()
 	drv->restoreFlash();
 	tmclimits.pid_torque_flux = getPower();
 	drv->setLimits(tmclimits);
-	//drv->setBiquadFlux(fluxbq);
+	//drv->setBiquadTorque(tmcbq_500hz_07q_25k);
 	drv->setExternalEncoderAllowed(true);
 
 

Firmware/FFBoard/UserExtensions/Inc/TMC4671.h

@@ -24,6 +24,7 @@
 #include "semaphore.hpp"
 #include "OutputPin.h"
 #include "cpp_target_config.h"
+#include "Filters.h"
 
 #define SPITIMEOUT 500
 #define TMC_THREAD_MEM 256
@@ -157,10 +158,10 @@ struct TMC4671MainConfig{
 };
 
 struct TMC4671PIDConf{
-	uint16_t fluxI		= 512;
-	uint16_t fluxP		= 512;
-	uint16_t torqueI	= 512;
-	uint16_t torqueP	= 512;
+	uint16_t fluxI		= 800;
+	uint16_t fluxP		= 700;
+	uint16_t torqueI	= 800;
+	uint16_t torqueP	= 700;
 	uint16_t velocityI	= 0;
 	uint16_t velocityP	= 256;
 	uint16_t positionI	= 0;
@@ -257,13 +258,26 @@ struct TMC4671HALLConf{
 	uint16_t dPhiMax = 10922;
 };
 
+// Use TMCL IDE with TMC4671 devkit to generate values
+// TODO rewrite as class to allow conversion from biquad
 struct TMC4671Biquad{
+
 	int32_t a1 = 0;
 	int32_t a2 = 0;
 	int32_t b0 = 0;
 	int32_t b1 = 0;
 	int32_t b2 = 0;
 	bool enable = false;
+
+	void from_bq(Biquad& bq,bool enable){
+		// Note: trinamic swapped the naming of b and a from the regular convention in the datasheet
+		a1 = (int32_t)(bq.b1 * (1 << 29));
+		a2 = (int32_t)(bq.b2 * (1 << 29));
+		b0 = (int32_t)(bq.a0 * (1 << 29));
+		b1 = (int32_t)(bq.a1 * (1 << 29));
+		b2 = (int32_t)(bq.a2 * (1 << 29));
+		this->enable = enable;
+	};
 };
 
 

Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp

@@ -1966,6 +1966,9 @@ TMC4671Limits TMC4671::getLimits(){
 	return curLimits;
 }
 
+/**
+ * Applies a biquad filter to the flux target
+ */
 void TMC4671::setBiquadFlux(TMC4671Biquad bq){
 	writeReg(0x4E, 25);
 	writeReg(0x4D, bq.a1);
@@ -1980,6 +1983,10 @@ void TMC4671::setBiquadFlux(TMC4671Biquad bq){
 	writeReg(0x4E, 31);
 	writeReg(0x4D, bq.enable & 0x1);
 }
+
+/**
+ * Applies a biquad filter to the pos target
+ */
 void TMC4671::setBiquadPos(TMC4671Biquad bq){
 	writeReg(0x4E, 1);
 	writeReg(0x4D, bq.a1);
@@ -1994,6 +2001,10 @@ void TMC4671::setBiquadPos(TMC4671Biquad bq){
 	writeReg(0x4E, 7);
 	writeReg(0x4D, bq.enable & 0x1);
 }
+
+/**
+ * Applies a biquad filter to the actual measured velocity
+ */
 void TMC4671::setBiquadVel(TMC4671Biquad bq){
 	writeReg(0x4E, 9);
 	writeReg(0x4D, bq.a1);
@@ -2008,6 +2019,10 @@ void TMC4671::setBiquadVel(TMC4671Biquad bq){
 	writeReg(0x4E, 15);
 	writeReg(0x4D, bq.enable & 0x1);
 }
+
+/**
+ * Applies a biquad filter to the torque target
+ */
 void TMC4671::setBiquadTorque(TMC4671Biquad bq){
 	writeReg(0x4E, 17);
 	writeReg(0x4D, bq.a1);