From 9644373f9ab357335316c700a0929f7962fb271f Mon Sep 17 00:00:00 2001
From: Vincent MANOUKIAN <10980775+manoukianv@users.noreply.github.com>
Date: Sat, 25 Apr 2026 10:37:12 +0200
Subject: [PATCH 1/8] docs: pure code cleanup, typing and Doxygen documentation

---
 Firmware/FFBoard/Inc/Axis.h                | 471 +++++++++++++++------
 Firmware/FFBoard/Inc/EffectsCalculator.h   | 225 ++++++++--
 Firmware/FFBoard/Inc/HidFFB.h              | 115 ++++-
 Firmware/FFBoard/Src/Axis.cpp              | 145 ++-----
 Firmware/FFBoard/Src/EffectsCalculator.cpp |  10 +-
 5 files changed, 671 insertions(+), 295 deletions(-)

diff --git a/Firmware/FFBoard/Inc/Axis.h b/Firmware/FFBoard/Inc/Axis.h
index 5b044c187..32b8bb8af 100644
--- a/Firmware/FFBoard/Inc/Axis.h
+++ b/Firmware/FFBoard/Inc/Axis.h
@@ -2,7 +2,7 @@
  * Axis.h
  *
  *  Created on: 21.01.2021
- *      Author: Yannick / Lidders
+ *      Author: Yannick / Lidders / Vincent
  */
 
 #ifndef SRC_AXIS_H_
@@ -10,7 +10,6 @@
 #include <FFBoardMain.h>
 #include <MotorPWM.h>
 #include "usb_hid_ffb_desc.h"
-#include "TMC4671.h"
 #include "PersistentStorage.h"
 #include "ButtonSource.h"
 #include "EncoderLocal.h"
@@ -34,18 +33,23 @@
 #define AXIS_SPEEDLIMITER_I 0.03
 #endif
 
-
+/**
+ * @brief Global control flags for all axes.
+ */
 struct Control_t {
-	bool emergency = false;
-	bool usb_disabled = true;
-	bool update_disabled = true;
-	bool request_update_disabled = false;
+	bool emergency = false;                         //!< Emergency stop is active.
+	bool usb_disabled = true;                       //!< FFB is disabled by USB.
+	bool update_disabled = true;            //!< FFB updates are disabled.
+	bool request_update_disabled = false; //!< A request to disable FFB updates is pending.
 //	bool usb_update_flag = false;
 //	bool update_flag = false;
-	bool resetEncoder = false;
+	bool resetEncoder = false;                      //!< A request to reset the encoder is pending.
 };
 
-struct AxisFlashAddrs
+/**
+ * @brief Defines the flash memory addresses for axis-specific settings.
+ */
+struct AxisFlashAddresses
 {
 	uint16_t config = ADR_AXIS1_CONFIG;
 	uint16_t maxSpeed = ADR_AXIS1_MAX_SPEED;
@@ -62,232 +66,435 @@ struct AxisFlashAddrs
 	uint16_t postprocess1 = ADR_AXIS1_POSTPROCESS1;
 };
 
+/**
+ * @brief Configuration for an axis, including driver and encoder types.
+ */
 struct AxisConfig
 {
-	uint8_t drvtype = 0;
-	uint8_t enctype = 0;
-	//bool invert = false;
+	uint8_t enctype = 0; //!< Encoder type ID.
+	uint8_t drvtype = 0; //!< Motor driver type ID.
 };
+
+/**
+ * @brief Holds the physical metrics of an axis at a point in time.
+ */
 struct metric_t {
-	float accel = 0;	// in deg/s²
-	float speed = 0; // in deg/s
-	int32_t pos_scaled_16b = 0; // scaled position as 16b int -0x7fff to 0x7fff matching FFB ranges
-	float pos_f = 0; // scaled position as float. -1 to 1 range
-	float posDegrees = 0; // Position in degrees. Not scaled to selected range
-	int32_t torque = 0; // total of effect + endstop torque
+	float accel = 0;          //!< Acceleration in deg/s².
+	float speed = 0;          //!< Speed in deg/s.
+	int32_t pos_scaled_16b = 0;       //!< Scaled position as a 16-bit integer (-0x7fff to 0x7fff).
+	float pos_f = 0;          //!< Scaled position as a float (-1.0 to 1.0).
+	float posDegrees = 0; //!< Position in degrees, not scaled to the selected range.
+	int32_t torque = 0;       //!< Total torque applied to the axis.
 };
 
-
+/**
+ * @brief Holds the current and previous metrics for an axis, used for calculating derivatives.
+ */
 struct axis_metric_t {
-	metric_t current;
-	metric_t previous;
+	metric_t current;  //!< Current metrics.
+	metric_t previous; //!< Metrics from the previous update cycle.
 };
 
+/**
+ * @brief Represents a gear ratio for scaling encoder values.
+ */
 struct GearRatio_t{
-	uint8_t denominator = 0;
-	uint8_t numerator = 0;
-	float gearRatio = 1.0;
+	uint8_t denominator = 0; //!< Denominator of the gear ratio.
+	uint8_t numerator = 0;   //!< Numerator of the gear ratio.
+	float gearRatio = 1.0;   //!< The calculated gear ratio (numerator/denominator).
 };
 
-
 enum class Axis_commands : uint32_t{
 	power=0x00,degrees=0x01,esgain,zeroenc,invert,idlespring,axisdamper,enctype,drvtype,
-	pos,maxspeed,maxtorquerate,fxratio,curtorque,curpos,curspd,curaccel,reductionScaler,
+	pos,curtorque,curpos,curspd,curaccel,
+	fxratio,reductionScaler,
 	filterSpeed, filterAccel, filterProfileId,cpr,axisfriction,axisinertia,
+	maxspeed, maxtorquerate,
 	expo,exposcale
 };
 
+/**
+ * @brief This class represents a single FFB axis.
+ * It handles the motor driver, encoder, and all related calculations for force feedback effects.
+ */
 class Axis : public PersistentStorage, public CommandHandler, public ErrorHandler
 {
 public:
+	/**
+	 * @brief Construct a new Axis object.
+	 * @param axis The character identifier for this axis (e.g., 'x', 'y').
+	 * @param control A pointer to the global control structure.
+	 */
 	Axis(char axis, volatile Control_t* control);
 	virtual ~Axis();
 
-	static ClassIdentifier info;
+	static ClassIdentifier info; //!< Static class identifier.
 	const ClassIdentifier getInfo();
 	const ClassType getClassType() override {return ClassType::Axis;};
 
-	virtual std::string getHelpstring() { return "FFB axis"	;}
-#ifdef TMC4671DRIVER
-	void setupTMC4671();
-#endif
+	virtual std::string getHelpstring() { return "FFB axis" ;}
+
 	// Dynamic classes
+	/**
+	 * @brief Sets the motor driver type.
+	 * @param drvtype The type of the motor driver.
+	 */
 	void setDrvType(uint8_t drvtype);
+
+	/**
+	 * @brief Sets the encoder type.
+	 * @param enctype The type of the encoder.
+	 */
 	void setEncType(uint8_t enctype);
+
+	/**
+	 * @brief Gets the motor driver type.
+	 * @return The type of the motor driver.
+	 */
 	uint8_t getDrvType();
+
+	/**
+	 * @brief Gets the encoder type.
+	 * @return The type of the encoder.
+	 */
 	uint8_t getEncType();
 
+	/**
+	 * @brief Gets the encoder instance.
+	 * @return A pointer to the encoder instance.
+	 */
 	Encoder* getEncoder();
+
+	/**
+	 * @brief Gets the motor driver instance.
+	 * @return A pointer to the motor driver instance.
+	 */
 	MotorDriver* getDriver();
 
-	void usbSuspend(); // Called on usb disconnect and suspend
-	void usbResume();  // Called on usb resume
+	/**
+	 * @brief Called on USB disconnect and suspend.
+	 */
+	void usbSuspend();
+
+	/**
+	 * @brief Called on USB resume. Enables the motor driver.
+	 */
+	void usbResume();
 
+	/**
+	 * @brief Saves axis settings to flash memory.
+	 * @override from PersistentStorage
+	 */
 	void saveFlash() override;
+
+	/**
+	 * @brief Restores axis settings from flash memory.
+	 * @override from PersistentStorage
+	 */
 	void restoreFlash() override;
 
-	void prepareForUpdate();  // called before the effects are calculated
-	void updateDriveTorque(); //int32_t effectTorque);
+	/**
+	 * @brief Prepares the axis for an update cycle. Called from the main loop before effects are calculated.
+	 * Reads the encoder, scales the value, and updates metrics (speed, acceleration).
+	 */
+	void prepareForUpdate();
+
+	/**
+	 * @brief Sends the final calculated torque to the motor driver.
+	 */
+	void updateDriveTorque();
+
+	/**
+	 * @brief Triggers an emergency stop. Disables the motor driver.
+	 * @param reset If true, resets the axis after stopping.
+	 */
 	void emergencyStop(bool reset);
 
+	/**
+	 * @brief Sets the position of the axis.
+	 * @param val The new position value.
+	 */
 	void setPos(uint16_t val);
+
+	/**
+	 * @brief Zeros the current encoder position.
+	 */
 	void zeroPos();
 
+	/**
+	 * @brief Checks if FFB is globally active.
+	 * @return true if FFB is active, false otherwise.
+	 */
 	bool getFfbActive();
 
+	/**
+	 * @brief Scales an encoder value.
+	 * @param angle The angle to scale.
+	 * @param degrees The total degrees of rotation.
+	 * @return A pair containing the scaled integer value and the float value.
+	 */
 	std::pair<int32_t,float> scaleEncValue(float angle, uint16_t degrees);
-	float 	getEncAngle(Encoder *enc);
-
 
+	/**
+	 * @brief Gets the angle from the encoder.
+	 * @param enc A pointer to the encoder.
+	 * @return The angle in degrees.
+	 */
+	float   getEncAngle(Encoder *enc);
+
+	/**
+	 * @brief Sets the maximum power (torque) of the motor.
+	 * @param power The new power value.
+	 */
 	void setPower(uint16_t power);
 
-
+	/**
+	 * @brief Callback for handling errors.
+	 * @param error The error that occurred.
+	 * @param cleared Whether the error has been cleared.
+	 * @override from ErrorHandler
+	 */
 	void errorCallback(const Error &error, bool cleared) override;
 
-	//ParseStatus command(ParsedCommand_old* cmd,std::string* reply) override;
+	/**
+	 * @brief Registers the commands for this axis with the command handler.
+	 */
 	void registerCommands();
-	CommandStatus command(const ParsedCommand& cmd,std::vector<CommandReply>& replies);
-
-	ClassChooser<MotorDriver> drv_chooser;
-	ClassChooser<Encoder> enc_chooser;
-
+	/**
+	 * @brief Handles command line interface commands for this axis.
+	 * @param cmd The parsed command.
+	 * @param replies A vector of replies to be sent back.
+	 * @return The status of the command execution.
+	 * @override from CommandHandler
+	 */
+	CommandStatus command(const ParsedCommand& cmd,std::vector<CommandReply>& replies) override;
+
+	ClassChooser<MotorDriver> driverChooser; //!< Class chooser for motor drivers.
+	ClassChooser<Encoder> encoderChooser;     //!< Class chooser for encoders.
+
+	/**
+	 * @brief Gets the last scaled encoder value.
+	 * @return The last scaled encoder value.
+	 */
 	int32_t getLastScaledEnc();
+
+	/**
+	 * @brief Resets the metrics of the axis.
+	 * @param new_pos The new position to reset to.
+	 */
 	void resetMetrics(float new_pos);
+
+	/**
+	 * @brief Updates the metrics of the axis.
+	 * @param new_pos The new position.
+	 */
 	void updateMetrics(float new_pos);
+
+	/**
+	 * @brief Updates the idle spring force.
+	 * @return The calculated idle spring force.
+	 */
 	int32_t updateIdleSpringForce();
+
+	/**
+	 * @brief Sets the idle spring strength.
+	 * @param spring The new spring strength.
+	 */
 	void setIdleSpringStrength(uint8_t spring);
+
+	/**
+	 * @brief Sets the strength and filter for an effect.
+	 * @param val The new strength value.
+	 * @param valToSet A reference to the value to be set.
+	 * @param filter A reference to the biquad filter.
+	 */
 	void setFxStrengthAndFilter(uint8_t val,uint8_t& valToSet, Biquad& filter);
+
+	/**
+	 * @brief Called before HID effects are calculated.
+	 */
 	void calculateAxisEffects(bool ffb_on);
-	int32_t getTorque(); // current torque scaled as a 32 bit signed value
-	int16_t updateEndstop();
 
-	int32_t calculateExpoTorque(int32_t torque);
+	/**
+	 * @brief Gets the current torque.
+	 * @return The current torque scaled as a 32-bit signed value.
+	 */
+	int32_t getTorque();
 
+	/**
+	 * @brief Calculate soft endstop effect.
+	 */
+	int16_t updateEndstop();
+
+	/**
+	 * @brief Starts a force fade-in.
+	 * @param start The starting force multiplier.
+	 * @param fadeTime The duration of the fade-in.
+	 */
 	void startForceFadeIn(float start = 0,float fadeTime = 0.5);
 
 	metric_t* getMetrics();
 
+	/**
+	 * @brief Sets the HID FFB effect torque.
+	 * @param torque The new FFB effect torque.
+	 */
 	void setEffectTorque(int32_t torque);
+
+	/**
+	 * @brief Updates the total torque.
+	 * @param totalTorque A pointer to the total torque value.
+	 * @return true if the torque was updated, false otherwise.
+	 */
 	bool updateTorque(int32_t* totalTorque);
 
 
 	void updateSamplerate(float newSamplerate);
 	void updateFilters(uint8_t profileId);
 
+	/**
+	 * @brief Sets the gear ratio.
+	 * @param numerator The numerator of the gear ratio.
+	 * @param denominator The denominator of the gear ratio.
+	 */
 	void setGearRatio(uint8_t numerator,uint8_t denominator);
 
-	static const std::vector<class_entry<MotorDriver>> axis1_drivers;
-	static const std::vector<class_entry<MotorDriver>> axis2_drivers;
+	static const std::vector<class_entry<MotorDriver>> axis1_drivers; //!< List of available motor drivers for the first axis.
+	static const std::vector<class_entry<MotorDriver>> axis2_drivers; //!< List of available motor drivers for the second axis.
 
 private:
-	// Axis damper is lower than default scale of HID Damper
+	// Internal constants
 	const float AXIS_DAMPER_RATIO = INTERNAL_SCALER_DAMPER * INTERNAL_AXIS_DAMPER_SCALER / 255.0;
 	const float AXIS_INERTIA_RATIO = INTERNAL_SCALER_INERTIA * INTERNAL_AXIS_INERTIA_SCALER / 255.0;
 
-	AxisFlashAddrs flashAddrs;
-	volatile Control_t* control;
+	// Private methods
+	/**
+	 * @brief Sets the degrees of rotation for the axis.
+	 * @param degrees The new range of rotation.
+	 */
+	void setDegrees(uint16_t degrees);
+	/**
+	 * @brief Returns the current power setting of the axis.
+	 * @return The power value.
+	 */
+	uint16_t getPower();
+	/**
+	 * @brief Returns the calculated torque scaler.
+	 * @return The torque scaler value.
+	 */
+	bool isInverted();
 
-	//TIM_HandleTypeDef *timer_update;
-	AxisConfig conf;
+	void updateTorqueScaler();
+	float getTorqueScaler();
 
-	std::unique_ptr<MotorDriver> drv = std::make_unique<MotorDriver>(); // dummy
-	std::shared_ptr<Encoder> enc = nullptr;
+	/**
+	 * @brief Sets the ratio between game effects and endstop force.
+	 * @param val The new ratio value (0-255).
+	 */
+	void setFxRatio(uint8_t val);
+	/**
+	 * @brief Sets the exponential torque curve.
+	 * @param val The new expo value.
+	 */
+	void setExpo(int val);
 
-	bool outOfBounds = false;
+	int32_t calculateExpoTorque(int32_t torque);
 
+	/**
+	 * @brief Decodes the axis configuration from a 16-bit integer stored in flash.
+	 * @param val The 16-bit encoded configuration value.
+	 */
 	static AxisConfig decodeConfFromInt(uint16_t val);
+	/**
+	 * @brief Encodes the axis configuration into a 16-bit integer for flash storage.
+	 * @param conf The AxisConfig struct to encode.
+	 * @return The encoded 16-bit value.
+	 */
 	static uint16_t encodeConfToInt(AxisConfig conf);
 
-	const Error outOfBoundsError = Error(ErrorCode::axisOutOfRange,ErrorType::warning,"Axis out of bounds");
+	// Member variables
+	AxisFlashAddresses flashAddresses;                      //!< Flash memory addresses for this axis.
+	volatile Control_t* control;            //!< Pointer to the global control structure.
+	AxisConfig conf;                                        //!< Configuration for this axis (driver and encoder types).
+	char axis;                                                      //!< Axis identifier ('X', 'Y', 'Z').
 
-	float forceFadeTime = 1.0;
-	float forceFadeCurMult = 1.0;
+	std::unique_ptr<MotorDriver> drv = std::make_unique<MotorDriver>(); //!< Unique pointer to the active motor driver.
+	std::shared_ptr<Encoder> enc = nullptr; //!< Shared pointer to the active encoder.
 
-#ifdef TMC4671DRIVER
-	TMC4671Limits tmclimits = TMC4671Limits({.pid_torque_flux_ddt = 32767,
-											 .pid_uq_ud = 30000,
-											 .pid_torque_flux = 30000,
-											 .pid_acc_lim = 2147483647,
-											 .pid_vel_lim = 2147483647,
-											 .pid_pos_low = -2147483647,
-											 .pid_pos_high = 2147483647});
-#endif
-	float encoderOffset = 0; // Offset for absolute encoders
-	uint16_t degreesOfRotation = 900;					// How many degrees of range for the full gamepad range
-	uint16_t lastdegreesOfRotation = degreesOfRotation; // Used to store the previous value
-	uint16_t nextDegreesOfRotation = degreesOfRotation; // Buffer when changing range
+	bool outOfBounds = false;                       //!< Flag indicating if the axis is out of its valid range.
+	const Error outOfBoundsError = Error(ErrorCode::axisOutOfRange,ErrorType::warning,"Axis out of bounds"); //!< Error object for out-of-bounds condition.
+
+	// Force fade-in effect
+	float forceFadeTime = 1.0;                      //!< Duration of the force fade-in in seconds.
+	float forceFadeCurMult = 1.0;                //!< Current multiplier for the force fade-in.
+
+	uint16_t degreesOfRotation = 900;               //!< Current degrees of rotation.
+	uint16_t lastdegreesOfRotation = degreesOfRotation; //!< Previous degrees of rotation (for smooth transitions).
+	uint16_t nextDegreesOfRotation = degreesOfRotation; //!< Target degrees of rotation.
 
 	// Limiters
-	uint16_t maxSpeedDegS  = 0; // Set to non zero to enable. example 1000. 8b * 10?
-	//float	 maxAccelDegSS = 0;
-	uint32_t maxTorqueRateMS = 0; // 8b * 128?
-	float speedLimiterP = AXIS_SPEEDLIMITER_P;
-	float speedLimiterI = AXIS_SPEEDLIMITER_I;
+	uint16_t maxSpeedDegS  = 0;             //!< Maximum speed in degrees per second. 0 to disable.
+	uint32_t maxTorqueRateMS = 0;           //!< Maximum torque rate of change per millisecond. 0 to disable.
 
-	float spdlimitreducerI = 0;
-	//float acclimitreducerI = 0;
-	//const uint8_t accelFactor = 10.0; // Conversion factor between internal and external acc limit
+	float speedLimiterP = AXIS_SPEEDLIMITER_P; //!< Proportional term for the speed limiter.
+	float speedLimiterI = AXIS_SPEEDLIMITER_I; //!< Integral term for the speed limiter.
 
-	void setDegrees(uint16_t degrees);
+	// Speed limiter PID
+	float spdlimitreducerI = 0;
 
-	uint16_t getPower();
-	float getTorqueScaler();
-	bool isInverted();
-	char axis;
+	// Axis metrics
+	axis_metric_t metric;                   //!< Current and previous physical metrics of the axis.
+	float _lastSpeed = 0;                   //!< Instantaneous speed from the last cycle.
 
+	// Torque components
+	int32_t effectTorque = 0;            //!< Torque from HID FFB effects.
+	int32_t axisEffectTorque = 0;     //!< Torque from mechanical effects.
 
-	// Merge normalized
-	axis_metric_t metric;
-	float _lastSpeed = 0;
-	int32_t effectTorque = 0;
-	int32_t axisEffectTorque = 0;
-	uint8_t fx_ratio_i = 204; // Reduce effects to a certain ratio of the total power to have a margin for the endstop. 80% = 204
-	uint16_t power = 5000;
-	float torqueScaler = 0; // power * fx_ratio as a ratio between 0 & 1
-	float effect_margin_scaler = 0;
-	bool invertAxis = true; // By default most motors and encoders count up CCW while gamepads are counting up CW.
-	uint8_t endstopStrength = 127; // Sets how much extra torque per count above endstop is added. High = stiff endstop. Low = softer
-	const float endstopGain = 25; // Overall max endstop intensity
+	// Power and scaling
+	uint16_t power = 5000;                  //!< Maximum motor power/torque.
+	uint8_t fx_ratio_i = 204;              //!< Ratio of HID effects vs. endstop effects (0-255).
+	float effect_margin_scaler = 0;    //!< Scaler for HID effects based on effectRatio.
+	float torqueScaler = 0;                 //!< Final torque scaler based on power.
 
+	// Axis configuration
+	bool invertAxis = true;                 //!< Invert axis direction.
+	uint8_t endstopStrength = 127;  //!< Stiffness of the endstop effect.
+	const float endstopGain = 25;   //!< Overall maximum endstop intensity.
 
-	uint8_t idlespringstrength = 127;
-	int16_t idlespringclip = 0;
-	float idlespringscale = 0;
-	bool motorWasNotReady = true;
+	// Idle spring effect
+	uint8_t idlespringstrength = 127; //!< Strength of the idle spring.
+	int16_t idlespringclip = 0;       //!< Maximum force for the idle spring.
+	float idlespringscale = 0;        //!< Scaler for the idle spring force.
+	bool motorWasNotReady = true;     //!< Flag to detect motor readiness transition.
 
+	// Filters
 	// TODO tune these and check if it is really stable and beneficial to the FFB. index 4 placeholder
-	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};
-	uint8_t filterProfileId = 1; // Default medium (1) as this is the most common encoder resolution and users can go lower or higher if required.
-	float filter_f = 1000; // 1khz default. should be set at runtime once the actual rate is known
-	const int32_t intFxClip = 20000;
-	uint8_t damperIntensity = 30;
-
-	uint8_t frictionIntensity = 0;
-	uint8_t inertiaIntensity = 0;
-
+	const std::array<biquad_constant_t,4> filterSpeedCst = { {{ 40, 55 }, { 70, 55 }, { 120, 55 }, {180, 55}} }; //!< Speed filter profiles.
+	const std::array<biquad_constant_t,4> filterAccelCst = { {{ 40, 30 }, { 55, 30 }, { 70, 30 }, {120, 55}} }; //!< Acceleration filter profiles.
+	const biquad_constant_t filterDamperCst = {60, 55};     //!< Damper filter constants.
+	const biquad_constant_t filterFrictionCst = {50, 20};   //!< Friction filter constants.
+	const biquad_constant_t filterInertiaCst = {20, 20};    //!< Inertia filter constants.
+	uint8_t filterProfileId = 1; //!< Currently selected filter profile ID.
+	float filter_f = 1000.0; // 1khz
+	const int32_t intFxClip = 20000; //!< Clipping value for internal effects.
+
+	// Internal effects intensity
+	uint8_t damperIntensity = 30;   //!< Intensity of the internal damper effect.
+	uint8_t frictionIntensity = 0;  //!< Intensity of the internal friction effect.
+	uint8_t inertiaIntensity = 0;   //!< Intensity of the internal inertia effect.
+
+	// Biquad filter instances
 	Biquad speedFilter = Biquad(BiquadType::lowpass, filterSpeedCst[filterProfileId].freq/filter_f, filterSpeedCst[filterProfileId].q/100.0, 0.0);
 	Biquad accelFilter = Biquad(BiquadType::lowpass, filterAccelCst[filterProfileId].freq/filter_f, filterAccelCst[filterProfileId].q/100.0, 0.0);
-	Biquad damperFilter = Biquad(BiquadType::lowpass, filterDamperCst.freq/filter_f, filterDamperCst.q / 100.0, 0.0); // enable on class constructor
-	Biquad frictionFilter = Biquad(BiquadType::lowpass, filterFrictionCst.freq/filter_f, filterFrictionCst.q / 100.0, 0.0); // enable on class constructor
-	Biquad inertiaFilter = Biquad(BiquadType::lowpass, filterInertiaCst.freq/filter_f, filterInertiaCst.q / 100.0, 0.0); // enable on class constructor
-
-
-	void setFxRatio(uint8_t val);
-	void updateTorqueScaler();
-
-	void setExpo(int val);
-
-
-	GearRatio_t gearRatio;
-
-	int expoValInt = 0; // expo v = val*2 => v<0 ? 1/-v : v
-	float expo = 1;
-	float expoScaler = 50; // 0.28 to 3.54
-
+	Biquad damperFilter = Biquad(BiquadType::lowpass, filterDamperCst.freq/filter_f, filterDamperCst.q / 100.0, 0.0);
+	Biquad frictionFilter = Biquad(BiquadType::lowpass, filterFrictionCst.freq/filter_f, filterFrictionCst.q / 100.0, 0.0);
+	Biquad inertiaFilter = Biquad(BiquadType::lowpass, filterInertiaCst.freq/filter_f, filterInertiaCst.q / 100.0, 0.0);
+
+	// Post-processing
+	GearRatio_t gearRatio;  //!< Gear ratio between encoder and axis.
+	int expoValInt = 0;              //!< Raw integer value for the expo curve.
+	float expo = 1;                 //!< Calculated exponent for the torque curve.
+	float expoScaler = 50;  //!< Scaler for the expo calculation : 0.28 to 3.54
 };
 
 #endif /* SRC_AXIS_H_ */
diff --git a/Firmware/FFBoard/Inc/EffectsCalculator.h b/Firmware/FFBoard/Inc/EffectsCalculator.h
index feff5f0a0..5e43c9285 100644
--- a/Firmware/FFBoard/Inc/EffectsCalculator.h
+++ b/Firmware/FFBoard/Inc/EffectsCalculator.h
@@ -8,7 +8,6 @@
 #ifndef EFFECTSCALCULATOR_H_
 #define EFFECTSCALCULATOR_H_
 
-//#include "ClassChooser.h"
 #include "ffb_defs.h"
 #include "PersistentStorage.h"
 #include "CommandHandler.h"
@@ -27,7 +26,9 @@
 class Axis;
 struct metric_t;
 
-// default effect gains
+/**
+ * @brief Default effect gains.
+ */
 struct effect_gain_t {
 	uint8_t friction = 254;
 	uint8_t spring = 64;
@@ -35,24 +36,33 @@ struct effect_gain_t {
 	uint8_t inertia = 127;
 };
 
+/**
+ * @brief Default effect scalers.
+ */
 struct effect_scaler_t {
-	float friction = 1.0; //0.4 * 40;
+	float friction = 1.0;
 	float spring = 16.0;
-	float damper = 4.0; //2 * 40 * 2
-	float inertia = 2.0;//0.5 * 40;
+	float damper = 4.0;
+	float inertia = 2.0;
 };
 
+/**
+ * @brief Biquad filter constants for different effect types.
+ */
 struct effect_biquad_t {
-	biquad_constant_t constant	= { 500, 70 };
-	biquad_constant_t friction 	= { 50, 20 };
-	biquad_constant_t damper 	= { 30, 40 };
-	biquad_constant_t inertia	= { 15, 20 };
+	biquad_constant_t constant      = { 500, 70 };
+	biquad_constant_t friction      = { 50, 20 };
+	biquad_constant_t damper        = { 30, 40 };
+	biquad_constant_t inertia       = { 15, 20 };
 };
 
+/**
+ * @brief Holds statistics for an effect type.
+ */
 struct effect_stat_t {
-	std::array<int16_t,MAX_AXIS> current={0};
-	std::array<int16_t,MAX_AXIS> max={0};
-	uint16_t nb=0;
+	std::array<int16_t,MAX_AXIS> current={0}; //!< Current force for each axis.
+	std::array<int16_t,MAX_AXIS> max={0};     //!< Maximum force for each axis.
+	uint16_t nb=0;                             //!< Number of samples.
 };
 
 enum class EffectsCalculator_commands : uint32_t {
@@ -62,6 +72,9 @@ enum class EffectsCalculator_commands : uint32_t {
 	monitorEffect, effectsDetails, effectsForces,
 };
 
+/**
+ * @brief This class calculates the final torque for each axis based on the received HID FFB effects.
+ */
 class EffectsCalculator: public PersistentStorage,
 						 public CommandHandler,
 						 cpp_freertos::Thread {
@@ -73,93 +86,215 @@ class EffectsCalculator: public PersistentStorage,
 	const ClassIdentifier getInfo();
 	const ClassType getClassType() override {return ClassType::Internal;};
 
-	void saveFlash();
-	void restoreFlash();
+	/**
+	 * @brief Saves effect settings to flash memory.
+	 * @override from PersistentStorage
+	 */
+	void saveFlash() override;
+	/**
+	 * @brief Restores effect settings from flash memory.
+	 * @override from PersistentStorage
+	 */
+	void restoreFlash() override;
 
+	/**
+	 * @brief Checks if the effects calculator is active.
+	 * @return true if active, false otherwise.
+	 */
 	bool isActive();
+	/**
+	 * @brief Sets the active state of the effects calculator.
+	 * @param active true to activate, false to deactivate.
+	 */
 	void setActive(bool active);
+	/**
+	 * @brief Calculates the combined force of all active effects for each axis.
+	 * @param axes A vector of unique pointers to the axis objects.
+	 */
 	void calculateEffects(std::vector<std::unique_ptr<Axis>> &axes);
+	/**
+	 * @brief Sets the filters for a specific effect.
+	 * @param effect A pointer to the FFB_Effect object.
+	 */
 	virtual void setFilters(FFB_Effect* effect);
+	/**
+	 * @brief Sets the global gain for all effects.
+	 * @param gain The gain value (0-255).
+	 */
 	void setGain(uint8_t gain);
+	/**
+	 * @brief Gets the global gain.
+	 * @return The global gain value.
+	 */
 	uint8_t getGain();
+	/**
+	 * @brief Logs an effect type as active or inactive.
+	 * @param type The effect type ID.
+	 * @param remove true to log as inactive, false as active.
+	 */
 	void logEffectType(uint8_t type,bool remove = false);
-	//void setDirectionEnableMask(uint8_t mask);
+	/**
+	 * @brief Calculates statistics for an effect type.
+	 * @param type The effect type ID.
+	 * @param force The calculated force value.
+	 * @param axis The axis ID.
+	 */
 	void calcStatsEffectType(uint8_t type, int32_t force,uint8_t axis);
+	/**
+	 * @brief Logs the state of an effect (active, inactive, etc.).
+	 * @param type The effect type ID.
+	 * @param state The state value.
+	 */
 	void logEffectState(uint8_t type,uint8_t state);
+	/**
+	 * @brief Resets the log of active effects.
+	 * @param reinit If true, reinitializes the log.
+	 */
 	void resetLoggedActiveEffects(bool reinit);
 
+	/**
+	 * @brief Finds a free effect slot in the effects array.
+	 * @param type The effect type ID.
+	 * @return The index of the free slot, or -1 if none found.
+	 */
 	int32_t find_free_effect(uint8_t type);
+	/**
+	 * @brief Frees an effect slot.
+	 * @param idx The index of the slot to free.
+	 */
 	void free_effect(uint16_t idx);
 
-	CommandStatus command(const ParsedCommand& cmd,std::vector<CommandReply>& replies);
+	/**
+	 * @brief Handles CLI commands for the effects calculator.
+	 * @param cmd The parsed command.
+	 * @param replies A vector of replies to be sent back.
+	 * @return The status of the command execution.
+	 * @override from CommandHandler
+	 */
+	CommandStatus command(const ParsedCommand& cmd,std::vector<CommandReply>& replies) override;
 	virtual std::string getHelpstring() { return "Controls internal FFB effects"; }
 
 	static const uint32_t max_effects = MAX_EFFECTS;
-	std::array<FFB_Effect,max_effects> effects; // Main effects storage
+	std::array<FFB_Effect,max_effects> effects; //!< Main effects storage array.
 
-	// Thread impl
-	void Run();
+	/**
+	 * @brief Main thread function for the effects calculator.
+	 */
+	void Run() override;
 
-	void updateSamplerate(float newSamplerate); // Must be called if update rate is changed to update filters and effects
+	/**
+	 * @brief Updates the internal sample rate and recalculates filters.
+	 * @param newSamplerate The new sample rate in Hz.
+	 */
+	void updateSamplerate(float newSamplerate);
 
 
 protected:
 
 private:
-	//uint8_t directionEnableMask = 0;
 	// Filters
-	effect_biquad_t filter[2];		// 0 is the default profile and the custom for CFFilter, CUSTOM_PROFILE_ID is the custom slot
-	uint8_t filterProfileId = 0;
-	uint32_t calcfrequency = 1000; 		// HID frequency 1khz
-	const float qfloatScaler = 0.01;
+	effect_biquad_t filter[2];              //!< Filter profiles: 0 is default, CUSTOM_PROFILE_ID is custom.
+	uint8_t filterProfileId = 0;            //!< Currently selected filter profile ID.
+	uint32_t calcfrequency = 1000;          //!< Calculation frequency in Hz (default 1kHz).
+	const float qfloatScaler = 0.01;        //!< Scaler for Q factor.
 
-	// Rescale factor for conditional effect to boost or decrease the intensity
-	uint8_t global_gain = 0xff;
-	effect_gain_t gain;
-	effect_scaler_t scaler;
-	uint8_t frictionPctSpeedToRampup = 25;	// define the max value of the range (0..5% of maxspeed) where torque is rampup on friction
+	// Intensity and scaling
+	uint8_t global_gain = 0xff;             //!< Global gain for all effects.
+	effect_gain_t gain;                     //!< Individual effect type gains.
+	effect_scaler_t scaler;                 //!< Individual effect type scalers.
+	uint8_t frictionPctSpeedToRampup = 25;  //!< Friction ramp-up threshold (percentage of max speed).
 
 	// FFB status
-	bool effects_active = false; // If FFB is on
-	uint32_t effects_used = 0;
-	std::array<effect_stat_t,12> effects_stats; // [0..12 effect types]
-	std::array<effect_stat_t,12> effects_statslast; // [0..12 effect types]
-	bool isMonitorEffect = false;
+	bool effects_active = false;            //!< true if FFB is active.
+	uint32_t effects_used = 0;              //!< Mask of currently used effect slots.
+	std::array<effect_stat_t,12> effects_stats;     //!< Statistics for each effect type (current cycle).
+	std::array<effect_stat_t,12> effects_statslast; //!< Statistics from the previous cycle.
+	bool isMonitorEffect = false;           //!< Flag for effect monitoring.
 
+	/**
+	 * @brief Calculates the force component for a specific effect on a specific axis.
+	 */
 	int32_t calcComponentForce(FFB_Effect *effect, int32_t forceVector, std::vector<std::unique_ptr<Axis>> &axes, uint8_t axis);
+	/**
+	 * @brief Calculates the force for a non-conditional effect (e.g., Constant Force, Sine).
+	 */
 	int32_t calcNonConditionEffectForce(FFB_Effect* effect);
+	/**
+	 * @brief Returns the speed ramp-up factor for friction.
+	 */
 	float speedRampupPct();
+	/**
+	 * @brief Calculates the force for a conditional effect (e.g., Spring, Damper, Friction).
+	 */
 	int32_t calcConditionEffectForce(FFB_Effect *effect, float metric, uint8_t gain, uint8_t idx, float scale, float angle_ratio);
+	/**
+	 * @brief Calculates the envelope magnitude for an effect.
+	 */
 	int32_t getEnvelopeMagnitude(FFB_Effect *effect);
+	/**
+	 * @brief Lists the currently used effects as a string.
+	 */
 	std::string listEffectsUsed(bool details = false,uint8_t axis = 0);
-	//std::string listForceEffects();
+	/**
+	 * @brief Checks and updates filter coefficients.
+	 */
 	void checkFilterCoeff(biquad_constant_t *filter, uint32_t freq,uint8_t q);
+	/**
+	 * @brief Updates filter settings for a specific effect type.
+	 */
 	void updateFilterSettingsForEffects(uint8_t type_effect);
 };
 
 /**
- * Helper interface class for common effects calculator related control functions
+ * @brief Helper interface class for common effects calculator related control functions.
  */
 class EffectsControlItf{
 public:
-	virtual void set_FFB(bool state) = 0; // Enables or disables FFB
+	/**
+	 * @brief Enables or disables FFB globally.
+	 */
+	virtual void set_FFB(bool state) = 0;
 	virtual void stop_FFB(){set_FFB(false);};
 	virtual void start_FFB(){set_FFB(true);};
+	/**
+	 * @brief Resets all FFB effects.
+	 */
 	virtual void reset_ffb() = 0;
-	virtual uint32_t getConstantForceRate(); // Returns an estimate of the constant force effect update rate in hz
-	virtual uint32_t getRate(); // Returns an estimate of the overall effect update speed in hz
+	/**
+	 * @brief Returns the estimated constant force update rate in Hz.
+	 */
+	virtual uint32_t getConstantForceRate();
+	/**
+	 * @brief Returns the estimated overall effect update speed in Hz.
+	 */
+	virtual uint32_t getRate();
+	/**
+	 * @brief Checks if FFB is active.
+	 */
 	virtual bool getFfbActive() = 0;
+	/**
+	 * @brief Sets the global FFB gain.
+	 */
 	virtual void set_gain(uint8_t gain) = 0;
+	/**
+	 * @brief Event handler for constant force updates.
+	 */
 	virtual void cfUpdateEvent();
+	/**
+	 * @brief Event handler for effect updates.
+	 */
 	virtual void fxUpdateEvent();
-	virtual void updateSamplerate(float newSamplerate) = 0; // Should be called when update loop rate is changed
+	/**
+	 * @brief Updates the sample rate for filters and effects.
+	 */
+	virtual void updateSamplerate(float newSamplerate) = 0;
 
 private:
-	FastMovingAverage<float> fxPeriodAvg{5};
-	FastMovingAverage<float> cfUpdatePeriodAvg{5};
+	FastMovingAverage<float> fxPeriodAvg{5};          //!< Average period between effect updates.
+	FastMovingAverage<float> cfUpdatePeriodAvg{5};    //!< Average period between constant force updates.
 
-	uint32_t lastFxUpdate = 0;
-	uint32_t lastCfUpdate = 0;
+	uint32_t lastFxUpdate = 0; //!< Time of last effect update.
+	uint32_t lastCfUpdate = 0; //!< Time of last constant force update.
 };
 
 #endif /* EFFECTSCALCULATOR_H_ */
diff --git a/Firmware/FFBoard/Inc/HidFFB.h b/Firmware/FFBoard/Inc/HidFFB.h
index 7dae12394..3e776958e 100644
--- a/Firmware/FFBoard/Inc/HidFFB.h
+++ b/Firmware/FFBoard/Inc/HidFFB.h
@@ -20,54 +20,139 @@
 #define Z_AXIS_ENABLE 4
 #define DIRECTION_ENABLE(AXES) (1 << AXES)
 
+/**
+ * @brief This class handles HID FFB reports from the USB host.
+ * It decodes the reports and updates the effects in the EffectsCalculator.
+ */
 class HidFFB: public UsbHidHandler, public EffectsControlItf {
 public:
+	/**
+	 * @brief Construct a new HidFFB object.
+	 * @param ec Shared pointer to the EffectsCalculator.
+	 * @param axisCount Number of FFB axes.
+	 */
 	HidFFB(std::shared_ptr<EffectsCalculator> ec,uint8_t axisCount);
 	virtual ~HidFFB();
 
+	/**
+	 * @brief Handles HID OUT reports.
+	 * @override from UsbHidHandler
+	 */
 	void hidOut(uint8_t report_id, hid_report_type_t report_type,const uint8_t* buffer, uint16_t bufsize) override;
+	/**
+	 * @brief Handles HID GET reports.
+	 * @override from UsbHidHandler
+	 */
 	uint16_t hidGet(uint8_t report_id, hid_report_type_t report_type,uint8_t* buffer, uint16_t reqlen) override;
 
-	bool getFfbActive();
+	/**
+	 * @brief Checks if FFB is active.
+	 * @return true if active, false otherwise.
+	 */
+	bool getFfbActive() override;
+	/**
+	 * @brief Sends a HID report.
+	 */
 	static bool HID_SendReport(uint8_t *report,uint16_t len);
 
-	void reset_ffb();
-	void start_FFB();
-	void stop_FFB();
-	void set_FFB(bool state);
-	void set_gain(uint8_t gain);
+	/**
+	 * @brief Resets all FFB effects.
+	 */
+	void reset_ffb() override;
+	/**
+	 * @brief Starts FFB.
+	 */
+	void start_FFB() override;
+	/**
+	 * @brief Stops FFB.
+	 */
+	void stop_FFB() override;
+	/**
+	 * @brief Enables or disables FFB.
+	 * @param state true to enable, false to disable.
+	 */
+	void set_FFB(bool state) override;
+	/**
+	 * @brief Sets the global FFB gain.
+	 * @param gain The gain value (0-255).
+	 */
+	void set_gain(uint8_t gain) override;
 	
+	/**
+	 * @brief Sends a status report for a specific effect.
+	 */
 	void sendStatusReport(uint8_t effect);
+	/**
+	 * @brief Sets the direction enable mask.
+	 */
 	void setDirectionEnableMask(uint8_t mask);
-	void updateSamplerate(float newSamplerate);
+	/**
+	 * @brief Updates the internal sample rate.
+	 * @param newSamplerate The new sample rate in Hz.
+	 */
+	void updateSamplerate(float newSamplerate) override;
 
 private:
 	// HID
 	std::shared_ptr<EffectsCalculator> effects_calc;
-	std::array<FFB_Effect,EffectsCalculator::max_effects>& effects; // Must be passed in constructor
+	std::array<FFB_Effect,EffectsCalculator::max_effects>& effects; //!< Reference to the effects array in EffectsCalculator.
+	
+	/**
+	 * @brief Handles the creation of a new effect.
+	 */
 	void new_effect(FFB_CreateNewEffect_Feature_Data_t* effect);
+	/**
+	 * @brief Handles freeing an effect slot.
+	 */
 	void free_effect(uint16_t id);
+	/**
+	 * @brief Decodes the FFB control command.
+	 */
 	void ffb_control(uint8_t cmd);
+	/**
+	 * @brief Decodes the set effect report.
+	 */
 	void set_effect(FFB_SetEffect_t* effect);
+	/**
+	 * @brief Decodes the set condition report.
+	 */
 	void set_condition(FFB_SetCondition_Data_t* cond);
+	/**
+	 * @brief Decodes the set envelope report.
+	 */
 	void set_envelope(FFB_SetEnvelope_Data_t* report);
+	/**
+	 * @brief Decodes the set ramp report.
+	 */
 	void set_ramp(FFB_SetRamp_Data_t* report);
+	/**
+	 * @brief Decodes the set constant force report.
+	 */
 	void set_constant_effect(FFB_SetConstantForce_Data_t* effect);
+	/**
+	 * @brief Decodes the set periodic report.
+	 */
 	void set_periodic(FFB_SetPeriodic_Data_t* report);
+	/**
+	 * @brief Decodes the effect operation report (start/stop/pause).
+	 */
 	void set_effect_operation(FFB_EffOp_Data_t* report);
 
 
+	/**
+	 * @brief Updates the filters for an effect.
+	 */
 	void set_filters(FFB_Effect* effect);
 
-	uint8_t directionEnableMask; // Has to be adjusted if bit is not last bit after axis enable bits
-	uint16_t used_effects = 0;
-	bool ffb_active = false;
-	FFB_BlockLoad_Feature_Data_t blockLoad_report;
-	FFB_PIDPool_Feature_Data_t pool_report;
+	uint8_t directionEnableMask; //!< Mask for enabled directions.
+	uint16_t used_effects = 0;   //!< Number of currently used effect slots.
+	bool ffb_active = false;      //!< true if FFB is active.
+	FFB_BlockLoad_Feature_Data_t blockLoad_report; //!< HID Feature report: Block Load.
+	FFB_PIDPool_Feature_Data_t pool_report;           //!< HID Feature report: PID Pool.
 
-	reportFFB_status_t reportFFBStatus;
+	reportFFB_status_t reportFFBStatus; //!< Holds the current FFB status.
 
-	uint8_t axisCount;
+	uint8_t axisCount; //!< Number of FFB axes.
 };
 
 #endif /* HIDFFB_H_ */
diff --git a/Firmware/FFBoard/Src/Axis.cpp b/Firmware/FFBoard/Src/Axis.cpp
index 41f487b28..54b2b83a0 100644
--- a/Firmware/FFBoard/Src/Axis.cpp
+++ b/Firmware/FFBoard/Src/Axis.cpp
@@ -86,30 +86,30 @@ const std::vector<class_entry<MotorDriver>> Axis::axis2_drivers =
 /**
  * Axis class manages motor drivers and passes effect torque to the motor drivers
  */
-Axis::Axis(char axis,volatile Control_t* control) :CommandHandler("axis", CLSID_AXIS), drv_chooser(MotorDriver::all_drivers),enc_chooser{Encoder::all_encoders}
+Axis::Axis(char axis,volatile Control_t* control) :CommandHandler("axis", CLSID_AXIS), driverChooser(MotorDriver::all_drivers),encoderChooser{Encoder::all_encoders}
 {
 	this->axis = axis;
 	this->control = control;
 	if (axis == 'X')
 	{
-		drv_chooser = ClassChooser<MotorDriver>(axis1_drivers);
+		driverChooser = ClassChooser<MotorDriver>(axis1_drivers);
 		setInstance(0);
-		this->flashAddrs = AxisFlashAddrs({ADR_AXIS1_CONFIG, ADR_AXIS1_MAX_SPEED, ADR_AXIS1_MAX_ACCEL,
+		this->flashAddresses = AxisFlashAddresses({ADR_AXIS1_CONFIG, ADR_AXIS1_MAX_SPEED, ADR_AXIS1_MAX_ACCEL,
 										   ADR_AXIS1_ENDSTOP, ADR_AXIS1_POWER, ADR_AXIS1_DEGREES,ADR_AXIS1_EFFECTS1,ADR_AXIS1_EFFECTS2,ADR_AXIS1_ENC_RATIO,
 										   ADR_AXIS1_SPEEDACCEL_FILTER,ADR_AXIS1_POSTPROCESS1});
 	}
 	else if (axis == 'Y')
 	{
-		drv_chooser = ClassChooser<MotorDriver>(axis2_drivers);
+		driverChooser = ClassChooser<MotorDriver>(axis2_drivers);
 		setInstance(1);
-		this->flashAddrs = AxisFlashAddrs({ADR_AXIS2_CONFIG, ADR_AXIS2_MAX_SPEED, ADR_AXIS2_MAX_ACCEL,
+		this->flashAddresses = AxisFlashAddresses({ADR_AXIS2_CONFIG, ADR_AXIS2_MAX_SPEED, ADR_AXIS2_MAX_ACCEL,
 										   ADR_AXIS2_ENDSTOP, ADR_AXIS2_POWER, ADR_AXIS2_DEGREES,ADR_AXIS2_EFFECTS1,ADR_AXIS2_EFFECTS2, ADR_AXIS2_ENC_RATIO,
 										   ADR_AXIS2_SPEEDACCEL_FILTER,ADR_AXIS2_POSTPROCESS1});
 	}
 	else if (axis == 'Z')
 	{
 		setInstance(2);
-		this->flashAddrs = AxisFlashAddrs({ADR_AXIS3_CONFIG, ADR_AXIS3_MAX_SPEED, ADR_AXIS3_MAX_ACCEL,
+		this->flashAddresses = AxisFlashAddresses({ADR_AXIS3_CONFIG, ADR_AXIS3_MAX_SPEED, ADR_AXIS3_MAX_ACCEL,
 										   ADR_AXIS3_ENDSTOP, ADR_AXIS3_POWER, ADR_AXIS3_DEGREES,ADR_AXIS3_EFFECTS1,ADR_AXIS3_EFFECTS2,ADR_AXIS3_ENC_RATIO,
 										   ADR_AXIS3_SPEEDACCEL_FILTER,ADR_AXIS3_POSTPROCESS1});
 	}
@@ -166,10 +166,10 @@ void Axis::registerCommands(){
  * Read parameters from flash and restore settings
  */
 void Axis::restoreFlash(){
-	//NormalizedAxis::restoreFlash();
+	// TODO: This seems to be a remnant of a previous architecture (NormalizedAxis). Confirm and remove.
 	// read all constants
 	uint16_t value;
-	if (Flash_Read(flashAddrs.config, &value)){
+	if (Flash_Read(flashAddresses.config, &value)){
 		this->conf = Axis::decodeConfFromInt(value);
 	}else{
 		pulseErrLed();
@@ -178,31 +178,31 @@ void Axis::restoreFlash(){
 	setDrvType(this->conf.drvtype);
 	setEncType(this->conf.enctype);
 
-	if (Flash_Read(flashAddrs.maxSpeed, &value)){
+	if (Flash_Read(flashAddresses.maxSpeed, &value)){
 		this->maxSpeedDegS = value;
 	}else{
 		pulseErrLed();
 	}
-//
-//	if (Flash_Read(flashAddrs.maxAccel, &value)){
-//		this->maxTorqueRateMS = value;
-//	}else{
-//		pulseErrLed();
-//	}
+
+	if (Flash_Read(flashAddresses.maxAccel, &value)){
+		this->maxTorqueRateMS = value;
+	}else{
+		pulseErrLed();
+	}
 
 
 	uint16_t esval, power;
-	if(Flash_Read(flashAddrs.endstop, &esval)) {
+	if(Flash_Read(flashAddresses.endstop, &esval)) {
 		fx_ratio_i = esval & 0xff;
 		endstopStrength = (esval >> 8) & 0xff;
 	}
 
 
-	if(Flash_Read(flashAddrs.power, &power)){
+	if(Flash_Read(flashAddresses.power, &power)){
 		setPower(power);
 	}
 	uint16_t deg_t;
-	if(Flash_Read(flashAddrs.degrees, &deg_t)){
+	if(Flash_Read(flashAddresses.degrees, &deg_t)){
 		this->degreesOfRotation = deg_t & 0x7fff;
 		this->invertAxis = (deg_t >> 15) & 0x1;
 		setDegrees(degreesOfRotation);
@@ -210,25 +210,25 @@ void Axis::restoreFlash(){
 
 
 	uint16_t effects;
-	if(Flash_Read(flashAddrs.effects1, &effects)){
+	if(Flash_Read(flashAddresses.effects1, &effects)){
 		setIdleSpringStrength(effects & 0xff);
 		setFxStrengthAndFilter((effects >> 8) & 0xff,damperIntensity,damperFilter);
 	}else{
 		setIdleSpringStrength(idlespringstrength); // Use default
 	}
 
-	if(Flash_Read(flashAddrs.effects2, &effects)){
+	if(Flash_Read(flashAddresses.effects2, &effects)){
 		setFxStrengthAndFilter(effects & 0xff,frictionIntensity,frictionFilter);
 		setFxStrengthAndFilter((effects >> 8) & 0xff,inertiaIntensity,inertiaFilter);
 	}
 
 	uint16_t ratio;
-	if(Flash_Read(flashAddrs.encoderRatio, &ratio)){
+	if(Flash_Read(flashAddresses.encoderRatio, &ratio)){
 		setGearRatio(ratio & 0xff, (ratio >> 8) & 0xff);
 	}
 
 	uint16_t filterStorage;
-	if (Flash_Read(flashAddrs.speedAccelFilter, &filterStorage))
+	if (Flash_Read(flashAddresses.speedAccelFilter, &filterStorage))
 	{
 		uint8_t profile = filterStorage & 0xFF;
 		this->filterProfileId = profile;
@@ -239,31 +239,31 @@ void Axis::restoreFlash(){
 	}
 
 	uint16_t pp1;
-	if(Flash_Read(flashAddrs.postprocess1, &pp1)){
+	if(Flash_Read(flashAddresses.postprocess1, &pp1)){
 		setExpo((int8_t)(pp1 & 0xff));
 	}
 
 }
 // Saves parameters to flash.
 void Axis::saveFlash(){
-	//NormalizedAxis::saveFlash();
-	Flash_Write(flashAddrs.config, Axis::encodeConfToInt(this->conf));
-	Flash_Write(flashAddrs.maxSpeed, this->maxSpeedDegS);
-//	Flash_Write(flashAddrs.maxAccel, (uint16_t)(this->maxTorqueRateMS));
-
-	Flash_Write(flashAddrs.endstop, fx_ratio_i | (endstopStrength << 8));
-	Flash_Write(flashAddrs.power, power);
-	Flash_Write(flashAddrs.degrees, (degreesOfRotation & 0x7fff) | (invertAxis << 15));
-	Flash_Write(flashAddrs.effects1, idlespringstrength | (damperIntensity << 8));
-	Flash_Write(flashAddrs.effects2, frictionIntensity | (inertiaIntensity << 8));
-	Flash_Write(flashAddrs.encoderRatio, gearRatio.numerator | (gearRatio.denominator << 8));
+	// TODO: This seems to be a remnant of a previous architecture (NormalizedAxis). Confirm and remove.
+	Flash_Write(flashAddresses.config, Axis::encodeConfToInt(this->conf));
+	Flash_Write(flashAddresses.maxSpeed, this->maxSpeedDegS);
+	Flash_Write(flashAddresses.maxAccel, (uint16_t)(this->maxTorqueRateMS));
+
+	Flash_Write(flashAddresses.endstop, fx_ratio_i | (endstopStrength << 8));
+	Flash_Write(flashAddresses.power, power);
+	Flash_Write(flashAddresses.degrees, (degreesOfRotation & 0x7fff) | (invertAxis << 15));
+	Flash_Write(flashAddresses.effects1, idlespringstrength | (damperIntensity << 8));
+	Flash_Write(flashAddresses.effects2, frictionIntensity | (inertiaIntensity << 8));
+	Flash_Write(flashAddresses.encoderRatio, gearRatio.numerator | (gearRatio.denominator << 8));
 
 	// save CF biquad
 	uint16_t filterStorage = (uint16_t)this->filterProfileId & 0xFF;
-	Flash_Write(flashAddrs.speedAccelFilter, filterStorage);
+	Flash_Write(flashAddresses.speedAccelFilter, filterStorage);
 
 	// Postprocessing
-	Flash_Write(flashAddrs.postprocess1, expoValInt & 0xff);
+	Flash_Write(flashAddresses.postprocess1, expoValInt & 0xff);
 
 }
 
@@ -375,16 +375,6 @@ void Axis::setPower(uint16_t power)
 {
 	this->power = power;
 	updateTorqueScaler();
-#ifdef TMC4671DRIVER
-	// Update hardware limits for TMC for safety
-	TMC4671 *drv = dynamic_cast<TMC4671 *>(this->drv.get());
-	if (drv != nullptr)
-	{
-		//tmclimits.pid_uq_ud = power;
-		//tmclimits.pid_torque_flux = power;
-		drv->setTorqueLimit(power);
-	}
-#endif
 }
 
 
@@ -393,12 +383,13 @@ void Axis::setPower(uint16_t power)
  */
 void Axis::setDrvType(uint8_t drvtype)
 {
-	if (!drv_chooser.isValidClassId(drvtype))
+	if (!driverChooser.isValidClassId(drvtype))
 	{
 		return;
 	}
 	cpp_freertos::CriticalSection::Enter();
-	this->drv.reset(drv_chooser.Create((uint16_t)drvtype));
+	MotorDriver* drv = driverChooser.Create((uint16_t)drvtype);
+	this->drv.reset(drv);
 	if (drv == nullptr)
 	{
 		cpp_freertos::CriticalSection::Exit();
@@ -410,12 +401,7 @@ void Axis::setDrvType(uint8_t drvtype)
 	if(!this->drv->hasIntegratedEncoder()){
 		this->drv->setEncoder(this->enc);
 	}
-#ifdef TMC4671DRIVER
-	if (dynamic_cast<TMC4671 *>(drv.get()))
-	{
-		setupTMC4671();
-	}
-#endif
+
 	if (!tud_connected())
 	{
 		control->usb_disabled = false;
@@ -428,25 +414,6 @@ void Axis::setDrvType(uint8_t drvtype)
 	cpp_freertos::CriticalSection::Exit();
 }
 
-#ifdef TMC4671DRIVER
-// Special tmc setup methods
-void Axis::setupTMC4671()
-{
-	TMC4671 *drv = static_cast<TMC4671 *>(this->drv.get());
-//	drv->setAxis(axis);
-	drv->setExternalEncoderAllowed(true);
-	drv->restoreFlash();
-	tmclimits.pid_torque_flux = getPower();
-	drv->setLimits(tmclimits);
-	//drv->setBiquadTorque(TMC4671Biquad(tmcbq_500hz_07q_25k));
-	
-
-	// Enable driver
-
-	drv->setMotionMode(MotionMode::torque);
-	drv->Start(); // Start thread
-}
-#endif
 
 
 /**
@@ -454,11 +421,11 @@ void Axis::setupTMC4671()
  */
 void Axis::setEncType(uint8_t enctype)
 {
-	if (enc_chooser.isValidClassId(enctype) && !drv->hasIntegratedEncoder())
+	if (encoderChooser.isValidClassId(enctype) && !drv->hasIntegratedEncoder())
 	{
 
 		this->conf.enctype = (enctype);
-		this->enc = std::shared_ptr<Encoder>(enc_chooser.Create(enctype)); // Make new encoder
+		this->enc = std::shared_ptr<Encoder>(encoderChooser.Create(enctype)); // Make new encoder
 		if(drv && !drv->hasIntegratedEncoder())
 			this->drv->setEncoder(this->enc);
 	}else{
@@ -747,11 +714,6 @@ bool Axis::updateTorque(int32_t* totalTorque) {
 		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();
-//		acclimitreducerI = clip<float,int32_t>( acclimitreducerI + ((accreducer * 0.02) * torqueScaler),0,power);
-
-
 		// Only reduce torque. Don't invert it to prevent oscillation
 		float torqueReduction = speedreducer * speedLimiterP + spdlimitreducerI;// accreducer * 0.025 + acclimitreducerI
 		if(torque > 0){
@@ -764,9 +726,9 @@ bool Axis::updateTorque(int32_t* totalTorque) {
 	}
 	// Torque slew rate limiter
 	if(maxTorqueRateMS > 0){
-		torque = clip<int32_t,int32_t>(torque, metric.previous.torque - maxTorqueRateMS,metric.previous.torque + maxTorqueRateMS);
+		torque = clip<int32_t,int32_t>(torque, metric.previous.torque - (int32_t)maxTorqueRateMS,metric.previous.torque + (int32_t)maxTorqueRateMS);
 	}
-//	if(torque - metric.previous.torque)
+
 	if(outOfBounds){
 		torque = 0;
 	}
@@ -864,14 +826,6 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector<CommandReply>&
 
 	case Axis_commands::degrees:
 		handleGetSetFunc(cmd, replies, degreesOfRotation, &Axis::setDegrees,this);
-//		if (cmd.type == CMDtype::get)
-//		{
-//			replies.emplace_back(degreesOfRotation);
-//		}
-//		else if (cmd.type == CMDtype::set)
-//		{
-//			setDegrees(cmd.val);
-//		}
 		break;
 
 	case Axis_commands::esgain:
@@ -940,7 +894,7 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector<CommandReply>&
 
 	case Axis_commands::enctype:
 		if(cmd.type == CMDtype::info){
-			enc_chooser.replyAvailableClasses(replies,this->getEncType());
+			encoderChooser.replyAvailableClasses(replies,this->getEncType());
 		}else if(cmd.type == CMDtype::get){
 			replies.emplace_back(this->getEncType());
 		}else if(cmd.type == CMDtype::set){
@@ -950,7 +904,7 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector<CommandReply>&
 
 	case Axis_commands::drvtype:
 		if(cmd.type == CMDtype::info){
-			drv_chooser.replyAvailableClasses(replies,this->getDrvType());
+			driverChooser.replyAvailableClasses(replies,this->getDrvType());
 		}else if(cmd.type == CMDtype::get){
 			replies.emplace_back(this->getDrvType());
 		}else if(cmd.type == CMDtype::set){
@@ -1043,13 +997,6 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector<CommandReply>&
 			if(this->getEncoder() != nullptr){
 				cpr = this->getEncoder()->getCpr();
 			}
-//#ifdef TMC4671DRIVER // CPR should be consistent with position. Maybe change TMC to prescale to encoder count or correct readout in UI
-//			TMC4671 *tmcdrv = dynamic_cast<TMC4671 *>(this->drv.get()); // Special case for TMC. Get the actual encoder resolution
-//			if (tmcdrv && tmcdrv->hasIntegratedEncoder())
-//			{
-//				cpr = tmcdrv->getEncCpr();
-//			}
-//#endif
 			replies.emplace_back(cpr);
 		}else{
 			return CommandStatus::ERR;
diff --git a/Firmware/FFBoard/Src/EffectsCalculator.cpp b/Firmware/FFBoard/Src/EffectsCalculator.cpp
index 5684fd975..1fe388673 100644
--- a/Firmware/FFBoard/Src/EffectsCalculator.cpp
+++ b/Firmware/FFBoard/Src/EffectsCalculator.cpp
@@ -104,11 +104,13 @@ An inertia condition uses axis acceleration as the metric.
 void EffectsCalculator::calculateEffects(std::vector<std::unique_ptr<Axis>> &axes)
 {
 	for (auto &axis : axes) {
-		axis->setEffectTorque(0);
 		axis->calculateAxisEffects(isActive());
 	}
 
 	if(!isActive()){
+		for (auto &axis : axes) {
+			axis->setEffectTorque(0);
+		}
 		return;
 	}
 
@@ -159,8 +161,8 @@ void EffectsCalculator::calculateEffects(std::vector<std::unique_ptr<Axis>> &axe
 	// Apply summed force to axes
 	for(uint8_t i=0 ; i < axisCount ; i++)
 	{
-		int32_t force = clip<int32_t, int32_t>(forces[i], -0x7fff, 0x7fff); // Clip
-		axes[i]->setEffectTorque(force);
+		forces[i] = clip<int32_t, int32_t>(forces[i], -0x7fff, 0x7fff); // Clip at effects summ
+		axes[i]->setEffectTorque(forces[i]);
 	}
 
 	effects_statslast = effects_stats;
@@ -715,7 +717,7 @@ std::string EffectsCalculator::listEffectsUsed(bool details,uint8_t axis){
 	} else {
 
 		bool firstItem = true;
-		for (int i=0;i < 12; i++) {
+		for (int i=0; i< 12; i++) {
 			if (!firstItem) effects_list += ", ";
 			effects_list += "{\"max\":" + std::to_string(effects_stats[i].max[axis]);
 			effects_list += ", \"curr\":" + std::to_string(effects_stats[i].current[axis]);

From 67e0270482e7516d80967ad75b99419325fe7768 Mon Sep 17 00:00:00 2001
From: Vincent MANOUKIAN <10980775+manoukianv@users.noreply.github.com>
Date: Sat, 25 Apr 2026 10:39:57 +0200
Subject: [PATCH 2/8] feat: add torque slew rate limiter and driver calibration

---
 Firmware/FFBoard/Inc/Axis.h          | 16 ++++++-
 Firmware/FFBoard/Inc/MotorDriver.h   | 69 +++++++++++++++++++++++---
 Firmware/FFBoard/Src/Axis.cpp        | 72 ++++++++++++++++++++++++----
 Firmware/FFBoard/Src/MotorDriver.cpp |  6 +++
 Firmware/scripts/memory_map.csv      | 11 ++---
 5 files changed, 150 insertions(+), 24 deletions(-)

diff --git a/Firmware/FFBoard/Inc/Axis.h b/Firmware/FFBoard/Inc/Axis.h
index 32b8bb8af..958c4bae1 100644
--- a/Firmware/FFBoard/Inc/Axis.h
+++ b/Firmware/FFBoard/Inc/Axis.h
@@ -54,6 +54,7 @@ struct AxisFlashAddresses
 	uint16_t config = ADR_AXIS1_CONFIG;
 	uint16_t maxSpeed = ADR_AXIS1_MAX_SPEED;
 	uint16_t maxAccel = ADR_AXIS1_MAX_ACCEL;
+	uint16_t maxSlewRateDrv = ADR_AXIS1_MAX_SLEWRATE_DRV;
 
 	uint16_t endstop = ADR_AXIS1_ENDSTOP;
 	uint16_t power = ADR_AXIS1_POWER;
@@ -109,7 +110,9 @@ enum class Axis_commands : uint32_t{
 	pos,curtorque,curpos,curspd,curaccel,
 	fxratio,reductionScaler,
 	filterSpeed, filterAccel, filterProfileId,cpr,axisfriction,axisinertia,
-	maxspeed, maxtorquerate,
+	maxspeed,slewrate,
+	calibrate_maxSlewRateDrv,
+	maxSlewRateDrv,
 	expo,exposcale
 };
 
@@ -400,6 +403,12 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 
 	int32_t calculateExpoTorque(int32_t torque);
 
+	/**
+	 * @brief Applies the torque slew rate limiter to the torque.
+	 * @param torque A reference to the torque value to be modified.
+	 */
+	void applyTorqueSlewRateLimiter(int64_t& torque);
+
 	/**
 	 * @brief Decodes the axis configuration from a 16-bit integer stored in flash.
 	 * @param val The 16-bit encoded configuration value.
@@ -433,6 +442,7 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	uint16_t nextDegreesOfRotation = degreesOfRotation; //!< Target degrees of rotation.
 
 	// Limiters
+	uint16_t maxSlewRate_Driver = MAX_SLEW_RATE;            //!< Maximum slew rate as measured by the driver (in units/ms).
 	uint16_t maxSpeedDegS  = 0;             //!< Maximum speed in degrees per second. 0 to disable.
 	uint32_t maxTorqueRateMS = 0;           //!< Maximum torque rate of change per millisecond. 0 to disable.
 
@@ -467,6 +477,10 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	float idlespringscale = 0;        //!< Scaler for the idle spring force.
 	bool motorWasNotReady = true;     //!< Flag to detect motor readiness transition.
 
+	// Slew rate calibration tracking: true when Axis requested a calibration and
+	// is waiting for the driver to finish measuring the max slew rate.
+	bool awaitingSlewCalibration = false;
+
 	// Filters
 	// TODO tune these and check if it is really stable and beneficial to the FFB. index 4 placeholder
 	const std::array<biquad_constant_t,4> filterSpeedCst = { {{ 40, 55 }, { 70, 55 }, { 120, 55 }, {180, 55}} }; //!< Speed filter profiles.
diff --git a/Firmware/FFBoard/Inc/MotorDriver.h b/Firmware/FFBoard/Inc/MotorDriver.h
index 0446ac3ba..0141f7ec1 100644
--- a/Firmware/FFBoard/Inc/MotorDriver.h
+++ b/Firmware/FFBoard/Inc/MotorDriver.h
@@ -14,7 +14,14 @@
 #include "Encoder.h"
 #include "memory"
 
+#define MAX_SLEW_RATE 65535
+
 class Encoder;
+
+/**
+ * @brief Base class for all motor drivers.
+ * This class defines the interface for controlling a motor and optionally its integrated encoder.
+ */
 class MotorDriver : public ChoosableClass{
 public:
 	MotorDriver(){};
@@ -25,25 +32,75 @@ class MotorDriver : public ChoosableClass{
 	const ClassType getClassType() override {return ClassType::Motordriver;};
 	static const std::vector<class_entry<MotorDriver>> all_drivers;
 
+	/**
+	 * @brief Sends a torque command to the motor.
+	 * @param power The torque value (signed 16-bit).
+	 */
 	virtual void turn(int16_t power);
+	/**
+	 * @brief Stops the motor (torque = 0).
+	 */
 	virtual void stopMotor();
+	/**
+	 * @brief Starts the motor driver.
+	 */
 	virtual void startMotor();
+	/**
+	 * @brief Triggers an emergency stop.
+	 * @param reset If true, resets the driver after stopping.
+	 */
 	virtual void emergencyStop(bool reset = false);
 
-	virtual bool motorReady(); // Returns true if the driver is active and ready to receive commands
+	/**
+	 * @brief Checks if the driver is ready to receive torque commands.
+	 * @return true if ready, false otherwise.
+	 */
+	virtual bool motorReady();
 
-	virtual Encoder* getEncoder(); // Encoder is managed by the motor driver. Must always return an encoder
+	/**
+	 * @brief Gets the encoder managed by this driver.
+	 * @return A pointer to the encoder instance.
+	 */
+	virtual Encoder* getEncoder();
 
 	/**
-	 * Can pass an external encoder if driver has no integrated encoder
-	 * This allows a driver to get an external encoder assigned if it requires one and has the capability of using external encoders
+	 * @brief Sets an external encoder for drivers that don't have an integrated one.
+	 * @param encoder A shared pointer to the external encoder.
 	 */
 	virtual void setEncoder(std::shared_ptr<Encoder>& encoder){drvEncoder = encoder;}
-	virtual bool hasIntegratedEncoder(); // Returns true if the driver has an integrated encoder. If false the axis will pass one to the driver
+
+	/**
+	 * @brief Checks if the driver has an integrated encoder.
+	 * @return true if integrated, false if external.
+	 */
+	virtual bool hasIntegratedEncoder();
+
+	/**
+	 * @brief Gets the hardware measured maximum slew rate of the driver.
+	 * @return The maximum slew rate in units/ms.
+	 */
+	virtual uint32_t getDrvSlewRate() { return MAX_SLEW_RATE; }
+
+	/**
+	 * @brief Starts a calibration procedure to measure the driver's max slew rate.
+	 * @return true if calibration started successfully.
+	 */
+	virtual bool startSlewRateCalibration() { return false; }
+
+	/**
+	 * @brief Performs initial setup and configuration of the driver.
+	 */
+	virtual void setupDriver() {}
+
+	/**
+	 * @brief Sets the maximum power/current limit for the driver.
+	 * @param power The power limit value.
+	 */
+	virtual void setPowerLimit(uint16_t power) {}
 
 
 protected:
-	std::shared_ptr<Encoder> drvEncoder = std::make_shared<Encoder>(); // Dummy encoder
+	std::shared_ptr<Encoder> drvEncoder = std::make_shared<Encoder>(); //!< Pointer to the encoder (integrated or external).
 };
 
 
diff --git a/Firmware/FFBoard/Src/Axis.cpp b/Firmware/FFBoard/Src/Axis.cpp
index 54b2b83a0..b22ae92e8 100644
--- a/Firmware/FFBoard/Src/Axis.cpp
+++ b/Firmware/FFBoard/Src/Axis.cpp
@@ -94,7 +94,7 @@ Axis::Axis(char axis,volatile Control_t* control) :CommandHandler("axis", CLSID_
 	{
 		driverChooser = ClassChooser<MotorDriver>(axis1_drivers);
 		setInstance(0);
-		this->flashAddresses = AxisFlashAddresses({ADR_AXIS1_CONFIG, ADR_AXIS1_MAX_SPEED, ADR_AXIS1_MAX_ACCEL,
+		this->flashAddresses = AxisFlashAddresses({ADR_AXIS1_CONFIG, ADR_AXIS1_MAX_SPEED, ADR_AXIS1_MAX_ACCEL, ADR_AXIS1_MAX_SLEWRATE_DRV,
 										   ADR_AXIS1_ENDSTOP, ADR_AXIS1_POWER, ADR_AXIS1_DEGREES,ADR_AXIS1_EFFECTS1,ADR_AXIS1_EFFECTS2,ADR_AXIS1_ENC_RATIO,
 										   ADR_AXIS1_SPEEDACCEL_FILTER,ADR_AXIS1_POSTPROCESS1});
 	}
@@ -102,14 +102,14 @@ Axis::Axis(char axis,volatile Control_t* control) :CommandHandler("axis", CLSID_
 	{
 		driverChooser = ClassChooser<MotorDriver>(axis2_drivers);
 		setInstance(1);
-		this->flashAddresses = AxisFlashAddresses({ADR_AXIS2_CONFIG, ADR_AXIS2_MAX_SPEED, ADR_AXIS2_MAX_ACCEL,
+		this->flashAddresses = AxisFlashAddresses({ADR_AXIS2_CONFIG, ADR_AXIS2_MAX_SPEED, ADR_AXIS2_MAX_ACCEL, ADR_AXIS2_MAX_SLEWRATE_DRV,
 										   ADR_AXIS2_ENDSTOP, ADR_AXIS2_POWER, ADR_AXIS2_DEGREES,ADR_AXIS2_EFFECTS1,ADR_AXIS2_EFFECTS2, ADR_AXIS2_ENC_RATIO,
 										   ADR_AXIS2_SPEEDACCEL_FILTER,ADR_AXIS2_POSTPROCESS1});
 	}
 	else if (axis == 'Z')
 	{
 		setInstance(2);
-		this->flashAddresses = AxisFlashAddresses({ADR_AXIS3_CONFIG, ADR_AXIS3_MAX_SPEED, ADR_AXIS3_MAX_ACCEL,
+		this->flashAddresses = AxisFlashAddresses({ADR_AXIS3_CONFIG, ADR_AXIS3_MAX_SPEED, ADR_AXIS3_MAX_ACCEL, ADR_AXIS3_MAX_SLEWRATE_DRV,
 										   ADR_AXIS3_ENDSTOP, ADR_AXIS3_POWER, ADR_AXIS3_DEGREES,ADR_AXIS3_EFFECTS1,ADR_AXIS3_EFFECTS2,ADR_AXIS3_ENC_RATIO,
 										   ADR_AXIS3_SPEEDACCEL_FILTER,ADR_AXIS3_POSTPROCESS1});
 	}
@@ -145,7 +145,7 @@ void Axis::registerCommands(){
 	registerCommand("drvtype", Axis_commands::drvtype, "Motor driver type get/set/list",CMDFLAG_GET | CMDFLAG_SET | CMDFLAG_INFOSTRING);
 	registerCommand("pos", Axis_commands::pos, "Encoder position",CMDFLAG_GET);
 	registerCommand("maxspeed", Axis_commands::maxspeed, "Speed limit in deg/s",CMDFLAG_GET | CMDFLAG_SET);
-	registerCommand("maxtorquerate", Axis_commands::maxtorquerate, "Torque rate limit in counts/ms",CMDFLAG_GET | CMDFLAG_SET);
+	registerCommand("slewrate", Axis_commands::slewrate, "Torque rate limit in counts/ms",CMDFLAG_GET | CMDFLAG_SET);
 	registerCommand("fxratio", Axis_commands::fxratio, "Effect ratio. Reduces game effects excluding endstop. 255=100%",CMDFLAG_GET | CMDFLAG_SET);
 	registerCommand("curtorque", Axis_commands::curtorque, "Axis torque",CMDFLAG_GET);
 	registerCommand("curpos", Axis_commands::curpos, "Axis position",CMDFLAG_GET);
@@ -190,6 +190,13 @@ void Axis::restoreFlash(){
 		pulseErrLed();
 	}
 
+	// save the max torque for the slew rate
+	if (Flash_Read(flashAddresses.maxSlewRateDrv, &value)){
+		this->maxSlewRate_Driver = value;
+	}else{
+		pulseErrLed();
+	}
+
 
 	uint16_t esval, power;
 	if(Flash_Read(flashAddresses.endstop, &esval)) {
@@ -250,6 +257,7 @@ void Axis::saveFlash(){
 	Flash_Write(flashAddresses.config, Axis::encodeConfToInt(this->conf));
 	Flash_Write(flashAddresses.maxSpeed, this->maxSpeedDegS);
 	Flash_Write(flashAddresses.maxAccel, (uint16_t)(this->maxTorqueRateMS));
+	Flash_Write(flashAddresses.maxSlewRateDrv, (uint16_t)(this->maxSlewRate_Driver));
 
 	Flash_Write(flashAddresses.endstop, fx_ratio_i | (endstopStrength << 8));
 	Flash_Write(flashAddresses.power, power);
@@ -396,6 +404,7 @@ void Axis::setDrvType(uint8_t drvtype)
 		return;
 	}
 	this->conf.drvtype = drvtype;
+	this->maxTorqueRateMS = drv->getDrvSlewRate();
 
 	// Pass encoder to driver again
 	if(!this->drv->hasIntegratedEncoder()){
@@ -725,9 +734,7 @@ bool Axis::updateTorque(int32_t* totalTorque) {
 		torque -= torqueReduction;
 	}
 	// Torque slew rate limiter
-	if(maxTorqueRateMS > 0){
-		torque = clip<int32_t,int32_t>(torque, metric.previous.torque - (int32_t)maxTorqueRateMS,metric.previous.torque + (int32_t)maxTorqueRateMS);
-	}
+	applyTorqueSlewRateLimiter(*(int64_t*)&torque); // Temporary cast for logic implementation
 
 	if(outOfBounds){
 		torque = 0;
@@ -754,6 +761,22 @@ bool Axis::updateTorque(int32_t* totalTorque) {
 	return (torqueChanged);
 }
 
+void Axis::applyTorqueSlewRateLimiter(int64_t& torque)
+{
+	// Limits the rate of change of the torque (slew rate), to smooths out sudden changes in torque.
+	// Essential for a natural feel and to prevent "clanking" noises.
+	if(maxTorqueRateMS == 0) {
+		return; // Limiter is disabled
+	}
+
+	// This prevents sudden torque jumps, resulting in a smoother feel.
+	const int64_t previousTorque = metric.previous.torque;
+	const int64_t maxTorqueChange = maxTorqueRateMS;
+
+	// The torque is clipped to be within the range of [previous torque - limit, previous torque + limit].
+	torque = clip<int64_t>(torque, previousTorque - maxTorqueChange, previousTorque + maxTorqueChange);
+}
+
 void Axis::updateSamplerate(float newSamplerate){
 	this->filter_f = newSamplerate;
 	this->updateFilters(this->filterProfileId); // Recalculate filters
@@ -932,8 +955,39 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector<CommandReply>&
 		handleGetSet(cmd, replies, this->maxSpeedDegS);
 		break;
 
-	case Axis_commands::maxtorquerate:
-		handleGetSet(cmd, replies, this->maxTorqueRateMS);
+	case Axis_commands::slewrate:
+		{
+			if(cmd.type == CMDtype::get){
+				// If driver has a more restrictive calibrated value, update the axis limit
+				if(maxSlewRate_Driver < this->maxTorqueRateMS) {
+					this->maxTorqueRateMS = maxSlewRate_Driver;
+				}
+				replies.emplace_back(this->maxTorqueRateMS);
+			}else if(cmd.type == CMDtype::set){
+				this->maxTorqueRateMS = clip<uint32_t,uint32_t>(cmd.val, 0, maxSlewRate_Driver);
+			}
+		}
+		break;
+
+	case Axis_commands::calibrate_maxSlewRateDrv:
+		{
+			if(cmd.type == CMDtype::get){
+				// Start calibration on driver and set awaiting flag if start is OK
+				if (drv->startSlewRateCalibration()) {
+					this->awaitingSlewCalibration = true;
+				} else {
+					// Inform user that calibration can't started
+					CommandHandler::broadcastCommandReply(CommandReply("Slew rate calibration unsupported",1), (uint32_t)Axis_commands::calibrate_maxSlewRateDrv, CMDtype::get);
+				}
+				replies.emplace_back(1); // ack
+			}
+			break;
+		}
+
+	case Axis_commands::maxSlewRateDrv:
+		if (cmd.type == CMDtype::get) {
+			replies.emplace_back(maxSlewRate_Driver);
+		}
 		break;
 
 	case Axis_commands::fxratio:
diff --git a/Firmware/FFBoard/Src/MotorDriver.cpp b/Firmware/FFBoard/Src/MotorDriver.cpp
index b7325de9d..cd27754cc 100644
--- a/Firmware/FFBoard/Src/MotorDriver.cpp
+++ b/Firmware/FFBoard/Src/MotorDriver.cpp
@@ -81,6 +81,12 @@ const ClassIdentifier MotorDriver::getInfo(){
 	return info;
 }
 
+/**
+ * Setup driver when is selected
+ */
+void MotorDriver::setupDriver(){
+
+}
 
 /**
  * Turn the motor with positive/negative power.
diff --git a/Firmware/scripts/memory_map.csv b/Firmware/scripts/memory_map.csv
index ccaca03f4..ce1383a95 100644
--- a/Firmware/scripts/memory_map.csv
+++ b/Firmware/scripts/memory_map.csv
@@ -62,6 +62,7 @@ Section comment,Name,Value,Comment on key,VirtAddVarTab,exportableFlashAddresses
 ,ADR_AXIS1_DEGREES,0x303,,1,1
 ,ADR_AXIS1_MAX_SPEED,0x304,// Store the max speed,1,1
 ,ADR_AXIS1_MAX_ACCEL,0x305,// Store the max accel,1,1
+,ADR_AXIS1_MAX_SLEWRATE_DRV,0x306,// Max slew rate for drv,1,1
 ,ADR_AXIS1_ENDSTOP,0x307,"// 0-7 endstop margin, 8-15 endstop stiffness",1,1
 ,ADR_AXIS1_EFFECTS1,0x308,"// 0-7 idlespring, 8-15 damper",1,1
 ,ADR_AXIS1_SPEEDACCEL_FILTER,0x309,// Speed/Accel filter Lowpass profile,1,1
@@ -88,18 +89,13 @@ Section comment,Name,Value,Comment on key,VirtAddVarTab,exportableFlashAddresses
 ,ADR_AXIS2_DEGREES,0x343,,1,1
 ,ADR_AXIS2_MAX_SPEED,0x344,// Store the max speed,1,1
 ,ADR_AXIS2_MAX_ACCEL,0x345,// Store the max accel,1,1
+,ADR_AXIS2_MAX_SLEWRATE_DRV,0x346,// Max slew rate for drv,1,1
 ,ADR_AXIS2_ENDSTOP,0x347,"// 0-7 endstop margin, 8-15 endstop stiffness",1,1
 ,ADR_AXIS2_EFFECTS1,0x348,"// 0-7 idlespring, 8-15 damper",1,1
 ,ADR_AXIS2_SPEEDACCEL_FILTER,0x349,// Speed/Accel filter Lowpass profile,1,1
 ,ADR_AXIS2_ENC_RATIO,0x34A,// Store the encoder ratio for an axis,1,1
 ,ADR_AXIS2_EFFECTS2,0x34B,"// 0-7 Friction, 8-15 Inertia",1,1
 ,ADR_AXIS2_POSTPROCESS1,0x34C,// 0-7 expo curve,1,1
-,,,,,
-,,,,,
-,,,,,
-,,,,,
-,,,,,
-,,,,,
 // TMC2,,,,,
 ,ADR_TMC2_MOTCONF,0x360,// 0-2: MotType 3-5: PhiE source 6-15: Poles,1,1
 ,ADR_TMC2_CPR,0x361,,1,1
@@ -120,14 +116,13 @@ Section comment,Name,Value,Comment on key,VirtAddVarTab,exportableFlashAddresses
 ,ADR_AXIS3_DEGREES,0x383,,1,1
 ,ADR_AXIS3_MAX_SPEED,0x384,// Store the max speed,1,1
 ,ADR_AXIS3_MAX_ACCEL,0x385,// Store the max accel,1,1
+,ADR_AXIS3_MAX_SLEWRATE_DRV,0x386,// Max slew rate for drv,1,1
 ,ADR_AXIS3_ENDSTOP,0x387,"// 0-7 endstop margin, 8-15 endstop stiffness",1,1
 ,ADR_AXIS3_EFFECTS1,0x388,"// 0-7 idlespring, 8-15 damper",1,1
 ,ADR_AXIS3_SPEEDACCEL_FILTER,0x389,// Speed/Accel filter Lowpass profile,1,1
 ,ADR_AXIS3_ENC_RATIO,0x38A,// Store the encoder ratio for an axis,1,1
 ,ADR_AXIS3_EFFECTS2,0x38B,"// 0-7 Friction, 8-15 Inertia",1,1
 ,ADR_AXIS3_POSTPROCESS1,0x38C,// 0-7 expo curve,1,1
-,,,,,
-,,,,,
 // TMC3,,,,,
 ,ADR_TMC3_MOTCONF,0x3A0,// 0-2: MotType 3-5: PhiE source 6-15: Poles,1,1
 ,ADR_TMC3_CPR,0x3A1,,1,1

From daee6973cc4e5e740dd94977e19404076b2d9312 Mon Sep 17 00:00:00 2001
From: Vincent MANOUKIAN <10980775+manoukianv@users.noreply.github.com>
Date: Sat, 25 Apr 2026 10:42:08 +0200
Subject: [PATCH 3/8] feat: refactor torque calculation into a 7-step pipeline

---
 Firmware/FFBoard/Src/Axis.cpp | 288 ++++++++++++++++++++--------------
 1 file changed, 173 insertions(+), 115 deletions(-)

diff --git a/Firmware/FFBoard/Src/Axis.cpp b/Firmware/FFBoard/Src/Axis.cpp
index b22ae92e8..73708df60 100644
--- a/Firmware/FFBoard/Src/Axis.cpp
+++ b/Firmware/FFBoard/Src/Axis.cpp
@@ -2,17 +2,32 @@
  * Axis.cpp
  *
  *  Created on: 31.01.2020
- *      Author: Yannick
+ *      Author: Yannick, Vincent
+ * 		
  */
 
 #include "Axis.h"
 #include "voltagesense.h"
+
+// Load the driver is they are declared in targer_constants.h
+#ifdef TMC4671DRIVER
 #include "TMC4671.h"
+#endif
+#ifdef PWMDRIVER
 #include "MotorPWM.h"
-#include "VescCAN.h"
+#endif
+#ifdef ODRIVE
 #include "ODriveCAN.h"
+#endif
+#ifdef VESC
+#include "VescCAN.h"
+#endif
+#ifdef SIMPLEMOTION
 #include "MotorSimplemotion.h"
+#endif
+#ifdef RMDCAN
 #include "RmdMotorCAN.h"
+#endif
 #include "critical.hpp"
 
 //////////////////////////////////////////////
@@ -109,16 +124,14 @@ Axis::Axis(char axis,volatile Control_t* control) :CommandHandler("axis", CLSID_
 	else if (axis == 'Z')
 	{
 		setInstance(2);
-		this->flashAddresses = AxisFlashAddresses({ADR_AXIS3_CONFIG, ADR_AXIS3_MAX_SPEED, ADR_AXIS3_MAX_ACCEL, ADR_AXIS3_MAX_SLEWRATE_DRV,
+	this->flashAddresses = AxisFlashAddresses({ADR_AXIS3_CONFIG, ADR_AXIS3_MAX_SPEED, ADR_AXIS3_MAX_ACCEL, ADR_AXIS3_MAX_SLEWRATE_DRV,
 										   ADR_AXIS3_ENDSTOP, ADR_AXIS3_POWER, ADR_AXIS3_DEGREES,ADR_AXIS3_EFFECTS1,ADR_AXIS3_EFFECTS2,ADR_AXIS3_ENC_RATIO,
 										   ADR_AXIS3_SPEEDACCEL_FILTER,ADR_AXIS3_POSTPROCESS1});
 	}
 
-
-	restoreFlash(); // Load parameters
 	CommandHandler::registerCommands(); // Internal commands
 	registerCommands();
-	updateTorqueScaler(); // In case no flash setting has been loaded yet
+	restoreFlash(); // Load parameters
 }
 
 Axis::~Axis()
@@ -184,6 +197,7 @@ void Axis::restoreFlash(){
 		pulseErrLed();
 	}
 
+	// save the max torque for the slew rate
 	if (Flash_Read(flashAddresses.maxAccel, &value)){
 		this->maxTorqueRateMS = value;
 	}else{
@@ -198,20 +212,20 @@ void Axis::restoreFlash(){
 	}
 
 
-	uint16_t esval, power;
-	if(Flash_Read(flashAddresses.endstop, &esval)) {
-		fx_ratio_i = esval & 0xff;
-		endstopStrength = (esval >> 8) & 0xff;
+	uint16_t endstopRawValue, power;
+	if(Flash_Read(flashAddresses.endstop, &endstopRawValue)) {
+		setEffectRatio(endstopRawValue & 0xff);
+		endstopStrength = (endstopRawValue >> 8) & 0xff;
 	}
 
 
 	if(Flash_Read(flashAddresses.power, &power)){
 		setPower(power);
 	}
-	uint16_t deg_t;
-	if(Flash_Read(flashAddresses.degrees, &deg_t)){
-		this->degreesOfRotation = deg_t & 0x7fff;
-		this->invertAxis = (deg_t >> 15) & 0x1;
+	uint16_t degreesRawValue;
+	if(Flash_Read(flashAddresses.degrees, &degreesRawValue)){
+		this->degreesOfRotation = degreesRawValue & 0x7fff;
+		this->invertAxis = (degreesRawValue >> 15) & 0x1;
 		setDegrees(degreesOfRotation);
 	}
 
@@ -221,7 +235,7 @@ void Axis::restoreFlash(){
 		setIdleSpringStrength(effects & 0xff);
 		setFxStrengthAndFilter((effects >> 8) & 0xff,damperIntensity,damperFilter);
 	}else{
-		setIdleSpringStrength(idlespringstrength); // Use default
+		setIdleSpringStrength(idleSpringStrength); // Use default
 	}
 
 	if(Flash_Read(flashAddresses.effects2, &effects)){
@@ -259,10 +273,10 @@ void Axis::saveFlash(){
 	Flash_Write(flashAddresses.maxAccel, (uint16_t)(this->maxTorqueRateMS));
 	Flash_Write(flashAddresses.maxSlewRateDrv, (uint16_t)(this->maxSlewRate_Driver));
 
-	Flash_Write(flashAddresses.endstop, fx_ratio_i | (endstopStrength << 8));
+	Flash_Write(flashAddresses.endstop, effectRatio | (endstopStrength << 8));
 	Flash_Write(flashAddresses.power, power);
 	Flash_Write(flashAddresses.degrees, (degreesOfRotation & 0x7fff) | (invertAxis << 15));
-	Flash_Write(flashAddresses.effects1, idlespringstrength | (damperIntensity << 8));
+	Flash_Write(flashAddresses.effects1, idleSpringStrength | (damperIntensity << 8));
 	Flash_Write(flashAddresses.effects2, frictionIntensity | (inertiaIntensity << 8));
 	Flash_Write(flashAddresses.encoderRatio, gearRatio.numerator | (gearRatio.denominator << 8));
 
@@ -271,7 +285,7 @@ void Axis::saveFlash(){
 	Flash_Write(flashAddresses.speedAccelFilter, filterStorage);
 
 	// Postprocessing
-	Flash_Write(flashAddresses.postprocess1, expoValInt & 0xff);
+	Flash_Write(flashAddresses.postprocess1, expoValue & 0xff);
 
 }
 
@@ -315,6 +329,7 @@ void Axis::prepareForUpdate(){
 		return;
 	}
 
+	// TODO: The motorReady() check was commented out. Review if this is still the desired behavior or if it should be restored.
 	//if (!drv->motorReady()) return;
 
 	float angle = getEncAngle(getEncoder());
@@ -347,6 +362,7 @@ void Axis::prepareForUpdate(){
 
 	}else if(abs(scaledEnc) <= 0x7fff) {
 		outOfBounds = false;
+		// TODO: This error clearing seems to have been moved to the errorCallback. Confirm this is correct and remove this line.
 		//ErrorHandler::clearError(outOfBoundsError);
 	}
 
@@ -358,6 +374,7 @@ void Axis::prepareForUpdate(){
 
 
 	this->updateMetrics(angle);
+	//this->updateHandsOffState();
 
 }
 
@@ -382,7 +399,11 @@ void Axis::updateDriveTorque(){
 void Axis::setPower(uint16_t power)
 {
 	this->power = power;
-	updateTorqueScaler();
+	torqueScaler = ((float)power / (float)0x7fff);
+	if (drv != nullptr)
+	{
+		drv->setPowerLimit(power);
+	}
 }
 
 
@@ -411,6 +432,7 @@ void Axis::setDrvType(uint8_t drvtype)
 		this->drv->setEncoder(this->enc);
 	}
 
+	drv->setupDriver();
 	if (!tud_connected())
 	{
 		control->usb_disabled = false;
@@ -539,16 +561,16 @@ int32_t Axis::getLastScaledEnc() {
  * Changes intensity of idle spring when FFB is off
  */
 int32_t Axis::updateIdleSpringForce() {
-	return clip<int32_t,int32_t>((int32_t)(-metric.current.pos_scaled_16b*idlespringscale),-idlespringclip,idlespringclip);
+	return clip<int32_t,int32_t>((int32_t)(-metric.current.pos_scaled_16b*idleSpringScale),-idleSpringClip,idleSpringClip);
 }
 
 /*
  * Set the strength of the spring effect if FFB is disabled
  */
 void Axis::setIdleSpringStrength(uint8_t spring){
-	idlespringstrength = spring;
-	idlespringclip = clip<int32_t,int32_t>((int32_t)spring*35,0,10000);
-	idlespringscale = 0.5f + ((float)spring * 0.01f);
+	idleSpringStrength = spring;
+	idleSpringClip = clip<int32_t,int32_t>((int32_t)spring*35,0,10000);
+	idleSpringScale = 0.5f + ((float)spring * 0.01f);
 }
 
 /**
@@ -562,26 +584,27 @@ void Axis::setFxStrengthAndFilter(uint8_t val,uint8_t& valToSet, Biquad& filter)
 }
 
 /**
- * Called before HID effects are calculated
- * Should calculate always on and idle effects specific to the axis like idlespring and friction
+ * Calculates the internal mechanical effects (damper, friction, inertia) that are always active.
+ * Called before HID effects are calculated.
+ * Also calculates idle spring when FFB is inactive.
  */
-void Axis::calculateAxisEffects(bool ffb_on){
-	axisEffectTorque = 0;
+void Axis::calculateMechanicalEffects(bool ffb_on){
+	mechanicalEffectTorque = 0;
 
 	if(!ffb_on){
-		axisEffectTorque += updateIdleSpringForce();
+		mechanicalEffectTorque += updateIdleSpringForce();
 	}
 
 	// Always active damper
 	if(damperIntensity != 0){
 		float speedFiltered = (metric.current.speed) * (float)damperIntensity * AXIS_DAMPER_RATIO;
-		axisEffectTorque -= damperFilter.process(clip<float, int32_t>(speedFiltered, -intFxClip, intFxClip));
+		mechanicalEffectTorque -= damperFilter.process(clip<float, int32_t>(speedFiltered, -internalFxClip, internalFxClip));
 	}
 
 	// Always active inertia
 	if(inertiaIntensity != 0){
 		float accelFiltered = metric.current.accel * (float)inertiaIntensity * AXIS_INERTIA_RATIO;
-		axisEffectTorque -= inertiaFilter.process(clip<float, int32_t>(accelFiltered, -intFxClip, intFxClip));
+		mechanicalEffectTorque -= inertiaFilter.process(clip<float, int32_t>(accelFiltered, -internalFxClip, internalFxClip));
 	}
 
 	// Always active friction. Based on effectsCalculator implementation
@@ -590,12 +613,12 @@ void Axis::calculateAxisEffects(bool ffb_on){
 		float speedRampupCeil = 4096;
 		float rampupFactor = 1.0;
 		if (fabs (speed) < speedRampupCeil) {								// if speed in the range to rampup we apply a sine curve
-			float phaseRad = M_PI * ((fabs (speed) / speedRampupCeil) - 0.5);// we start to compute the normalized angle (speed / normalizedSpeed@5%) and translate it of -1/2PI to translate sin on 1/2 periode
-			rampupFactor = ( 1 + sin(phaseRad ) ) / 2;						// sin value is -1..1 range, we translate it to 0..2 and we scale it by 2
+			float phaseRad = M_PI * ((fabsf (speed) / speedRampupCeil) - 0.5f);// we start to compute the normalized angle (speed / normalizedSpeed@5%) and translate it of -1/2PI to translate sin on 1/2 periode
+			rampupFactor = ( 1.0f + sinf(phaseRad ) ) / 2.0f;			// sin value is -1..1 range, we translate it to 0..2 and we scale it by 2
 		}
 		int8_t sign = speed >= 0 ? 1 : -1;
 		float force = (float)frictionIntensity * rampupFactor * sign * INTERNAL_AXIS_FRICTION_SCALER * 32;
-		axisEffectTorque -= frictionFilter.process(clip<float, int32_t>(force, -intFxClip, intFxClip));
+		mechanicalEffectTorque -= frictionFilter.process(clip<float, int32_t>(force, -internalFxClip, internalFxClip));
 	}
 
 }
@@ -603,9 +626,9 @@ void Axis::calculateAxisEffects(bool ffb_on){
 /**
  * Changes the ratio of effects to endstop strength. 255 = same strength, 0 = no effects
  */
-void Axis::setFxRatio(uint8_t val) {
-	fx_ratio_i = val;
-	updateTorqueScaler();
+void Axis::setEffectRatio(uint8_t val) {
+	effectRatio = val;
+	effectRatioScaler = ((float)effectRatio/255.0);
 }
 
 /**
@@ -635,8 +658,8 @@ void Axis::updateMetrics(float new_pos) { // pos is degrees
 	// compute speed and accel from raw instant speed normalized
 	float currentSpeed = (new_pos - metric.previous.posDegrees) * this->filter_f; // deg/s
 	metric.current.speed = speedFilter.process(currentSpeed);
-	metric.current.accel = accelFilter.process((currentSpeed - _lastSpeed))* this->filter_f; // deg/s/s
-	_lastSpeed = currentSpeed;
+	metric.current.accel = accelFilter.process((currentSpeed - previousFrameSpeed))* this->filter_f; // deg/s/s
+	previousFrameSpeed = currentSpeed;
 
 }
 
@@ -647,7 +670,7 @@ uint16_t Axis::getPower(){
 }
 
 /**
- * Calculates an exponential torque correction curve
+ * Calculates an exponential torque correction curve and scale for FFBEffect
  */
 int32_t Axis::calculateExpoTorque(int32_t torque){
 	float torquef = (float)torque / (float)0x7fff; // This down and upscaling may introduce float artifacts. Do this before scaling down.
@@ -658,17 +681,29 @@ int32_t Axis::calculateExpoTorque(int32_t torque){
 	}
 }
 
-void  Axis::updateTorqueScaler() {
-	effect_margin_scaler = ((float)fx_ratio_i/255.0);
-	torqueScaler = ((float)power / (float)0x7fff);
-}
+int64_t Axis::calculateFFBTorque() {
 
-float Axis::getTorqueScaler(){
-	return torqueScaler;
-}
+	int64_t torque = this->ffbEffectTorque;
 
+	// 1. Game Clipping detection
+	// If the game sends more than the theoretical maximum (+/- 32767), the signal is clipped at the source.
+	if(abs(torque) >= 0x7fff){
+		pulseClipLed(); // Visual alert: game signal is clipping
+	}
 
-int32_t Axis::getTorque() { return metric.previous.torque; }
+	// 2. Apply Expo (Linearization or sensation curve)
+	if(expo != 1){
+		torque = calculateExpoTorque(torque);
+	}
+
+	// 3. Game specific gain (effectRatioScaler)
+	// Scale the FFB from game only (allows lowering game effects without lowering endstops)
+	torque = (int64_t)((float)torque * effectRatioScaler);
+
+	return torque;
+}
+
+int32_t Axis::getTorque() { return metric.current.torque; }
 
 bool Axis::isInverted() {
 	return invertAxis;
@@ -677,20 +712,21 @@ bool Axis::isInverted() {
 /**
  * Calculate soft endstop effect
  */
-int16_t Axis::updateEndstop(){
-	int8_t clipdir = cliptest<int32_t,int32_t>(metric.current.pos_scaled_16b, -0x7fff, 0x7fff);
-	if(clipdir == 0){
+int32_t Axis::calculateEndstopTorque(){
+	// TODO Check the type int8_t and the range clipping is -0x7fff..0x7fff
+	int8_t clipDirection = cliptest<int32_t,int32_t>(metric.current.pos_scaled_16b, -0x7fff, 0x7fff);
+	if(clipDirection == 0){
 		return 0;
 	}
-	float addtorque = clipdir*metric.current.posDegrees - (float)this->degreesOfRotation/2.0; // degress of rotation counts total range so multiply by 2
-	addtorque *= (float)endstopStrength * endstopGain; // Apply endstop gain for stiffness.
-	addtorque *= -clipdir;
+	float endstopTorque = clipDirection*metric.current.posDegrees - (float)this->degreesOfRotation/2.0; // degress of rotation counts total range so multiply by 2
+	endstopTorque *= (float)endstopStrength * endstopGain; // Apply endstop gain for stiffness.
+	endstopTorque *= -clipDirection;
 
-	return clip<int32_t,int32_t>(addtorque,-0x7fff,0x7fff);
+	return clip<int32_t,int32_t>(endstopTorque,-0x7fff,0x7fff);
 }
 
-void Axis::setEffectTorque(int32_t torque) {
-	effectTorque = torque;
+void Axis::setFfbEffectTorque(int64_t torque) {
+	this->ffbEffectTorque = torque;
 }
 
 /** pass in ptr to receive the sum of the effects + endstop torque
@@ -699,68 +735,62 @@ void Axis::setEffectTorque(int32_t torque) {
 
 bool Axis::updateTorque(int32_t* totalTorque) {
 
-	if(abs(effectTorque) >= 0x7fff){
-		pulseClipLed();
-	}
-
-	// Scale effect torque
-	int32_t torque = effectTorque; // Game effects
-	if(expo != 1){
-		torque = calculateExpoTorque(torque);
-	}
-	torque *= effect_margin_scaler;
-	torque += axisEffectTorque; // Independent effects
-	torque += updateEndstop();
-	torque *= torqueScaler; // Scale to power
-
-
-	// TODO speed and accel limiters
-	if(maxSpeedDegS > 0){
-
-		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)((metric.current.speed*torqueSign) - (float)maxSpeedDegS) *  ((float)0x7FFF / maxSpeedDegS);
-		spdlimitreducerI = clip<float,int32_t>( spdlimitreducerI + ((speedreducer * speedLimiterI) * torqueScaler),0,power);
-
-		// Only reduce torque. Don't invert it to prevent oscillation
-		float torqueReduction = speedreducer * speedLimiterP + spdlimitreducerI;// accreducer * 0.025 + acclimitreducerI
-		if(torque > 0){
-			torqueReduction = clip<float,int32_t>(torqueReduction,0,torque);
-		}else{
-			torqueReduction = clip<float,int32_t>(-torqueReduction,torque,0);
-		}
-
-		torque -= torqueReduction;
-	}
-	// Torque slew rate limiter
-	applyTorqueSlewRateLimiter(*(int64_t*)&torque); // Temporary cast for logic implementation
-
+	// STEP 1: Process FFB torque from the game (via helper function)
+	// (Reconstructed by CMSIS Spline, Expo applied, and scaled by FFB ratio)
+	int64_t torque = calculateFFBTorque();
+
+	// STEP 2: Mix in local mechanical effects
+	// (Damper, Friction, Inertia generated locally at high frequency)
+	torque += mechanicalEffectTorque;
+	
+	// STEP 3: Add endstops
+	// Note: Historically endstops are added before the Master Scaler.
+	// If hard endstops (like Simucube) are desired, this should be moved to STEP 5.
+	torque += calculateEndstopTorque();
+
+	// STEP 4: Master Volume Scaling
+	// Map the virtual signal (+/- 32767) to the physical power limit ("power")
+	torque = (int64_t)((float)torque * torqueScaler);
+
+	// STEP 5: Safety limits (Fade-in, Out of bounds)
+	// Applied BEFORE dynamic limiters so that abrupt cuts are smoothed by the Slew Rate.
 	if(outOfBounds){
 		torque = 0;
 	}
 
-	// Fade in
-	if(forceFadeCurMult < 1){
-		torque = torque * forceFadeCurMult;
-		forceFadeCurMult += forceFadeTime / this->filter_f; // Fade time
+	// Apply a fade-in effect for a smooth force ramp-up on startup or recovery.
+	// Increases forceFadeMultiplier progressively based on forceFadeDuration and sample rate.
+	if(forceFadeMultiplier < 1.0f){
+		torque = (int64_t)((float)torque * forceFadeMultiplier);
+		forceFadeMultiplier += forceFadeDuration / this->filter_f;
 	}
 
-	// Torque calculated. Now sending to driver
-	torque = (invertAxis) ? -torque : torque;
-	metric.current.torque = torque;
-	torque = clip<int32_t, int32_t>(torque, -power, power);
+	// STEP 6: Dynamic limiters (Speed & Slew Rate)
+	// CRITICAL: Slew Rate compares the target value with "metric.previous.torque"
+	// (which is the clipped physical torque from the previous cycle).
+	// It MUST be applied on the final scaled torque!
+	torque -= applySpeedLimiterTorque(torque);
+	applyTorqueSlewRateLimiter(torque);
 
-	bool torqueChanged = metric.current.torque != metric.previous.torque;
+	// STEP 7: Axis inversion and final hardware clipping
+	torque = (invertAxis) ? -torque : torque;
 
-	if (abs(torque) == power){
-		pulseClipLed();
+	int32_t torqueAfterClipping = clip<int32_t, int32_t>((int32_t)torque, -power, power);
+	
+	if (torqueAfterClipping != torque){
+		pulseClipLed(); // Visual alert: MOTOR cannot provide requested power (Hardware clipping)
 	}
 
-	*totalTorque = torque;
-	return (torqueChanged);
+	// Store the actually applied torque for the next iteration (used by the slew rate limiter).
+	metric.current.torque = torqueAfterClipping; 
+	
+	// return result
+	*totalTorque = torqueAfterClipping;
+	
+	return (metric.current.torque != metric.previous.torque);
 }
 
+
 void Axis::applyTorqueSlewRateLimiter(int64_t& torque)
 {
 	// Limits the rate of change of the torque (slew rate), to smooths out sudden changes in torque.
@@ -777,6 +807,34 @@ void Axis::applyTorqueSlewRateLimiter(int64_t& torque)
 	torque = clip<int64_t>(torque, previousTorque - maxTorqueChange, previousTorque + maxTorqueChange);
 }
 
+int64_t Axis::applySpeedLimiterTorque(int64_t& torque){
+	// Speed Limiter: A PI controller to reduce torque when speed exceeds maxSpeedDegS.
+	// The limiter only acts when torque is applied in the direction of movement.
+
+	// if limiter is disabled, return
+	if(maxSpeedDegS <= 0) {
+		return 0;
+	}
+
+	int64_t resultTorque = 0;
+
+	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)((metric.current.speed*torqueSign) - (float)maxSpeedDegS) *  ((float)0x7FFF / maxSpeedDegS);
+	speedLimitReducerI = clip<float,int32_t>( speedLimitReducerI + ((speedreducer * speedLimiterI) * torqueScaler),0,power);
+
+	// Only reduce torque. Don't invert it to prevent oscillation
+	float torqueReduction = speedreducer * speedLimiterP + speedLimitReducerI;// accreducer * 0.025 + acclimitreducerI
+	if(torque > 0){
+		resultTorque = clip<float,int64_t>(torqueReduction,0,torque);
+	}else{
+		resultTorque = clip<float,int64_t>(-torqueReduction,torque,0);
+	}
+
+	return resultTorque;
+}
+
 void Axis::updateSamplerate(float newSamplerate){
 	this->filter_f = newSamplerate;
 	this->updateFilters(this->filterProfileId); // Recalculate filters
@@ -797,8 +855,8 @@ void Axis::updateFilters(uint8_t profileId){
  * Starts fading in force from start to 1 over fadeTime
  */
 void Axis::startForceFadeIn(float start,float fadeTime){
-	this->forceFadeTime = fadeTime;
-	this->forceFadeCurMult = clip<float>(start, 0, 1);
+	this->forceFadeDuration = fadeTime;
+	this->forceFadeMultiplier = clip<float>(start, 0, 1);
 }
 
 
@@ -809,9 +867,9 @@ void Axis::setDegrees(uint16_t degrees){
 
 	degrees &= 0x7fff;
 	if(degrees == 0){
-		nextDegreesOfRotation = lastdegreesOfRotation;
+		nextDegreesOfRotation = previousDegreesOfRotation;
 	}else{
-		lastdegreesOfRotation = degreesOfRotation;
+		previousDegreesOfRotation = degreesOfRotation;
 		nextDegreesOfRotation = degrees;
 	}
 }
@@ -819,7 +877,7 @@ void Axis::setDegrees(uint16_t degrees){
 
 void Axis::setExpo(int val){
 	val = clip(val, -127, 127);
-	expoValInt = val;
+	expoValue = val;
 	if(val == 0){
 		expo = 1; // Explicitly force expo off
 		return;
@@ -874,7 +932,7 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector<CommandReply>&
 	case Axis_commands::idlespring:
 		if (cmd.type == CMDtype::get)
 		{
-			replies.emplace_back(idlespringstrength);
+			replies.emplace_back(idleSpringStrength);
 		}
 		else if (cmd.type == CMDtype::set)
 		{
@@ -992,9 +1050,9 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector<CommandReply>&
 
 	case Axis_commands::fxratio:
 		if(cmd.type == CMDtype::get){
-			replies.emplace_back(this->fx_ratio_i);
+			replies.emplace_back(this->effectRatio);
 		}else if(cmd.type == CMDtype::set){
-			setFxRatio(cmd.val);
+			setEffectRatio(cmd.val);
 		}
 		break;
 
@@ -1058,7 +1116,7 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector<CommandReply>&
 		break;
 
 	case Axis_commands::expo:
-		handleGetSetFunc(cmd, replies, expoValInt, &Axis::setExpo, this); // need to also provide the expoScaler constant
+		handleGetSetFunc(cmd, replies, expoValue, &Axis::setExpo, this); // need to also provide the expoScaler constant
 		break;
 
 	case Axis_commands::exposcale:

From a8dba6b06592430217389d95e394af339d99b4e8 Mon Sep 17 00:00:00 2001
From: Vincent MANOUKIAN <10980775+manoukianv@users.noreply.github.com>
Date: Sat, 25 Apr 2026 10:43:32 +0200
Subject: [PATCH 4/8] feat: add ARM CMSIS-DSP library infrastructure and
 optimized filters

---
 .github/workflows/build-firmware.yml    |   2 +
 .gitmodules                             |   3 +
 Firmware/FFBoard/Inc/Filters.h          |  23 +-
 Firmware/FFBoard/Inc/ffb_defs.h         |   1 +
 Firmware/FFBoard/Src/Filters.cpp        | 121 +++++++--
 Firmware/Libraries/CMSIS-DSP            |   1 +
 Firmware/Makefile                       |  24 +-
 Firmware/Targets/F407VG/.cproject       | 316 ++++++++++++++++++++++--
 Firmware/Targets/F407VG/.project        |   5 +
 Firmware/Targets/F407VG_DISCO/.cproject |  18 +-
 Firmware/Targets/F411RE/.cproject       |   4 +
 11 files changed, 463 insertions(+), 55 deletions(-)
 create mode 160000 Firmware/Libraries/CMSIS-DSP

diff --git a/.github/workflows/build-firmware.yml b/.github/workflows/build-firmware.yml
index d3e3377b1..e31bb5ccc 100644
--- a/.github/workflows/build-firmware.yml
+++ b/.github/workflows/build-firmware.yml
@@ -32,6 +32,8 @@ jobs:
     steps:
       # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it
       - uses: actions/checkout@v4
+        with:
+          submodules: 'recursive'
       
       - {uses: ./.github/actions/build-firmware, with: {target: '${{ matrix.target }}', path: 'Output'}}
 
diff --git a/.gitmodules b/.gitmodules
index ed7c594be..313b67a9d 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -6,3 +6,6 @@
 	path = Hardware
 	url = https://github.com/Ultrawipf/OpenFFBoard-hardware.git
 	branch = master
+[submodule "Firmware/Libraries/CMSIS-DSP"]
+	path = Firmware/Libraries/CMSIS-DSP
+	url = https://github.com/ARM-software/CMSIS-DSP.git
diff --git a/Firmware/FFBoard/Inc/Filters.h b/Firmware/FFBoard/Inc/Filters.h
index 83a31bf17..92c2a7959 100644
--- a/Firmware/FFBoard/Inc/Filters.h
+++ b/Firmware/FFBoard/Inc/Filters.h
@@ -2,13 +2,17 @@
  * Filters.h
  *
  *  Created on: Feb 13, 2020
- *      Author: Yannick
+ *      Author: Yannick, Vincent
  */
 
 #ifndef FILTERS_H_
 #define FILTERS_H_
 #include "cppmain.h"
 
+#ifdef USE_DSP_FUNCTIONS
+#include "arm_math.h"
+#endif
+
 #ifdef __cplusplus
 
 // Frequency in hz, q in float q*100. Example: Q 0.5 -> 50
@@ -40,14 +44,27 @@ class Biquad{
     float getFc() const;
     void setQ(float Q);
     float getQ() const;
+    void setPeakGain(float peakGainDB);
     void calcBiquad(void);
 
-protected:
+#ifdef USE_DSP_FUNCTIONS
+    const float* getCoeffs() const { return pCoeffs; }
+#endif
 
+protected:
     BiquadType type;
-    float a0, a1, a2, b1, b2;
     float Fc, Q, peakGain;
+
+#ifdef USE_DSP_FUNCTIONS
+    // CMSIS-DSP instance
+    arm_biquad_casd_df1_inst_f32 S;
+    float32_t pCoeffs[5];
+    float32_t pState[4]; // For a single biquad stage (DF1 float requires 4 states)
+#else
     float z1, z2;
+    float a0, a1, a2, b1, b2;
+#endif
+
 };
 
 
diff --git a/Firmware/FFBoard/Inc/ffb_defs.h b/Firmware/FFBoard/Inc/ffb_defs.h
index f6f0a8eef..aa58dd978 100644
--- a/Firmware/FFBoard/Inc/ffb_defs.h
+++ b/Firmware/FFBoard/Inc/ffb_defs.h
@@ -11,6 +11,7 @@
 #include "cppmain.h"
 #include "Filters.h"
 #include "constants.h" // For #define MAX_AXIS
+
 #define FFB_ID_OFFSET 0x00
 #define MAX_EFFECTS 40
 
diff --git a/Firmware/FFBoard/Src/Filters.cpp b/Firmware/FFBoard/Src/Filters.cpp
index a9c59330a..1eb2644d6 100644
--- a/Firmware/FFBoard/Src/Filters.cpp
+++ b/Firmware/FFBoard/Src/Filters.cpp
@@ -2,20 +2,33 @@
  * Filters.cpp
  *
  *  Created on: Feb 13, 2020
- *      Author: Yannick
+ *      Author: Yannick, Vincent
  *
  *  Based on http://www.earlevel.com/main/2012/11/26/biquad-c-source-code/
  */
 
 #include "Filters.h"
-
 #include <math.h>
 
+#ifdef USE_DSP_FUNCTIONS
+#include "arm_math.h"
+#endif
 
 Biquad::Biquad(){
-	z1 = z2 = 0.0;
+#ifdef USE_DSP_FUNCTIONS
+    // Clear state
+    memset(pState, 0, sizeof(pState));
+	// Initialize the CMSIS-DSP biquad instance
+    arm_biquad_cascade_df1_init_f32(&S, 1, pCoeffs, pState);
+#else
+    z1 = 0.0f;
+    z2 = 0.0f; 
+#endif
 }
 Biquad::Biquad(BiquadType type, float Fc, float Q, float peakGainDB) {
+#ifdef USE_DSP_FUNCTIONS
+    arm_biquad_cascade_df1_init_f32(&S, 1, pCoeffs, pState);
+#endif
     setBiquad(type, Fc, Q, peakGainDB);
 }
 
@@ -49,13 +62,23 @@ float Biquad::getQ() const {
 	return this->Q;
 }
 
+void Biquad::setPeakGain(float peakGainDB) {
+    this->peakGain = peakGainDB;
+    calcBiquad();
+}
+
 /**
  * Calculates one step of the filter and returns the output
  */
 float Biquad::process(float in) {
-	float out = in * a0 + z1;
+    float out;
+#ifdef USE_DSP_FUNCTIONS
+    arm_biquad_cascade_df1_f32(&S, &in, &out, 1);
+#else
+	out = in * a0 + z1;
     z1 = in * a1 + z2 - b1 * out;
     z2 = in * a2 - b2 * out;
+#endif
     return out;
 }
 
@@ -72,11 +95,18 @@ void Biquad::setBiquad(BiquadType type, float Fc, float Q, float peakGainDB) {
  * Updates parameters and resets the biquad filter
  */
 void Biquad::calcBiquad(void) {
+    float norm;
+    float K, V;
+#ifdef USE_DSP_FUNCTIONS
+    float a0 = 0.0f, a1 = 0.0f, a2 = 0.0f, b1 = 0.0f, b2 = 0.0f;
+    V = powf(10, fabsf(peakGain) / 20.0f);
+    K = arm_sin_f32(PI * Fc) / arm_cos_f32(PI * Fc);
+#else
 	z1 = 0.0;
 	z2 = 0.0;
-    float norm;
-    float V = pow(10, fabs(peakGain) / 20.0);
-    float K = tan(M_PI * Fc);
+    V = pow(10, fabs(peakGain) / 20.0);
+    K = tan(M_PI * Fc);
+#endif
     switch (this->type) {
         case BiquadType::lowpass:
             norm = 1 / (1 + K / Q + K * K);
@@ -134,41 +164,84 @@ void Biquad::calcBiquad(void) {
             break;
         case BiquadType::lowshelf:
             if (peakGain >= 0) {    // boost
-                norm = 1 / (1 + sqrt(2) * K + K * K);
-                a0 = (1 + sqrt(2*V) * K + V * K * K) * norm;
+				float sqrt2, sqrt2V;
+#ifdef USE_DSP_FUNCTIONS
+				arm_sqrt_f32(2, &sqrt2);
+				arm_sqrt_f32(2*V, &sqrt2V);
+#else
+                sqrt2 = sqrt(2);
+                sqrt2V = sqrt(2*V);
+#endif
+                norm = 1 / (1 + sqrt2 * K + K * K);
+                a0 = (1 + sqrt2V * K + V * K * K) * norm;
                 a1 = 2 * (V * K * K - 1) * norm;
-                a2 = (1 - sqrt(2*V) * K + V * K * K) * norm;
+                a2 = (1 - sqrt2V * K + V * K * K) * norm;
                 b1 = 2 * (K * K - 1) * norm;
-                b2 = (1 - sqrt(2) * K + K * K) * norm;
+                b2 = (1 - sqrt2 * K + K * K) * norm;
             }
             else {    // cut
-                norm = 1 / (1 + sqrt(2*V) * K + V * K * K);
-                a0 = (1 + sqrt(2) * K + K * K) * norm;
+				float sqrt2, sqrt2V;
+#ifdef USE_DSP_FUNCTIONS
+				arm_sqrt_f32(2, &sqrt2);
+				arm_sqrt_f32(2*V, &sqrt2V);
+#else
+                sqrt2 = sqrt(2);
+                sqrt2V = sqrt(2*V);
+#endif
+                norm = 1 / (1 + sqrt2V * K + V * K * K);
+                a0 = (1 + sqrt2 * K + K * K) * norm;
                 a1 = 2 * (K * K - 1) * norm;
-                a2 = (1 - sqrt(2) * K + K * K) * norm;
+                a2 = (1 - sqrt2 * K + K * K) * norm;
                 b1 = 2 * (V * K * K - 1) * norm;
-                b2 = (1 - sqrt(2*V) * K + V * K * K) * norm;
+                b2 = (1 - sqrt2V * K + V * K * K) * norm;
             }
             break;
         case BiquadType::highshelf:
             if (peakGain >= 0) {    // boost
-                norm = 1 / (1 + sqrt(2) * K + K * K);
-                a0 = (V + sqrt(2*V) * K + K * K) * norm;
+				float sqrt2, sqrt2V;
+#ifdef USE_DSP_FUNCTIONS
+				arm_sqrt_f32(2, &sqrt2);
+				arm_sqrt_f32(2*V, &sqrt2V);
+#else
+                sqrt2 = sqrt(2);
+                sqrt2V = sqrt(2*V);
+#endif
+                norm = 1 / (1 + sqrt2 * K + K * K);
+                a0 = (V + sqrt2V * K + K * K) * norm;
                 a1 = 2 * (K * K - V) * norm;
-                a2 = (V - sqrt(2*V) * K + K * K) * norm;
+                a2 = (V - sqrt2V * K + K * K) * norm;
                 b1 = 2 * (K * K - 1) * norm;
-                b2 = (1 - sqrt(2) * K + K * K) * norm;
+                b2 = (1 - sqrt2 * K + K * K) * norm;
             }
             else {    // cut
-                norm = 1 / (V + sqrt(2*V) * K + K * K);
-                a0 = (1 + sqrt(2) * K + K * K) * norm;
+				float sqrt2, sqrt2V;
+#ifdef USE_DSP_FUNCTIONS
+				arm_sqrt_f32(2, &sqrt2);
+				arm_sqrt_f32(2*V, &sqrt2V);
+#else
+                sqrt2 = sqrt(2);
+                sqrt2V = sqrt(2*V);
+#endif
+                norm = 1 / (V + sqrt2V * K + K * K);
+                a0 = (1 + sqrt2 * K + K * K) * norm;
                 a1 = 2 * (K * K - 1) * norm;
-                a2 = (1 - sqrt(2) * K + K * K) * norm;
+                a2 = (1 - sqrt2 * K + K * K) * norm;
                 b1 = 2 * (K * K - V) * norm;
-                b2 = (V - sqrt(2*V) * K + K * K) * norm;
+                b2 = (V - sqrt2V * K + K * K) * norm;
             }
             break;
     }
 
-    return;
+#ifdef USE_DSP_FUNCTIONS
+    // Store coefficients in the format required by CMSIS-DSP: {b0, b1, b2, -a1, -a2}
+    // Note the negated feedback coefficients a1 and a2.
+    pCoeffs[0] = a0;
+    pCoeffs[1] = a1;
+    pCoeffs[2] = a2;
+    pCoeffs[3] = -b1;
+    pCoeffs[4] = -b2;
+
+	// Reset state
+	memset(pState, 0, sizeof(pState));
+#endif
 }
diff --git a/Firmware/Libraries/CMSIS-DSP b/Firmware/Libraries/CMSIS-DSP
new file mode 160000
index 000000000..1ba19d14a
--- /dev/null
+++ b/Firmware/Libraries/CMSIS-DSP
@@ -0,0 +1 @@
+Subproject commit 1ba19d14a6491169b4d2873e1a0a59486c7636c6
diff --git a/Firmware/Makefile b/Firmware/Makefile
index b5e4fb33c..a3b53944a 100644
--- a/Firmware/Makefile
+++ b/Firmware/Makefile
@@ -104,7 +104,6 @@ $(TARGET_DIR)/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F \
 $(TARGET_DIR)/Drivers/CMSIS/Device/ST/STM32F4xx/Include \
 $(TARGET_DIR)/Drivers/CMSIS/Include 
 
-
 C_INCLUDES := $(addprefix -I, $(C_INCLUDES))
 
 #######################################
@@ -152,10 +151,29 @@ AS_DEFS =
 C_DEFS =  \
 -DSTM32_THREAD_SAFE_STRATEGY=4 \
 -DUSE_HAL_DRIVER 
+
 ifeq ($(MCU_TARGET),F411RE)
-C_DEFS += -DSTM32F411xE
+C_DEFS += -DSTM32F411xE -DUSE_DSP_FUNCTIONS
 else
-C_DEFS += -DSTM32F407xx
+C_DEFS += -DSTM32F407xx -DUSE_DSP_FUNCTIONS
+endif
+
+# DSP library configuration
+ifneq (,$(filter -DUSE_DSP_FUNCTIONS,$(C_DEFS)))
+DSP_LIB_PATH = Libraries/CMSIS-DSP
+C_INCLUDES += -I$(DSP_LIB_PATH)/Include -I$(DSP_LIB_PATH)/PrivateInclude
+C_DEFS += -DARM_MATH_LOOPUNROLL -DARM_MATH_CORTEX_M4
+
+# Add required CMSIS-DSP source modules
+C_SOURCES += $(wildcard $(DSP_LIB_PATH)/Source/BasicMathFunctions/arm_*.c) \
+             $(wildcard $(DSP_LIB_PATH)/Source/CommonTables/arm_*.c) \
+             $(wildcard $(DSP_LIB_PATH)/Source/FastMathFunctions/arm_*.c) \
+             $(wildcard $(DSP_LIB_PATH)/Source/FilteringFunctions/arm_*.c) \
+             $(wildcard $(DSP_LIB_PATH)/Source/ControllerFunctions/arm_*.c) \
+             $(wildcard $(DSP_LIB_PATH)/Source/InterpolationFunctions/arm_*.c)
+
+# Filter out unnecessary or incompatible files
+C_SOURCES := $(filter-out %_f16.c %_f64.c %_mve.c, $(C_SOURCES))
 endif
 
 # compile gcc flags
diff --git a/Firmware/Targets/F407VG/.cproject b/Firmware/Targets/F407VG/.cproject
index 81af2569f..de32c7675 100644
--- a/Firmware/Targets/F407VG/.cproject
+++ b/Firmware/Targets/F407VG/.cproject
@@ -25,11 +25,11 @@
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.707765906" name="Floating-point unit" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.value.fpv4-sp-d16" valueType="enumerated"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.1812098537" name="Floating-point ABI" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.value.hard" valueType="enumerated"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board.1543989768" name="Board" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board" useByScannerDiscovery="false" value="genericBoard" valueType="string"/>
-							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.1721792342" name="Defaults" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults" useByScannerDiscovery="false" value="com.st.stm32cube.ide.common.services.build.inputs.revA.1.0.6 || Debug || true || Executable || com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.base.gnu-tools-for-stm32 || STM32F407VGTx || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../Middlewares/Third_Party/FreeRTOS/Source/include | ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F | ../Drivers/CMSIS/Include | ../Core/Inc | ../Drivers/STM32F4xx_HAL_Driver/Inc | ../Drivers/CMSIS/Device/ST/STM32F4xx/Include | ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 | ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy | ../Core/ThreadSafe || ../Core/Inc | ../Drivers/STM32F4xx_HAL_Driver/Inc | ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy | ../Middlewares/Third_Party/FreeRTOS/Source/include | ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 | ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F | ../Drivers/CMSIS/Device/ST/STM32F4xx/Include | ../Drivers/CMSIS/Include ||  || STM32_THREAD_SAFE_STRATEGY=4 | USE_HAL_DRIVER | STM32F407xx ||  || Core/ThreadSafe | Drivers | Core/Startup | Middlewares | Core ||  ||  || ${workspace_loc:/${ProjName}/STM32F407VGTX_FLASH.ld} || true || NonSecure ||  || secure_nsclib.o ||  || FreeRTOS_AllowLockFromIRQ ||  ||  || " valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.1721792342" name="Defaults" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults" useByScannerDiscovery="false" value="com.st.stm32cube.ide.common.services.build.inputs.revA.1.0.6 || Debug || true || Executable || com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.base.gnu-tools-for-stm32 || STM32F407VGTx || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../Middlewares/Third_Party/FreeRTOS/Source/include | ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F | ../Drivers/CMSIS/Include | ../Core/Inc | ../Drivers/STM32F4xx_HAL_Driver/Inc | ../Drivers/CMSIS/Device/ST/STM32F4xx/Include | ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 | ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy | ../Core/ThreadSafe | ../Middlewares/ST/ARM/DSP/Inc || ../Core/Inc | ../Drivers/STM32F4xx_HAL_Driver/Inc | ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy | ../Middlewares/Third_Party/FreeRTOS/Source/include | ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 | ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F | ../Drivers/CMSIS/Device/ST/STM32F4xx/Include | ../Drivers/CMSIS/Include | ../Middlewares/ST/ARM/DSP/Inc ||  || STM32_THREAD_SAFE_STRATEGY=4 | USE_HAL_DRIVER | STM32F407xx ||  || Core/ThreadSafe | Drivers | Core/Startup | Middlewares | Core ||  || ../Middlewares/ST/ARM/DSP/Lib/libarm_cortexM4lf_math.a || ${workspace_loc:/${ProjName}/STM32F407VGTX_FLASH.ld} || true || NonSecure ||  || secure_nsclib.o ||  || FreeRTOS_AllowLockFromIRQ ||  ||  || " valueType="string"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat.396811763" name="Use float with printf from newlib-nano (-u _printf_float)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat" useByScannerDiscovery="false" value="true" valueType="boolean"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.converthex.678535481" name="Convert to Intel Hex file (-O ihex)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.converthex" useByScannerDiscovery="false" value="true" valueType="boolean"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.runtimelibrary_cpp.1430587733" name="Runtime library" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.runtimelibrary_cpp" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.runtimelibrary_cpp.value.standard_c_nano_cpp" valueType="enumerated"/>
-							<option id="com.st.stm32cube.ide.mcu.debug.option.cpuclock.1577055487" superClass="com.st.stm32cube.ide.mcu.debug.option.cpuclock" useByScannerDiscovery="false" value="168" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.debug.option.cpuclock.1577055487" name="Cpu clock frequence" superClass="com.st.stm32cube.ide.mcu.debug.option.cpuclock" useByScannerDiscovery="false" value="168" valueType="string"/>
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform.1473732877" isAbstract="false" osList="all" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform"/>
 							<builder buildPath="${workspace_loc:/F407VG}/Debug" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder.757539434" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder"/>
 							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.654005408" name="MCU GCC Assembler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler">
@@ -37,7 +37,7 @@
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.definedsymbols.101926958" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="DEBUG"/>
 								</option>
-								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths.83450388" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths" valueType="includePath">
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths.1289904115" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths" valueType="includePath">
 									<listOptionValue builtIn="false" value="../Core/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy"/>
@@ -54,11 +54,14 @@
 								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.962386684" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.value.og" valueType="enumerated"/>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols.1000272167" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
-									<listOptionValue builtIn="false" value="DEBUG"/>
+									<listOptionValue builtIn="false" value="ARM_MATH_CM4"/>
 									<listOptionValue builtIn="false" value="STM32F407xx"/>
 									<listOptionValue builtIn="false" value="STM32_THREAD_SAFE_STRATEGY=4"/>
+									<listOptionValue builtIn="false" value="DEBUG"/>
 								</option>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths.799389639" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths" useByScannerDiscovery="false" valueType="includePath">
+									<listOptionValue builtIn="false" value="../../../Libraries/CMSIS-DSP/Include"/>
+									<listOptionValue builtIn="false" value="../../../Libraries/CMSIS-DSP/PrivateInclude"/>
 									<listOptionValue builtIn="false" value="../Core/Inc"/>
 									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class/midi"/>
 									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class/audio"/>
@@ -87,6 +90,8 @@
 								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level.2037698556" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level.value.o0" valueType="enumerated"/>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.includepaths.999187480" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.includepaths" useByScannerDiscovery="false" valueType="includePath">
 									<listOptionValue builtIn="false" value="../Core/Inc"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/OpenFFBoard/Libraries/CMSIS-DSP/Include}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/OpenFFBoard/Libraries/CMSIS-DSP/PrivateInclude}&quot;"/>
 									<listOptionValue builtIn="false" value="../../../FFBoard/UserExtensions/Inc"/>
 									<listOptionValue builtIn="false" value="../../../FFBoard/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc"/>
@@ -109,6 +114,8 @@
 								</option>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.definedsymbols.36088191" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
+									<listOptionValue builtIn="false" value="ARM_MATH_CM4"/>
+									<listOptionValue builtIn="false" value="USE_DSP_FUNCTIONS"/>
 									<listOptionValue builtIn="false" value="DEBUG"/>
 									<listOptionValue builtIn="false" value="STM32F407xx"/>
 									<listOptionValue builtIn="false" value="STM32_THREAD_SAFE_STRATEGY=4"/>
@@ -120,6 +127,12 @@
 							</tool>
 							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.1483418087" name="MCU GCC Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker">
 								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.script.510702054" name="Linker Script (-T)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.script" value="${workspace_loc:/${ProjName}/STM32F407VGTX_FLASH.ld}" valueType="string"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.directories.1626202026" name="Library search path (-L)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.directories" valueType="libPaths">
+									<listOptionValue builtIn="false" value="../Middlewares/ST/ARM/DSP/Lib"/>
+								</option>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.libraries.483798684" name="Libraries (-l)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.libraries" valueType="libs">
+									<listOptionValue builtIn="false" value=":libarm_cortexM4lf_math.a"/>
+								</option>
 								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.input.1194172900" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.input">
 									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
 									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
@@ -130,6 +143,10 @@
 								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.systemcalls.526908104" name="System calls" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.systemcalls" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.systemcalls.value.minimalimplementation" valueType="enumerated"/>
 								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.gcsections.974981920" name="Discard unused sections (-Wl,--gc-sections)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.gcsections" useByScannerDiscovery="false" value="true" valueType="boolean"/>
 								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.cref.1072959917" name="Add symbol cross reference table to map file (-Wl,--cref)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.cref" useByScannerDiscovery="false" value="true" valueType="boolean"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.libraries.966972385" name="Libraries (-l)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.libraries" valueType="libs">
+									<listOptionValue builtIn="false" value="m"/>
+								</option>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="true" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.directories.728597271" name="Library search path (-L)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.directories" valueType="libPaths"/>
 								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.input.1351043086" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.input">
 									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
 									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
@@ -218,17 +235,18 @@
 						</toolChain>
 					</folderInfo>
 					<sourceEntries>
-						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="FFBoard"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
-						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
+						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="FFBoard"/>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
+						<entry excluding="**/_*.c|**/*_neon.c|**/*_mve.c|**/*_mve_*.c|**/*Functions.c|**/CommonTables.c|**/Distance.c|**/SVM.c|**/Bayes.c" flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="DSP_Source"/>
 					</sourceEntries>
 				</configuration>
 			</storageModule>
 			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
 		</cconfiguration>
 		<cconfiguration id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.1432874368">
-			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.1432874368" moduleId="org.eclipse.cdt.core.settings" name="Release">
+			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.1432874368" moduleId="org.eclipse.cdt.core.settings" name="Release_old">
 				<externalSettings/>
 				<extensions>
 					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
@@ -240,7 +258,7 @@
 				</extensions>
 			</storageModule>
 			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-				<configuration artifactExtension="elf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug" cleanCommand="rm -rf" description="" errorParsers="org.eclipse.cdt.core.GASErrorParser;org.eclipse.cdt.core.GmakeErrorParser;org.eclipse.cdt.core.GLDErrorParser;org.eclipse.cdt.core.CWDLocator;org.eclipse.cdt.core.GCCErrorParser" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.1432874368" name="Release" parent="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug">
+				<configuration artifactExtension="elf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug" cleanCommand="rm -rf" description="" errorParsers="org.eclipse.cdt.core.GASErrorParser;org.eclipse.cdt.core.GmakeErrorParser;org.eclipse.cdt.core.GLDErrorParser;org.eclipse.cdt.core.CWDLocator;org.eclipse.cdt.core.GCCErrorParser" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.1432874368" name="Release_old" parent="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug">
 					<folderInfo id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.1432874368." name="/" resourcePath="">
 						<toolChain id="com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.exe.debug.219764043" name="MCU ARM GCC" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.exe.debug">
 							<option id="com.st.stm32cube.ide.mcu.option.internal.toolchain.type.1480479977" name="Internal Toolchain Type" superClass="com.st.stm32cube.ide.mcu.option.internal.toolchain.type" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.base.gnu-tools-for-stm32" valueType="string"/>
@@ -251,11 +269,11 @@
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.148418543" name="Floating-point unit" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.value.fpv4-sp-d16" valueType="enumerated"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.210656568" name="Floating-point ABI" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.value.hard" valueType="enumerated"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board.1776500793" name="Board" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board" useByScannerDiscovery="false" value="genericBoard" valueType="string"/>
-							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.1132536473" name="Defaults" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults" useByScannerDiscovery="false" value="com.st.stm32cube.ide.common.services.build.inputs.revA.1.0.6 || Release || true || Executable || com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.base.gnu-tools-for-stm32 || STM32F407VGTx || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../Middlewares/Third_Party/FreeRTOS/Source/include | ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F | ../Drivers/CMSIS/Include | ../Core/Inc | ../Drivers/STM32F4xx_HAL_Driver/Inc | ../Drivers/CMSIS/Device/ST/STM32F4xx/Include | ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 | ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy | ../Core/ThreadSafe || ../Core/Inc | ../Drivers/STM32F4xx_HAL_Driver/Inc | ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy | ../Middlewares/Third_Party/FreeRTOS/Source/include | ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 | ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F | ../Drivers/CMSIS/Device/ST/STM32F4xx/Include | ../Drivers/CMSIS/Include ||  || STM32_THREAD_SAFE_STRATEGY=4 | USE_HAL_DRIVER | STM32F407xx ||  || Core/ThreadSafe | Drivers | Core/Startup | Middlewares | Core ||  ||  || ${workspace_loc:/${ProjName}/STM32F407VGTX_FLASH.ld} || true || NonSecure ||  || secure_nsclib.o ||  || FreeRTOS_AllowLockFromIRQ ||  ||  || " valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.1132536473" name="Defaults" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults" useByScannerDiscovery="false" value="com.st.stm32cube.ide.common.services.build.inputs.revA.1.0.6 || Release || true || Executable || com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.base.gnu-tools-for-stm32 || STM32F407VGTx || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../Middlewares/Third_Party/FreeRTOS/Source/include | ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F | ../Drivers/CMSIS/Include | ../Core/Inc | ../Drivers/STM32F4xx_HAL_Driver/Inc | ../Drivers/CMSIS/Device/ST/STM32F4xx/Include | ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 | ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy | ../Core/ThreadSafe | ../Middlewares/ST/ARM/DSP/Inc || ../Core/Inc | ../Drivers/STM32F4xx_HAL_Driver/Inc | ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy | ../Middlewares/Third_Party/FreeRTOS/Source/include | ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 | ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F | ../Drivers/CMSIS/Device/ST/STM32F4xx/Include | ../Drivers/CMSIS/Include | ../Middlewares/ST/ARM/DSP/Inc ||  || STM32_THREAD_SAFE_STRATEGY=4 | USE_HAL_DRIVER | STM32F407xx ||  || Core/ThreadSafe | Drivers | Core/Startup | Middlewares | Core ||  || ../Middlewares/ST/ARM/DSP/Lib/libarm_cortexM4lf_math.a || ${workspace_loc:/${ProjName}/STM32F407VGTX_FLASH.ld} || true || NonSecure ||  || secure_nsclib.o ||  || FreeRTOS_AllowLockFromIRQ ||  ||  || " valueType="string"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat.1586695396" name="Use float with printf from newlib-nano (-u _printf_float)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat" useByScannerDiscovery="false" value="true" valueType="boolean"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.converthex.1723306711" name="Convert to Intel Hex file (-O ihex)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.converthex" useByScannerDiscovery="false" value="true" valueType="boolean"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.runtimelibrary_cpp.692226491" name="Runtime library" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.runtimelibrary_cpp" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.runtimelibrary_cpp.value.standard_c_nano_cpp" valueType="enumerated"/>
-							<option id="com.st.stm32cube.ide.mcu.debug.option.cpuclock.1545102677" superClass="com.st.stm32cube.ide.mcu.debug.option.cpuclock" useByScannerDiscovery="false" value="168" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.debug.option.cpuclock.1545102677" name="Cpu clock frequence" superClass="com.st.stm32cube.ide.mcu.debug.option.cpuclock" useByScannerDiscovery="false" value="168" valueType="string"/>
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform.830438003" isAbstract="false" osList="all" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform"/>
 							<builder buildPath="${workspace_loc:/F407VG}/Debug" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder.587985951" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder"/>
 							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.1451593181" name="MCU GCC Assembler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler">
@@ -263,7 +281,7 @@
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.definedsymbols.763722981" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="DEBUG"/>
 								</option>
-								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths.1048516646" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths" valueType="includePath">
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths.1328579848" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths" valueType="includePath">
 									<listOptionValue builtIn="false" value="../Core/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy"/>
@@ -280,12 +298,16 @@
 								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.1695955147" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.value.o2" valueType="enumerated"/>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols.53417333" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
+									<listOptionValue builtIn="false" value="USE_DSP_FUNCTIONS"/>
+									<listOptionValue builtIn="false" value="ARM_MATH_CM4"/>
 									<listOptionValue builtIn="false" value="STM32F407xx"/>
 									<listOptionValue builtIn="false" value="STM32_THREAD_SAFE_STRATEGY=4"/>
 									<listOptionValue builtIn="false" value="DEBUG"/>
 								</option>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths.509062604" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths" useByScannerDiscovery="false" valueType="includePath">
 									<listOptionValue builtIn="false" value="../Core/Inc"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/OpenFFBoard/Libraries/CMSIS-DSP/Include}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/OpenFFBoard/Libraries/CMSIS-DSP/PrivateInclude}&quot;"/>
 									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class/midi"/>
 									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class/audio"/>
 									<listOptionValue builtIn="false" value="../../../FFBoard/UserExtensions/Inc"/>
@@ -316,6 +338,8 @@
 								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level.1420093106" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level.value.os" valueType="enumerated"/>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.includepaths.1985529570" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.includepaths" useByScannerDiscovery="false" valueType="includePath">
 									<listOptionValue builtIn="false" value="../Core/Inc"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/OpenFFBoard/Libraries/CMSIS-DSP/Include}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/OpenFFBoard/Libraries/CMSIS-DSP/PrivateInclude}&quot;"/>
 									<listOptionValue builtIn="false" value="../../../FFBoard/UserExtensions/Inc"/>
 									<listOptionValue builtIn="false" value="../../../FFBoard/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc"/>
@@ -338,6 +362,8 @@
 								</option>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.definedsymbols.1842866947" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
+									<listOptionValue builtIn="false" value="USE_DSP_FUNCTIONS"/>
+									<listOptionValue builtIn="false" value="ARM_MATH_CM4"/>
 									<listOptionValue builtIn="false" value="DEBUG"/>
 									<listOptionValue builtIn="false" value="STM32F407xx"/>
 									<listOptionValue builtIn="false" value="STM32_THREAD_SAFE_STRATEGY=4"/>
@@ -353,6 +379,12 @@
 							</tool>
 							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.1233815650" name="MCU GCC Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker">
 								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.script.1517368836" name="Linker Script (-T)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.script" value="${workspace_loc:/${ProjName}/STM32F407VGTX_FLASH.ld}" valueType="string"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.directories.1255046971" name="Library search path (-L)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.directories" valueType="libPaths">
+									<listOptionValue builtIn="false" value="../Middlewares/ST/ARM/DSP/Lib"/>
+								</option>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.libraries.568556050" name="Libraries (-l)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.libraries" valueType="libs">
+									<listOptionValue builtIn="false" value=":libarm_cortexM4lf_math.a"/>
+								</option>
 								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.input.2036654442" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.input">
 									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
 									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
@@ -363,6 +395,10 @@
 								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.systemcalls.1493733321" name="System calls" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.systemcalls" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.systemcalls.value.minimalimplementation" valueType="enumerated"/>
 								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.gcsections.342932735" name="Discard unused sections (-Wl,--gc-sections)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.gcsections" useByScannerDiscovery="false" value="true" valueType="boolean"/>
 								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.cref.1953736270" name="Add symbol cross reference table to map file (-Wl,--cref)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.cref" useByScannerDiscovery="false" value="true" valueType="boolean"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="true" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.directories.2006090705" name="Library search path (-L)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.directories" valueType="libPaths"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.libraries.1065976636" name="Libraries (-l)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.libraries" valueType="libs">
+									<listOptionValue builtIn="false" value="m"/>
+								</option>
 								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.input.441406744" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.input">
 									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
 									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
@@ -451,10 +487,254 @@
 						</toolChain>
 					</folderInfo>
 					<sourceEntries>
-						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="FFBoard"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
+						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="FFBoard"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
+					</sourceEntries>
+				</configuration>
+			</storageModule>
+			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
+		</cconfiguration>
+		<cconfiguration id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.1363922842">
+			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.1363922842" moduleId="org.eclipse.cdt.core.settings" name="Release">
+				<externalSettings/>
+				<extensions>
+					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+				</extensions>
+			</storageModule>
+			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+				<configuration artifactExtension="elf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug" cleanCommand="rm -rf" description="" errorParsers="org.eclipse.cdt.core.GASErrorParser;org.eclipse.cdt.core.GmakeErrorParser;org.eclipse.cdt.core.GLDErrorParser;org.eclipse.cdt.core.CWDLocator;org.eclipse.cdt.core.GCCErrorParser" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.1363922842" name="Release" parent="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug">
+					<folderInfo id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.1363922842." name="/" resourcePath="">
+						<toolChain id="com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.exe.debug.1285660580" name="MCU ARM GCC" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.exe.debug">
+							<option id="com.st.stm32cube.ide.mcu.option.internal.toolchain.type.9052958" name="Internal Toolchain Type" superClass="com.st.stm32cube.ide.mcu.option.internal.toolchain.type" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.base.gnu-tools-for-stm32" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.option.internal.toolchain.version.596919784" name="Internal Toolchain Version" superClass="com.st.stm32cube.ide.mcu.option.internal.toolchain.version" useByScannerDiscovery="false" value="7-2018-q2-update" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_mcu.1292320274" name="MCU" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_mcu" useByScannerDiscovery="false" value="STM32F407VGTx" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_cpuid.878820511" name="CPU" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_cpuid" useByScannerDiscovery="false" value="0" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_coreid.1967971095" name="Core" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_coreid" useByScannerDiscovery="false" value="0" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.924415620" name="Floating-point unit" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.value.fpv4-sp-d16" valueType="enumerated"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.1287477197" name="Floating-point ABI" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.value.hard" valueType="enumerated"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board.1325145601" name="Board" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board" useByScannerDiscovery="false" value="genericBoard" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.1220367116" name="Defaults" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults" useByScannerDiscovery="false" value="com.st.stm32cube.ide.common.services.build.inputs.revA.1.0.6 || Debug || true || Executable || com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.base.gnu-tools-for-stm32 || STM32F407VGTx || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../Middlewares/Third_Party/FreeRTOS/Source/include | ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F | ../Drivers/CMSIS/Include | ../Core/Inc | ../Drivers/STM32F4xx_HAL_Driver/Inc | ../Drivers/CMSIS/Device/ST/STM32F4xx/Include | ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 | ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy | ../Core/ThreadSafe | ../Middlewares/ST/ARM/DSP/Inc || ../Core/Inc | ../Drivers/STM32F4xx_HAL_Driver/Inc | ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy | ../Middlewares/Third_Party/FreeRTOS/Source/include | ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 | ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F | ../Drivers/CMSIS/Device/ST/STM32F4xx/Include | ../Drivers/CMSIS/Include | ../Middlewares/ST/ARM/DSP/Inc ||  || STM32_THREAD_SAFE_STRATEGY=4 | USE_HAL_DRIVER | STM32F407xx ||  || Core/ThreadSafe | Drivers | Core/Startup | Middlewares | Core ||  || ../Middlewares/ST/ARM/DSP/Lib/libarm_cortexM4lf_math.a || ${workspace_loc:/${ProjName}/STM32F407VGTX_FLASH.ld} || true || NonSecure ||  || secure_nsclib.o ||  || FreeRTOS_AllowLockFromIRQ ||  ||  || " valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat.322597623" name="Use float with printf from newlib-nano (-u _printf_float)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat" useByScannerDiscovery="false" value="true" valueType="boolean"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.converthex.1320135450" name="Convert to Intel Hex file (-O ihex)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.converthex" useByScannerDiscovery="false" value="true" valueType="boolean"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.runtimelibrary_cpp.283885057" name="Runtime library" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.runtimelibrary_cpp" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.runtimelibrary_cpp.value.standard_c_nano_cpp" valueType="enumerated"/>
+							<option id="com.st.stm32cube.ide.mcu.debug.option.cpuclock.497164895" name="Cpu clock frequence" superClass="com.st.stm32cube.ide.mcu.debug.option.cpuclock" useByScannerDiscovery="false" value="168" valueType="string"/>
+							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform.805320124" isAbstract="false" osList="all" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform"/>
+							<builder buildPath="${workspace_loc:/F407VG}/Debug" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder.1882593924" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.915241620" name="MCU GCC Assembler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.debuglevel.1602242812" name="Debug level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.debuglevel" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.debuglevel.value.g3" valueType="enumerated"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.definedsymbols.1577881387" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
+									<listOptionValue builtIn="false" value="DEBUG"/>
+								</option>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths.835111385" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths" valueType="includePath">
+									<listOptionValue builtIn="false" value="../Core/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/include"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32F4xx/Include"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
+								</option>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input.735096978" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input"/>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.1904052172" name="MCU GCC Compiler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel.141503941" name="Debug level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel.value.g" valueType="enumerated"/>
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.2090674708" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.value.o2" valueType="enumerated"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols.1446367934" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
+									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
+									<listOptionValue builtIn="false" value="ARM_MATH_CM4"/>
+									<listOptionValue builtIn="false" value="STM32F407xx"/>
+									<listOptionValue builtIn="false" value="STM32_THREAD_SAFE_STRATEGY=4"/>
+									<listOptionValue builtIn="false" value="DEBUG"/>
+								</option>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths.516098490" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths" useByScannerDiscovery="false" valueType="includePath">
+									<listOptionValue builtIn="false" value="../../../Libraries/CMSIS-DSP/Include"/>
+									<listOptionValue builtIn="false" value="../../../Libraries/CMSIS-DSP/PrivateInclude"/>
+									<listOptionValue builtIn="false" value="../Core/Inc"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class/midi"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class/audio"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/UserExtensions/Inc"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32F4xx/Include"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/include"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class/cdc"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class/hid"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/common"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/device"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/osal"/>
+									<listOptionValue builtIn="false" value="../Core/ThreadSafe"/>
+								</option>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.935491259" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/>
+							</tool>
+							<tool commandLinePattern="${COMMAND} ${INPUTS} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT}" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.516975373" name="MCU G++ Compiler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.debuglevel.1465240701" name="Debug level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.debuglevel" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.debuglevel.value.g" valueType="enumerated"/>
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level.1061474810" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level.value.os" valueType="enumerated"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.includepaths.1087483447" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.includepaths" useByScannerDiscovery="false" valueType="includePath">
+									<listOptionValue builtIn="false" value="../Core/Inc"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/OpenFFBoard/Libraries/CMSIS-DSP/Include}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/OpenFFBoard/Libraries/CMSIS-DSP/PrivateInclude}&quot;"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/UserExtensions/Inc"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32F4xx/Include"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/include"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class/cdc"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class/hid"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/common"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/device"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/osal"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class/midi"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class/audio"/>
+									<listOptionValue builtIn="false" value="../Core/ThreadSafe"/>
+								</option>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.definedsymbols.1534998435" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
+									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
+									<listOptionValue builtIn="false" value="ARM_MATH_CM4"/>
+									<listOptionValue builtIn="false" value="USE_DSP_FUNCTIONS"/>
+									<listOptionValue builtIn="false" value="DEBUG"/>
+									<listOptionValue builtIn="false" value="STM32F407xx"/>
+									<listOptionValue builtIn="false" value="STM32_THREAD_SAFE_STRATEGY=4"/>
+								</option>
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.nortti.40677291" name="Disable generation of information about every class with virtual functions (-fno-rtti)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.nortti" useByScannerDiscovery="false" value="false" valueType="boolean"/>
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.languagestandard.1536805651" name="Language standard" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.languagestandard" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.languagestandard.value.gnupp20" valueType="enumerated"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="true" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.otherflags.1835380360" name="Other flags" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.otherflags" useByScannerDiscovery="true" valueType="stringList"/>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp.723528923" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp"/>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.760287782" name="MCU GCC Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.script.118645055" name="Linker Script (-T)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.script" value="${workspace_loc:/${ProjName}/STM32F407VGTX_FLASH.ld}" valueType="string"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.directories.479305905" name="Library search path (-L)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.directories" valueType="libPaths">
+									<listOptionValue builtIn="false" value="../Middlewares/ST/ARM/DSP/Lib"/>
+								</option>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.libraries.2037434941" name="Libraries (-l)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.libraries" valueType="libs">
+									<listOptionValue builtIn="false" value=":libarm_cortexM4lf_math.a"/>
+								</option>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.input.1668802858" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.input">
+									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
+									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
+								</inputType>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.139516070" name="MCU G++ Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.script.1730750587" name="Linker Script (-T)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.script" useByScannerDiscovery="false" value="${workspace_loc:/${ProjName}/STM32F407VGTX_FLASH.ld}" valueType="string"/>
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.systemcalls.540672951" name="System calls" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.systemcalls" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.systemcalls.value.minimalimplementation" valueType="enumerated"/>
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.gcsections.575378861" name="Discard unused sections (-Wl,--gc-sections)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.gcsections" useByScannerDiscovery="false" value="true" valueType="boolean"/>
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.cref.2072296631" name="Add symbol cross reference table to map file (-Wl,--cref)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.cref" useByScannerDiscovery="false" value="true" valueType="boolean"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.libraries.244341576" name="Libraries (-l)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.libraries" valueType="libs">
+									<listOptionValue builtIn="false" value="m"/>
+								</option>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="true" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.directories.1691175730" name="Library search path (-L)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.directories" valueType="libPaths"/>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.input.2126731635" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.input">
+									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
+									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
+								</inputType>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.archiver.826324806" name="MCU GCC Archiver" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.archiver"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.size.670905681" name="MCU Size" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.size"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objdump.listfile.275232008" name="MCU Output Converter list file" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objdump.listfile"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.hex.1286489487" name="MCU Output Converter Hex" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.hex"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.binary.24936505" name="MCU Output Converter Binary" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.binary"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.verilog.1810710601" name="MCU Output Converter Verilog" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.verilog"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.srec.1326355484" name="MCU Output Converter Motorola S-rec" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.srec"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.symbolsrec.497686729" name="MCU Output Converter Motorola S-rec with symbols" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.symbolsrec"/>
+						</toolChain>
+					</folderInfo>
+					<folderInfo id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.1363922842.FFBoard/USB/portable/st/stm32_fsdev" name="/" resourcePath="FFBoard/USB/portable/st/stm32_fsdev">
+						<toolChain id="com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.exe.debug.536918603" name="MCU ARM GCC" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.exe.debug" unusedChildren="">
+							<option id="com.st.stm32cube.ide.mcu.option.internal.toolchain.type.1579538038.546525032" name="Internal Toolchain Type" superClass="com.st.stm32cube.ide.mcu.option.internal.toolchain.type.1579538038"/>
+							<option id="com.st.stm32cube.ide.mcu.option.internal.toolchain.version.217964872.1443037302" name="Internal Toolchain Version" superClass="com.st.stm32cube.ide.mcu.option.internal.toolchain.version.217964872"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_mcu.1707466200.1136697719" name="MCU" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_mcu.1707466200"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_cpuid.846936463.1884296605" name="CPU" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_cpuid.846936463"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_coreid.761213199.1893072223" name="Core" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_coreid.761213199"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.707765906.405463958" name="Floating-point unit" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.707765906"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.1812098537.970212700" name="Floating-point ABI" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.1812098537"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board.1543989768.927096507" name="Board" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board.1543989768"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.1721792342.1856243064" name="Defaults" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.1721792342"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat.396811763.753093289" name="Use float with printf from newlib-nano (-u _printf_float)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat.396811763"/>
+							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform.1340737974" isAbstract="false" osList="all" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.1184941639" name="MCU GCC Assembler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.915241620">
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input.1781363974" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input"/>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.1642804515" name="MCU GCC Compiler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.1904052172">
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths.1162066718" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths" useByScannerDiscovery="false" valueType="includePath">
+									<listOptionValue builtIn="false" value="../Core/Inc"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/UserExtensions/Inc"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32F4xx/Include"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/include"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class/cdc"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class/hid"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/common"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/device"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/portable/st/stm32_fsdev"/>
+								</option>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.90423837" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.1165102383" name="MCU G++ Compiler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.516975373">
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.includepaths.729977927" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.includepaths" useByScannerDiscovery="false" valueType="includePath">
+									<listOptionValue builtIn="false" value="../Core/Inc"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/UserExtensions/Inc"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32F4xx/Include"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/include"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class/cdc"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class/hid"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/common"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/portable/st/stm32_fsdev"/>
+									<listOptionValue builtIn="false" value="../../../FFBoard/USB/device"/>
+								</option>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp.794126394" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp"/>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.1472897188" name="MCU GCC Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.760287782"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.1674326894" name="MCU G++ Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.139516070"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.archiver.786812465" name="MCU GCC Archiver" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.archiver.826324806"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.size.1330056767" name="MCU Size" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.size.670905681"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objdump.listfile.1222203945" name="MCU Output Converter list file" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objdump.listfile.275232008"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.hex.273039636" name="MCU Output Converter Hex" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.hex.1286489487"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.binary.76139356" name="MCU Output Converter Binary" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.binary.24936505"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.verilog.1631979959" name="MCU Output Converter Verilog" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.verilog.1810710601"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.srec.1381731762" name="MCU Output Converter Motorola S-rec" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.srec.1326355484"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.symbolsrec.1678273395" name="MCU Output Converter Motorola S-rec with symbols" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.symbolsrec.497686729"/>
+						</toolChain>
+					</folderInfo>
+					<sourceEntries>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
+						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="FFBoard"/>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
+						<entry excluding="**/_*.c|**/*_neon.c|**/*_mve.c|**/*_mve_*.c|**/*Functions.c|**/CommonTables.c|**/Distance.c|**/SVM.c|**/Bayes.c" flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="DSP_Source"/>
 					</sourceEntries>
 				</configuration>
 			</storageModule>
@@ -477,22 +757,22 @@
 	<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
 	<storageModule moduleId="scannerConfiguration">
 		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-		<scannerConfigBuildInfo instanceId="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588;com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.96783383;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp.1072682784">
+		<scannerConfigBuildInfo instanceId="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.1432874368;com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.1432874368.;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.960683736;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp.307847074">
 			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
 		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588;com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.1195385770;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.1234902516">
+			<autodiscovery enabled="false" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
 		<scannerConfigBuildInfo instanceId="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.825670163;com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.825670163.;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.1407060800;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.136927613">
 			<autodiscovery enabled="false" problemReportingEnabled="true" selectedProfileId=""/>
 		</scannerConfigBuildInfo>
-		<scannerConfigBuildInfo instanceId="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.1432874368;com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.1432874368.;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.452034158;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.1868352148">
+		<scannerConfigBuildInfo instanceId="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588;com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.96783383;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp.1072682784">
 			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
 		</scannerConfigBuildInfo>
 		<scannerConfigBuildInfo instanceId="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.825670163;com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.825670163.;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.1403538269;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp.1805261403">
 			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
 		</scannerConfigBuildInfo>
-		<scannerConfigBuildInfo instanceId="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588;com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.1195385770;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.1234902516">
-			<autodiscovery enabled="false" problemReportingEnabled="true" selectedProfileId=""/>
-		</scannerConfigBuildInfo>
-		<scannerConfigBuildInfo instanceId="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.1432874368;com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.1432874368.;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.960683736;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp.307847074">
+		<scannerConfigBuildInfo instanceId="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.1432874368;com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.957220588.1432874368.;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.452034158;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.1868352148">
 			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
 		</scannerConfigBuildInfo>
 	</storageModule>
diff --git a/Firmware/Targets/F407VG/.project b/Firmware/Targets/F407VG/.project
index 404f8451d..2d2516fe2 100644
--- a/Firmware/Targets/F407VG/.project
+++ b/Firmware/Targets/F407VG/.project
@@ -37,5 +37,10 @@
 			<type>2</type>
 			<locationURI>PARENT-2-PROJECT_LOC/FFBoard</locationURI>
 		</link>
+		<link>
+			<name>DSP_Source</name>
+			<type>2</type>
+			<locationURI>PARENT-2-PROJECT_LOC/Libraries/CMSIS-DSP/Source</locationURI>
+		</link>
 	</linkedResources>
 </projectDescription>
diff --git a/Firmware/Targets/F407VG_DISCO/.cproject b/Firmware/Targets/F407VG_DISCO/.cproject
index d2e451124..7e2f13097 100644
--- a/Firmware/Targets/F407VG_DISCO/.cproject
+++ b/Firmware/Targets/F407VG_DISCO/.cproject
@@ -29,7 +29,7 @@
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat.396811763" name="Use float with printf from newlib-nano (-u _printf_float)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat" useByScannerDiscovery="false" value="true" valueType="boolean"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.converthex.678535481" name="Convert to Intel Hex file (-O ihex)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.converthex" useByScannerDiscovery="false" value="true" valueType="boolean"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.runtimelibrary_cpp.1430587733" name="Runtime library" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.runtimelibrary_cpp" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.runtimelibrary_cpp.value.standard_c_nano_cpp" valueType="enumerated"/>
-							<option id="com.st.stm32cube.ide.mcu.debug.option.cpuclock.202304661" superClass="com.st.stm32cube.ide.mcu.debug.option.cpuclock" useByScannerDiscovery="false" value="168" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.debug.option.cpuclock.202304661" name="Cpu clock frequence" superClass="com.st.stm32cube.ide.mcu.debug.option.cpuclock" useByScannerDiscovery="false" value="168" valueType="string"/>
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform.1473732877" isAbstract="false" osList="all" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform"/>
 							<builder buildPath="${workspace_loc:/F407VG}/Debug" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder.757539434" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder"/>
 							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.654005408" name="MCU GCC Assembler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler">
@@ -37,7 +37,7 @@
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.definedsymbols.101926958" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="DEBUG"/>
 								</option>
-								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths.172064398" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths" valueType="includePath">
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths.172064398" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths" valueType="includePath">
 									<listOptionValue builtIn="false" value="../Core/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy"/>
@@ -60,6 +60,8 @@
 								</option>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths.799389639" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths" useByScannerDiscovery="false" valueType="includePath">
 									<listOptionValue builtIn="false" value="../Core/Inc"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/OpenFFBoard/Libraries/CMSIS-DSP/Include}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/OpenFFBoard/Libraries/CMSIS-DSP/PrivateInclude}&quot;"/>
 									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class/midi"/>
 									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class/audio"/>
 									<listOptionValue builtIn="false" value="../../../FFBoard/UserExtensions/Inc"/>
@@ -87,6 +89,8 @@
 								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level.2037698556" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level.value.og" valueType="enumerated"/>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.includepaths.999187480" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.includepaths" useByScannerDiscovery="false" valueType="includePath">
 									<listOptionValue builtIn="false" value="../Core/Inc"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/OpenFFBoard/Libraries/CMSIS-DSP/Include}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/OpenFFBoard/Libraries/CMSIS-DSP/PrivateInclude}&quot;"/>
 									<listOptionValue builtIn="false" value="../../../FFBoard/UserExtensions/Inc"/>
 									<listOptionValue builtIn="false" value="../../../FFBoard/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc"/>
@@ -151,9 +155,9 @@
 						<toolChain id="com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.exe.debug.1455690153" name="MCU ARM GCC" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.exe.debug" unusedChildren="">
 							<option id="com.st.stm32cube.ide.mcu.option.internal.toolchain.type.1579538038.1392462805" name="Internal Toolchain Type" superClass="com.st.stm32cube.ide.mcu.option.internal.toolchain.type.1579538038"/>
 							<option id="com.st.stm32cube.ide.mcu.option.internal.toolchain.version.217964872.1102815799" name="Internal Toolchain Version" superClass="com.st.stm32cube.ide.mcu.option.internal.toolchain.version.217964872"/>
-							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_mcu.1707466200.295685066" name="Mcu" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_mcu.1707466200"/>
-							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_cpuid.846936463.905713779" name="CpuId" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_cpuid.846936463"/>
-							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_coreid.761213199.1218557261" name="CpuCoreId" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_coreid.761213199"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_mcu.1707466200.295685066" name="MCU" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_mcu.1707466200"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_cpuid.846936463.905713779" name="CPU" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_cpuid.846936463"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_coreid.761213199.1218557261" name="Core" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_coreid.761213199"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.707765906.2076926955" name="Floating-point unit" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.707765906"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.1812098537.1745863964" name="Floating-point ABI" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.1812098537"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board.1543989768.1336695815" name="Board" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board.1543989768"/>
@@ -257,7 +261,7 @@
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat.1586695396" name="Use float with printf from newlib-nano (-u _printf_float)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat" useByScannerDiscovery="false" value="true" valueType="boolean"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.converthex.1723306711" name="Convert to Intel Hex file (-O ihex)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.converthex" useByScannerDiscovery="false" value="true" valueType="boolean"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.runtimelibrary_cpp.692226491" name="Runtime library" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.runtimelibrary_cpp" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.runtimelibrary_cpp.value.standard_c_nano_cpp" valueType="enumerated"/>
-							<option id="com.st.stm32cube.ide.mcu.debug.option.cpuclock.1945594623" superClass="com.st.stm32cube.ide.mcu.debug.option.cpuclock" useByScannerDiscovery="false" value="168" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.debug.option.cpuclock.1945594623" name="Cpu clock frequence" superClass="com.st.stm32cube.ide.mcu.debug.option.cpuclock" useByScannerDiscovery="false" value="168" valueType="string"/>
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform.830438003" isAbstract="false" osList="all" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform"/>
 							<builder buildPath="${workspace_loc:/F407VG}/Debug" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder.587985951" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder"/>
 							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.1451593181" name="MCU GCC Assembler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler">
@@ -265,7 +269,7 @@
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.definedsymbols.763722981" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="DEBUG"/>
 								</option>
-								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths.207317094" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths" valueType="includePath">
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths.207317094" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths" valueType="includePath">
 									<listOptionValue builtIn="false" value="../Core/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy"/>
diff --git a/Firmware/Targets/F411RE/.cproject b/Firmware/Targets/F411RE/.cproject
index f092253bb..66701d103 100644
--- a/Firmware/Targets/F411RE/.cproject
+++ b/Firmware/Targets/F411RE/.cproject
@@ -49,6 +49,8 @@
 								</option>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths.1392609053" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths" useByScannerDiscovery="false" valueType="includePath">
 									<listOptionValue builtIn="false" value="../Core/Inc"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/OpenFFBoard/Libraries/CMSIS-DSP/Include}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/OpenFFBoard/Libraries/CMSIS-DSP/PrivateInclude}&quot;"/>
 									<listOptionValue builtIn="false" value="../../../FFBoard/UserExtensions/Inc"/>
 									<listOptionValue builtIn="false" value="../../../FFBoard/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc"/>
@@ -85,6 +87,8 @@
 									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/include"/>
 									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F"/>
 									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/OpenFFBoard/Libraries/CMSIS-DSP/Include}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/OpenFFBoard/Libraries/CMSIS-DSP/PrivateInclude}&quot;"/>
 									<listOptionValue builtIn="false" value="../../../FFBoard/USB"/>
 									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class/cdc"/>
 									<listOptionValue builtIn="false" value="../../../FFBoard/USB/class"/>

From 1a061b76f40aa1292074253f2921008c0599a208 Mon Sep 17 00:00:00 2001
From: Vincent MANOUKIAN <10980775+manoukianv@users.noreply.github.com>
Date: Sat, 25 Apr 2026 11:26:49 +0200
Subject: [PATCH 5/8] feat: add FFB reconstruction filter and DSP optimizations

---
 Firmware/FFBoard/Inc/Axis.h                | 153 ++++----
 Firmware/FFBoard/Inc/EffectsCalculator.h   | 246 ++++++++-----
 Firmware/FFBoard/Inc/Filters.h             |   3 +-
 Firmware/FFBoard/Inc/HidFFB.h              | 135 +++++---
 Firmware/FFBoard/Inc/ffb_defs.h            |   5 +
 Firmware/FFBoard/Src/Axis.cpp              |  74 +++-
 Firmware/FFBoard/Src/EffectsCalculator.cpp | 384 +++++++++++++++++----
 Firmware/FFBoard/Src/Filters.cpp           |   2 +-
 Firmware/FFBoard/Src/HidFFB.cpp            |  28 +-
 9 files changed, 750 insertions(+), 280 deletions(-)

diff --git a/Firmware/FFBoard/Inc/Axis.h b/Firmware/FFBoard/Inc/Axis.h
index 958c4bae1..514dffd65 100644
--- a/Firmware/FFBoard/Inc/Axis.h
+++ b/Firmware/FFBoard/Inc/Axis.h
@@ -3,6 +3,8 @@
  *
  *  Created on: 21.01.2021
  *      Author: Yannick / Lidders / Vincent
+ * 
+ *  Release 27.10.25: Vincent, add reconstruction filter, equalizer, slew rate.
  */
 
 #ifndef SRC_AXIS_H_
@@ -23,6 +25,11 @@
 #include "ExtiHandler.h"
 #include "EffectsCalculator.h"
 
+#ifdef USE_DSP_FUNCTIONS
+#include "dsp/controller_functions.h"
+#include "dsp/fast_math_functions.h"
+#endif
+
 #define INTERNAL_AXIS_DAMPER_SCALER 0.7
 #define INTERNAL_AXIS_FRICTION_SCALER 0.7
 #define INTERNAL_AXIS_INERTIA_SCALER 0.7
@@ -37,13 +44,13 @@
  * @brief Global control flags for all axes.
  */
 struct Control_t {
-	bool emergency = false;                         //!< Emergency stop is active.
-	bool usb_disabled = true;                       //!< FFB is disabled by USB.
-	bool update_disabled = true;            //!< FFB updates are disabled.
+	bool emergency = false;				//!< Emergency stop is active.
+	bool usb_disabled = true;			//!< FFB is disabled by USB.
+	bool update_disabled = true;		//!< FFB updates are disabled.
 	bool request_update_disabled = false; //!< A request to disable FFB updates is pending.
 //	bool usb_update_flag = false;
 //	bool update_flag = false;
-	bool resetEncoder = false;                      //!< A request to reset the encoder is pending.
+	bool resetEncoder = false;			//!< A request to reset the encoder is pending.
 };
 
 /**
@@ -72,22 +79,22 @@ struct AxisFlashAddresses
  */
 struct AxisConfig
 {
-	uint8_t enctype = 0; //!< Encoder type ID.
 	uint8_t drvtype = 0; //!< Motor driver type ID.
+	uint8_t enctype = 0; //!< Encoder type ID.
 };
-
 /**
  * @brief Holds the physical metrics of an axis at a point in time.
  */
 struct metric_t {
-	float accel = 0;          //!< Acceleration in deg/s².
-	float speed = 0;          //!< Speed in deg/s.
-	int32_t pos_scaled_16b = 0;       //!< Scaled position as a 16-bit integer (-0x7fff to 0x7fff).
-	float pos_f = 0;          //!< Scaled position as a float (-1.0 to 1.0).
+	float accel = 0;	  //!< Acceleration in deg/s².
+	float speed = 0;	  //!< Speed in deg/s.
+	int32_t pos_scaled_16b = 0;	  //!< Scaled position as a 16-bit integer (-0x7fff to 0x7fff).
+	float pos_f = 0;	  //!< Scaled position as a float (-1.0 to 1.0).
 	float posDegrees = 0; //!< Position in degrees, not scaled to the selected range.
-	int32_t torque = 0;       //!< Total torque applied to the axis.
+	int32_t torque = 0;	  //!< Total torque applied to the axis.
 };
 
+
 /**
  * @brief Holds the current and previous metrics for an axis, used for calculating derivatives.
  */
@@ -135,8 +142,7 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	const ClassIdentifier getInfo();
 	const ClassType getClassType() override {return ClassType::Axis;};
 
-	virtual std::string getHelpstring() { return "FFB axis" ;}
-
+	virtual std::string getHelpstring() { return "FFB axis"	;}
 	// Dynamic classes
 	/**
 	 * @brief Sets the motor driver type.
@@ -243,7 +249,7 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	 * @param enc A pointer to the encoder.
 	 * @return The angle in degrees.
 	 */
-	float   getEncAngle(Encoder *enc);
+	float 	getEncAngle(Encoder *enc);
 
 	/**
 	 * @brief Sets the maximum power (torque) of the motor.
@@ -314,9 +320,11 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	void setFxStrengthAndFilter(uint8_t val,uint8_t& valToSet, Biquad& filter);
 
 	/**
-	 * @brief Called before HID effects are calculated.
+	 * @brief Calculates the mechanical effects (damper, friction, inertia) that are always active.
+	 * These are separate from the HID FFB effects sent by the game.
+	 * @param ffb_on A flag indicating if FFB is on.
 	 */
-	void calculateAxisEffects(bool ffb_on);
+	void calculateMechanicalEffects(bool ffb_on);
 
 	/**
 	 * @brief Gets the current torque.
@@ -325,9 +333,16 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	int32_t getTorque();
 
 	/**
-	 * @brief Calculate soft endstop effect.
+	 * @brief Calculates the endstop torque.
+	 * @return The calculated endstop torque.
 	 */
-	int16_t updateEndstop();
+	int32_t calculateEndstopTorque();
+
+	/**
+	 * @brief Calculates the FFB torque exponential torque, from the input torque and apply expo and scaler
+	 * @return The calculated exponential torque.
+	 */
+	int64_t calculateFFBTorque();
 
 	/**
 	 * @brief Starts a force fade-in.
@@ -339,10 +354,10 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	metric_t* getMetrics();
 
 	/**
-	 * @brief Sets the HID FFB effect torque.
-	 * @param torque The new FFB effect torque.
+	 * @brief Sets the FFB effect torque.
+	 * @param torque The new FFB effect torque from the EffectsCalculator.
 	 */
-	void setEffectTorque(int32_t torque);
+	void setFfbEffectTorque(int64_t torque);
 
 	/**
 	 * @brief Updates the total torque.
@@ -386,15 +401,11 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	 * @return The torque scaler value.
 	 */
 	bool isInverted();
-
-	void updateTorqueScaler();
-	float getTorqueScaler();
-
 	/**
-	 * @brief Sets the ratio between game effects and endstop force.
+	 * @brief Sets the ratio between game effects and endstop force and updates the internal torque scaler based on power and fx_ratio
 	 * @param val The new ratio value (0-255).
 	 */
-	void setFxRatio(uint8_t val);
+	void setEffectRatio(uint8_t val);
 	/**
 	 * @brief Sets the exponential torque curve.
 	 * @param val The new expo value.
@@ -402,13 +413,17 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	void setExpo(int val);
 
 	int32_t calculateExpoTorque(int32_t torque);
-
+	/**
+	 * @brief Applies the speed limiter PI controller to the torque.
+	 * @param torque A reference to the torque value to be modified.
+	 * @return torque update to apply to reduced de speed.
+	 */
+	int64_t applySpeedLimiterTorque(int64_t& torque);
 	/**
 	 * @brief Applies the torque slew rate limiter to the torque.
 	 * @param torque A reference to the torque value to be modified.
 	 */
 	void applyTorqueSlewRateLimiter(int64_t& torque);
-
 	/**
 	 * @brief Decodes the axis configuration from a 16-bit integer stored in flash.
 	 * @param val The 16-bit encoded configuration value.
@@ -421,60 +436,66 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	 */
 	static uint16_t encodeConfToInt(AxisConfig conf);
 
+
+
 	// Member variables
-	AxisFlashAddresses flashAddresses;                      //!< Flash memory addresses for this axis.
-	volatile Control_t* control;            //!< Pointer to the global control structure.
-	AxisConfig conf;                                        //!< Configuration for this axis (driver and encoder types).
-	char axis;                                                      //!< Axis identifier ('X', 'Y', 'Z').
+	AxisFlashAddresses flashAddresses;			//!< Flash memory addresses for this axis.
+	volatile Control_t* control;		//!< Pointer to the global control structure.
+	AxisConfig conf;					//!< Configuration for this axis (driver and encoder types).
+	char axis;							//!< Axis identifier ('X', 'Y', 'Z').
 
 	std::unique_ptr<MotorDriver> drv = std::make_unique<MotorDriver>(); //!< Unique pointer to the active motor driver.
-	std::shared_ptr<Encoder> enc = nullptr; //!< Shared pointer to the active encoder.
+	std::shared_ptr<Encoder> enc = nullptr;	//!< Shared pointer to the active encoder.
 
-	bool outOfBounds = false;                       //!< Flag indicating if the axis is out of its valid range.
+	bool outOfBounds = false;			//!< Flag indicating if the axis is out of its valid range.
 	const Error outOfBoundsError = Error(ErrorCode::axisOutOfRange,ErrorType::warning,"Axis out of bounds"); //!< Error object for out-of-bounds condition.
 
 	// Force fade-in effect
-	float forceFadeTime = 1.0;                      //!< Duration of the force fade-in in seconds.
-	float forceFadeCurMult = 1.0;                //!< Current multiplier for the force fade-in.
+	float forceFadeDuration = 1.0;			//!< Duration of the force fade-in in seconds.
+	float forceFadeMultiplier = 1.0;		//!< Current multiplier for the force fade-in.
 
-	uint16_t degreesOfRotation = 900;               //!< Current degrees of rotation.
-	uint16_t lastdegreesOfRotation = degreesOfRotation; //!< Previous degrees of rotation (for smooth transitions).
+	float encoderOffset = 0; //!< Offset for absolute encoders.
+	uint16_t degreesOfRotation = 900;		//!< Current degrees of rotation.
+	uint16_t previousDegreesOfRotation = degreesOfRotation; //!< Previous degrees of rotation (for smooth transitions).
 	uint16_t nextDegreesOfRotation = degreesOfRotation; //!< Target degrees of rotation.
 
 	// Limiters
-	uint16_t maxSlewRate_Driver = MAX_SLEW_RATE;            //!< Maximum slew rate as measured by the driver (in units/ms).
-	uint16_t maxSpeedDegS  = 0;             //!< Maximum speed in degrees per second. 0 to disable.
-	uint32_t maxTorqueRateMS = 0;           //!< Maximum torque rate of change per millisecond. 0 to disable.
+	uint16_t maxSlewRate_Driver = MAX_SLEW_RATE;		//!< Maximum slew rate as measured by the driver (in units/ms).
+	uint16_t maxSpeedDegS  = 0;		//!< Maximum speed in degrees per second. 0 to disable.
+	uint32_t maxTorqueRateMS = 0;		//!< Maximum torque rate of change per millisecond. 0 to disable.
 
 	float speedLimiterP = AXIS_SPEEDLIMITER_P; //!< Proportional term for the speed limiter.
 	float speedLimiterI = AXIS_SPEEDLIMITER_I; //!< Integral term for the speed limiter.
-
+#ifdef USE_DSP_FUNCTIONS
+	arm_pid_instance_f32 speedLimiterPID; //!< PID instance for the speed limiter.
+#else
 	// Speed limiter PID
-	float spdlimitreducerI = 0;
+	float speedLimitReducerI = 0;
+#endif
 
 	// Axis metrics
-	axis_metric_t metric;                   //!< Current and previous physical metrics of the axis.
-	float _lastSpeed = 0;                   //!< Instantaneous speed from the last cycle.
+	axis_metric_t metric;			//!< Current and previous physical metrics of the axis.
+	float previousFrameSpeed = 0;			//!< Instantaneous speed from the last cycle, used for acceleration calculation.
 
 	// Torque components
-	int32_t effectTorque = 0;            //!< Torque from HID FFB effects.
-	int32_t axisEffectTorque = 0;     //!< Torque from mechanical effects.
+	int64_t ffbEffectTorque = 0;		//!< Torque from HID FFB effects.
+	int32_t mechanicalEffectTorque = 0;	//!< Torque from mechanical effects (damper, friction, inertia).
 
 	// Power and scaling
-	uint16_t power = 5000;                  //!< Maximum motor power/torque.
-	uint8_t fx_ratio_i = 204;              //!< Ratio of HID effects vs. endstop effects (0-255).
-	float effect_margin_scaler = 0;    //!< Scaler for HID effects based on effectRatio.
-	float torqueScaler = 0;                 //!< Final torque scaler based on power.
+	uint16_t power = 5000;			//!< Maximum motor power/torque.
+	uint8_t effectRatio = 204;		//!< Ratio of HID effects vs. endstop effects (0-255).
+	float effectRatioScaler = 0;	//!< Scaler for HID effects based on effectRatio.
+	float torqueScaler = 0;			//!< Final torque scaler based on power.
 
 	// Axis configuration
-	bool invertAxis = true;                 //!< Invert axis direction.
-	uint8_t endstopStrength = 127;  //!< Stiffness of the endstop effect.
-	const float endstopGain = 25;   //!< Overall maximum endstop intensity.
+	bool invertAxis = true;			//!< Invert axis direction.
+	uint8_t endstopStrength = 127;	//!< Stiffness of the endstop effect.
+	const float endstopGain = 25;	//!< Overall maximum endstop intensity.
 
 	// Idle spring effect
-	uint8_t idlespringstrength = 127; //!< Strength of the idle spring.
-	int16_t idlespringclip = 0;       //!< Maximum force for the idle spring.
-	float idlespringscale = 0;        //!< Scaler for the idle spring force.
+	uint8_t idleSpringStrength = 127; //!< Strength of the idle spring.
+	int16_t idleSpringClip = 0;       //!< Maximum force for the idle spring.
+	float idleSpringScale = 0;        //!< Scaler for the idle spring force.
 	bool motorWasNotReady = true;     //!< Flag to detect motor readiness transition.
 
 	// Slew rate calibration tracking: true when Axis requested a calibration and
@@ -490,12 +511,12 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	const biquad_constant_t filterInertiaCst = {20, 20};    //!< Inertia filter constants.
 	uint8_t filterProfileId = 1; //!< Currently selected filter profile ID.
 	float filter_f = 1000.0; // 1khz
-	const int32_t intFxClip = 20000; //!< Clipping value for internal effects.
+	const int32_t internalFxClip = 20000; //!< Clipping value for internal effects.
 
 	// Internal effects intensity
-	uint8_t damperIntensity = 30;   //!< Intensity of the internal damper effect.
-	uint8_t frictionIntensity = 0;  //!< Intensity of the internal friction effect.
-	uint8_t inertiaIntensity = 0;   //!< Intensity of the internal inertia effect.
+	uint8_t damperIntensity = 30;	//!< Intensity of the internal damper effect.
+	uint8_t frictionIntensity = 0;	//!< Intensity of the internal friction effect.
+	uint8_t inertiaIntensity = 0;	//!< Intensity of the internal inertia effect.
 
 	// Biquad filter instances
 	Biquad speedFilter = Biquad(BiquadType::lowpass, filterSpeedCst[filterProfileId].freq/filter_f, filterSpeedCst[filterProfileId].q/100.0, 0.0);
@@ -505,10 +526,10 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	Biquad inertiaFilter = Biquad(BiquadType::lowpass, filterInertiaCst.freq/filter_f, filterInertiaCst.q / 100.0, 0.0);
 
 	// Post-processing
-	GearRatio_t gearRatio;  //!< Gear ratio between encoder and axis.
-	int expoValInt = 0;              //!< Raw integer value for the expo curve.
-	float expo = 1;                 //!< Calculated exponent for the torque curve.
-	float expoScaler = 50;  //!< Scaler for the expo calculation : 0.28 to 3.54
+	GearRatio_t gearRatio;	//!< Gear ratio between encoder and axis.
+	int expoValue = 0;		//!< Raw integer value for the expo curve. Formula: v = val*2 => v<0 ? 1/-v : v
+	float expo = 1;			//!< Calculated exponent for the torque curve.
+	float expoScaler = 50;	//!< Scaler for the expo calculation : 0.28 to 3.54
 };
 
 #endif /* SRC_AXIS_H_ */
diff --git a/Firmware/FFBoard/Inc/EffectsCalculator.h b/Firmware/FFBoard/Inc/EffectsCalculator.h
index 5e43c9285..e22d71cd6 100644
--- a/Firmware/FFBoard/Inc/EffectsCalculator.h
+++ b/Firmware/FFBoard/Inc/EffectsCalculator.h
@@ -8,6 +8,7 @@
 #ifndef EFFECTSCALCULATOR_H_
 #define EFFECTSCALCULATOR_H_
 
+//#include "ClassChooser.h"
 #include "ffb_defs.h"
 #include "PersistentStorage.h"
 #include "CommandHandler.h"
@@ -26,8 +27,15 @@
 class Axis;
 struct metric_t;
 
+enum class ReconFilterMode : uint8_t {
+	NO_RECONSTRUCTION = 0,
+    LINEAR_INTERPOLATION = 1, 	// More responsive, attempts to match the game's signal precisely. Can feel "grainy" if slew rate is high.
+	SPLINE_CUBIC_NATURAL = 2,	// Highest fidelity, smooth curve through last 4 points. More CPU intensive.
+    SPLINE_CUBIC_HERMITE = 3  	// Mixed solution, good fidelity and optimized timing
+};
+
 /**
- * @brief Default effect gains.
+ * @brief Default gains for conditional effects.
  */
 struct effect_gain_t {
 	uint8_t friction = 254;
@@ -37,32 +45,32 @@ struct effect_gain_t {
 };
 
 /**
- * @brief Default effect scalers.
+ * @brief Scaler values for conditional effects to adjust their intensity.
  */
 struct effect_scaler_t {
-	float friction = 1.0;
+	float friction = 1.0; //0.4 * 40;
 	float spring = 16.0;
-	float damper = 4.0;
-	float inertia = 2.0;
+	float damper = 4.0; //2 * 40 * 2
+	float inertia = 2.0;//0.5 * 40;
 };
 
 /**
- * @brief Biquad filter constants for different effect types.
+ * @brief Biquad filter coefficients for various effects.
  */
 struct effect_biquad_t {
-	biquad_constant_t constant      = { 500, 70 };
-	biquad_constant_t friction      = { 50, 20 };
-	biquad_constant_t damper        = { 30, 40 };
-	biquad_constant_t inertia       = { 15, 20 };
+	biquad_constant_t constant	= { 500, 70 };
+	biquad_constant_t friction 	= { 50, 20 };
+	biquad_constant_t damper 	= { 30, 40 };
+	biquad_constant_t inertia	= { 15, 20 };
 };
 
 /**
- * @brief Holds statistics for an effect type.
+ * @brief Statistics for each effect type.
  */
 struct effect_stat_t {
-	std::array<int16_t,MAX_AXIS> current={0}; //!< Current force for each axis.
-	std::array<int16_t,MAX_AXIS> max={0};     //!< Maximum force for each axis.
-	uint16_t nb=0;                             //!< Number of samples.
+	std::array<int16_t,MAX_AXIS> current={0}; //!< Current force value.
+	std::array<int16_t,MAX_AXIS> max={0};     //!< Maximum force value since reset.
+	uint16_t nb=0;                           //!< Number of times this effect has been used.
 };
 
 enum class EffectsCalculator_commands : uint32_t {
@@ -70,10 +78,12 @@ enum class EffectsCalculator_commands : uint32_t {
 	damper_f, damper_q, friction_f, friction_q, inertia_f, inertia_q, filterProfileId,
 	frictionPctSpeedToRampup,
 	monitorEffect, effectsDetails, effectsForces,
+	reconFilterMode
 };
 
 /**
- * @brief This class calculates the final torque for each axis based on the received HID FFB effects.
+ * @brief This class is responsible for calculating the forces of all active FFB effects.
+ * It runs in its own thread to perform the calculations.
  */
 class EffectsCalculator: public PersistentStorage,
 						 public CommandHandler,
@@ -82,7 +92,11 @@ class EffectsCalculator: public PersistentStorage,
 	EffectsCalculator();
 	virtual ~EffectsCalculator();
 
-	static ClassIdentifier info;
+	static ClassIdentifier info; //!< Static class identifier.
+	/**
+	 * @brief Returns the class identifier.
+	 * @return The class identifier.
+	 */
 	const ClassIdentifier getInfo();
 	const ClassType getClassType() override {return ClassType::Internal;};
 
@@ -91,6 +105,7 @@ class EffectsCalculator: public PersistentStorage,
 	 * @override from PersistentStorage
 	 */
 	void saveFlash() override;
+
 	/**
 	 * @brief Restores effect settings from flash memory.
 	 * @override from PersistentStorage
@@ -102,70 +117,91 @@ class EffectsCalculator: public PersistentStorage,
 	 * @return true if active, false otherwise.
 	 */
 	bool isActive();
+
 	/**
 	 * @brief Sets the active state of the effects calculator.
-	 * @param active true to activate, false to deactivate.
+	 * @param active The new active state.
 	 */
 	void setActive(bool active);
+
 	/**
-	 * @brief Calculates the combined force of all active effects for each axis.
-	 * @param axes A vector of unique pointers to the axis objects.
+	 * @brief This is the main calculation method. It iterates through all active effects and sums up the forces for each axis.
+	 * @param axes A reference to the vector of axes.
 	 */
 	void calculateEffects(std::vector<std::unique_ptr<Axis>> &axes);
+
 	/**
 	 * @brief Sets the filters for a specific effect.
-	 * @param effect A pointer to the FFB_Effect object.
+	 * @param effect A pointer to the effect.
 	 */
 	virtual void setFilters(FFB_Effect* effect);
+
 	/**
 	 * @brief Sets the global gain for all effects.
-	 * @param gain The gain value (0-255).
+	 * @param gain The new gain value.
 	 */
 	void setGain(uint8_t gain);
+
 	/**
 	 * @brief Gets the global gain.
-	 * @return The global gain value.
+	 * @return The current global gain value.
 	 */
 	uint8_t getGain();
+
 	/**
-	 * @brief Logs an effect type as active or inactive.
-	 * @param type The effect type ID.
-	 * @param remove true to log as inactive, false as active.
+	 * @brief Logs the usage of an effect type for statistics.
+	 * @param type The effect type.
+	 * @param remove True to decrement the usage count, false to increment.
 	 */
 	void logEffectType(uint8_t type,bool remove = false);
+
 	/**
-	 * @brief Calculates statistics for an effect type.
-	 * @param type The effect type ID.
-	 * @param force The calculated force value.
-	 * @param axis The axis ID.
+	 * @brief Calculates and stores statistics for a given effect type.
+	 * @param type The effect type.
+	 * @param force The calculated force.
+	 * @param axis The axis index.
 	 */
 	void calcStatsEffectType(uint8_t type, int32_t force,uint8_t axis);
+
 	/**
-	 * @brief Logs the state of an effect (active, inactive, etc.).
-	 * @param type The effect type ID.
-	 * @param state The state value.
+	 * @brief Logs the state of an effect (start/stop).
+	 * @param type The effect type.
+	 * @param state The new state.
 	 */
 	void logEffectState(uint8_t type,uint8_t state);
+
 	/**
-	 * @brief Resets the log of active effects.
-	 * @param reinit If true, reinitializes the log.
+	 * @brief Resets the statistics of active effects.
+	 * @param reinit If true, re-initializes the stats based on currently active effects.
 	 */
 	void resetLoggedActiveEffects(bool reinit);
 
 	/**
-	 * @brief Finds a free effect slot in the effects array.
-	 * @param type The effect type ID.
-	 * @return The index of the free slot, or -1 if none found.
+	 * @brief Finds a free slot for a new effect.
+	 * @param type The type of the effect.
+	 * @return The index of the free slot, or -1 if no slot is available.
 	 */
 	int32_t find_free_effect(uint8_t type);
+
 	/**
 	 * @brief Frees an effect slot.
-	 * @param idx The index of the slot to free.
+	 * @param idx The index of the effect to free.
 	 */
 	void free_effect(uint16_t idx);
 
 	/**
-	 * @brief Handles CLI commands for the effects calculator.
+     * @brief Met à jour les tampons d'interpolation pour un effet non-conditionnel.
+     * Appelé par le handler HID (ex: 60Hz).
+     * @param effect Pointeur vers l'effet à modifier.
+     * @param new_magnitude Nouvelle magnitude/amplitude cible.
+     * @param new_offset Nouvel offset cible (utilisé uniquement si is_periodic est true).
+     * @param is_periodic Si true, met à jour les deux tampons (mag + offset).
+     */
+    void updateEffectReconstruction(FFB_Effect* effect, float new_magnitude, float new_offset, bool is_periodic);
+
+
+	/**
+	 * @brief Handles command line interface commands for the effects calculator.
 	 * @param cmd The parsed command.
 	 * @param replies A vector of replies to be sent back.
 	 * @return The status of the command execution.
@@ -178,123 +214,167 @@ class EffectsCalculator: public PersistentStorage,
 	std::array<FFB_Effect,max_effects> effects; //!< Main effects storage array.
 
 	/**
-	 * @brief Main thread function for the effects calculator.
+	 * @brief The main loop for the effects calculator thread.
+	 * @override from cpp_freertos::Thread
 	 */
 	void Run() override;
 
-	/**
-	 * @brief Updates the internal sample rate and recalculates filters.
-	 * @param newSamplerate The new sample rate in Hz.
-	 */
-	void updateSamplerate(float newSamplerate);
+	void updateSamplerate(float newSamplerate); // Must be called if update rate is changed to update filters and effects
 
 
 protected:
 
 private:
 	// Filters
-	effect_biquad_t filter[2];              //!< Filter profiles: 0 is default, CUSTOM_PROFILE_ID is custom.
-	uint8_t filterProfileId = 0;            //!< Currently selected filter profile ID.
-	uint32_t calcfrequency = 1000;          //!< Calculation frequency in Hz (default 1kHz).
-	const float qfloatScaler = 0.01;        //!< Scaler for Q factor.
+	effect_biquad_t filter[2];		//!< Filter coefficients for CFFilter, default (0) and custom (1) profiles.
+	uint8_t filterProfileId = 0;	//!< Currently active filter profile (0 or 1).
+	uint32_t calcfrequency = 1000; //!< Calculation frequency in Hz (matches HID rate 1khz).
+	const float qfloatScaler = 0.01;	 //!< Scaler for Q factor.
 
-	// Intensity and scaling
-	uint8_t global_gain = 0xff;             //!< Global gain for all effects.
-	effect_gain_t gain;                     //!< Individual effect type gains.
-	effect_scaler_t scaler;                 //!< Individual effect type scalers.
-	uint8_t frictionPctSpeedToRampup = 25;  //!< Friction ramp-up threshold (percentage of max speed).
+	// Gains and scalers
+	uint8_t global_gain = 0xff;		//!< Global gain for all effects.
+	effect_gain_t gain;				//!< Per-effect type gains.
+	effect_scaler_t scaler;			//!< Per-effect type intensity scalers.
+	uint8_t frictionPctSpeedToRampup = 25;	//!< Speed percentage for friction ramp-up. : define the max value of the range (0..5% of maxspeed) where torque is rampup on friction
 
 	// FFB status
-	bool effects_active = false;            //!< true if FFB is active.
-	uint32_t effects_used = 0;              //!< Mask of currently used effect slots.
-	std::array<effect_stat_t,12> effects_stats;     //!< Statistics for each effect type (current cycle).
-	std::array<effect_stat_t,12> effects_statslast; //!< Statistics from the previous cycle.
-	bool isMonitorEffect = false;           //!< Flag for effect monitoring.
+	bool effects_active = false;	//!< True if FFB is globally active.
+	uint32_t effects_used = 0;		//!< Bitmask of effect types used since reset.
+	std::array<effect_stat_t,12> effects_stats; //!< Statistics for each effect type, [0..12 effect types].
+	std::array<effect_stat_t,12> effects_statslast; //!< Statistics from the previous cycle, [0..12 effect types].
+	bool isMonitorEffect = false;	//!< Flag to enable effect monitoring.
 
 	/**
-	 * @brief Calculates the force component for a specific effect on a specific axis.
+	 * @brief Calculates the force for a single component of an effect on a specific axis.
+	 * Dispatches between conditional and non-conditional effects.
 	 */
-	int32_t calcComponentForce(FFB_Effect *effect, int32_t forceVector, std::vector<std::unique_ptr<Axis>> &axes, uint8_t axis);
+	int32_t calculateEffectForceOnAxis(FFB_Effect *effect, int32_t forceVector, std::vector<std::unique_ptr<Axis>> &axes, uint8_t axis);
+
 	/**
-	 * @brief Calculates the force for a non-conditional effect (e.g., Constant Force, Sine).
+	 * @brief Calculates the base force for non-conditional effects (Constant, Ramp, Periodic).
 	 */
 	int32_t calcNonConditionEffectForce(FFB_Effect* effect);
+
 	/**
-	 * @brief Returns the speed ramp-up factor for friction.
+	 * @brief Calculates the speed threshold for friction ramp-up.
 	 */
 	float speedRampupPct();
+
 	/**
-	 * @brief Calculates the force for a conditional effect (e.g., Spring, Damper, Friction).
+	 * @brief Calculates the force for conditional effects (Spring, Damper, Inertia, Friction).
 	 */
 	int32_t calcConditionEffectForce(FFB_Effect *effect, float metric, uint8_t gain, uint8_t idx, float scale, float angle_ratio);
+
 	/**
-	 * @brief Calculates the envelope magnitude for an effect.
+	 * @brief Calculates the magnitude of an effect with an envelope.
 	 */
-	int32_t getEnvelopeMagnitude(FFB_Effect *effect);
+	int32_t getEnvelopeMagnitude(FFB_Effect *effect, int32_t baseMagnitude);
+
 	/**
-	 * @brief Lists the currently used effects as a string.
+	 * @brief Generates a string listing the effects used.
 	 */
 	std::string listEffectsUsed(bool details = false,uint8_t axis = 0);
+
 	/**
-	 * @brief Checks and updates filter coefficients.
+	 * @brief Validates and sanitizes filter coefficients.
 	 */
 	void checkFilterCoeff(biquad_constant_t *filter, uint32_t freq,uint8_t q);
+
 	/**
-	 * @brief Updates filter settings for a specific effect type.
+	 * @brief Updates the filters for all active effects of a specific type.
 	 */
 	void updateFilterSettingsForEffects(uint8_t type_effect);
+
+	// --- Reconstruction Filter ---
+	ReconFilterMode reconFilterMode = ReconFilterMode::NO_RECONSTRUCTION;	//!< Current reconstruction filter mode.
+
+	/**
+	 * @brief Internal method to push a new value into the reconstruction filter.
+	 * @param state Pointer to the reconstruction filter state.
+	 * @param newValue The new value to push into the filter.
+	 */
+	void pushReconstructionSample(ReconFilterState* state, float newValue);
+
+	/**
+	 * @brief Retrieves the interpolated value from the reconstruction filter.
+	 * @param state Pointer to the reconstruction filter state.
+	 * @param fallbackValue Value to return if the filter is not ready.
+	 * @return The interpolated value from the filter.
+	 */
+	float evaluateReconstructionFilter(ReconFilterState* state, float fallbackValue);
 };
 
+/**
+ * Helper interface class for common effects calculator related control functions
+ */
 /**
  * @brief Helper interface class for common effects calculator related control functions.
+ * This class provides a common interface for controlling FFB effects.
  */
 class EffectsControlItf{
 public:
 	/**
-	 * @brief Enables or disables FFB globally.
+	 * @brief Enables or disables FFB.
+	 * @param state The new state of the FFB.
 	 */
 	virtual void set_FFB(bool state) = 0;
+
+	/**
+	 * @brief Stops the FFB.
+	 */
 	virtual void stop_FFB(){set_FFB(false);};
+
+	/**
+	 * @brief Starts the FFB.
+	 */
 	virtual void start_FFB(){set_FFB(true);};
+
 	/**
 	 * @brief Resets all FFB effects.
 	 */
 	virtual void reset_ffb() = 0;
+
 	/**
-	 * @brief Returns the estimated constant force update rate in Hz.
+	 * @brief Returns an estimate of the constant force effect update rate in Hz.
+	 * @return The estimated update rate.
 	 */
 	virtual uint32_t getConstantForceRate();
+
 	/**
-	 * @brief Returns the estimated overall effect update speed in Hz.
+	 * @brief Returns an estimate of the overall effect update speed in Hz.
+	 * @return The estimated update rate.
 	 */
 	virtual uint32_t getRate();
+
 	/**
 	 * @brief Checks if FFB is active.
+	 * @return true if FFB is active, false otherwise.
 	 */
 	virtual bool getFfbActive() = 0;
+
 	/**
-	 * @brief Sets the global FFB gain.
+	 * @brief Sets the global gain for all effects.
+	 * @param gain The new gain value.
 	 */
 	virtual void set_gain(uint8_t gain) = 0;
+
 	/**
-	 * @brief Event handler for constant force updates.
+	 * @brief To be called when a constant force update event occurs. Used for rate calculation.
 	 */
 	virtual void cfUpdateEvent();
+
 	/**
-	 * @brief Event handler for effect updates.
+	 * @brief To be called when any FFB update event occurs. Used for rate calculation.
 	 */
 	virtual void fxUpdateEvent();
-	/**
-	 * @brief Updates the sample rate for filters and effects.
-	 */
-	virtual void updateSamplerate(float newSamplerate) = 0;
+	virtual void updateSamplerate(float newSamplerate) = 0; // Should be called when update loop rate is changed
 
 private:
-	FastMovingAverage<float> fxPeriodAvg{5};          //!< Average period between effect updates.
-	FastMovingAverage<float> cfUpdatePeriodAvg{5};    //!< Average period between constant force updates.
+	FastMovingAverage<float> fxPeriodAvg{5};
+	FastMovingAverage<float> cfUpdatePeriodAvg{5};
 
-	uint32_t lastFxUpdate = 0; //!< Time of last effect update.
-	uint32_t lastCfUpdate = 0; //!< Time of last constant force update.
+	uint32_t lastFxUpdate = 0;	//!< Timestamp of the last FFB update.
+	uint32_t lastCfUpdate = 0;	//!< Timestamp of the last constant force update.
 };
 
 #endif /* EFFECTSCALCULATOR_H_ */
diff --git a/Firmware/FFBoard/Inc/Filters.h b/Firmware/FFBoard/Inc/Filters.h
index 92c2a7959..e2ec290e8 100644
--- a/Firmware/FFBoard/Inc/Filters.h
+++ b/Firmware/FFBoard/Inc/Filters.h
@@ -10,7 +10,8 @@
 #include "cppmain.h"
 
 #ifdef USE_DSP_FUNCTIONS
-#include "arm_math.h"
+#include "arm_math_types.h"
+#include "dsp/filtering_functions.h"
 #endif
 
 #ifdef __cplusplus
diff --git a/Firmware/FFBoard/Inc/HidFFB.h b/Firmware/FFBoard/Inc/HidFFB.h
index 3e776958e..bb0d0cfcd 100644
--- a/Firmware/FFBoard/Inc/HidFFB.h
+++ b/Firmware/FFBoard/Inc/HidFFB.h
@@ -21,138 +21,183 @@
 #define DIRECTION_ENABLE(AXES) (1 << AXES)
 
 /**
- * @brief This class handles HID FFB reports from the USB host.
- * It decodes the reports and updates the effects in the EffectsCalculator.
+ * @brief This class handles the Force Feedback (FFB) HID communication.
+ * It inherits from UsbHidHandler to process HID reports and from EffectsControlItf to control FFB effects.
+ * It is responsible for parsing HID FFB reports from the host and managing the lifecycle of FFB effects.
  */
 class HidFFB: public UsbHidHandler, public EffectsControlItf {
 public:
 	/**
 	 * @brief Construct a new HidFFB object.
-	 * @param ec Shared pointer to the EffectsCalculator.
-	 * @param axisCount Number of FFB axes.
+	 * @param ec A shared pointer to the EffectsCalculator instance.
+	 * @param axisCount The number of axes to be controlled.
 	 */
 	HidFFB(std::shared_ptr<EffectsCalculator> ec,uint8_t axisCount);
 	virtual ~HidFFB();
 
 	/**
-	 * @brief Handles HID OUT reports.
+	 * @brief Handles outgoing HID reports from the host (PC -> Device). This is the main entry point for FFB commands.
+	 * @param report_id The report ID.
+	 * @param report_type The type of the report.
+	 * @param buffer A pointer to the report data.
+	 * @param bufsize The size of the report data.
 	 * @override from UsbHidHandler
 	 */
 	void hidOut(uint8_t report_id, hid_report_type_t report_type,const uint8_t* buffer, uint16_t bufsize) override;
+
 	/**
-	 * @brief Handles HID GET reports.
+	 * @brief Handles incoming HID GET_FEATURE requests from the host (Device -> PC).
+	 * Used for status reports like block load and PID pool.
+	 * @param report_id The report ID.
+	 * @param report_type The type of the report.
+	 * @param buffer A pointer to the buffer where the report data should be stored.
+	 * @param reqlen The requested length of the report.
+	 * @return The actual length of the report.
 	 * @override from UsbHidHandler
 	 */
 	uint16_t hidGet(uint8_t report_id, hid_report_type_t report_type,uint8_t* buffer, uint16_t reqlen) override;
 
 	/**
-	 * @brief Checks if FFB is active.
-	 * @return true if active, false otherwise.
+	 * @brief Checks if the FFB is active.
+	 * @return true if FFB is active, false otherwise.
+	 * @override from EffectsControlItf
 	 */
 	bool getFfbActive() override;
+
 	/**
-	 * @brief Sends a HID report.
+	 * @brief Sends a HID report to the host.
+	 * @param report A pointer to the report data.
+	 * @param len The length of the report data.
+	 * @return true if the report was sent successfully, false otherwise.
 	 */
 	static bool HID_SendReport(uint8_t *report,uint16_t len);
 
 	/**
 	 * @brief Resets all FFB effects.
+	 * @override from EffectsControlItf
 	 */
 	void reset_ffb() override;
+
 	/**
-	 * @brief Starts FFB.
+	 * @brief Starts the FFB.
+	 * @override from EffectsControlItf
 	 */
 	void start_FFB() override;
+
 	/**
-	 * @brief Stops FFB.
+	 * @brief Stops the FFB.
+	 * @override from EffectsControlItf
 	 */
 	void stop_FFB() override;
+
 	/**
-	 * @brief Enables or disables FFB.
-	 * @param state true to enable, false to disable.
+	 * @brief Sets the FFB state (active/inactive).
+	 * @param state The new state of the FFB.
+	 * @override from EffectsControlItf
 	 */
 	void set_FFB(bool state) override;
+
 	/**
-	 * @brief Sets the global FFB gain.
-	 * @param gain The gain value (0-255).
+	 * @brief Sets the global gain for all FFB effects.
+	 * @param gain The new gain value (0-255).
+	 * @override from EffectsControlItf
 	 */
 	void set_gain(uint8_t gain) override;
 	
 	/**
-	 * @brief Sends a status report for a specific effect.
+	 * @brief Sends a status report for a specific effect to the host.
+	 * @param effect The ID of the effect.
 	 */
 	void sendStatusReport(uint8_t effect);
+
 	/**
-	 * @brief Sets the direction enable mask.
+	 * @brief Sets the direction enable mask for the axes.
+	 * This mask determines how direction parameters in HID reports are interpreted.
+	 * @param mask The new direction enable mask.
 	 */
 	void setDirectionEnableMask(uint8_t mask);
-	/**
-	 * @brief Updates the internal sample rate.
-	 * @param newSamplerate The new sample rate in Hz.
-	 */
-	void updateSamplerate(float newSamplerate) override;
+	void updateSamplerate(float newSamplerate);
 
 private:
-	// HID
-	std::shared_ptr<EffectsCalculator> effects_calc;
-	std::array<FFB_Effect,EffectsCalculator::max_effects>& effects; //!< Reference to the effects array in EffectsCalculator.
-	
+	// HID processing methods
+	std::shared_ptr<EffectsCalculator> effects_calc; //!< A shared pointer to the EffectsCalculator instance.
+	std::array<FFB_Effect,EffectsCalculator::max_effects>& effects; //!< A reference to the array of FFB effects, managed by EffectsCalculator, Must be passed in constructor.
+
 	/**
-	 * @brief Handles the creation of a new effect.
+	 * @brief Handles the "Create New Effect" HID report. Allocates a new effect.
+	 * @param effect Pointer to the HID report data.
 	 */
 	void new_effect(FFB_CreateNewEffect_Feature_Data_t* effect);
+
 	/**
-	 * @brief Handles freeing an effect slot.
+	 * @brief Frees an effect slot. This is called when a HID_ID_BLKFRREP is received.
+	 * @param id The block index of the effect to free.
 	 */
 	void free_effect(uint16_t id);
+
 	/**
-	 * @brief Decodes the FFB control command.
+	 * @brief Handles FFB control commands (Enable, Disable, Stop, Reset, etc.).
+	 * @param cmd The control command byte.
 	 */
 	void ffb_control(uint8_t cmd);
+
 	/**
-	 * @brief Decodes the set effect report.
+	 * @brief Handles the "Set Effect" HID report. Configures a previously created effect.
+	 * @param effect Pointer to the HID report data.
 	 */
 	void set_effect(FFB_SetEffect_t* effect);
+
 	/**
-	 * @brief Decodes the set condition report.
+	 * @brief Handles the "Set Condition" HID report (Spring, Damper, Friction, Inertia).
+	 * @param cond Pointer to the HID report data.
 	 */
 	void set_condition(FFB_SetCondition_Data_t* cond);
+
 	/**
-	 * @brief Decodes the set envelope report.
+	 * @brief Handles the "Set Envelope" HID report.
+	 * @param report Pointer to the HID report data.
 	 */
 	void set_envelope(FFB_SetEnvelope_Data_t* report);
+
 	/**
-	 * @brief Decodes the set ramp report.
+	 * @brief Handles the "Set Ramp" HID report.
+	 * @param report Pointer to the HID report data.
 	 */
 	void set_ramp(FFB_SetRamp_Data_t* report);
+
 	/**
-	 * @brief Decodes the set constant force report.
+	 * @brief Handles the "Set Constant Force" HID report.
+	 * @param effect Pointer to the HID report data.
 	 */
 	void set_constant_effect(FFB_SetConstantForce_Data_t* effect);
+
 	/**
-	 * @brief Decodes the set periodic report.
+	 * @brief Handles the "Set Periodic" HID report (Sine, Square, etc.).
+	 * @param report Pointer to the HID report data.
 	 */
 	void set_periodic(FFB_SetPeriodic_Data_t* report);
+
 	/**
-	 * @brief Decodes the effect operation report (start/stop/pause).
+	 * @brief Handles the "Effect Operation" HID report (Start, Stop, Start Solo).
+	 * @param report Pointer to the HID report data.
 	 */
 	void set_effect_operation(FFB_EffOp_Data_t* report);
 
-
 	/**
-	 * @brief Updates the filters for an effect.
+	 * @brief Sets the default filters for a new effect based on its type.
+	 * @param effect Pointer to the effect to configure.
 	 */
 	void set_filters(FFB_Effect* effect);
 
-	uint8_t directionEnableMask; //!< Mask for enabled directions.
-	uint16_t used_effects = 0;   //!< Number of currently used effect slots.
-	bool ffb_active = false;      //!< true if FFB is active.
-	FFB_BlockLoad_Feature_Data_t blockLoad_report; //!< HID Feature report: Block Load.
-	FFB_PIDPool_Feature_Data_t pool_report;           //!< HID Feature report: PID Pool.
+	uint8_t directionEnableMask; //!< Mask to enable/disable directions for axes. Adjusted based on axis count.
+	uint16_t used_effects = 0; //!< The number of currently active effects.
+	bool ffb_active = false; //!< Flag indicating if FFB is globally active.
+	FFB_BlockLoad_Feature_Data_t blockLoad_report; //!< HID report structure for block load status.
+	FFB_PIDPool_Feature_Data_t pool_report; //!< HID report structure for PID pool status.
 
-	reportFFB_status_t reportFFBStatus; //!< Holds the current FFB status.
+	reportFFB_status_t reportFFBStatus; //!< HID report structure for general FFB status.
 
-	uint8_t axisCount; //!< Number of FFB axes.
+	uint8_t axisCount; //!< The number of axes supported by this FFB instance.
 };
 
 #endif /* HIDFFB_H_ */
diff --git a/Firmware/FFBoard/Inc/ffb_defs.h b/Firmware/FFBoard/Inc/ffb_defs.h
index aa58dd978..20013804c 100644
--- a/Firmware/FFBoard/Inc/ffb_defs.h
+++ b/Firmware/FFBoard/Inc/ffb_defs.h
@@ -12,6 +12,11 @@
 #include "Filters.h"
 #include "constants.h" // For #define MAX_AXIS
 
+#ifdef USE_DSP_FUNCTIONS
+#include "arm_math_types.h"
+#include "dsp/interpolation_functions.h"
+#endif
+
 #define FFB_ID_OFFSET 0x00
 #define MAX_EFFECTS 40
 
diff --git a/Firmware/FFBoard/Src/Axis.cpp b/Firmware/FFBoard/Src/Axis.cpp
index 73708df60..ff7262278 100644
--- a/Firmware/FFBoard/Src/Axis.cpp
+++ b/Firmware/FFBoard/Src/Axis.cpp
@@ -103,6 +103,14 @@ const std::vector<class_entry<MotorDriver>> Axis::axis2_drivers =
  */
 Axis::Axis(char axis,volatile Control_t* control) :CommandHandler("axis", CLSID_AXIS), driverChooser(MotorDriver::all_drivers),encoderChooser{Encoder::all_encoders}
 {
+
+#ifdef USE_DSP_FUNCTIONS
+	speedLimiterPID.Kp = speedLimiterP;
+	speedLimiterPID.Ki = speedLimiterI;
+	speedLimiterPID.Kd = 0.0f;
+	arm_pid_init_f32(&speedLimiterPID, 1);
+#endif
+
 	this->axis = axis;
 	this->control = control;
 	if (axis == 'X')
@@ -129,6 +137,11 @@ Axis::Axis(char axis,volatile Control_t* control) :CommandHandler("axis", CLSID_
 										   ADR_AXIS3_SPEEDACCEL_FILTER,ADR_AXIS3_POSTPROCESS1});
 	}
 
+	// Initialize equalizer filters
+	/*for (uint8_t idx = 0; idx < num_eq_bands; idx++) {
+		eqFilters[idx].setBiquad(BiquadType::peak, eq_frequencies[idx] / filter_f, 1.0, 0.0);
+	}*/
+
 	CommandHandler::registerCommands(); // Internal commands
 	registerCommands();
 	restoreFlash(); // Load parameters
@@ -613,8 +626,13 @@ void Axis::calculateMechanicalEffects(bool ffb_on){
 		float speedRampupCeil = 4096;
 		float rampupFactor = 1.0;
 		if (fabs (speed) < speedRampupCeil) {								// if speed in the range to rampup we apply a sine curve
+#ifdef USE_DSP_FUNCTIONS
+			float phaseRad = PI * ((fabsf (speed) / speedRampupCeil) - 0.5f);// we start to compute the normalized angle (speed / normalizedSpeed@5%) and translate it of -1/2PI to translate sin on 1/2 periode
+			rampupFactor = ( 1.0f + arm_sin_f32(phaseRad ) ) / 2.0f;			// sin value is -1..1 range, we translate it to 0..2 and we scale it by 2
+#else
 			float phaseRad = M_PI * ((fabsf (speed) / speedRampupCeil) - 0.5f);// we start to compute the normalized angle (speed / normalizedSpeed@5%) and translate it of -1/2PI to translate sin on 1/2 periode
 			rampupFactor = ( 1.0f + sinf(phaseRad ) ) / 2.0f;			// sin value is -1..1 range, we translate it to 0..2 and we scale it by 2
+#endif
 		}
 		int8_t sign = speed >= 0 ? 1 : -1;
 		float force = (float)frictionIntensity * rampupFactor * sign * INTERNAL_AXIS_FRICTION_SCALER * 32;
@@ -642,6 +660,10 @@ void Axis::resetMetrics(float new_pos= 0) { // pos is degrees
 	// Reset filters
 	speedFilter.calcBiquad();
 	accelFilter.calcBiquad();
+
+#ifdef USE_DSP_FUNCTIONS
+	arm_pid_reset_f32(&speedLimiterPID);	// reset the PID limit
+#endif
 }
 
 /**
@@ -703,7 +725,7 @@ int64_t Axis::calculateFFBTorque() {
 	return torque;
 }
 
-int32_t Axis::getTorque() { return metric.current.torque; }
+int32_t Axis::getTorque() { return metric.current.torque; } // Fix: move from previous to current
 
 bool Axis::isInverted() {
 	return invertAxis;
@@ -765,16 +787,20 @@ bool Axis::updateTorque(int32_t* totalTorque) {
 		forceFadeMultiplier += forceFadeDuration / this->filter_f;
 	}
 
-	// STEP 6: Dynamic limiters (Speed & Slew Rate)
-	// CRITICAL: Slew Rate compares the target value with "metric.previous.torque"
-	// (which is the clipped physical torque from the previous cycle).
-	// It MUST be applied on the final scaled torque!
+	// STEP 6: Dynamic limiters (Speed)
+	// Must be applied while torque is still in the Game Coordinate System (matching current.speed).
 	torque -= applySpeedLimiterTorque(torque);
-	applyTorqueSlewRateLimiter(torque);
 
-	// STEP 7: Axis inversion and final hardware clipping
+	// STEP 7: Axis inversion and Slew Rate Limiter
+	// Axis inversion must be done BEFORE Slew Rate because previous.torque is in hardware CS.
 	torque = (invertAxis) ? -torque : torque;
 
+	// CRITICAL: Slew Rate compares the target value with "metric.previous.torque"
+	// (which is the clipped physical torque from the previous cycle).
+	// It MUST be applied on the final physical torque after inversion!
+	applyTorqueSlewRateLimiter(torque);
+
+	// STEP 8: Final hardware clipping
 	int32_t torqueAfterClipping = clip<int32_t, int32_t>((int32_t)torque, -power, power);
 	
 	if (torqueAfterClipping != torque){
@@ -805,7 +831,7 @@ void Axis::applyTorqueSlewRateLimiter(int64_t& torque)
 
 	// The torque is clipped to be within the range of [previous torque - limit, previous torque + limit].
 	torque = clip<int64_t>(torque, previousTorque - maxTorqueChange, previousTorque + maxTorqueChange);
-}
+		}
 
 int64_t Axis::applySpeedLimiterTorque(int64_t& torque){
 	// Speed Limiter: A PI controller to reduce torque when speed exceeds maxSpeedDegS.
@@ -818,6 +844,29 @@ int64_t Axis::applySpeedLimiterTorque(int64_t& torque){
 
 	int64_t resultTorque = 0;
 
+#ifdef USE_DSP_FUNCTIONS
+	float effectiveSpeed = metric.current.speed * (torque > 0 ? 1.0f : -1.0f);
+	if (effectiveSpeed > maxSpeedDegS)
+	{
+		// --- PI Controller Logic ---
+		// 1. Calculate the error term (how much we are over the speed limit).
+		float speedError = effectiveSpeed - maxSpeedDegS;
+
+		// 2. Calculate the total reduction amount using the PID controller.
+		float reductionAmount = arm_pid_f32(&speedLimiterPID, speedError);
+
+		// 3. Apply the reduction to the main torque.
+		// We must only reduce the magnitude of the torque, not invert it.
+		reductionAmount = clip(reductionAmount, 0.0f, fabsf((float)torque));
+		if(torque > 0) {
+			resultTorque = reductionAmount;
+		} else {
+			resultTorque = -reductionAmount;
+		}
+	} else {
+		arm_pid_reset_f32(&speedLimiterPID); // Reset PID if not active
+	}
+#else
 	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);
@@ -831,6 +880,7 @@ int64_t Axis::applySpeedLimiterTorque(int64_t& torque){
 	}else{
 		resultTorque = clip<float,int64_t>(-torqueReduction,torque,0);
 	}
+#endif
 
 	return resultTorque;
 }
@@ -1109,6 +1159,14 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector<CommandReply>&
 			if(this->getEncoder() != nullptr){
 				cpr = this->getEncoder()->getCpr();
 			}
+			// TODO: For TMC4671 drivers, CPR reporting might be inconsistent. Investigate if a prescale is needed or if the UI should handle the readout correction.
+//#ifdef TMC4671DRIVER // CPR should be consistent with position. Maybe change TMC to prescale to encoder count or correct readout in UI
+//			TMC4671 *tmcdrv = dynamic_cast<TMC4671 *>(this->drv.get()); // Special case for TMC. Get the actual encoder resolution
+//			if (tmcdrv && tmcdrv->hasIntegratedEncoder())
+//			{
+//				cpr = tmcdrv->getEncCpr();
+//			}
+//#endif
 			replies.emplace_back(cpr);
 		}else{
 			return CommandStatus::ERR;
diff --git a/Firmware/FFBoard/Src/EffectsCalculator.cpp b/Firmware/FFBoard/Src/EffectsCalculator.cpp
index 1fe388673..22472b3ec 100644
--- a/Firmware/FFBoard/Src/EffectsCalculator.cpp
+++ b/Firmware/FFBoard/Src/EffectsCalculator.cpp
@@ -11,6 +11,16 @@
 #include "Axis.h"
 #include "ledEffects.h"
 
+#ifdef USE_DSP_FUNCTIONS
+#include "arm_math_types.h"
+#include "dsp/fast_math_functions.h"
+#include "dsp/interpolation_functions.h"
+#define MATH_PI PI
+#define MATH_SIN(x) arm_sin_f32(x)
+#else
+#define MATH_PI M_PI
+#define MATH_SIN(x) sinf(x)
+#endif
 
 #define EFFECT_STATE_INACTIVE 0
 
@@ -49,6 +59,7 @@ EffectsCalculator::EffectsCalculator() : CommandHandler("fx", CLSID_EFFECTSCALC)
 	registerCommand("filterProfile_id", EffectsCalculator_commands::filterProfileId, "Conditional effects filter profile: 0 default; 1 custom", CMDFLAG_GET | CMDFLAG_SET);
 
 	registerCommand("frictionPctSpeedToRampup", EffectsCalculator_commands::frictionPctSpeedToRampup, "% of max speed for gradual increase", CMDFLAG_GET | CMDFLAG_SET);
+	registerCommand("reconFilterMode", EffectsCalculator_commands::reconFilterMode, "Recon. filter: 0=None, 1=Linear, 2=CubicNatural, 3=CubicHermite", CMDFLAG_GET | CMDFLAG_SET);
 
 	//this->Start(); // Enable if we want to periodically monitor
 }
@@ -103,69 +114,59 @@ An inertia condition uses axis acceleration as the metric.
  */
 void EffectsCalculator::calculateEffects(std::vector<std::unique_ptr<Axis>> &axes)
 {
-	for (auto &axis : axes) {
-		axis->calculateAxisEffects(isActive());
-	}
-
-	if(!isActive()){
-		for (auto &axis : axes) {
-			axis->setEffectTorque(0);
-		}
-		return;
-	}
-
-	int32_t force = 0;
 	int axisCount = axes.size();
 	int32_t forces[MAX_AXIS] = {0};
 
-	for (uint8_t i = 0; i < effects_stats.size(); i++)
-	{
-		effects_stats[i].current = {0}; // Reset active effect forces
-	}
+	if(isActive()){
+		int32_t force = 0;
+		for (uint8_t i = 0; i < effects_stats.size(); i++)
+		{
+			effects_stats[i].current = {0}; // Reset active effect forces
+		}
 
-	for (uint8_t fxi = 0; fxi < MAX_EFFECTS; fxi++)
-	{
-		FFB_Effect *effect = &effects[fxi];
+		for (uint8_t fxi = 0; fxi < MAX_EFFECTS; fxi++)
+		{
+			FFB_Effect *effect = &effects[fxi];
 
-		// Effect activated and not infinite (0 or 0xffff)
-		if (effect->state != EFFECT_STATE_INACTIVE && effect->duration != FFB_EFFECT_DURATION_INFINITE && effect->duration != 0){
-			// Start delay not yet reached
-			if(HAL_GetTick() < effect->startTime){
-				continue;
+			// Effect activated and not infinite (0 or 0xffff)
+			if (effect->state != EFFECT_STATE_INACTIVE && effect->duration != FFB_EFFECT_DURATION_INFINITE && effect->duration != 0){
+				// Start delay not yet reached
+				if(HAL_GetTick() < effect->startTime){
+					continue;
+				}
+				// If effect has expired make inactive
+				if (HAL_GetTick() - effect->startTime > effect->duration)
+				{
+					effect->state = EFFECT_STATE_INACTIVE;
+					for(uint8_t axis=0 ; axis < axisCount ; axis++)
+						calcStatsEffectType(effect->type, 0,axis); // record a 0 on the ended force
+				}
 			}
-			// If effect has expired make inactive
-			if (HAL_GetTick() - effect->startTime > effect->duration)
+
+			// Filter out inactive effects
+			if (effect->state == EFFECT_STATE_INACTIVE)
 			{
-				effect->state = EFFECT_STATE_INACTIVE;
-				for(uint8_t axis=0 ; axis < axisCount ; axis++)
-					calcStatsEffectType(effect->type, 0,axis); // record a 0 on the ended force
+				continue;
 			}
-		}
 
-		// Filter out inactive effects
-		if (effect->state == EFFECT_STATE_INACTIVE)
-		{
-			continue;
-		}
+			force = calcNonConditionEffectForce(effect);	// Compute the effect force
 
-		force = calcNonConditionEffectForce(effect);
-
-		for(uint8_t axis=0 ; axis < axisCount ; axis++) // Calculate effects for all axes
-		{
-			int32_t axisforce = calcComponentForce(effect, force, axes, axis);
-			calcStatsEffectType(effect->type, axisforce,axis);
-			forces[axis] += axisforce; // Do not clip yet to allow effects to subtract force correctly. Will not overflow as maxeffects * 0x7fff is less than int32 range
+			for(uint8_t axis=0 ; axis < axisCount ; axis++) // Calculate effects for all axes
+			{
+				int32_t axisforce = calculateEffectForceOnAxis(effect, force, axes, axis);
+				calcStatsEffectType(effect->type, axisforce,axis);
+				forces[axis] += axisforce; // Do not clip yet to allow effects to subtract force correctly. Will not overflow as maxeffects * 0x7fff is less than int32 range
+			}
 		}
+		effects_statslast = effects_stats;
 	}
 
 	// Apply summed force to axes
 	for(uint8_t i=0 ; i < axisCount ; i++)
 	{
-		forces[i] = clip<int32_t, int32_t>(forces[i], -0x7fff, 0x7fff); // Clip at effects summ
-		axes[i]->setEffectTorque(forces[i]);
+		axes[i]->calculateMechanicalEffects(isActive());
+		axes[i]->setFfbEffectTorque(forces[i]);
 	}
-
-	effects_statslast = effects_stats;
 }
 
 /**
@@ -173,13 +174,45 @@ void EffectsCalculator::calculateEffects(std::vector<std::unique_ptr<Axis>> &axe
  * Periodic and constant effects
  */
 int32_t EffectsCalculator::calcNonConditionEffectForce(FFB_Effect *effect) {
-	int32_t force_vector = 0;
-	int32_t magnitude = effect->magnitude;
 
-	// If using an envelope modulate the magnitude based on time
-	if(effect->useEnvelope){
-		magnitude = getEnvelopeMagnitude(effect);
+	// Sanity check effect type must be non-conditional
+	// Avoid calculating reconstructed forces for conditional effects here
+	if (effect->type == FFB_EFFECT_SPRING ||
+		effect->type == FFB_EFFECT_DAMPER ||
+		effect->type == FFB_EFFECT_FRICTION ||
+		effect->type == FFB_EFFECT_INERTIA) {
+		return 0;
 	}
+
+	int32_t force_vector = 0;
+	
+    int32_t magnitude = 0;
+    int32_t offset_lrf = 0;
+
+    // Only calculate reconstruction and envelope if the effect actually uses them
+    if (effect->type != FFB_EFFECT_RAMP) {
+        // Get interpolated magnitude (or amplitude)
+        float interpolated_magnitude = evaluateReconstructionFilter(
+            &effect->recon_magnitude,    // Use the new structure
+            (float)effect->magnitude     // Fallback value (for NONE mode)
+        );
+
+        // Get interpolated offset
+        float interpolated_offset_float = evaluateReconstructionFilter(
+            &effect->recon_offset,       // Use the new structure
+            (float)effect->offset		 // Fallback value (for NONE mode)
+        );
+        offset_lrf = (int32_t)interpolated_offset_float;
+
+        // Magnitude with envelope if used
+        if(effect->useEnvelope){
+            magnitude = getEnvelopeMagnitude(effect, (int32_t)interpolated_magnitude);
+        } else {
+            magnitude = (int32_t)interpolated_magnitude;
+        }
+    }
+
+
 	switch (effect->type){
 
 	case FFB_EFFECT_CONSTANT:
@@ -200,14 +233,14 @@ int32_t EffectsCalculator::calcNonConditionEffectForce(FFB_Effect *effect) {
 	{
 		uint32_t elapsed_time = HAL_GetTick() - effect->startTime; // Square is ms aligned
 		int32_t force = ((elapsed_time + effect->phase) % ((uint32_t)effect->period + 2)) < (uint32_t)(effect->period + 2) / 2 ? -magnitude : magnitude;
-		force_vector = force + effect->offset;
+		force_vector = force + offset_lrf;
 		break;
 	}
 
 	case FFB_EFFECT_TRIANGLE:
 	{
 		int32_t force = 0;
-		int32_t offset = effect->offset;
+		int32_t offset = offset_lrf;
 		float elapsed_time = micros() - ((float)effect->startTime*1000.0);
 		uint32_t phase = effect->phase;
 		uint32_t period = effect->period;
@@ -230,7 +263,7 @@ int32_t EffectsCalculator::calcNonConditionEffectForce(FFB_Effect *effect) {
 
 	case FFB_EFFECT_SAWTOOTHUP:
 	{
-		float offset = effect->offset;
+		float offset = offset_lrf;
 		float elapsed_time = micros() - ((float)effect->startTime*1000.0);
 		uint32_t phase = effect->phase;
 		uint32_t period = effect->period;
@@ -248,7 +281,7 @@ int32_t EffectsCalculator::calcNonConditionEffectForce(FFB_Effect *effect) {
 
 	case FFB_EFFECT_SAWTOOTHDOWN:
 	{
-		float offset = effect->offset;
+		float offset = offset_lrf;
 		float elapsed_time = micros() - ((float)effect->startTime*1000.0);
 		float phase = effect->phase;
 		uint32_t period = effect->period;
@@ -269,8 +302,8 @@ int32_t EffectsCalculator::calcNonConditionEffectForce(FFB_Effect *effect) {
 		float t = (micros()/1000.0) - (float)effect->startTime;
 		float freq = 1.0f / (float)(std::max<uint16_t>(effect->period, 2));
 		float phase = (float)effect->phase / (float)35999; //degrees
-		float sine = sinf(2.0 * M_PI * (t * freq + phase)) * magnitude;
-		force_vector = (int32_t)(effect->offset + sine);
+		float sine = MATH_SIN(2.0f * MATH_PI * (t * freq + phase)) * magnitude;
+		force_vector = (int32_t)(offset_lrf + sine);
 		break;
 	}
 	default:
@@ -284,10 +317,15 @@ int32_t EffectsCalculator::calcNonConditionEffectForce(FFB_Effect *effect) {
 
 
 /**
- * Calculates the force of an effect
+ * @brief Calculates the final force of a single effect on a specific axis.
+ * It applies directional scaling and computes conditional effects (spring, damper, etc.) based on the axis's metrics.
+ * @param effect The effect to calculate.
+ * @param forceVector The base force from a non-conditional calculation (e.g., sine wave value).
+ * @param axes The list of all axes.
+ * @param axis The index of the axis to calculate the force for.
+ * @return The calculated torque for the axis.
  */
-
-int32_t EffectsCalculator::calcComponentForce(FFB_Effect *effect, int32_t forceVector, std::vector<std::unique_ptr<Axis>> &axes, uint8_t axis)
+int32_t EffectsCalculator::calculateEffectForceOnAxis(FFB_Effect *effect, int32_t forceVector, std::vector<std::unique_ptr<Axis>> &axes, uint8_t axis)
 {
 	int32_t result_torque = 0;
 //	uint16_t direction;
@@ -362,9 +400,8 @@ int32_t EffectsCalculator::calcComponentForce(FFB_Effect *effect, int32_t forceV
 			float rampupFactor = 1.0;
 			if (fabs (speed) < speedRampupCeil) {								// if speed in the range to rampup we apply a sinus curbe to ramup
 
-				float phaseRad = M_PI * ((fabs (speed) / speedRampupCeil) - 0.5);// we start to compute the normalized angle (speed / normalizedSpeed@5%) and translate it of -1/2PI to translate sin on 1/2 periode
-				rampupFactor = ( 1 + sin(phaseRad ) ) / 2;						// sin value is -1..1 range, we translate it to 0..2 and we scale it by 2
-
+				float phaseRad = MATH_PI * ((fabsf (speed) / speedRampupCeil) - 0.5f);// we start to compute the normalized angle (speed / normalizedSpeed@5%) and translate it of -1/2PI to translate sin on 1/2 periode
+				rampupFactor = ( 1.0f + MATH_SIN(phaseRad ) ) / 2.0f;			// sin value is -1..1 range, we translate it to 0..2 and we scale it by 2
 			}
 
 			int8_t sign = speed >= 0 ? 1 : -1;
@@ -449,12 +486,12 @@ int32_t EffectsCalculator::calcConditionEffectForce(FFB_Effect *effect, float  m
  * until the fade time where the strength changes to the fade level until the stop time of the effect.
  * Infinite effects can't have an envelope and return the normal magnitude.
  */
-int32_t EffectsCalculator::getEnvelopeMagnitude(FFB_Effect *effect)
+int32_t EffectsCalculator::getEnvelopeMagnitude(FFB_Effect *effect, int32_t baseMagnitude)
 {
 	if(effect->duration == FFB_EFFECT_DURATION_INFINITE || effect->duration == 0){
 		return effect->magnitude; // Effect is infinite. envelope is invalid
 	}
-	int32_t scaler = abs(effect->magnitude);
+	int32_t scaler = abs(baseMagnitude);
 	uint32_t elapsed_time = HAL_GetTick() - effect->startTime;
 	if (elapsed_time < effect->attackTime && effect->attackTime != 0)
 	{
@@ -468,7 +505,7 @@ int32_t EffectsCalculator::getEnvelopeMagnitude(FFB_Effect *effect)
 		scaler /= (int32_t)effect->fadeTime;
 		scaler += effect->fadeLevel;
 	}
-	scaler = signbit(effect->magnitude) ? -scaler : scaler; // Follow original sign of magnitude because envelope has no sign (important for constant force)
+	scaler = signbit(baseMagnitude) ? -scaler : scaler; // Follow original sign of magnitude because envelope has no sign (important for constant force)
 	return scaler;
 }
 
@@ -518,6 +555,200 @@ void EffectsCalculator::setFilters(FFB_Effect *effect){
 	}
 }
 
+// ==============================================================================
+// --- Reconstruction Filter ---
+// ==============================================================================
+
+void EffectsCalculator::updateEffectReconstruction(FFB_Effect* effect, float new_magnitude, float new_offset, bool is_periodic)
+{
+    // Push new magnitude into recon structure
+    pushReconstructionSample(&effect->recon_magnitude, new_magnitude);
+
+    // Push new offset into recon structure if periodic
+    if (is_periodic) {
+        pushReconstructionSample(&effect->recon_offset, new_offset);
+    }
+}
+
+void EffectsCalculator::pushReconstructionSample(ReconFilterState* state, float newValue)
+{
+    uint32_t now_us = micros();
+
+    // Shift the spline points
+    for (int i = 0; i < 3; ++i) {
+        state->spline_x[i] = state->spline_x[i + 1];
+        state->spline_y[i] = state->spline_y[i + 1];
+    }
+
+    // Add the new point
+    state->spline_x[3] = (float)now_us;
+    state->spline_y[3] = newValue;
+
+    // FIll in initial points if not ready yet at 60Hz, so 16.66ms intervals
+    if (!state->isSplineReady) {
+        float fake_time_step = 16666.0f; // 60Hz
+        state->spline_x[0] = state->spline_x[3] - 3.0f * fake_time_step;
+        state->spline_y[0] = state->spline_y[3];
+        state->spline_x[1] = state->spline_x[3] - 2.0f * fake_time_step;
+        state->spline_y[1] = state->spline_y[3];
+        state->spline_x[2] = state->spline_x[3] - 1.0f * fake_time_step;
+        state->spline_y[2] = state->spline_y[3];
+        state->isSplineReady = true; // Ready after first fill
+    }
+
+    // Initialize spline if needed
+    if (reconFilterMode == ReconFilterMode::SPLINE_CUBIC_NATURAL) {
+#ifdef USE_DSP_FUNCTIONS
+        arm_spline_init_f32(
+            &state->spline_instance,
+            ARM_SPLINE_NATURAL,
+            state->spline_x,
+            state->spline_y,
+            4,
+            state->spline_y2,
+            state->spline_scratch
+        );
+
+		state->spline_arm_initialized = true;
+		
+#endif
+    }
+}
+
+float EffectsCalculator::evaluateReconstructionFilter(ReconFilterState* state, float fallbackValue)
+{
+    // 'reconFilterMode' is a member variable of the class
+
+    float resulting_value = fallbackValue; // Default value, "NONE"
+    uint32_t now_us = micros();
+    float last_known_time = state->spline_x[3]; // The most recent point
+
+    // If no samples were pushed (e.g. SerialFFB or simple test mode), use fallback
+    if (!state->isSplineReady) {
+        return fallbackValue;
+    }
+
+    // 50ms timeout (if the game is paused, etc.)
+    if (now_us > last_known_time + 50000) {
+        return state->spline_y[3]; // Maintain the last value
+    }
+
+    switch(reconFilterMode) {
+        case ReconFilterMode::NO_RECONSTRUCTION:
+            resulting_value = state->spline_y[3]; // The most recent (no filter)
+            break;
+
+        case ReconFilterMode::LINEAR_INTERPOLATION:
+        {
+			// "Safe" Linear Interpolation (Delayed by 1 sample)
+			// To absolutely guarantee NO OVERSHOOT, we must not extrapolate.
+			// We accept a strictly defined transport delay of 1 sample.
+			//
+			// Logic: We are currently at time 'now_us', which is AFTER the latest update 't1'.
+			// We don't know the future, so we replay the ramp from V0 to V1, but we start it NOW.
+			// We use the previous interval duration (t1 - t0) as our best guess for when the
+			// NEXT packet (t2) will arrive, ensuring we reach V1 exactly when t2 is expected.
+
+			float t0 = state->spline_x[2]; // Time of penultimate sample (T-1)
+			float t1 = state->spline_x[3]; // Time of latest sample (T)
+			float v0 = state->spline_y[2]; // Value of penultimate sample (V0)
+			float v1 = state->spline_y[3]; // Value of latest sample (V1)
+
+			// Estimate the expected duration until the next packet arrives.
+			float expected_interval = t1 - t0;
+
+			// Sanity check: avoid division by zero if updates are too fast (e.g. double-send bugs)
+			if (expected_interval < 1.0f) {
+				resulting_value = v1;
+				break;
+			}
+
+			// Calculate progress based on time elapsed SINCE t1.
+			float time_since_t1 = (float)(now_us - t1);
+			float progress = time_since_t1 / expected_interval;
+
+			// Clamp progress to [0.0, 1.0] to eliminate ANY overshoot.
+			progress = clip<float>(progress, 0.0f, 1.0f);
+
+			// Interpolate between the two KNOWN, SAFE past values.
+			resulting_value = v0 + progress * (v1 - v0);
+			break;
+        }
+
+#ifdef USE_DSP_FUNCTIONS
+        // if DSP is enabled we have SPLINE available, else use Hermite
+        case ReconFilterMode::SPLINE_CUBIC_NATURAL:
+        {
+        	//	 If spline is not ready, return last known value
+			if (!state->spline_arm_initialized) {
+				 resulting_value = state->spline_y[3];
+				 break;
+			}
+
+			float32_t interpolated_torque_f = 0.0f;
+			float32_t now_f = (float)now_us;
+
+			// Clamp time to known range to avoid extrapolation
+            float32_t interp_time = clip<float32_t>(now_f, state->spline_x[0], state->spline_x[3]);
+
+			// Perform the spline interpolation using CMSIS-DSP
+            arm_spline_f32(&state->spline_instance, &interp_time, &interpolated_torque_f, 1);
+
+			// Return the interpolated value
+            resulting_value = interpolated_torque_f;
+            break;
+        }
+#endif
+
+        case ReconFilterMode::SPLINE_CUBIC_HERMITE:
+#ifndef USE_DSP_FUNCTIONS
+        // if the DSP is not available, we use Hermite for spline
+        case ReconFilterMode::SPLINE_CUBIC_NATURAL:
+#endif
+        {
+            // Hermite interpolation is made between P1 (idx 1) and P2 (idx 2)
+            const float p1 = state->spline_y[1];
+            const float p2 = state->spline_y[2];
+            const float t1 = state->spline_x[1];
+            const float t2 = state->spline_x[2];
+
+			// sanity check to not divide by zero and clamp for not overshoot
+            float interval = t2 - t1;
+            if (interval <= 0) {
+                resulting_value = p1;
+                break;
+            }
+            float t = ((float)now_us - t1) / interval;
+            t = clip<float>(t, 0.0f, 1.0f);
+
+            // Tangents (Catmull-Rom)
+            float m1, m2;
+            float dt_m1 = state->spline_x[2] - state->spline_x[0];
+            float dt_m2 = state->spline_x[3] - state->spline_x[1];
+
+			// Compute tangents safely
+            if (dt_m1 > 0) m1 = (state->spline_y[2] - state->spline_y[0]) / dt_m1; else m1 = 0;
+            if (dt_m2 > 0) m2 = (state->spline_y[3] - state->spline_y[1]) / dt_m2; else m2 = 0;
+
+			// Scale tangents by interval
+            m1 *= interval;
+            m2 *= interval;
+
+			// Hermite basis functions
+            float tSq = t * t;
+            float tCub = tSq * t;
+            float h_00 = 2*tCub - 3*tSq + 1;
+            float h_10 = tCub - 2*tSq + t;
+            float h_01 = -2*tCub + 3*tSq;
+            float h_11 = tCub - tSq;
+
+			// Final interpolation
+            resulting_value = h_00 * p1 + h_10 * m1 + h_01 * p2 + h_11 * m2;
+            break;
+        }
+    }
+    return resulting_value;
+}
 
 void EffectsCalculator::setGain(uint8_t gain)
 {
@@ -526,8 +757,6 @@ void EffectsCalculator::setGain(uint8_t gain)
 
 uint8_t EffectsCalculator::getGain() { return global_gain; }
 
-
-
 /*
  * Read parameters from flash and restore settings
  */
@@ -580,6 +809,11 @@ void EffectsCalculator::restoreFlash()
 		frictionPctSpeedToRampup = (effects & 0xff);
 	}
 
+	// Read reconstruction parameters
+	if(Flash_Read(ADR_FFB_RECONSTRUCTION_FILTER, &effects)){
+		reconFilterMode = (ReconFilterMode)(effects & 0x03);
+	}
+
 }
 
 // Saves parameters to flash
@@ -620,6 +854,9 @@ void EffectsCalculator::saveFlash()
 	effects = frictionPctSpeedToRampup | (filterProfileId << 8);
 	Flash_Write(ADR_FFB_EFFECTS3, effects);
 
+	// Save reconstruction parameters
+	effects = (uint16_t)(reconFilterMode);
+	Flash_Write(ADR_FFB_RECONSTRUCTION_FILTER, effects);
 }
 
 void EffectsCalculator::checkFilterCoeff(biquad_constant_t *filter, uint32_t freq,uint8_t q)
@@ -717,7 +954,7 @@ std::string EffectsCalculator::listEffectsUsed(bool details,uint8_t axis){
 	} else {
 
 		bool firstItem = true;
-		for (int i=0; i< 12; i++) {
+		for (int i=0;i < 12; i++) {
 			if (!firstItem) effects_list += ", ";
 			effects_list += "{\"max\":" + std::to_string(effects_stats[i].max[axis]);
 			effects_list += ", \"curr\":" + std::to_string(effects_stats[i].current[axis]);
@@ -945,6 +1182,15 @@ CommandStatus EffectsCalculator::command(const ParsedCommand& cmd,std::vector<Co
 		}
 		break;
 
+	case EffectsCalculator_commands::reconFilterMode:
+        if (cmd.type == CMDtype::get) {
+            replies.emplace_back((uint32_t)reconFilterMode);
+        } else if (cmd.type == CMDtype::set) {
+            uint32_t mode = clip<uint32_t, uint32_t>(cmd.val, 0, 3);
+            reconFilterMode = (ReconFilterMode)mode;
+        }
+        break;
+
 	default:
 		return CommandStatus::NOT_FOUND;
 	}
diff --git a/Firmware/FFBoard/Src/Filters.cpp b/Firmware/FFBoard/Src/Filters.cpp
index 1eb2644d6..7f647bc3f 100644
--- a/Firmware/FFBoard/Src/Filters.cpp
+++ b/Firmware/FFBoard/Src/Filters.cpp
@@ -11,7 +11,7 @@
 #include <math.h>
 
 #ifdef USE_DSP_FUNCTIONS
-#include "arm_math.h"
+#include "dsp/fast_math_functions.h"
 #endif
 
 Biquad::Biquad(){
diff --git a/Firmware/FFBoard/Src/HidFFB.cpp b/Firmware/FFBoard/Src/HidFFB.cpp
index 5b12a41f7..cf2f5a8e8 100644
--- a/Firmware/FFBoard/Src/HidFFB.cpp
+++ b/Firmware/FFBoard/Src/HidFFB.cpp
@@ -12,6 +12,18 @@
 #include "cppmain.h"
 #include <math.h>
 
+#ifdef USE_DSP_FUNCTIONS
+#include "arm_math_types.h"
+#include "dsp/fast_math_functions.h"
+#define MATH_PI PI
+#define MATH_SIN(x) arm_sin_f32(x)
+#define MATH_COS(x) arm_cos_f32(x)
+#else
+#define MATH_PI M_PI
+#define MATH_SIN(x) sinf(x)
+#define MATH_COS(x) cosf(x)
+#endif
+
 HidFFB::HidFFB(std::shared_ptr<EffectsCalculator> ec,uint8_t axisCount) : effects_calc(ec), effects(ec->effects),axisCount(axisCount)
 {
 	directionEnableMask = 1 << axisCount; // Direction enable bit is last bit after axis enable bits
@@ -236,6 +248,7 @@ void HidFFB::set_constant_effect(FFB_SetConstantForce_Data_t* data){
 	FFB_Effect& effect_p = effects[data->effectBlockIndex-1];
 
 	effect_p.magnitude = data->magnitude;
+	effects_calc->updateEffectReconstruction(&effect_p, (float)data->magnitude, 0.0f, false);
 //	if(effect_p.state == 0){
 //		effect_p.state = 1; // Force start effect
 //	}
@@ -315,15 +328,15 @@ void HidFFB::set_effect(FFB_SetEffect_t* effect){
 
 
 	if(!overridesCondition){
-		float phaseX = M_PI*2.0 * (effect->directionX/36000.0f);
+		float phaseX = MATH_PI*2.0f * (effect->directionX/36000.0f);
 
-		effect_p->axisMagnitudes[0] = directionEnable ? sin(phaseX) : (effect->enableAxis & X_AXIS_ENABLE ? (effect->directionX - 18000.0f) / 18000.0f : 0); // Angular vector if dirEnable used otherwise linear or 0 if axis enabled
-		effect_p->axisMagnitudes[1] = directionEnable ? -cos(phaseX) : (effect->enableAxis & Y_AXIS_ENABLE ? -(effect->directionY - 18000.0f) / 18000.0f : 0);
+		effect_p->axisMagnitudes[0] = directionEnable ? MATH_SIN(phaseX) : (effect->enableAxis & X_AXIS_ENABLE ? (effect->directionX - 18000.0f) / 18000.0f : 0); // Angular vector if dirEnable used otherwise linear or 0 if axis enabled
+		effect_p->axisMagnitudes[1] = directionEnable ? -MATH_COS(phaseX) : (effect->enableAxis & Y_AXIS_ENABLE ? -(effect->directionY - 18000.0f) / 18000.0f : 0);
 	}
 
 #if MAX_AXIS == 3
-	float phaseY = M_PI*2.0 * (effect->directionY/36000.0);
-	effect_p->axisMagnitudes[3] = directionEnable ? sin(phaseY) : (effect->enableAxis & Z_AXIS_ENABLE ? (effect->directionZ - 18000.0f) / 18000.0f : 0);
+	float phaseY = MATH_PI*2.0f * (effect->directionY/36000.0f);
+	effect_p->axisMagnitudes[3] = directionEnable ? MATH_SIN(phaseY) : (effect->enableAxis & Z_AXIS_ENABLE ? (effect->directionZ - 18000.0f) / 18000.0f : 0);
 #endif
 	if(effect->duration == 0){ // Fix for games assuming 0 is infinite
 		effect_p->duration = FFB_EFFECT_DURATION_INFINITE;
@@ -453,10 +466,11 @@ void HidFFB::set_periodic(FFB_SetPeriodic_Data_t* report){
 	FFB_Effect* effect = &effects[report->effectBlockIndex-1];
 
 	effect->period = clip<uint32_t,uint32_t>(report->period,1,0x7fff); // Period is never 0
+	effect->phase = report->phase;
+
 	effect->magnitude = report->magnitude;
 	effect->offset = report->offset;
-	effect->phase = report->phase;
-	//effect->counter = 0;
+	effects_calc->updateEffectReconstruction(effect, (float)report->magnitude, (float)report->offset, true);
 }
 
 

From a307cf906589207b885714b4ea13dda7796c2949 Mon Sep 17 00:00:00 2001
From: Vincent MANOUKIAN <10980775+manoukianv@users.noreply.github.com>
Date: Sat, 25 Apr 2026 11:26:59 +0200
Subject: [PATCH 6/8] feat: final TMC optimizations, project configs and build
 tools

---
 Firmware/FFBoard/Inc/MotorDriver.h            | 84 ++++++-------------
 Firmware/FFBoard/Inc/ffb_defs.h               | 23 ++++-
 Firmware/FFBoard/Inc/flash_helpers.h          |  4 +-
 Firmware/FFBoard/Src/HidCommandInterface.cpp  |  2 +-
 Firmware/FFBoard/UserExtensions/Inc/TMC4671.h | 25 +++++-
 .../UserExtensions/Inc/eeprom_addresses.h     |  8 +-
 .../FFBoard/UserExtensions/Src/FFBHIDMain.cpp |  4 +-
 .../FFBoard/UserExtensions/Src/TMC4671.cpp    | 32 ++++++-
 .../UserExtensions/Src/eeprom_addresses.c     |  8 ++
 .../.settings/stm32cubeide.project.prefs      |  4 +-
 .../F407VG/Core/ThreadSafe/newlib_lock_glue.c | 12 +++
 .../Targets/F407VG/OpenFFBoard_F407VG.ioc     |  2 +
 Firmware/make_windows.ps1                     | 34 ++++++++
 Firmware/scripts/generate_memory.py           |  8 +-
 Firmware/scripts/memory_map.csv               |  9 ++
 15 files changed, 180 insertions(+), 79 deletions(-)
 create mode 100644 Firmware/make_windows.ps1

diff --git a/Firmware/FFBoard/Inc/MotorDriver.h b/Firmware/FFBoard/Inc/MotorDriver.h
index 0141f7ec1..b643a66ac 100644
--- a/Firmware/FFBoard/Inc/MotorDriver.h
+++ b/Firmware/FFBoard/Inc/MotorDriver.h
@@ -2,26 +2,21 @@
  * MotorDriver.h
  *
  *  Created on: Feb 1, 2020
- *      Author: Yannick
+ *      Author: Yannick, Vincent
  */
 
 #ifndef MOTORDRIVER_H_
 #define MOTORDRIVER_H_
 
+#define MAX_SLEW_RATE 65535
+
 #include "cppmain.h"
 #include "ChoosableClass.h"
 #include "PersistentStorage.h"
 #include "Encoder.h"
 #include "memory"
 
-#define MAX_SLEW_RATE 65535
-
 class Encoder;
-
-/**
- * @brief Base class for all motor drivers.
- * This class defines the interface for controlling a motor and optionally its integrated encoder.
- */
 class MotorDriver : public ChoosableClass{
 public:
 	MotorDriver(){};
@@ -32,75 +27,46 @@ class MotorDriver : public ChoosableClass{
 	const ClassType getClassType() override {return ClassType::Motordriver;};
 	static const std::vector<class_entry<MotorDriver>> all_drivers;
 
-	/**
-	 * @brief Sends a torque command to the motor.
-	 * @param power The torque value (signed 16-bit).
-	 */
+	virtual void setupDriver();
 	virtual void turn(int16_t power);
-	/**
-	 * @brief Stops the motor (torque = 0).
-	 */
 	virtual void stopMotor();
-	/**
-	 * @brief Starts the motor driver.
-	 */
 	virtual void startMotor();
-	/**
-	 * @brief Triggers an emergency stop.
-	 * @param reset If true, resets the driver after stopping.
-	 */
 	virtual void emergencyStop(bool reset = false);
 
+	virtual void setPowerLimit(uint16_t power){}; // specific motor driver manager power, this is used to send powerLimit to the driver, like TMC4671.
+	
+	// 
 	/**
-	 * @brief Checks if the driver is ready to receive torque commands.
-	 * @return true if ready, false otherwise.
+	 * Slew rate calibration interface (no-op by default)
+	 * If driver can't calibration the max slew rate will by MAX_SLEW_RATE (65535)
+	 * @return false is the driver not implemented this process or true if calibration start
 	 */
-	virtual bool motorReady();
-
+	virtual bool startSlewRateCalibration() { return false; };
 	/**
-	 * @brief Gets the encoder managed by this driver.
-	 * @return A pointer to the encoder instance.
+	 * Check if calibration is in process
+	 * @return the state of calibration process
 	 */
-	virtual Encoder* getEncoder();
-
-	/**
-	 * @brief Sets an external encoder for drivers that don't have an integrated one.
-	 * @param encoder A shared pointer to the external encoder.
-	 */
-	virtual void setEncoder(std::shared_ptr<Encoder>& encoder){drvEncoder = encoder;}
-
-	/**
-	 * @brief Checks if the driver has an integrated encoder.
-	 * @return true if integrated, false if external.
-	 */
-	virtual bool hasIntegratedEncoder();
-
+	virtual bool isSlewRateCalibrationInProgress() { return false; };
 	/**
-	 * @brief Gets the hardware measured maximum slew rate of the driver.
-	 * @return The maximum slew rate in units/ms.
+	 * Get the value of the Slew Rate after a calibration
+	 * @return 
 	 */
-	virtual uint32_t getDrvSlewRate() { return MAX_SLEW_RATE; }
+	virtual uint16_t getDrvSlewRate() { return MAX_SLEW_RATE; };
+	
+	virtual bool motorReady(); // Returns true if the driver is active and ready to receive commands
 
-	/**
-	 * @brief Starts a calibration procedure to measure the driver's max slew rate.
-	 * @return true if calibration started successfully.
-	 */
-	virtual bool startSlewRateCalibration() { return false; }
+	virtual Encoder* getEncoder(); // Encoder is managed by the motor driver. Must always return an encoder
 
 	/**
-	 * @brief Performs initial setup and configuration of the driver.
+	 * Can pass an external encoder if driver has no integrated encoder
+	 * This allows a driver to get an external encoder assigned if it requires one and has the capability of using external encoders
 	 */
-	virtual void setupDriver() {}
-
-	/**
-	 * @brief Sets the maximum power/current limit for the driver.
-	 * @param power The power limit value.
-	 */
-	virtual void setPowerLimit(uint16_t power) {}
+	virtual void setEncoder(std::shared_ptr<Encoder>& encoder){drvEncoder = encoder;}
+	virtual bool hasIntegratedEncoder(); // Returns true if the driver has an integrated encoder. If false the axis will pass one to the driver
 
 
 protected:
-	std::shared_ptr<Encoder> drvEncoder = std::make_shared<Encoder>(); //!< Pointer to the encoder (integrated or external).
+	std::shared_ptr<Encoder> drvEncoder = std::make_shared<Encoder>(); // Dummy encoder
 };
 
 
diff --git a/Firmware/FFBoard/Inc/ffb_defs.h b/Firmware/FFBoard/Inc/ffb_defs.h
index 20013804c..44edda135 100644
--- a/Firmware/FFBoard/Inc/ffb_defs.h
+++ b/Firmware/FFBoard/Inc/ffb_defs.h
@@ -201,7 +201,25 @@ typedef struct
 
 } __attribute__((packed)) reportFFB_status_t;
 
-
+/**
+ * @brief Contient l'état et les tampons pour
+ * l'interpolation d'un seul paramètre (ex: magnitude ou offset).
+ */
+typedef struct
+{
+#ifdef USE_DSP_FUNCTIONS
+	float32_t spline_x[4] = {0}; 		// Buffer time(en us)
+	float32_t spline_y[4] = {0}; 		// Buffer value
+	float32_t spline_y2[4] = {0};    	// Buffer for Spline Natural
+	float32_t spline_scratch[8] = {0}; 	// Buffer for Spline Natural
+	arm_spline_instance_f32 spline_instance; // Instance pour CMSIS-DSP
+	bool spline_arm_initialized = false;	 // CMSIS-DSP initialized ?
+#else
+	float spline_x[4] = {0}; 		// Buffer time(en us)
+	float spline_y[4] = {0}; 		// Buffer value
+#endif
+	bool isSplineReady = false;	// Le tampon est-il plein ?
+} ReconFilterState;
 
 typedef struct
 	{
@@ -339,6 +357,9 @@ typedef struct
 	uint16_t samplePeriod = 0;
 	bool useEnvelope = false;
 	bool useSingleCondition = true;
+
+	ReconFilterState recon_magnitude; // State pour Magnitude (ou Amplitude)
+    ReconFilterState recon_offset;    // State pour Offset (périodiques)
 } FFB_Effect;
 
 
diff --git a/Firmware/FFBoard/Inc/flash_helpers.h b/Firmware/FFBoard/Inc/flash_helpers.h
index 68961e711..e40469531 100644
--- a/Firmware/FFBoard/Inc/flash_helpers.h
+++ b/Firmware/FFBoard/Inc/flash_helpers.h
@@ -2,7 +2,7 @@
  * flash_helpers.h
  *
  *  Created on: 31.01.2020
- *      Author: Yannick
+ *      Author: Yannick, Vincent
  */
 
 #ifndef FLASH_HELPERS_H_
@@ -32,6 +32,8 @@ bool Flash_ReadWriteDefault(uint16_t adr,uint16_t *buf,uint16_t def); // returns
 void Flash_Dump(std::vector<std::tuple<uint16_t,uint16_t>> *result,bool includeAll = false);
 bool Flash_Format();
 
+
+
 bool OTP_Write(uint16_t adroffset,uint64_t dat);
 bool OTP_Read(uint16_t adroffset,uint64_t* dat);
 
diff --git a/Firmware/FFBoard/Src/HidCommandInterface.cpp b/Firmware/FFBoard/Src/HidCommandInterface.cpp
index 418c3a032..9dbbbc6ca 100644
--- a/Firmware/FFBoard/Src/HidCommandInterface.cpp
+++ b/Firmware/FFBoard/Src/HidCommandInterface.cpp
@@ -193,7 +193,7 @@ void HID_CommandInterface::hidCmdCallback(HID_CMD_Data_t* data){
 		for(CommandHandler* handler : handlers){
 			ParsedCommand newCmd = cmd;
 			newCmd.target = handler;
-			if(newCmd.target == nullptr || !(cmd.target->isValidCommandId(cmd.cmdId, CMDFLAG_STR_ONLY))){
+			if(newCmd.target == nullptr || !(newCmd.target->isValidCommandId(cmd.cmdId, CMDFLAG_STR_ONLY))){
 				data->type = HidCmdType::notFound;
 				//sendHidCmd(data); // Send back error
 				this->outBuffer.push_back(*data);
diff --git a/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h b/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h
index fdde1fa2b..222da9e6b 100644
--- a/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h
+++ b/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h
@@ -2,7 +2,7 @@
  * TMC4671.h
  *
  *  Created on: Feb 1, 2020
- *      Author: Yannick
+ *      Author: Yannick, Vincent
  */
 
 #ifndef TMC4671_H_
@@ -235,7 +235,7 @@ struct TMC4671PIDConf{
 struct TMC4671Limits{
 	uint16_t pid_torque_flux_ddt	= 32767;
 	uint16_t pid_uq_ud				= 30000;
-	uint16_t pid_torque_flux		= 32767;
+	uint16_t pid_torque_flux		= 30000;
 	uint32_t pid_acc_lim			= 2147483647;
 	uint32_t pid_vel_lim			= 2147483647;
 	int32_t pid_pos_low				= -2147483647;
@@ -345,6 +345,17 @@ class TMC4671Biquad{
 	}
 	TMC4671Biquad(const TMC4671Biquad_t bq) : params(bq){}
 	TMC4671Biquad(const Biquad& bq,bool enable = true){
+#ifdef USE_DSP_FUNCTIONS
+		const float* coeffs = bq.getCoeffs();
+		// coeffs is in order {b0, b1, b2, -a1, -a2}
+		// TMC expects {b0, b1, b2, a1, a2} with a1 and a2 negated
+		this->params.b0 = (int32_t)(coeffs[0] * (float)(1 << 29));
+		this->params.b1 = (int32_t)(coeffs[1] * (float)(1 << 29));
+		this->params.b2 = (int32_t)(coeffs[2] * (float)(1 << 29));
+		this->params.a1 = (int32_t)(coeffs[3] * (float)(1 << 29));
+		this->params.a2 = (int32_t)(coeffs[4] * (float)(1 << 29));
+		this->params.enable = bq.getFc() > 0 ? enable : false;
+#else
 		// Note: trinamic swapped the naming of b and a from the regular convention in the datasheet and a and b are possibly inverse to b in our filter class
 		this->params.a1 = -(int32_t)(bq.b1 * (float)(1 << 29));
 		this->params.a2 = -(int32_t)(bq.b2 * (float)(1 << 29));
@@ -352,6 +363,7 @@ class TMC4671Biquad{
 		this->params.b1 = (int32_t)(bq.a1 * (float)(1 << 29));
 		this->params.b2 = (int32_t)(bq.a2 * (float)(1 << 29));
 		this->params.enable = bq.getFc() > 0 ? enable : false;
+#endif
 	}
 	void enable(bool enable){
 		params.enable = enable;
@@ -412,6 +424,7 @@ friend class TMCDebugBridge;
 
 	TMC4671MainConfig conf;
 
+	void setupDriver();
 	bool initialize();
 	void initializeWithPower();
 
@@ -485,15 +498,20 @@ friend class TMCDebugBridge;
 	void stopMotor();
 	void startMotor();
 
+	void setPowerLimit(uint16_t power) override;
+
 	void emergencyStop(bool reset);
 	bool emergency = false;
 	bool estopTriggered = false;
 	void turn(int16_t power);
+
+	int16_t getVelocityControllerTorque();
 	int16_t nextFlux = 0;
 	int16_t idleFlux = 0;
 	uint16_t maxOffsetFlux = 0;
 
 	int16_t bangInitPower = 5000; // Default current in setup routines
+	uint16_t maxPowerAxis = 0;
 
 	int16_t controlFluxDissipate();
 	const float fluxDissipationLimit = 1000;
@@ -629,6 +647,7 @@ friend class TMCDebugBridge;
 	static std::span<const TMC4671HardwareTypeConf> tmc4671_hw_configs; // Can override in external target file
 
 private:
+	uint8_t drv_address = 0;
 	OutputPin enablePin = OutputPin(*DRV_ENABLE_GPIO_Port,DRV_ENABLE_Pin);
 	const Error indexNotHitError = Error(ErrorCode::encoderIndexMissed,ErrorType::critical,"Encoder index missed");
 	const Error lowVoltageError = Error(ErrorCode::undervoltage,ErrorType::warning,"Low motor voltage");
@@ -737,5 +756,3 @@ class TMC_2 : public TMC4671 {
 };
 #endif
 #endif /* TMC4671_H_ */
-
-
diff --git a/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h b/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h
index d05638476..bed80e3af 100644
--- a/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h
+++ b/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h
@@ -13,11 +13,11 @@
 
 #include "main.h"
 // Change this to the amount of currently registered variables
-#define NB_OF_VAR 164
+#define NB_OF_VAR 168
 extern const uint16_t VirtAddVarTab[NB_OF_VAR];
 
 // Amount of variables in exportable list
-#define NB_EXPORTABLE_ADR 149
+#define NB_EXPORTABLE_ADR 153
 extern const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR];
 
 
@@ -88,6 +88,7 @@ uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data) will return 1 if
 #define ADR_FFB_EFFECTS1 0x284 // 0-7 inertia, 8-15 friction
 #define ADR_FFB_EFFECTS2 0x285 // 0-7 spring, 8-15 damper
 #define ADR_FFB_EFFECTS3 0x286 // 0-7 friction ramp up zone, 8-9 filterProfile
+#define ADR_FFB_RECONSTRUCTION_FILTER 0x287 // 0-1 recon filter mode
 // Button Sources:
 #define ADR_ADS111X_CONF1 0x290
 // How many axis configured 1-3
@@ -98,6 +99,7 @@ uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data) will return 1 if
 #define ADR_AXIS1_DEGREES 0x303
 #define ADR_AXIS1_MAX_SPEED 0x304 // Store the max speed
 #define ADR_AXIS1_MAX_ACCEL 0x305 // Store the max accel
+#define ADR_AXIS1_MAX_SLEWRATE_DRV 0x306 // Max slew rate for drv
 #define ADR_AXIS1_ENDSTOP 0x307 // 0-7 endstop margin, 8-15 endstop stiffness
 #define ADR_AXIS1_EFFECTS1 0x308 // 0-7 idlespring, 8-15 damper
 #define ADR_AXIS1_SPEEDACCEL_FILTER 0x309 // Speed/Accel filter Lowpass profile
@@ -124,6 +126,7 @@ uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data) will return 1 if
 #define ADR_AXIS2_DEGREES 0x343
 #define ADR_AXIS2_MAX_SPEED 0x344 // Store the max speed
 #define ADR_AXIS2_MAX_ACCEL 0x345 // Store the max accel
+#define ADR_AXIS2_MAX_SLEWRATE_DRV 0x346 // Max slew rate for drv
 #define ADR_AXIS2_ENDSTOP 0x347 // 0-7 endstop margin, 8-15 endstop stiffness
 #define ADR_AXIS2_EFFECTS1 0x348 // 0-7 idlespring, 8-15 damper
 #define ADR_AXIS2_SPEEDACCEL_FILTER 0x349 // Speed/Accel filter Lowpass profile
@@ -150,6 +153,7 @@ uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data) will return 1 if
 #define ADR_AXIS3_DEGREES 0x383
 #define ADR_AXIS3_MAX_SPEED 0x384 // Store the max speed
 #define ADR_AXIS3_MAX_ACCEL 0x385 // Store the max accel
+#define ADR_AXIS3_MAX_SLEWRATE_DRV 0x386 // Max slew rate for drv
 #define ADR_AXIS3_ENDSTOP 0x387 // 0-7 endstop margin, 8-15 endstop stiffness
 #define ADR_AXIS3_EFFECTS1 0x388 // 0-7 idlespring, 8-15 damper
 #define ADR_AXIS3_SPEEDACCEL_FILTER 0x389 // Speed/Accel filter Lowpass profile
diff --git a/Firmware/FFBoard/UserExtensions/Src/FFBHIDMain.cpp b/Firmware/FFBoard/UserExtensions/Src/FFBHIDMain.cpp
index 295815f91..31e6afc96 100644
--- a/Firmware/FFBoard/UserExtensions/Src/FFBHIDMain.cpp
+++ b/Firmware/FFBoard/UserExtensions/Src/FFBHIDMain.cpp
@@ -2,7 +2,7 @@
  * FFBWheel.cpp
  *
  *  Created on: 31.01.2020
- *      Author: Yannick / Lidders
+ *      Author: Yannick / Lidders / Vincent
  */
 
 #include <FFBHIDMain.h>
@@ -29,7 +29,7 @@ const FFBHIDMain::FFB_update_rates FFBHIDMain::ffbrates; // Default rates
  * setFFBEffectsCalc must be called in constructor of derived class to finish the setup
  */
 FFBHIDMain::FFBHIDMain(uint8_t axisCount,bool hidAxis32b) :
-		Thread("FFBMAIN", 256, 30),
+		Thread("FFBMAIN", 312, 30),
 		SelectableInputs(ButtonSource::all_buttonsources,AnalogSource::all_analogsources),
 		axisCount(axisCount),hidAxis32b(hidAxis32b)
 {
diff --git a/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp b/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp
index 1d7be6823..55c824792 100644
--- a/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp
+++ b/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp
@@ -2,7 +2,7 @@
  * TMC4671.cpp
  *
  *  Created on: Feb 1, 2020
- *      Author: Yannick
+ *      Author: Yannick, Vincent
  */
 
 #include "TMC4671.h"
@@ -49,6 +49,7 @@ ClassIdentifier TMC4671::info = {
 TMC4671::TMC4671(SPIPort& spiport,OutputPin cspin,uint8_t address) :
 		CommandHandler("tmc", CLSID_MOT_TMC0,address-1), SPIDevice{motor_spi,cspin},Thread("TMC", TMC_THREAD_MEM, TMC_THREAD_PRIO)
 {
+	this->drv_address = address;
 	CommandHandler::setCommandsEnabled(false);
 	setAddress(address);
 	registerCommands();
@@ -239,6 +240,18 @@ int32_t TMC4671::getTmcVM(){
 	return ((float)agpiA_VM * conf.hwconf.vmScaler) * 1000;
 }
 
+void TMC4671::setupDriver() {
+	// Configuration specific to TMC4671 upon starting the motor
+	// The power limit is set by the Axis class via setPowerLimit during initialization.
+
+	this->setExternalEncoderAllowed(true);
+	this->restoreFlash();
+	this->setLimits(curLimits);
+
+	// Start driver
+	this->setMotionMode(MotionMode::torque);
+	this->Start(); // Start thread
+}
 
 /**
  * Sets all parameters of the driver at startup. Only has to be called once when the driver is detected
@@ -2046,8 +2059,11 @@ void TMC4671::setTorque(int16_t torque){
 	if(curMotionMode != MotionMode::torque){
 		setMotionMode(MotionMode::torque,true);
 	}
-	updateReg(0x64,torque,0xffff,16);
+
+	// Update main torque setpoint
+	updateReg(0x64, torque, 0xffff, 16);
 }
+
 int16_t TMC4671::getTorque(){
 	return readReg(0x64) >> 16;
 }
@@ -2068,10 +2084,16 @@ void TMC4671::setFluxTorque(int16_t flux, int16_t torque){
 #ifdef TMC4671_TORQUE_USE_ASYNC
 	writeRegAsync(0x64, (flux & 0xffff) | (torque << 16));
 #else
+	// Update main flux and torque setpoints
 	writeReg(0x64, (flux & 0xffff) | (torque << 16));
 #endif
 }
 
+int16_t TMC4671::getVelocityControllerTorque(){
+    writeReg(0x6F, 4); // INTERIM_ADDR = 4 for PIDOUT_TARGET_TORQUE
+    return (int16_t)readReg(0x6E); // Read from INTERIM_DATA
+}
+
 void TMC4671::setFluxTorqueFF(int16_t flux, int16_t torque){
 	if(curMotionMode != MotionMode::torque){
 		setMotionMode(MotionMode::torque,true);
@@ -2123,6 +2145,11 @@ void TMC4671::setUqUdLimit(uint16_t limit){
 	writeReg(0x5D, limit);
 }
 
+void TMC4671::setPowerLimit(uint16_t power) {
+	maxPowerAxis = power;
+	setTorqueLimit(power);
+}
+
 void TMC4671::setTorqueLimit(uint16_t limit){
 	this->curLimits.pid_torque_flux = limit;
 	bangInitPower = (float)limit*0.75;
@@ -3105,7 +3132,6 @@ CommandStatus TMC4671::command(const ParsedCommand& cmd,std::vector<CommandReply
 	default:
 		return CommandStatus::NOT_FOUND;
 	}
-
 	return status;
 
 
diff --git a/Firmware/FFBoard/UserExtensions/Src/eeprom_addresses.c b/Firmware/FFBoard/UserExtensions/Src/eeprom_addresses.c
index d111d712a..2a0e83a21 100644
--- a/Firmware/FFBoard/UserExtensions/Src/eeprom_addresses.c
+++ b/Firmware/FFBoard/UserExtensions/Src/eeprom_addresses.c
@@ -70,6 +70,7 @@ const uint16_t VirtAddVarTab[NB_OF_VAR] =
 	ADR_FFB_EFFECTS1, // 0-7 inertia, 8-15 friction
 	ADR_FFB_EFFECTS2, // 0-7 spring, 8-15 damper
 	ADR_FFB_EFFECTS3, // 0-7 friction ramp up zone, 8-9 filterProfile
+	ADR_FFB_RECONSTRUCTION_FILTER, // 0-1 recon filter mode
 // Button Sources:
 	ADR_ADS111X_CONF1,
 // How many axis configured 1-3
@@ -80,6 +81,7 @@ const uint16_t VirtAddVarTab[NB_OF_VAR] =
 	ADR_AXIS1_DEGREES,
 	ADR_AXIS1_MAX_SPEED, // Store the max speed
 	ADR_AXIS1_MAX_ACCEL, // Store the max accel
+	ADR_AXIS1_MAX_SLEWRATE_DRV, // Max slew rate for drv
 	ADR_AXIS1_ENDSTOP, // 0-7 endstop margin, 8-15 endstop stiffness
 	ADR_AXIS1_EFFECTS1, // 0-7 idlespring, 8-15 damper
 	ADR_AXIS1_SPEEDACCEL_FILTER, // Speed/Accel filter Lowpass profile
@@ -106,6 +108,7 @@ const uint16_t VirtAddVarTab[NB_OF_VAR] =
 	ADR_AXIS2_DEGREES,
 	ADR_AXIS2_MAX_SPEED, // Store the max speed
 	ADR_AXIS2_MAX_ACCEL, // Store the max accel
+	ADR_AXIS2_MAX_SLEWRATE_DRV, // Max slew rate for drv
 	ADR_AXIS2_ENDSTOP, // 0-7 endstop margin, 8-15 endstop stiffness
 	ADR_AXIS2_EFFECTS1, // 0-7 idlespring, 8-15 damper
 	ADR_AXIS2_SPEEDACCEL_FILTER, // Speed/Accel filter Lowpass profile
@@ -132,6 +135,7 @@ const uint16_t VirtAddVarTab[NB_OF_VAR] =
 	ADR_AXIS3_DEGREES,
 	ADR_AXIS3_MAX_SPEED, // Store the max speed
 	ADR_AXIS3_MAX_ACCEL, // Store the max accel
+	ADR_AXIS3_MAX_SLEWRATE_DRV, // Max slew rate for drv
 	ADR_AXIS3_ENDSTOP, // 0-7 endstop margin, 8-15 endstop stiffness
 	ADR_AXIS3_EFFECTS1, // 0-7 idlespring, 8-15 damper
 	ADR_AXIS3_SPEEDACCEL_FILTER, // Speed/Accel filter Lowpass profile
@@ -270,6 +274,7 @@ const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR] =
 	ADR_FFB_EFFECTS1, // 0-7 inertia, 8-15 friction
 	ADR_FFB_EFFECTS2, // 0-7 spring, 8-15 damper
 	ADR_FFB_EFFECTS3, // 0-7 friction ramp up zone, 8-9 filterProfile
+	ADR_FFB_RECONSTRUCTION_FILTER, // 0-1 recon filter mode
 // Button Sources:
 	ADR_ADS111X_CONF1,
 // How many axis configured 1-3
@@ -280,6 +285,7 @@ const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR] =
 	ADR_AXIS1_DEGREES,
 	ADR_AXIS1_MAX_SPEED, // Store the max speed
 	ADR_AXIS1_MAX_ACCEL, // Store the max accel
+	ADR_AXIS1_MAX_SLEWRATE_DRV, // Max slew rate for drv
 	ADR_AXIS1_ENDSTOP, // 0-7 endstop margin, 8-15 endstop stiffness
 	ADR_AXIS1_EFFECTS1, // 0-7 idlespring, 8-15 damper
 	ADR_AXIS1_SPEEDACCEL_FILTER, // Speed/Accel filter Lowpass profile
@@ -306,6 +312,7 @@ const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR] =
 	ADR_AXIS2_DEGREES,
 	ADR_AXIS2_MAX_SPEED, // Store the max speed
 	ADR_AXIS2_MAX_ACCEL, // Store the max accel
+	ADR_AXIS2_MAX_SLEWRATE_DRV, // Max slew rate for drv
 	ADR_AXIS2_ENDSTOP, // 0-7 endstop margin, 8-15 endstop stiffness
 	ADR_AXIS2_EFFECTS1, // 0-7 idlespring, 8-15 damper
 	ADR_AXIS2_SPEEDACCEL_FILTER, // Speed/Accel filter Lowpass profile
@@ -332,6 +339,7 @@ const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR] =
 	ADR_AXIS3_DEGREES,
 	ADR_AXIS3_MAX_SPEED, // Store the max speed
 	ADR_AXIS3_MAX_ACCEL, // Store the max accel
+	ADR_AXIS3_MAX_SLEWRATE_DRV, // Max slew rate for drv
 	ADR_AXIS3_ENDSTOP, // 0-7 endstop margin, 8-15 endstop stiffness
 	ADR_AXIS3_EFFECTS1, // 0-7 idlespring, 8-15 damper
 	ADR_AXIS3_SPEEDACCEL_FILTER, // Speed/Accel filter Lowpass profile
diff --git a/Firmware/Targets/F407VG/.settings/stm32cubeide.project.prefs b/Firmware/Targets/F407VG/.settings/stm32cubeide.project.prefs
index 5b3603498..7272f8f0e 100644
--- a/Firmware/Targets/F407VG/.settings/stm32cubeide.project.prefs
+++ b/Firmware/Targets/F407VG/.settings/stm32cubeide.project.prefs
@@ -1,4 +1,4 @@
-2F62501ED4689FB349E356AB974DBE57=E2380525C25FCA23F64F38676B957D9B
+2F62501ED4689FB349E356AB974DBE57=2E846C232C86F8091E1D1C41786923FD
 8DF89ED150041C4CBC7CB9A9CAA90856=2E846C232C86F8091E1D1C41786923FD
-DC22A860405A8BF2F2C095E5B6529F12=948244613B091C43745821314F11043C
+DC22A860405A8BF2F2C095E5B6529F12=22CA254BACDE848B9B8194C2EFC7A52E
 eclipse.preferences.version=1
diff --git a/Firmware/Targets/F407VG/Core/ThreadSafe/newlib_lock_glue.c b/Firmware/Targets/F407VG/Core/ThreadSafe/newlib_lock_glue.c
index 4ed4b9ba0..3d490072f 100644
--- a/Firmware/Targets/F407VG/Core/ThreadSafe/newlib_lock_glue.c
+++ b/Firmware/Targets/F407VG/Core/ThreadSafe/newlib_lock_glue.c
@@ -36,6 +36,8 @@
 /* Includes ------------------------------------------------------------------*/
 #include <cmsis_compiler.h>
 
+/* Private functions prototypes ----------------------------------------------*/
+__WEAK void Error_Handler(void);
 /* Private functions ---------------------------------------------------------*/
 
 /**
@@ -71,6 +73,12 @@ __WEAK void Error_Handler(void)
 /** See struct __lock definition */
 #define STM32_LOCK_PARAMETER(lock) (&(lock)->lock_data)
 
+/* shared variables for bare metal allow lock ------------------------------------------------------*/
+#if defined(STM32_THREAD_SAFE_BAREMETAL_ALLOW_LOCKS) && (STM32_THREAD_SAFE_BAREMETAL_ALLOW_LOCKS != 0)
+uint32_t gflag = 0;
+uint32_t call_counter = 0;
+#endif /* defined(STM32_THREAD_SAFE_BAREMETAL_ALLOW_LOCKS) && (STM32_THREAD_SAFE_BAREMETAL_ALLOW_LOCKS != 0) */
+
 /* Private variables ---------------------------------------------------------*/
 struct __lock
 {
@@ -341,6 +349,10 @@ typedef struct
 /** Mutex used in __cxa_guard_acquire, __cxa_guard_release and __cxa_guard_abort */
 static LockingData_t __cxa_guard_mutex = LOCKING_DATA_INIT;
 
+/* Private functions prototype ---------------------------------------------------------*/
+int __cxa_guard_acquire(CxaGuardObject_t *guard_object);
+void __cxa_guard_abort(CxaGuardObject_t *guard_object);
+void __cxa_guard_release(CxaGuardObject_t *guard_object);
 /* Private functions ---------------------------------------------------------*/
 
 /**
diff --git a/Firmware/Targets/F407VG/OpenFFBoard_F407VG.ioc b/Firmware/Targets/F407VG/OpenFFBoard_F407VG.ioc
index 59e0251c3..656b179eb 100644
--- a/Firmware/Targets/F407VG/OpenFFBoard_F407VG.ioc
+++ b/Firmware/Targets/F407VG/OpenFFBoard_F407VG.ioc
@@ -859,6 +859,7 @@ SPI3.Direction=SPI_DIRECTION_2LINES
 SPI3.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,BaudRatePrescaler
 SPI3.Mode=SPI_MODE_MASTER
 SPI3.VirtualType=VM_MASTER
+
 TIM1.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_ENABLE
 TIM1.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1
 TIM1.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2
@@ -942,6 +943,7 @@ VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2
 VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2
 VP_IWDG_VS_IWDG.Mode=IWDG_Activate
 VP_IWDG_VS_IWDG.Signal=IWDG_VS_IWDG
+
 VP_SYS_VS_tim7.Mode=TIM7
 VP_SYS_VS_tim7.Signal=SYS_VS_tim7
 VP_TIM13_VS_ClockSourceINT.Mode=Enable_Timer
diff --git a/Firmware/make_windows.ps1 b/Firmware/make_windows.ps1
new file mode 100644
index 000000000..36265d208
--- /dev/null
+++ b/Firmware/make_windows.ps1
@@ -0,0 +1,34 @@
+# 1. Setup the entry point
+$stPath = "C:\ST"
+
+Write-Host "--- Looking for compilation tools in $stPath ---" -ForegroundColor Cyan
+
+# 2. Find folder 'bin' which contain the make.exe (the lastest one)
+$makeBin = Get-ChildItem -Path $stPath -Filter "make.exe" -Recurse -ErrorAction SilentlyContinue | 
+           Sort-Object LastWriteTime -Descending | 
+           Select-Object -First 1 -ExpandProperty DirectoryName
+
+# 3. find folder 'bin' for arm-none-eabi-gcc.exe (the lastest one)
+$gccBin = Get-ChildItem -Path $stPath -Filter "arm-none-eabi-gcc.exe" -Recurse -ErrorAction SilentlyContinue | 
+          Sort-Object LastWriteTime -Descending | 
+          Select-Object -First 1 -ExpandProperty DirectoryName
+
+if ($makeBin -and $gccBin) {
+    Write-Host "[OK] Make found : $makeBin" -ForegroundColor Green
+    Write-Host "[OK] GCC found : $gccBin" -ForegroundColor Green
+
+    # 4. Add paths to PATH file
+    $env:PATH = "$makeBin;$gccBin;" + $env:PATH
+
+    Write-Host "--- Launch compilation for all target ---" -ForegroundColor Yellow
+    
+    # 5. Make all targets
+    make MCU_TARGET=F407VG clean
+    make MCU_TARGET=F407VG -j8
+    make MCU_TARGET=F407VG_DISCO clean
+    make MCU_TARGET=F407VG_DISCO -j8
+    make MCU_TARGET=F411RE clean
+    make MCU_TARGET=F411RE -j8
+} else {
+    Write-Host "[ERROR] Can't detect make tools. Check than STM32CubeIDE is in directory $stPath" -ForegroundColor Red
+}
\ No newline at end of file
diff --git a/Firmware/scripts/generate_memory.py b/Firmware/scripts/generate_memory.py
index 5b31a90ff..9e3bd3eed 100644
--- a/Firmware/scripts/generate_memory.py
+++ b/Firmware/scripts/generate_memory.py
@@ -8,8 +8,8 @@
  *  Created on: 24.01.2020		
  *  Author: Yannick		
  *		
- *	/!\ Generated from the file memory_map.csv 
-   / ! \ DO NOT EDIT THIS DIRECTLY !!!	
+ *	/!\\ Generated from the file memory_map.csv 
+   / ! \\ DO NOT EDIT THIS DIRECTLY !!!	
  */		
 		
 #include "eeprom_addresses.h"		
@@ -22,8 +22,8 @@
  *  Created on: 24.01.2020		
  *  Author: Yannick		
  *		
- *	/!\ Generated from the file memory_map.csv 
-   / ! \ DO NOT EDIT THIS DIRECTLY !!!	
+ *	/!\\ Generated from the file memory_map.csv 
+   / ! \\ DO NOT EDIT THIS DIRECTLY !!!	
  */			
 		
 #ifndef EEPROM_ADDRESSES_H_
diff --git a/Firmware/scripts/memory_map.csv b/Firmware/scripts/memory_map.csv
index ce1383a95..822ba9c93 100644
--- a/Firmware/scripts/memory_map.csv
+++ b/Firmware/scripts/memory_map.csv
@@ -52,6 +52,7 @@ Section comment,Name,Value,Comment on key,VirtAddVarTab,exportableFlashAddresses
 ,ADR_FFB_EFFECTS1,0x284,"// 0-7 inertia, 8-15 friction",1,1
 ,ADR_FFB_EFFECTS2,0x285,"// 0-7 spring, 8-15 damper",1,1
 ,ADR_FFB_EFFECTS3,0x286,"// 0-7 friction ramp up zone, 8-9 filterProfile",1,1
+,ADR_FFB_RECONSTRUCTION_FILTER,0x287,// 0-1 recon filter mode,1,1
 // Button Sources:,,,,,
 ,ADR_ADS111X_CONF1,0x290,,1,1
 // How many axis configured 1-3,,,,,
@@ -96,6 +97,12 @@ Section comment,Name,Value,Comment on key,VirtAddVarTab,exportableFlashAddresses
 ,ADR_AXIS2_ENC_RATIO,0x34A,// Store the encoder ratio for an axis,1,1
 ,ADR_AXIS2_EFFECTS2,0x34B,"// 0-7 Friction, 8-15 Inertia",1,1
 ,ADR_AXIS2_POSTPROCESS1,0x34C,// 0-7 expo curve,1,1
+,,,,,
+,,,,,
+,,,,,
+,,,,,
+,,,,,
+,,,,,
 // TMC2,,,,,
 ,ADR_TMC2_MOTCONF,0x360,// 0-2: MotType 3-5: PhiE source 6-15: Poles,1,1
 ,ADR_TMC2_CPR,0x361,,1,1
@@ -123,6 +130,8 @@ Section comment,Name,Value,Comment on key,VirtAddVarTab,exportableFlashAddresses
 ,ADR_AXIS3_ENC_RATIO,0x38A,// Store the encoder ratio for an axis,1,1
 ,ADR_AXIS3_EFFECTS2,0x38B,"// 0-7 Friction, 8-15 Inertia",1,1
 ,ADR_AXIS3_POSTPROCESS1,0x38C,// 0-7 expo curve,1,1
+,,,,,
+,,,,,
 // TMC3,,,,,
 ,ADR_TMC3_MOTCONF,0x3A0,// 0-2: MotType 3-5: PhiE source 6-15: Poles,1,1
 ,ADR_TMC3_CPR,0x3A1,,1,1

From df181e32f2597b5a4994e56ed8c0ca58dfc1997d Mon Sep 17 00:00:00 2001
From: Vincent MANOUKIAN <10980775+manoukianv@users.noreply.github.com>
Date: Thu, 28 May 2026 19:09:24 +0200
Subject: [PATCH 7/8] refactor: simplify Biquad filter implementations and TMC
 init

---
 Firmware/FFBoard/Inc/EffectsCalculator.h       |  2 +-
 Firmware/FFBoard/Inc/Filters.h                 | 11 +++++++----
 Firmware/FFBoard/Inc/ffb_defs.h                |  8 ++++----
 Firmware/FFBoard/Src/Filters.cpp               | 18 +++++++-----------
 Firmware/FFBoard/UserExtensions/Inc/TMC4671.h  | 18 ++----------------
 .../FFBoard/UserExtensions/Src/TMC4671.cpp     |  5 -----
 6 files changed, 21 insertions(+), 41 deletions(-)

diff --git a/Firmware/FFBoard/Inc/EffectsCalculator.h b/Firmware/FFBoard/Inc/EffectsCalculator.h
index e22d71cd6..f84f61b7e 100644
--- a/Firmware/FFBoard/Inc/EffectsCalculator.h
+++ b/Firmware/FFBoard/Inc/EffectsCalculator.h
@@ -190,7 +190,7 @@ class EffectsCalculator: public PersistentStorage,
 	void free_effect(uint16_t idx);
 
 	/**
-     * @brief Met à jour les tampons d'interpolation pour un effet non-conditionnel.
+     * @brief Updates interpolation buffers for unconditional effect.
      * Appelé par le handler HID (ex: 60Hz).
      * @param effect Pointeur vers l'effet à modifier.
      * @param new_magnitude Nouvelle magnitude/amplitude cible.
diff --git a/Firmware/FFBoard/Inc/Filters.h b/Firmware/FFBoard/Inc/Filters.h
index e2ec290e8..e658a7b9d 100644
--- a/Firmware/FFBoard/Inc/Filters.h
+++ b/Firmware/FFBoard/Inc/Filters.h
@@ -48,22 +48,25 @@ class Biquad{
     void setPeakGain(float peakGainDB);
     void calcBiquad(void);
 
-#ifdef USE_DSP_FUNCTIONS
     const float* getCoeffs() const { return pCoeffs; }
-#endif
 
 protected:
     BiquadType type;
     float Fc, Q, peakGain;
 
+    union {
+        float pCoeffs[5];
+        struct {
+            float a0, a1, a2, b1, b2;
+        };
+    };
+
 #ifdef USE_DSP_FUNCTIONS
     // CMSIS-DSP instance
     arm_biquad_casd_df1_inst_f32 S;
-    float32_t pCoeffs[5];
     float32_t pState[4]; // For a single biquad stage (DF1 float requires 4 states)
 #else
     float z1, z2;
-    float a0, a1, a2, b1, b2;
 #endif
 
 };
diff --git a/Firmware/FFBoard/Inc/ffb_defs.h b/Firmware/FFBoard/Inc/ffb_defs.h
index 44edda135..f00e2b7fc 100644
--- a/Firmware/FFBoard/Inc/ffb_defs.h
+++ b/Firmware/FFBoard/Inc/ffb_defs.h
@@ -202,8 +202,8 @@ typedef struct
 } __attribute__((packed)) reportFFB_status_t;
 
 /**
- * @brief Contient l'état et les tampons pour
- * l'interpolation d'un seul paramètre (ex: magnitude ou offset).
+ * @brief Contains state and buffers for
+ * the interpolation of a single parameter (eg: magnitude or offset).
  */
 typedef struct
 {
@@ -212,13 +212,13 @@ typedef struct
 	float32_t spline_y[4] = {0}; 		// Buffer value
 	float32_t spline_y2[4] = {0};    	// Buffer for Spline Natural
 	float32_t spline_scratch[8] = {0}; 	// Buffer for Spline Natural
-	arm_spline_instance_f32 spline_instance; // Instance pour CMSIS-DSP
+	arm_spline_instance_f32 spline_instance; // Instance for CMSIS-DSP
 	bool spline_arm_initialized = false;	 // CMSIS-DSP initialized ?
 #else
 	float spline_x[4] = {0}; 		// Buffer time(en us)
 	float spline_y[4] = {0}; 		// Buffer value
 #endif
-	bool isSplineReady = false;	// Le tampon est-il plein ?
+	bool isSplineReady = false;	// buffer is full ?
 } ReconFilterState;
 
 typedef struct
diff --git a/Firmware/FFBoard/Src/Filters.cpp b/Firmware/FFBoard/Src/Filters.cpp
index 7f647bc3f..937fc78e8 100644
--- a/Firmware/FFBoard/Src/Filters.cpp
+++ b/Firmware/FFBoard/Src/Filters.cpp
@@ -76,8 +76,8 @@ float Biquad::process(float in) {
     arm_biquad_cascade_df1_f32(&S, &in, &out, 1);
 #else
 	out = in * a0 + z1;
-    z1 = in * a1 + z2 - b1 * out;
-    z2 = in * a2 - b2 * out;
+    z1 = in * a1 + z2 + b1 * out;
+    z2 = in * a2 + b2 * out;
 #endif
     return out;
 }
@@ -98,7 +98,6 @@ void Biquad::calcBiquad(void) {
     float norm;
     float K, V;
 #ifdef USE_DSP_FUNCTIONS
-    float a0 = 0.0f, a1 = 0.0f, a2 = 0.0f, b1 = 0.0f, b2 = 0.0f;
     V = powf(10, fabsf(peakGain) / 20.0f);
     K = arm_sin_f32(PI * Fc) / arm_cos_f32(PI * Fc);
 #else
@@ -232,15 +231,12 @@ void Biquad::calcBiquad(void) {
             break;
     }
 
-#ifdef USE_DSP_FUNCTIONS
-    // Store coefficients in the format required by CMSIS-DSP: {b0, b1, b2, -a1, -a2}
-    // Note the negated feedback coefficients a1 and a2.
-    pCoeffs[0] = a0;
-    pCoeffs[1] = a1;
-    pCoeffs[2] = a2;
-    pCoeffs[3] = -b1;
-    pCoeffs[4] = -b2;
+    // Invert feedback coefficients to match CMSIS-DSP expected format {-b1, -b2}
+    // and to simplify the TMC configuration.
+    b1 = -b1;
+    b2 = -b2;
 
+#ifdef USE_DSP_FUNCTIONS
 	// Reset state
 	memset(pState, 0, sizeof(pState));
 #endif
diff --git a/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h b/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h
index 222da9e6b..3abce6502 100644
--- a/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h
+++ b/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h
@@ -345,25 +345,12 @@ class TMC4671Biquad{
 	}
 	TMC4671Biquad(const TMC4671Biquad_t bq) : params(bq){}
 	TMC4671Biquad(const Biquad& bq,bool enable = true){
-#ifdef USE_DSP_FUNCTIONS
-		const float* coeffs = bq.getCoeffs();
-		// coeffs is in order {b0, b1, b2, -a1, -a2}
-		// TMC expects {b0, b1, b2, a1, a2} with a1 and a2 negated
-		this->params.b0 = (int32_t)(coeffs[0] * (float)(1 << 29));
-		this->params.b1 = (int32_t)(coeffs[1] * (float)(1 << 29));
-		this->params.b2 = (int32_t)(coeffs[2] * (float)(1 << 29));
-		this->params.a1 = (int32_t)(coeffs[3] * (float)(1 << 29));
-		this->params.a2 = (int32_t)(coeffs[4] * (float)(1 << 29));
-		this->params.enable = bq.getFc() > 0 ? enable : false;
-#else
-		// Note: trinamic swapped the naming of b and a from the regular convention in the datasheet and a and b are possibly inverse to b in our filter class
-		this->params.a1 = -(int32_t)(bq.b1 * (float)(1 << 29));
-		this->params.a2 = -(int32_t)(bq.b2 * (float)(1 << 29));
 		this->params.b0 = (int32_t)(bq.a0 * (float)(1 << 29));
 		this->params.b1 = (int32_t)(bq.a1 * (float)(1 << 29));
 		this->params.b2 = (int32_t)(bq.a2 * (float)(1 << 29));
+		this->params.a1 = (int32_t)(bq.b1 * (float)(1 << 29));
+		this->params.a2 = (int32_t)(bq.b2 * (float)(1 << 29));
 		this->params.enable = bq.getFc() > 0 ? enable : false;
-#endif
 	}
 	void enable(bool enable){
 		params.enable = enable;
@@ -505,7 +492,6 @@ friend class TMCDebugBridge;
 	bool estopTriggered = false;
 	void turn(int16_t power);
 
-	int16_t getVelocityControllerTorque();
 	int16_t nextFlux = 0;
 	int16_t idleFlux = 0;
 	uint16_t maxOffsetFlux = 0;
diff --git a/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp b/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp
index 55c824792..24cbcd4d5 100644
--- a/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp
+++ b/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp
@@ -2089,11 +2089,6 @@ void TMC4671::setFluxTorque(int16_t flux, int16_t torque){
 #endif
 }
 
-int16_t TMC4671::getVelocityControllerTorque(){
-    writeReg(0x6F, 4); // INTERIM_ADDR = 4 for PIDOUT_TARGET_TORQUE
-    return (int16_t)readReg(0x6E); // Read from INTERIM_DATA
-}
-
 void TMC4671::setFluxTorqueFF(int16_t flux, int16_t torque){
 	if(curMotionMode != MotionMode::torque){
 		setMotionMode(MotionMode::torque,true);

From 690deb7b3d504e6efadafba8c4ff20f69b6d3ed1 Mon Sep 17 00:00:00 2001
From: Vincent MANOUKIAN <10980775+manoukianv@users.noreply.github.com>
Date: Thu, 28 May 2026 19:20:54 +0200
Subject: [PATCH 8/8] refactor(axis): unify DSP and non-DSP logic in
 applySpeedLimiterTorque

---
 Firmware/FFBoard/Inc/Axis.h   | 10 +++---
 Firmware/FFBoard/Src/Axis.cpp | 58 ++++++++++++++++-------------------
 2 files changed, 31 insertions(+), 37 deletions(-)

diff --git a/Firmware/FFBoard/Inc/Axis.h b/Firmware/FFBoard/Inc/Axis.h
index 514dffd65..246593d4a 100644
--- a/Firmware/FFBoard/Inc/Axis.h
+++ b/Firmware/FFBoard/Inc/Axis.h
@@ -342,7 +342,7 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	 * @brief Calculates the FFB torque exponential torque, from the input torque and apply expo and scaler
 	 * @return The calculated exponential torque.
 	 */
-	int64_t calculateFFBTorque();
+	int32_t calculateFFBTorque();
 
 	/**
 	 * @brief Starts a force fade-in.
@@ -357,7 +357,7 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	 * @brief Sets the FFB effect torque.
 	 * @param torque The new FFB effect torque from the EffectsCalculator.
 	 */
-	void setFfbEffectTorque(int64_t torque);
+	void setFfbEffectTorque(int32_t torque);
 
 	/**
 	 * @brief Updates the total torque.
@@ -418,12 +418,12 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	 * @param torque A reference to the torque value to be modified.
 	 * @return torque update to apply to reduced de speed.
 	 */
-	int64_t applySpeedLimiterTorque(int64_t& torque);
+	int32_t applySpeedLimiterTorque(int32_t& torque);
 	/**
 	 * @brief Applies the torque slew rate limiter to the torque.
 	 * @param torque A reference to the torque value to be modified.
 	 */
-	void applyTorqueSlewRateLimiter(int64_t& torque);
+	void applyTorqueSlewRateLimiter(int32_t& torque);
 	/**
 	 * @brief Decodes the axis configuration from a 16-bit integer stored in flash.
 	 * @param val The 16-bit encoded configuration value.
@@ -478,7 +478,7 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	float previousFrameSpeed = 0;			//!< Instantaneous speed from the last cycle, used for acceleration calculation.
 
 	// Torque components
-	int64_t ffbEffectTorque = 0;		//!< Torque from HID FFB effects.
+	int32_t ffbEffectTorque = 0;		//!< Torque from HID FFB effects.
 	int32_t mechanicalEffectTorque = 0;	//!< Torque from mechanical effects (damper, friction, inertia).
 
 	// Power and scaling
diff --git a/Firmware/FFBoard/Src/Axis.cpp b/Firmware/FFBoard/Src/Axis.cpp
index ff7262278..187142ebc 100644
--- a/Firmware/FFBoard/Src/Axis.cpp
+++ b/Firmware/FFBoard/Src/Axis.cpp
@@ -703,9 +703,9 @@ int32_t Axis::calculateExpoTorque(int32_t torque){
 	}
 }
 
-int64_t Axis::calculateFFBTorque() {
+int32_t Axis::calculateFFBTorque() {
 
-	int64_t torque = this->ffbEffectTorque;
+	int32_t torque = this->ffbEffectTorque;
 
 	// 1. Game Clipping detection
 	// If the game sends more than the theoretical maximum (+/- 32767), the signal is clipped at the source.
@@ -720,7 +720,7 @@ int64_t Axis::calculateFFBTorque() {
 
 	// 3. Game specific gain (effectRatioScaler)
 	// Scale the FFB from game only (allows lowering game effects without lowering endstops)
-	torque = (int64_t)((float)torque * effectRatioScaler);
+	torque = (int32_t)((float)torque * effectRatioScaler);
 
 	return torque;
 }
@@ -747,7 +747,7 @@ int32_t Axis::calculateEndstopTorque(){
 	return clip<int32_t,int32_t>(endstopTorque,-0x7fff,0x7fff);
 }
 
-void Axis::setFfbEffectTorque(int64_t torque) {
+void Axis::setFfbEffectTorque(int32_t torque) {
 	this->ffbEffectTorque = torque;
 }
 
@@ -759,7 +759,7 @@ bool Axis::updateTorque(int32_t* totalTorque) {
 
 	// STEP 1: Process FFB torque from the game (via helper function)
 	// (Reconstructed by CMSIS Spline, Expo applied, and scaled by FFB ratio)
-	int64_t torque = calculateFFBTorque();
+	int32_t torque = calculateFFBTorque();
 
 	// STEP 2: Mix in local mechanical effects
 	// (Damper, Friction, Inertia generated locally at high frequency)
@@ -772,7 +772,7 @@ bool Axis::updateTorque(int32_t* totalTorque) {
 
 	// STEP 4: Master Volume Scaling
 	// Map the virtual signal (+/- 32767) to the physical power limit ("power")
-	torque = (int64_t)((float)torque * torqueScaler);
+	torque = (int32_t)((float)torque * torqueScaler);
 
 	// STEP 5: Safety limits (Fade-in, Out of bounds)
 	// Applied BEFORE dynamic limiters so that abrupt cuts are smoothed by the Slew Rate.
@@ -783,7 +783,7 @@ bool Axis::updateTorque(int32_t* totalTorque) {
 	// Apply a fade-in effect for a smooth force ramp-up on startup or recovery.
 	// Increases forceFadeMultiplier progressively based on forceFadeDuration and sample rate.
 	if(forceFadeMultiplier < 1.0f){
-		torque = (int64_t)((float)torque * forceFadeMultiplier);
+		torque = (int32_t)((float)torque * forceFadeMultiplier);
 		forceFadeMultiplier += forceFadeDuration / this->filter_f;
 	}
 
@@ -817,7 +817,7 @@ bool Axis::updateTorque(int32_t* totalTorque) {
 }
 
 
-void Axis::applyTorqueSlewRateLimiter(int64_t& torque)
+void Axis::applyTorqueSlewRateLimiter(int32_t& torque)
 {
 	// Limits the rate of change of the torque (slew rate), to smooths out sudden changes in torque.
 	// Essential for a natural feel and to prevent "clanking" noises.
@@ -826,14 +826,14 @@ void Axis::applyTorqueSlewRateLimiter(int64_t& torque)
 	}
 
 	// This prevents sudden torque jumps, resulting in a smoother feel.
-	const int64_t previousTorque = metric.previous.torque;
-	const int64_t maxTorqueChange = maxTorqueRateMS;
+	const int32_t previousTorque = metric.previous.torque;
+	const int32_t maxTorqueChange = maxTorqueRateMS;
 
 	// The torque is clipped to be within the range of [previous torque - limit, previous torque + limit].
-	torque = clip<int64_t>(torque, previousTorque - maxTorqueChange, previousTorque + maxTorqueChange);
+	torque = clip<int32_t>(torque, previousTorque - maxTorqueChange, previousTorque + maxTorqueChange);
 		}
 
-int64_t Axis::applySpeedLimiterTorque(int64_t& torque){
+int32_t Axis::applySpeedLimiterTorque(int32_t& torque){
 	// Speed Limiter: A PI controller to reduce torque when speed exceeds maxSpeedDegS.
 	// The limiter only acts when torque is applied in the direction of movement.
 
@@ -842,18 +842,23 @@ int64_t Axis::applySpeedLimiterTorque(int64_t& torque){
 		return 0;
 	}
 
-	int64_t resultTorque = 0;
-
-#ifdef USE_DSP_FUNCTIONS
+	int32_t resultTorque = 0;
 	float effectiveSpeed = metric.current.speed * (torque > 0 ? 1.0f : -1.0f);
+
 	if (effectiveSpeed > maxSpeedDegS)
 	{
-		// --- PI Controller Logic ---
 		// 1. Calculate the error term (how much we are over the speed limit).
 		float speedError = effectiveSpeed - maxSpeedDegS;
+		float reductionAmount = 0.0f;
 
-		// 2. Calculate the total reduction amount using the PID controller.
-		float reductionAmount = arm_pid_f32(&speedLimiterPID, speedError);
+		// 2. Calculate the total reduction amount using the PI controller.
+#ifdef USE_DSP_FUNCTIONS
+		reductionAmount = arm_pid_f32(&speedLimiterPID, speedError);
+#else
+		float speedreducer = speedError * ((float)0x7FFF / maxSpeedDegS);
+		speedLimitReducerI = clip<float,int32_t>( speedLimitReducerI + ((speedreducer * speedLimiterI) * torqueScaler),0,power);
+		reductionAmount = speedreducer * speedLimiterP + speedLimitReducerI;
+#endif
 
 		// 3. Apply the reduction to the main torque.
 		// We must only reduce the magnitude of the torque, not invert it.
@@ -864,23 +869,12 @@ int64_t Axis::applySpeedLimiterTorque(int64_t& torque){
 			resultTorque = -reductionAmount;
 		}
 	} else {
+#ifdef USE_DSP_FUNCTIONS
 		arm_pid_reset_f32(&speedLimiterPID); // Reset PID if not active
-	}
 #else
-	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)((metric.current.speed*torqueSign) - (float)maxSpeedDegS) *  ((float)0x7FFF / maxSpeedDegS);
-	speedLimitReducerI = clip<float,int32_t>( speedLimitReducerI + ((speedreducer * speedLimiterI) * torqueScaler),0,power);
-
-	// Only reduce torque. Don't invert it to prevent oscillation
-	float torqueReduction = speedreducer * speedLimiterP + speedLimitReducerI;// accreducer * 0.025 + acclimitreducerI
-	if(torque > 0){
-		resultTorque = clip<float,int64_t>(torqueReduction,0,torque);
-	}else{
-		resultTorque = clip<float,int64_t>(-torqueReduction,torque,0);
-	}
+		speedLimitReducerI = 0; // Reset I term if not active
 #endif
+	}
 
 	return resultTorque;
 }