fix: tmc init in axis, and starting initialisation

Author: manoukianv

Firmware/FFBoard/Inc/MotorDriver.h

@@ -27,6 +27,7 @@ class MotorDriver : public ChoosableClass{
 	const ClassType getClassType() override {return ClassType::Motordriver;};
 	static const std::vector<class_entry<MotorDriver>> all_drivers;
 
+	virtual void setupDriver();
 	virtual void turn(int16_t power);
 	virtual void stopMotor();
 	virtual void startMotor();

Firmware/FFBoard/Src/Axis.cpp

@@ -430,7 +430,7 @@ void Axis::setDrvType(uint8_t drvtype)
 		return;
 	}
 	cpp_freertos::CriticalSection::Enter();
-	this->drv.reset(drv_chooser.Create((uint16_t)drvtype));
+	this->drv.reset(driverChooser.Create((uint16_t)drvtype));
 	if (drv == nullptr)
 	{
 		cpp_freertos::CriticalSection::Exit();
@@ -443,6 +443,9 @@ void Axis::setDrvType(uint8_t drvtype)
 	if(!this->drv->hasIntegratedEncoder()){
 		this->drv->setEncoder(this->enc);
 	}
+
+	drv->setupDriver();
+	
 	if (!tud_connected())
 	{
 		control->usb_disabled = false;

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.

Firmware/FFBoard/UserExtensions/Inc/TMC4671.h

@@ -424,6 +424,7 @@ friend class TMCDebugBridge;
 
 	TMC4671MainConfig conf;
 
+	void setupDriver();
 	bool initialize();
 	void initializeWithPower();
 
@@ -503,6 +504,7 @@ friend class TMCDebugBridge;
 	bool emergency = false;
 	bool estopTriggered = false;
 	void turn(int16_t power);
+
 	int16_t getVelocityControllerTorque();
 	int16_t nextFlux = 0;
 	int16_t idleFlux = 0;

Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp

@@ -240,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
@@ -1675,13 +1687,6 @@ void TMC4671::startMotor(){
 	}
 	// Start driver if powered and emergency flag reset
 	if(hasPower() && !emergency){
-		// Configuration specific to TMC4671 upon starting the motor
-		// The power limit is set by the Axis class via setPowerLimit during initialization.
-		// We call setLimits() here to ensure all other limits (velocity, acceleration, etc.) are applied from the defaults.
-		setExternalEncoderAllowed(true);
-		setLimits(curLimits);
-		setMotionMode(MotionMode::torque);
-		
 		setPwm(TMC_PwmMode::PWM_FOC); // enable foc
 		enablePin.set();
 		setMotionMode(nextMotionMode,true);