From 3cbfa91d1e7a881c03c0902fe5cb4a65c886461d Mon Sep 17 00:00:00 2001
From: Yannick Richter <git@y-richter.de>
Date: Tue, 12 Nov 2024 16:08:41 +0100
Subject: [PATCH] Speed limiter tuning, filter and gain adjustments. Removed
 averaging.

---
 Firmware/FFBoard/Inc/Axis.h   | 15 ++++++++++-----
 Firmware/FFBoard/Src/Axis.cpp |  8 ++++----
 2 files changed, 14 insertions(+), 9 deletions(-)

diff --git a/Firmware/FFBoard/Inc/Axis.h b/Firmware/FFBoard/Inc/Axis.h
index 1c366ce6..18d6c177 100644
--- a/Firmware/FFBoard/Inc/Axis.h
+++ b/Firmware/FFBoard/Inc/Axis.h
@@ -23,11 +23,16 @@
 #include "ClassChooser.h"
 #include "ExtiHandler.h"
 #include "EffectsCalculator.h"
-#include "FastAvg.h"
 
 #define INTERNAL_AXIS_DAMPER_SCALER 0.7
 #define INTERNAL_AXIS_FRICTION_SCALER 0.7
 #define INTERNAL_AXIS_INERTIA_SCALER 0.7
+#ifndef AXIS_SPEEDLIMITER_P
+#define AXIS_SPEEDLIMITER_P 0.3
+#endif
+#ifndef AXIS_SPEEDLIMITER_I
+#define AXIS_SPEEDLIMITER_I 0.03
+#endif
 
 
 struct Control_t {
@@ -226,7 +231,8 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	uint16_t maxSpeedDegS  = 0; // Set to non zero to enable. example 1000. 8b * 10?
 	//float	 maxAccelDegSS = 0;
 	uint32_t maxTorqueRateMS = 0; // 8b * 128?
-	const float speedLimiterP = 0.2;
+	float speedLimiterP = AXIS_SPEEDLIMITER_P;
+	float speedLimiterI = AXIS_SPEEDLIMITER_I;
 
 	float spdlimitreducerI = 0;
 	//float acclimitreducerI = 0;
@@ -260,8 +266,8 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	bool motorWasNotReady = true;
 
 	// TODO tune these and check if it is really stable and beneficial to the FFB. index 4 placeholder
-	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 std::array<biquad_constant_t,4> filterSpeedCst = { {{ 40, 55 }, { 70, 55 }, { 120, 55 }, {180, 55}} };
+	const std::array<biquad_constant_t,4> filterAccelCst = { {{ 40, 30 }, { 55, 30 }, { 70, 30 }, {120, 55}} };
 	const biquad_constant_t filterDamperCst = {60, 55};
 	const biquad_constant_t filterFrictionCst = {50, 20};
 	const biquad_constant_t filterInertiaCst = {20, 20};
@@ -269,7 +275,6 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	const float filter_f = 1000; // 1khz
 	const int32_t intFxClip = 20000;
 	uint8_t damperIntensity = 30;
-	FastAvg<float,5> spdlimiterAvg;
 
 	uint8_t frictionIntensity = 0;
 	uint8_t inertiaIntensity = 0;
diff --git a/Firmware/FFBoard/Src/Axis.cpp b/Firmware/FFBoard/Src/Axis.cpp
index 39639bf3..03b4deea 100644
--- a/Firmware/FFBoard/Src/Axis.cpp
+++ b/Firmware/FFBoard/Src/Axis.cpp
@@ -705,9 +705,9 @@ bool Axis::updateTorque(int32_t* totalTorque) {
 
 		float torqueSign = torque > 0 ? 1 : -1; // Used to prevent metrics against the force to go into the limiter
 		// Speed. Mostly tuned...
-		spdlimiterAvg.addValue(metric.current.speed);
-		float speedreducer = (float)((spdlimiterAvg.getAverage()*torqueSign) - (float)maxSpeedDegS) *  ((float)0x7FFF / maxSpeedDegS);
-		spdlimitreducerI = clip<float,int32_t>( spdlimitreducerI + ((speedreducer * 0.015) * torqueScaler),0,power);
+		//spdlimiterAvg.addValue(metric.current.speed);
+		float speedreducer = (float)((metric.current.speed*torqueSign) - (float)maxSpeedDegS) *  ((float)0x7FFF / maxSpeedDegS);
+		spdlimitreducerI = clip<float,int32_t>( spdlimitreducerI + ((speedreducer * speedLimiterI) * torqueScaler),0,power);
 
 		// Accel limit. Not really useful. Maybe replace with torque slew rate limit?
 //		float accreducer = (float)((metric.current.accel*torqueSign) - (float)maxAccelDegSS) * getAccelScalerNormalized();
@@ -949,7 +949,7 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector<CommandReply>&
 		}
 		else if (cmd.type == CMDtype::set)
 		{
-			uint32_t value = clip<uint32_t, uint32_t>(cmd.val, 0, 2);
+			uint32_t value = clip<uint32_t, uint32_t>(cmd.val, 0, filterSpeedCst.size()-1);
 			this->filterProfileId = value;
 			speedFilter.setFc(filterSpeedCst[this->filterProfileId].freq / filter_f);
 			speedFilter.setQ(filterSpeedCst[this->filterProfileId].q / 100.0);