feat: implement slew rate detector and limiter for motor drivers

Author: manoukianv

Firmware/FFBoard/Inc/Axis.h

@@ -60,6 +60,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;
@@ -122,6 +123,8 @@ enum class Axis_commands : uint32_t{
 	fxratio,reductionScaler,
 	filterSpeed, filterAccel, filterProfileId,cpr,axisfriction,axisinertia,
 	maxspeed,slewrate,
+	calibrate_maxSlewRateDrv,
+	maxSlewRateDrv,
 	expo,exposcale,
 	equalizer,eqb1,eqb2,eqb3,eqb4,eqb5,eqb6,
 	handsoff, handsoff_speed, handsoff_accel
@@ -474,6 +477,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.
 
@@ -503,14 +507,17 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	// Axis configuration
 	bool invertAxis = true;			//!< Invert axis direction.
 	uint8_t endstopStrength = 127;	//!< Stiffness of the endstop effect.
-	static constexpr float ENDSTOP_GAIN = 25.0f;	//!< Overall maximum endstop intensity.
+	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.
 	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
@@ -521,7 +528,7 @@ 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
-	static constexpr int32_t INTERNAL_FX_CLIP = 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.

Firmware/FFBoard/Src/Axis.cpp

@@ -117,7 +117,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,
 										   ADR_AXIS1_EQ1,ADR_AXIS1_EQ2,ADR_AXIS1_EQ3,
@@ -128,7 +128,7 @@ 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,
 										   ADR_AXIS2_EQ1,ADR_AXIS2_EQ2,ADR_AXIS2_EQ3,
@@ -138,7 +138,7 @@ 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,
+	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,
 										   ADR_AXIS3_EQ1,ADR_AXIS3_EQ2,ADR_AXIS3_EQ3,
@@ -207,6 +207,9 @@ void Axis::registerCommands(){
 	registerCommand("handsoff", Axis_commands::handsoff, "Hands-off enable", CMDFLAG_GET | CMDFLAG_SET);
 	registerCommand("handsoff_speed", Axis_commands::handsoff_speed, "Hoff speed thrld (deg/s)", CMDFLAG_GET | CMDFLAG_SET);
 	registerCommand("handsoff_accel", Axis_commands::handsoff_accel, "Hoff accel std dev thrld (float, val/1000)", CMDFLAG_GET | CMDFLAG_SET);
+
+	registerCommand("maxSlewRateDrv", Axis_commands::maxSlewRateDrv, "Max driver torque in counts/ms",CMDFLAG_GET);
+	registerCommand("calibrate_maxSlewRateDrv", Axis_commands::calibrate_maxSlewRateDrv, "Start driver slewRate calib", CMDFLAG_GET);
 }
 
 /*
@@ -238,6 +241,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 endstopRawValue, power;
 	if(Flash_Read(flashAddresses.endstop, &endstopRawValue)) {
@@ -322,6 +332,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, effectRatio | (endstopStrength << 8));
 	Flash_Write(flashAddresses.power, power);
@@ -435,6 +446,29 @@ void Axis::prepareForUpdate(){
 		startForceFadeIn(0, 1.0);
 	}
 
+	// Check for pending slew rate calibration result
+	if(this->awaitingSlewCalibration){
+		// If driver reports calibration finished, retrieve measured value and persist
+		if(!drv->isSlewRateCalibrationInProgress()){
+			// Get value from drv
+			this->maxSlewRate_Driver =  drv->getDrvSlewRate();
+
+			// If the driver's max slew rate is lowest thant current max flew rate, cap the value and send the new value to the UI
+			if (this->maxSlewRate_Driver < this->maxTorqueRateMS) {
+				this->maxTorqueRateMS = this->maxSlewRate_Driver;
+				CommandHandler::broadcastCommandReply(CommandReply(this->maxTorqueRateMS), (uint32_t)Axis_commands::slewrate, CMDtype::get);
+			}
+			
+			// Broadcast a friendly completion message and the numeric value
+			CommandHandler::broadcastCommandReply(CommandReply("Slew rate calibration complete",0), (uint32_t)Axis_commands::calibrate_maxSlewRateDrv, CMDtype::get);
+			CommandHandler::broadcastCommandReply(CommandReply(this->maxSlewRate_Driver), (uint32_t)Axis_commands::maxSlewRateDrv, CMDtype::get);
+
+			
+			this->awaitingSlewCalibration = false;
+		}
+	}
+
+
 	this->updateMetrics(angle);
 	this->updateHandsOffState();
 
@@ -499,6 +533,7 @@ void Axis::setDrvType(uint8_t drvtype)
 	old_drv_ptr = this->drv.release(); // Detach the old driver safely
 	this->drv.reset(drv_new);          // Attach the new driver
 	this->conf.drvtype = drvtype;
+	this->maxTorqueRateMS = drv_new->getDrvSlewRate();
 	cpp_freertos::CriticalSection::Exit();
 
 	// Delete the old driver outside of the critical section to avoid blocking destructors or FreeRTOS issues
@@ -671,13 +706,13 @@ void Axis::calculateMechanicalEffects(bool ffb_on){
 	// Always active damper
 	if(damperIntensity != 0){
 		float speedFiltered = (metric.current.speed) * (float)damperIntensity * AXIS_DAMPER_RATIO;
-		mechanicalEffectTorque -= damperFilter.process(clip<float, int32_t>(speedFiltered, -INTERNAL_FX_CLIP, INTERNAL_FX_CLIP));
+		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;
-		mechanicalEffectTorque -= inertiaFilter.process(clip<float, int32_t>(accelFiltered, -INTERNAL_FX_CLIP, INTERNAL_FX_CLIP));
+		mechanicalEffectTorque -= inertiaFilter.process(clip<float, int32_t>(accelFiltered, -internalFxClip, internalFxClip));
 	}
 
 	// Always active friction. Based on effectsCalculator implementation
@@ -696,7 +731,7 @@ void Axis::calculateMechanicalEffects(bool ffb_on){
 		}
 		int8_t sign = speed >= 0 ? 1 : -1;
 		float force = (float)frictionIntensity * rampupFactor * sign * INTERNAL_AXIS_FRICTION_SCALER * 32;
-		mechanicalEffectTorque -= frictionFilter.process(clip<float, int32_t>(force, -INTERNAL_FX_CLIP, INTERNAL_FX_CLIP));
+		mechanicalEffectTorque -= frictionFilter.process(clip<float, int32_t>(force, -internalFxClip, internalFxClip));
 	}
 
 }
@@ -805,7 +840,7 @@ int32_t Axis::calculateEndstopTorque(){
 		return 0;
 	}
 	float endstopTorque = clipDirection*metric.current.posDegrees - (float)this->degreesOfRotation/2.0; // degress of rotation counts total range so multiply by 2
-	endstopTorque *= (float)endstopStrength * ENDSTOP_GAIN; // Apply endstop gain for stiffness.
+	endstopTorque *= (float)endstopStrength * endstopGain; // Apply endstop gain for stiffness.
 	endstopTorque *= -clipDirection;
 
 	return clip<int32_t,int32_t>(endstopTorque,-0x7fff,0x7fff);
@@ -1132,13 +1167,38 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector<CommandReply>&
 	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 = cmd.val;
+				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:
 		if(cmd.type == CMDtype::get){
 			replies.emplace_back(this->effectRatio);
@@ -1200,6 +1260,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;

Firmware/FFBoard/UserExtensions/Inc/TMC4671.h

@@ -32,6 +32,11 @@
 #define TMC_THREAD_PRIO 25 		// Must be higher than main thread
 #define TMC_ADCOFFSETFAIL 5000 	// How much offset from 0x7fff to allow before a calibration is failed
 
+#ifdef COGGING_TABLE_FLASH_START_ADDRESS
+// --- Constants for anti-cogging calibration ---
+#define CALIB_SPEED 50 	 	    // Slow speed in RPM used for calibration.
+#endif
+
 extern SPI_HandleTypeDef HSPIDRV;
 
 #ifdef TIM_TMC
@@ -57,13 +62,25 @@ extern TIM_HandleTypeDef TIM_TMC;
 #define TIM_TMC_ARR 250
 #endif
 
-enum class TMC_ControlState : uint32_t {uninitialized,waitPower,Shutdown,Running,EncoderInit,EncoderFinished,HardError,OverTemp,IndexSearch,FullCalibration,ExternalEncoderInit,Pidautotune, NONE
+enum class TMC_ControlState : uint32_t {uninitialized,waitPower,Shutdown,Running,EncoderInit,EncoderFinished,HardError,OverTemp,IndexSearch,FullCalibration,ExternalEncoderInit,Pidautotune
+#ifdef COGGING_TABLE_FLASH_START_ADDRESS
+	,CoggingCalibration
+#endif
+	,SlewRateCalibration, NONE
 };
 
 enum class TMC_PwmMode : uint8_t {off = 0,HSlow_LShigh = 1, HShigh_LSlow = 2, res2 = 3, res3 = 4, PWM_LS = 5, PWM_HS = 6, PWM_FOC = 7};
 
 enum class TMC_StartupType{NONE,coldStart,warmStart};
 
+enum class CoggingState : uint8_t { Init, ForwardWait, ForwardMeasure, BackwardWait, BackwardMeasure, Compute };
+
+struct CoggingCalibData {
+	int32_t temp_fw[CALIB_MAP_SIZE] = {0};
+	int32_t temp_bw[CALIB_MAP_SIZE] = {0};
+	uint16_t counts_fw[CALIB_MAP_SIZE] = {0};
+	uint16_t counts_bw[CALIB_MAP_SIZE] = {0};
+};
 
 enum class TMC_GpioMode{DebugSpi,DSAdcClkOut,DSAdcClkIn,Aout_Bin,Ain_Bout,Aout_Bout,Ain_Bin};
 enum class MotorType : uint8_t {NONE=0,DC=1,STEPPER=2,BLDC=3};
@@ -265,6 +282,9 @@ struct TMC4671FlashAddrs{
 	uint16_t encOffset = ADR_TMC1_ENC_OFFSET;
 	uint16_t phieOffset = ADR_TMC1_PHIE_OFS;
 	uint16_t torqueFilter = ADR_TMC1_TRQ_FILT;
+#ifdef COGGING_TABLE_FLASH_START_ADDRESS
+	uint16_t coggingEnable = ADR_TMC1_COGGING_CAL;
+#endif
 };
 
 struct TMC4671ABNConf{
@@ -395,7 +415,10 @@ class TMC4671 :
 		torqueP,torqueI,fluxP,fluxI,velocityP,velocityI,posP,posI,
 		tmctype,pidPrec,phiesrc,fluxoffset,seqpi,tmcIscale,encdir,temp,reg,
 		svpwm,fullCalibration,calibrated,abnindexenabled,findIndex,getState,encpol,combineEncoder,invertForce,vmTmc,
-		extphie,torqueFilter_mode,torqueFilter_f,torqueFilter_q,pidautotune,fluxbrake,pwmfreq
+		extphie,torqueFilter_mode,torqueFilter_f,torqueFilter_q,pidautotune,fluxbrake,pwmfreq,
+#ifdef COGGING_TABLE_FLASH_START_ADDRESS
+		cogging,calibrateCogging, coggingTable
+#endif
 	};
 
 #ifdef TMCDEBUG
@@ -457,6 +480,9 @@ friend class TMCDebugBridge;
 	bool checkEncoder();
 	void calibrateAenc();
 	void calibrateEncoder();
+#ifdef COGGING_TABLE_FLASH_START_ADDRESS
+	void calibrateCogging();
+#endif
 
 	void setEncoderType(EncoderType_TMC type);
 	uint32_t getEncCpr();
@@ -488,6 +514,11 @@ friend class TMCDebugBridge;
 
 	void setBBM(uint8_t bbml,uint8_t bbmh);
 
+	
+	// Slew rate calibration control
+	uint16_t getDrvSlewRate();
+	bool startSlewRateCalibration();
+	bool isSlewRateCalibrationInProgress();
 
 #ifdef TIM_TMC
 	void timerElapsed(TIM_HandleTypeDef* htim);
@@ -696,6 +727,23 @@ friend class TMCDebugBridge;
 
 	uint8_t spi_buf[5] = {0};
 
+#ifdef COGGING_TABLE_FLASH_START_ADDRESS
+	// Cogging Calibration
+	int16_t data_cogging[CALIB_MAP_SIZE] = {0};
+	bool cogging_enabled = false;
+	void saveCoggingTable();
+	void clearCoggingTable();
+
+	CoggingState coggingCalibState = CoggingState::Init;
+	std::unique_ptr<CoggingCalibData> coggingData = nullptr;
+	uint32_t calibStartTime = 0;
+	MotionMode prevCalibMode = MotionMode::stop;
+	void handleStateCoggingCalibration();
+#endif
+
+	uint16_t maxSlewRate = MAX_SLEW_RATE; // in mA/ms
+	void measureMaxSlewRate();
+
 	enum class PidTuneState : uint8_t { Init, RampFluxP, TuneFluxI_Pulse, TuneFluxI_Measure, Done };
 	PidTuneState pidTuneState = PidTuneState::Init;
 	uint32_t pidTuneStartTime = 0;