fix: resolve boot-up deadlocks and refactor TMC state machine to non-blocking

Refactored Axis::setDrvType to instantiate driver outside critical section, preventing FreeRTOS crashes. Converted TMC4671 state machine to an asynchronous, non-blocking model. Updated existing handlers (WaitPower, Running, FullCalibration, PID AutoTune) to use a tick-based approach. Refactored header constants to use constexpr and UPPERCASE naming. Integrated updated documentation.

Author: manoukianv

Firmware/FFBoard/Inc/Axis.h

@@ -60,7 +60,6 @@ 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;
@@ -123,8 +122,6 @@ 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
@@ -477,7 +474,6 @@ 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.
 
@@ -507,17 +503,14 @@ 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.
-	const float endstopGain = 25;	//!< Overall maximum endstop intensity.
+	static constexpr float ENDSTOP_GAIN = 25.0f;	//!< 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
@@ -528,7 +521,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
-	const int32_t internalFxClip = 20000; //!< Clipping value for internal effects.
+	static constexpr int32_t INTERNAL_FX_CLIP = 20000; //!< Clipping value for internal effects.
 
 	// Internal effects intensity
 	uint8_t damperIntensity = 30;	//!< Intensity of the internal damper effect.

Firmware/FFBoard/Inc/flash_helpers.h

@@ -32,11 +32,6 @@ 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();
 
-#ifdef COGGING_TABLE_FLASH_START_ADDRESS
-bool Flash_WriteCoggingTable(uint8_t table_idx, int16_t* data);
-bool Flash_ReadCoggingTable(uint8_t table_idx, int16_t* data);
-#endif
-
 bool OTP_Write(uint16_t adroffset,uint64_t dat);
 bool OTP_Read(uint16_t adroffset,uint64_t* dat);
 

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, ADR_AXIS1_MAX_SLEWRATE_DRV,
+		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,
 										   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, ADR_AXIS2_MAX_SLEWRATE_DRV,
+		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,
 										   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, ADR_AXIS3_MAX_SLEWRATE_DRV,
+	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,
 										   ADR_AXIS3_EQ1,ADR_AXIS3_EQ2,ADR_AXIS3_EQ3,
@@ -207,9 +207,6 @@ 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);
 }
 
 /*
@@ -241,13 +238,6 @@ 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)) {
@@ -332,7 +322,6 @@ 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);
@@ -446,29 +435,6 @@ 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();
 
@@ -512,33 +478,44 @@ void Axis::setDrvType(uint8_t drvtype)
 	{
 		return;
 	}
-	cpp_freertos::CriticalSection::Enter();
-	MotorDriver* drv = driverChooser.Create((uint16_t)drvtype);
-	this->drv.reset(drv);
-	if (drv == nullptr)
+
+	// Create the driver outside of the critical section to avoid FreeRTOS issues (semaphore take with interrupts disabled)
+	MotorDriver* drv_new = driverChooser.Create((uint16_t)drvtype);
+	
+	if (drv_new == nullptr)
 	{
-		cpp_freertos::CriticalSection::Exit();
 		return;
 	}
+
+	// Pre-initialization outside of critical section
+	if(!drv_new->hasIntegratedEncoder()){
+		drv_new->setEncoder(this->enc);
+	}
+	drv_new->setupDriver();
+
+	// Use critical section only for atomic replacement of the driver pointer
+	MotorDriver* old_drv_ptr = nullptr;
+	cpp_freertos::CriticalSection::Enter();
+	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->getDrvSlewRate();
+	cpp_freertos::CriticalSection::Exit();
 
-	// Pass encoder to driver again
-	if(!this->drv->hasIntegratedEncoder()){
-		this->drv->setEncoder(this->enc);
+	// Delete the old driver outside of the critical section to avoid blocking destructors or FreeRTOS issues
+	if (old_drv_ptr != nullptr) {
+		delete old_drv_ptr;
 	}
 
-	drv->setupDriver();
+	// Perform SPI communications (start/suspend) outside of the critical section
 	if (!tud_connected())
 	{
 		control->usb_disabled = false;
 		this->usbSuspend();
 	}
 	else
 	{
-		drv->startMotor();
+		drv_new->startMotor();
 	}
-	cpp_freertos::CriticalSection::Exit();
 }
 
 
@@ -694,13 +671,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, -internalFxClip, internalFxClip));
+		mechanicalEffectTorque -= damperFilter.process(clip<float, int32_t>(speedFiltered, -INTERNAL_FX_CLIP, INTERNAL_FX_CLIP));
 	}
 
 	// Always active inertia
 	if(inertiaIntensity != 0){
 		float accelFiltered = metric.current.accel * (float)inertiaIntensity * AXIS_INERTIA_RATIO;
-		mechanicalEffectTorque -= inertiaFilter.process(clip<float, int32_t>(accelFiltered, -internalFxClip, internalFxClip));
+		mechanicalEffectTorque -= inertiaFilter.process(clip<float, int32_t>(accelFiltered, -INTERNAL_FX_CLIP, INTERNAL_FX_CLIP));
 	}
 
 	// Always active friction. Based on effectsCalculator implementation
@@ -719,7 +696,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, -internalFxClip, internalFxClip));
+		mechanicalEffectTorque -= frictionFilter.process(clip<float, int32_t>(force, -INTERNAL_FX_CLIP, INTERNAL_FX_CLIP));
 	}
 
 }
@@ -828,7 +805,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 * endstopGain; // Apply endstop gain for stiffness.
+	endstopTorque *= (float)endstopStrength * ENDSTOP_GAIN; // Apply endstop gain for stiffness.
 	endstopTorque *= -clipDirection;
 
 	return clip<int32_t,int32_t>(endstopTorque,-0x7fff,0x7fff);
@@ -1155,35 +1132,10 @@ 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 = 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
+				this->maxTorqueRateMS = cmd.val;
 			}
-			break;
-		}
-
-	case Axis_commands::maxSlewRateDrv:
-		if (cmd.type == CMDtype::get) {
-			replies.emplace_back(maxSlewRate_Driver);
 		}
 		break;
 
@@ -1248,14 +1200,6 @@ 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/Src/flash_helpers.cpp

@@ -2,13 +2,14 @@
  * flash_helpers.cpp
  *
  *  Created on: 31.01.2020
- *      Author: Yannick
+ *      Author: Yannick, Vincent
  */
 #include "flash_helpers.h"
 #include "eeprom_addresses.h"
 #include <vector>
 #include "mutex.hpp"
 #include <span>
+#include <cstring>
 
 cpp_freertos::MutexStandard flashMutex;
 // Flash helpers
@@ -152,7 +153,7 @@ bool I2C_EEPROM_Write16(uint16_t devAdr,uint16_t adr, uint16_t dat){
 /**
  * Helper function to read 16b data from i2c eeproms with different block sizes
  */
-bool I2C_EEPROM_Read16(uint16_t devAdr,uint16_t adr,uint16_t *buf, bool checkempty){
+bool I2C_EEPROM_Read16(uint16_t devAdr,uint16_t adr, uint16_t *buf, bool checkempty){
 	uint32_t adrAbs = (adr * sizeof(*buf)/I2C_EEPROM_DATA_SIZE);
 	assert(adrAbs < I2C_EEPROM_SIZE);
 	while(!i2cport_int.isDeviceReady(&i2cdeveeprom, devAdr, 100, I2C_EEPROM_TIMEOUT,false)){
@@ -242,7 +243,9 @@ __weak bool Flash_Format(){
 __weak bool Flash_Init(){
 	return true;
 }
+
 #endif
+
 /*
  * Dumps all set variables from flash to a vector
  * does by default not include certain calibration constants that could break something if imported on a different board