Fix: semaphore issue lock with ITR and static power when there is several TMC Class enabled

Author: manoukianv

Firmware/FFBoard/Src/Axis.cpp

@@ -512,33 +512,45 @@ 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();
+	this->maxTorqueRateMS = drv_new->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();
 }
 
 

Firmware/FFBoard/UserExtensions/Inc/TMC4671.h

@@ -702,6 +702,7 @@ friend class TMCDebugBridge;
 	bool encHallRestored = false;
 	bool canChangeHwType 	= true; // Allows changing the hardware version by commands
 	//int32_t phiEOffsetRestored = 0; //-0x8000 to 0x7fff
+	uint8_t powerCheckCounter = 0;
 	uint8_t calibrationFailCount = 2;
 
 	int16_t externalEncoderPhieOffset = 0; // PhiE offset for external encoders

Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp

@@ -220,9 +220,12 @@ void TMC4671::restoreFlash(){
 	}
 
 #ifdef COGGING_TABLE_FLASH_START_ADDRESS
-	uint16_t cogging;
-	Flash_Read(flashAddrs.coggingEnable, &cogging);
-	cogging_enabled = cogging & 0x01;
+	uint16_t cogging = 0; // Initialize to avoid random stack data
+	if(Flash_Read(flashAddrs.coggingEnable, &cogging)) {
+		cogging_enabled = cogging & 0x01;
+	} else {
+		cogging_enabled = false;
+	}
 
 	Flash_ReadCoggingTable(this->drv_address - 1, this->data_cogging);
 #endif
@@ -509,14 +512,13 @@ void TMC4671::Run(){
 		{
 			allowStateChange = false;
 			pulseClipLed(); // blink led
-			static uint8_t powerCheckCounter = 0;
 			// if powered check ADCs and go to encoder calibration
 			if(!hasPower() || emergency){
-				powerCheckCounter = 0;
+				this->powerCheckCounter = 0;
 				Delay(250);
 				break;
 			}
-			if(++powerCheckCounter > 5 && !powerInitialized){
+			if(++this->powerCheckCounter > 5 && !powerInitialized){
 				initializeWithPower();
 			}
 			if(powerInitialized){