diff --git a/Firmware/FFBoard/Inc/Axis.h b/Firmware/FFBoard/Inc/Axis.h index 246593d4a..ec47025cc 100644 --- a/Firmware/FFBoard/Inc/Axis.h +++ b/Firmware/FFBoard/Inc/Axis.h @@ -61,7 +61,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; @@ -114,12 +113,9 @@ struct GearRatio_t{ enum class Axis_commands : uint32_t{ power=0x00,degrees=0x01,esgain,zeroenc,invert,idlespring,axisdamper,enctype,drvtype, - pos,curtorque,curpos,curspd,curaccel, - fxratio,reductionScaler, + pos,fxratio,curtorque,curpos,curspd,curaccel,reductionScaler, filterSpeed, filterAccel, filterProfileId,cpr,axisfriction,axisinertia, - maxspeed,slewrate, - calibrate_maxSlewRateDrv, - maxSlewRateDrv, + maxspeed,maxtorquerate, expo,exposcale }; @@ -419,11 +415,6 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle * @return torque update to apply to reduced de speed. */ 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(int32_t& torque); /** * @brief Decodes the axis configuration from a 16-bit integer stored in flash. * @param val The 16-bit encoded configuration value. @@ -459,8 +450,7 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle 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). + // Axis parameters 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. @@ -498,9 +488,6 @@ 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 diff --git a/Firmware/FFBoard/Inc/MotorDriver.h b/Firmware/FFBoard/Inc/MotorDriver.h index b643a66ac..80cf1627b 100644 --- a/Firmware/FFBoard/Inc/MotorDriver.h +++ b/Firmware/FFBoard/Inc/MotorDriver.h @@ -35,23 +35,10 @@ class MotorDriver : public ChoosableClass{ virtual void setPowerLimit(uint16_t power){}; // specific motor driver manager power, this is used to send powerLimit to the driver, like TMC4671. - // /** - * 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 + * Check if any calibration process is currently active. */ - virtual bool startSlewRateCalibration() { return false; }; - /** - * Check if calibration is in process - * @return the state of calibration process - */ - virtual bool isSlewRateCalibrationInProgress() { return false; }; - /** - * Get the value of the Slew Rate after a calibration - * @return - */ - virtual uint16_t getDrvSlewRate() { return MAX_SLEW_RATE; }; + virtual bool isCalibrationInProgress() { return false; }; virtual bool motorReady(); // Returns true if the driver is active and ready to receive commands diff --git a/Firmware/FFBoard/Inc/flash_helpers.h b/Firmware/FFBoard/Inc/flash_helpers.h index e40469531..908b31852 100644 --- a/Firmware/FFBoard/Inc/flash_helpers.h +++ b/Firmware/FFBoard/Inc/flash_helpers.h @@ -33,6 +33,10 @@ void Flash_Dump(std::vector> *result,bool includeA bool Flash_Format(); +#ifdef COGGING_TABLE_FLASH_START_ADDRESS +bool Flash_WriteCoggingTable(uint8_t table_idx, void* data); +bool Flash_ReadCoggingTable(uint8_t table_idx, void* data); +#endif bool OTP_Write(uint16_t adroffset,uint64_t dat); bool OTP_Read(uint16_t adroffset,uint64_t* dat); diff --git a/Firmware/FFBoard/Src/Axis.cpp b/Firmware/FFBoard/Src/Axis.cpp index 187142ebc..2a629af89 100644 --- a/Firmware/FFBoard/Src/Axis.cpp +++ b/Firmware/FFBoard/Src/Axis.cpp @@ -117,7 +117,7 @@ Axis::Axis(char axis,volatile Control_t* control) :CommandHandler("axis", CLSID_ { driverChooser = ClassChooser(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}); } @@ -125,14 +125,14 @@ Axis::Axis(char axis,volatile Control_t* control) :CommandHandler("axis", CLSID_ { driverChooser = ClassChooser(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}); } 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}); } @@ -171,7 +171,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("slewrate", Axis_commands::slewrate, "Torque rate limit in counts/ms",CMDFLAG_GET | CMDFLAG_SET); + registerCommand("maxtorquerate", Axis_commands::maxtorquerate, "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); @@ -210,19 +210,13 @@ void Axis::restoreFlash(){ pulseErrLed(); } - // save the max torque for the slew rate if (Flash_Read(flashAddresses.maxAccel, &value)){ this->maxTorqueRateMS = value; }else{ 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; @@ -284,8 +278,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); Flash_Write(flashAddresses.degrees, (degreesOfRotation & 0x7fff) | (invertAxis << 15)); @@ -364,7 +356,7 @@ void Axis::prepareForUpdate(){ int32_t scaledEnc; std::tie(scaledEnc,std::ignore) = scaleEncValue(angle, degreesOfRotation); - if (abs(scaledEnc) > 0xffff && drv->motorReady()){ + if (abs(scaledEnc) > 0xffff && drv->motorReady() && !drv->isCalibrationInProgress()){ // We are way off. Shut down drv->stopMotor(); pulseErrLed(); @@ -400,10 +392,9 @@ void Axis::errorCallback(const Error &error, bool cleared){ void Axis::updateDriveTorque(){ - // totalTorque = effectTorque + endstopTorque int32_t totalTorque; - bool torqueChanged = updateTorque(&totalTorque); - if (torqueChanged && drv->motorReady()){ + updateTorque(&totalTorque); + if (drv->motorReady()){ // Send to motor driver drv->turn(totalTorque); } @@ -429,23 +420,36 @@ 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(); - // Pass encoder to driver again - if(!this->drv->hasIntegratedEncoder()){ - this->drv->setEncoder(this->enc); + cpp_freertos::CriticalSection::Exit(); + + // 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; @@ -453,9 +457,8 @@ void Axis::setDrvType(uint8_t drvtype) } else { - drv->startMotor(); + drv_new->startMotor(); } - cpp_freertos::CriticalSection::Exit(); } @@ -604,6 +607,10 @@ void Axis::setFxStrengthAndFilter(uint8_t val,uint8_t& valToSet, Biquad& filter) void Axis::calculateMechanicalEffects(bool ffb_on){ mechanicalEffectTorque = 0; + if (outOfBounds || drv->isCalibrationInProgress() || !drv->motorReady()) { + return; + } + if(!ffb_on){ mechanicalEffectTorque += updateIdleSpringForce(); } @@ -780,59 +787,38 @@ bool Axis::updateTorque(int32_t* totalTorque) { torque = 0; } - // 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 = (int32_t)((float)torque * forceFadeMultiplier); forceFadeMultiplier += forceFadeDuration / this->filter_f; } // STEP 6: Dynamic limiters (Speed) - // Must be applied while torque is still in the Game Coordinate System (matching current.speed). torque -= applySpeedLimiterTorque(torque); - // 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; + // Torque slew rate limiter + if(maxTorqueRateMS > 0){ + torque = clip(torque, metric.previous.torque - maxTorqueRateMS,metric.previous.torque + maxTorqueRateMS); + } - // 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); + if(invertAxis){ + torque = -torque; + } - // STEP 8: Final hardware clipping int32_t torqueAfterClipping = clip((int32_t)torque, -power, power); if (torqueAfterClipping != torque){ - pulseClipLed(); // Visual alert: MOTOR cannot provide requested power (Hardware clipping) + pulseClipLed(); } - // Store the actually applied torque for the next iteration (used by the slew rate limiter). - metric.current.torque = torqueAfterClipping; + metric.previous.torque = torqueAfterClipping; + metric.current.torque = torqueAfterClipping; - // return result *totalTorque = torqueAfterClipping; return (metric.current.torque != metric.previous.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. - if(maxTorqueRateMS == 0) { - return; // Limiter is disabled - } - - // This prevents sudden torque jumps, resulting in a smoother feel. - 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(torque, previousTorque - maxTorqueChange, previousTorque + maxTorqueChange); - } - 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. @@ -1057,39 +1043,8 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector& handleGetSet(cmd, replies, this->maxSpeedDegS); break; - 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(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); - } + case Axis_commands::maxtorquerate: + handleGetSet(cmd, replies, this->maxTorqueRateMS); break; case Axis_commands::fxratio: diff --git a/Firmware/FFBoard/Src/CommandHandler.cpp b/Firmware/FFBoard/Src/CommandHandler.cpp index 95cf4d823..4dfc33d91 100644 --- a/Firmware/FFBoard/Src/CommandHandler.cpp +++ b/Firmware/FFBoard/Src/CommandHandler.cpp @@ -57,36 +57,38 @@ std::string CommandHandler::getCommandsHelpstring(){ } if(registeredCommands.empty()){ - helpstring += "No commands."; + helpstring.append("No commands."); }else{ - helpstring += "cmd\tflags\tdescription\n\n"; + helpstring.append("cmd\tflags\tdescription\n\n"); for(CmdHandlerCommanddef& cmd : registeredCommands){ if(cmd.helpstring != nullptr && cmd.cmd != nullptr){ - helpstring += std::string(cmd.cmd); - helpstring += "\t("; + helpstring.append(cmd.cmd); + helpstring.append("\t("); if(cmd.flags & CMDFLAG_GET){ - helpstring+= " R"; + helpstring.append(" R"); } if(cmd.flags & CMDFLAG_SET){ - helpstring+= " W"; + helpstring.append(" W"); } if(cmd.flags & CMDFLAG_SETADR){ - helpstring+= " WA"; + helpstring.append(" WA"); } if(cmd.flags & CMDFLAG_GETADR){ - helpstring+= " RA"; + helpstring.append(" RA"); } if(cmd.flags & CMDFLAG_INFOSTRING){ - helpstring+= " I"; + helpstring.append(" I"); } if(cmd.flags & CMDFLAG_STR_ONLY){ - helpstring+= " STR"; + helpstring.append(" STR"); } if(cmd.flags & CMDFLAG_DEBUG){ - helpstring+= " DBG"; + helpstring.append(" DBG"); } - helpstring.append(" )\t").append(std::string(cmd.helpstring)).append("\n"); + helpstring.append(" )\t"); + helpstring.append(cmd.helpstring); + helpstring.append("\n"); } } @@ -543,7 +545,6 @@ void CommandHandler::registerCommand_INT(const char* cmd,const uint32_t cmdid,co } this->registeredCommands.emplace_back(cmd, help,cmdid,flags); - this->registeredCommands.shrink_to_fit(); } /** diff --git a/Firmware/FFBoard/Src/CommandInterface.cpp b/Firmware/FFBoard/Src/CommandInterface.cpp index 0427cd0d0..98cb97d8a 100644 --- a/Firmware/FFBoard/Src/CommandInterface.cpp +++ b/Firmware/FFBoard/Src/CommandInterface.cpp @@ -240,7 +240,7 @@ void StringCommandInterface::generateReplyFromCmd(std::string& replyPart,const P */ -CDC_CommandInterface::CDC_CommandInterface() : StringCommandInterface(CDC_CMD_BUFFER_SIZE), Thread("CDCCMD", 512, 37) { +CDC_CommandInterface::CDC_CommandInterface() : StringCommandInterface(CDC_CMD_BUFFER_SIZE), Thread("CDCCMD", 384, 37) { parser.setClearBufferTimeout(parserTimeout); this->Start(); } diff --git a/Firmware/FFBoard/Src/Encoder.cpp b/Firmware/FFBoard/Src/Encoder.cpp index 157363308..1f693e83e 100644 --- a/Firmware/FFBoard/Src/Encoder.cpp +++ b/Firmware/FFBoard/Src/Encoder.cpp @@ -65,10 +65,19 @@ int32_t Encoder::getPosAbs(){ * Returns a float position in rotations */ float Encoder::getPos_f(){ - if(getCpr() == 0){ - return 0.0; // cpr not set. + int32_t cpr = this->getCpr(); + if(cpr == 0){ + return 0.0f; // cpr not set. } - return (float)this->getPos() / (float)this->getCpr(); + int32_t pos = this->getPos(); + + // Split turns and remainder to prevent 24-bit float mantissa truncation + // when the absolute position becomes very large (e.g., >16-bit encoders after several turns) + // The optimisation is unused during simulation (range is 900°), but is recommended for motor + // calibration. + int32_t turns = pos / cpr; + int32_t remainder = pos % cpr; + return (float)turns + ((float)remainder / (float)cpr); } /** diff --git a/Firmware/FFBoard/Src/FFBoardMainCommandThread.cpp b/Firmware/FFBoard/Src/FFBoardMainCommandThread.cpp index 13512c4ac..d0421e3fb 100644 --- a/Firmware/FFBoard/Src/FFBoardMainCommandThread.cpp +++ b/Firmware/FFBoard/Src/FFBoardMainCommandThread.cpp @@ -30,7 +30,7 @@ Error FFBoardMainCommandThread::cmdExecError = Error(ErrorCode::cmdExecutionErro FFBoardMainCommandThread* commandThread; // Note: allocate enough memory for the command thread to store replies -FFBoardMainCommandThread::FFBoardMainCommandThread(FFBoardMain* mainclass) : Thread("CMD_MAIN",700, 32) { +FFBoardMainCommandThread::FFBoardMainCommandThread(FFBoardMain* mainclass) : Thread("CMD_MAIN", 600, 32) { //main = mainclass; commandThread = this; this->Start(); @@ -98,8 +98,8 @@ void FFBoardMainCommandThread::executeCommands(std::vector& comma CmdHandlerCommanddef* cmdDef = handler->getCommandFromId(cmd.cmdId); // check flags - bool validFlags = cmdDef != nullptr; - if(cmdDef->flags & ( CMDFLAG_GET | CMDFLAG_SET | CMDFLAG_INFOSTRING | CMDFLAG_GETADR | CMDFLAG_SETADR) ){ + bool validFlags = (cmdDef != nullptr); + if(validFlags && (cmdDef->flags & ( CMDFLAG_GET | CMDFLAG_SET | CMDFLAG_INFOSTRING | CMDFLAG_GETADR | CMDFLAG_SETADR)) ){ // A type flag is preset. check it validFlags = static_cast(cmd.type) & cmdDef->flags; // type uses the same flag values } diff --git a/Firmware/FFBoard/Src/flash_helpers.cpp b/Firmware/FFBoard/Src/flash_helpers.cpp index 3e64de7d1..211ce62d0 100644 --- a/Firmware/FFBoard/Src/flash_helpers.cpp +++ b/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 #include "mutex.hpp" #include +#include cpp_freertos::MutexStandard flashMutex; // Flash helpers @@ -260,6 +261,78 @@ __weak bool Flash_Format(){ __weak bool Flash_Init(){ return true; } + +#endif +#if defined(COGGING_TABLE_FLASH_START_ADDRESS) + +// Statically allocated buffer for the cogging region (approx 1KB for 3 tables of harmonics) +static uint8_t cogging_flash_buffer[1024]; + +/** + * @brief Writes a single cogging table to flash using a safe read-modify-write procedure. + * @param table_idx Index of the table to write (0 to MAX_COGGING_TABLES-1). + * @param data Pointer to the harmonic data to be written. + * @return true on success, false on failure. + */ +bool Flash_WriteCoggingTable(uint8_t table_idx, void* data) { + if (table_idx >= MAX_COGGING_TABLES) { + return false; + } + + // Use a statically allocated buffer to avoid stack overflow and heap allocation errors. + const uint32_t region_size = sizeof(cogging_flash_buffer); + uint8_t* temp_buffer = cogging_flash_buffer; + + // 1. Read the entire existing region from flash into the buffer + memcpy(temp_buffer, (void*)COGGING_TABLE_FLASH_START_ADDRESS, region_size); + + // 2. Copy the new table data into the buffer at the correct offset + uint32_t offset = table_idx * COGGING_TABLE_SIZE; + if (offset + COGGING_TABLE_SIZE > region_size) return false; + + memcpy(temp_buffer + offset, data, COGGING_TABLE_SIZE); + + // 3. Erase the flash sector + HAL_FLASH_Unlock(); + FLASH_EraseInitTypeDef pEraseInit; + pEraseInit.TypeErase = FLASH_TYPEERASE_SECTORS; + pEraseInit.Sector = COGGING_TABLE_FLASH_SECTOR; + pEraseInit.NbSectors = 1; + pEraseInit.VoltageRange = VOLTAGE_RANGE_3; + uint32_t SectorError = 0; + if (HAL_FLASHEx_Erase(&pEraseInit, &SectorError) != HAL_OK) { + HAL_FLASH_Lock(); + return false; + } + + // 4. Write the modified buffer back to flash + for (uint32_t i = 0; i < region_size / sizeof(uint32_t); ++i) { + uint32_t write_address = COGGING_TABLE_FLASH_START_ADDRESS + i * sizeof(uint32_t); + uint32_t word_to_write = ((uint32_t*)temp_buffer)[i]; + if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, write_address, word_to_write) != HAL_OK) { + HAL_FLASH_Lock(); + return false; + } + } + + HAL_FLASH_Lock(); + return true; +} + +/** + * @brief Reads a single cogging table from its dedicated flash area. + * @param table_idx Index of the table to read (0 to MAX_COGGING_TABLES-1). + * @param data Pointer to a buffer where the harmonic data will be stored. + * @return true on success, false on failure. + */ +bool Flash_ReadCoggingTable(uint8_t table_idx, void* data) { + if (table_idx >= MAX_COGGING_TABLES) { + return false; + } + uint32_t address = COGGING_TABLE_FLASH_START_ADDRESS + (table_idx * COGGING_TABLE_SIZE); + memcpy(data, (void*)address, COGGING_TABLE_SIZE); + return true; +} #endif /* * Dumps all set variables from flash to a vector diff --git a/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h b/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h index 3abce6502..4b3c4ffd1 100644 --- a/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h +++ b/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h @@ -23,14 +23,27 @@ #include "TimerHandler.h" #include #include "semaphore.hpp" + +#ifdef USE_DSP_FUNCTIONS +#include "dsp/controller_functions.h" +#include "dsp/fast_math_functions.h" +#endif #include #include "cpp_target_config.h" #include "Filters.h" #define SPITIMEOUT 500 -#define TMC_THREAD_MEM 256 -#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 +#define TMC_THREAD_MEM 1024 +#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 COGGING_CALIB_LUT_RESOLUTION 2880 // Resolution for legacy protocol communication +#define COGGING_CALIB_TIME_PER_REV_S 20 // Time in seconds to complete one revolution (8s = 7.5 RPM) +#define COGGING_CALIB_DFT_HARMONICS 128 // Number of harmonics to analyze during calibration +#define COGGING_CALIB_ENABLE_ID_DIAG // Enable Point 1 diagnostic (Id axis analysis) +#endif extern SPI_HandleTypeDef HSPIDRV; @@ -38,6 +51,11 @@ extern SPI_HandleTypeDef HSPIDRV; extern TIM_HandleTypeDef TIM_TMC; #endif +#ifdef TIM_CALIBRATION +// Hardware timer configuration used for non-blocking anticogging torque calibration. +extern TIM_HandleTypeDef TIM_CALIBRATION; +#endif + #ifndef TMC4671_DEFAULT_CURRENT_SCALER #define TMC4671_DEFAULT_CURRENT_SCALER 0 #endif @@ -57,15 +75,30 @@ extern TIM_HandleTypeDef TIM_TMC; #define TIM_TMC_ARR 250 #endif +struct Harmonic { + float32_t amplitude; + float32_t phase; + uint16_t order; +}; + +struct CoggingCalibData { + float32_t iq_sums[1024]; + float32_t id_sums[1024]; + uint16_t counts[1024]; +}; -enum class TMC_ControlState : uint32_t {uninitialized,waitPower,Shutdown,Running,EncoderInit,EncoderFinished,HardError,OverTemp,IndexSearch,FullCalibration,ExternalEncoderInit,Pidautotune}; +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 + , 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 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}; enum class PhiE : uint8_t {ext=1,openloop=2,abn=3,hall=5,aenc=6,aencE=7,NONE,extEncoder}; enum class MotionMode : uint8_t {stop=0,torque=1,velocity=2,position=3,prbsflux=4,prbstorque=5,prbsvelocity=6,uqudext=8,encminimove=9,NONE}; @@ -265,6 +298,10 @@ 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; + uint16_t coggingScale = ADR_TMC1_COGGING_SCALE; +#endif }; struct TMC4671ABNConf{ @@ -372,7 +409,8 @@ struct TMC4671BiquadFilters{ class TMC4671 : public MotorDriver, public PersistentStorage, public Encoder, public CommandHandler, public SPIDevice, public ExtiHandler, public cpp_freertos::Thread,ErrorHandler -#ifdef TIM_TMC +#if defined(TIM_TMC) || defined(TIM_CALIBRATION) + // Inherit from TimerHandler to receive hardware timer elapsed interrupts ,public TimerHandler #endif { @@ -382,7 +420,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, coggingScale, coggingSpeedP, coggingSpeedI +#endif }; #ifdef TMCDEBUG @@ -444,6 +485,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(); @@ -475,8 +519,12 @@ friend class TMCDebugBridge; void setBBM(uint8_t bbml,uint8_t bbmh); + + + bool isCalibrationInProgress() override; -#ifdef TIM_TMC +#if defined(TIM_TMC) || defined(TIM_CALIBRATION) + // Callback triggered when a hardware timer expires void timerElapsed(TIM_HandleTypeDef* htim); #endif @@ -500,7 +548,7 @@ friend class TMCDebugBridge; uint16_t maxPowerAxis = 0; int16_t controlFluxDissipate(); - const float fluxDissipationLimit = 1000; + static constexpr float FLUX_DISSIPATION_LIMIT = 1000.0f; void setTorque(int16_t torque); @@ -515,6 +563,7 @@ friend class TMCDebugBridge; void setFluxTorque(int16_t flux, int16_t torque); void setFluxTorqueFF(int16_t flux, int16_t torque); std::pair getActualTorqueFlux(); + int32_t getActualFlux(); int32_t getActualTorque(); @@ -621,7 +670,7 @@ friend class TMCDebugBridge; protected: class TMC_ExternalEncoderUpdateThread : public cpp_freertos::Thread{ public: - TMC_ExternalEncoderUpdateThread(TMC4671* tmc); + TMC_ExternalEncoderUpdateThread(TMC4671* tmc); //~TMC_ExternalEncoderUpdateThread(); void Run(); void updateFromIsr(); @@ -635,14 +684,15 @@ friend class TMCDebugBridge; 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"); - const Error communicationError = Error(ErrorCode::tmcCommunicationError, ErrorType::warning, "TMC not responding"); - const Error estopError = Error(ErrorCode::emergencyStop, ErrorType::critical, "TMC emergency stop triggered"); + const Error INDEX_NOT_HIT_ERROR = Error(ErrorCode::encoderIndexMissed,ErrorType::critical,"Encoder index missed"); + const Error LOW_VOLTAGE_ERROR = Error(ErrorCode::undervoltage,ErrorType::warning,"Low motor voltage"); + const Error COMMUNICATION_ERROR = Error(ErrorCode::tmcCommunicationError, ErrorType::warning, "TMC not responding"); + const Error ESTOP_ERROR = Error(ErrorCode::emergencyStop, ErrorType::critical, "TMC emergency stop triggered"); TMC_ControlState state = TMC_ControlState::uninitialized; TMC_ControlState laststate = TMC_ControlState::uninitialized; TMC_ControlState requestedState = TMC_ControlState::Shutdown; + TMC_ControlState postPowerState = TMC_ControlState::NONE; MotionMode curMotionMode = MotionMode::stop; MotionMode lastMotionMode = MotionMode::stop; MotionMode nextMotionMode = MotionMode::stop; @@ -665,6 +715,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 @@ -674,12 +725,41 @@ friend class TMCDebugBridge; bool recalibrationRequired = false; uint8_t enc_retry = 0; - uint8_t enc_retry_max = 3; + static constexpr uint8_t ENC_RETRY_MAX = 3; uint32_t lastStatTime = 0; + int32_t cached_pos = 0; uint8_t spi_buf[5] = {0}; +#ifdef COGGING_TABLE_FLASH_START_ADDRESS + // Cogging Calibration + Harmonic cogging_harmonics[COGGING_HARMONICS_COUNT]; + bool cogging_enabled = false; + float cogging_scale = 0.5f; + int32_t last_anticogging_torque = 0; + void saveCoggingTable(); + void clearCoggingTable(); + + std::unique_ptr coggingData = nullptr; + uint32_t calibStartTime = 0; + MotionMode prevCalibMode = MotionMode::stop; + float coggingSpeedP = 0.0f; + float coggingSpeedI = 0.0f; + void handleStateCoggingCalibration(); +#endif + + + enum class PidTuneState : uint8_t { Init, RampFluxP, TuneFluxI_Pulse, TuneFluxI_Measure, Done }; + PidTuneState pidTuneState = PidTuneState::Init; + uint32_t pidTuneStartTime = 0; + uint16_t tuneFluxI = 0, tuneFluxP = 100; + int32_t tuneMeasurePeak = 0; + PhiE lastPidTunePhiE = PhiE::NONE; + MotionMode lastPidTuneMode = MotionMode::stop; + TMC4671PIDConf pidTuneNewPids; + void handleStatePidAutoTune(); + void initAdc(uint16_t mdecA, uint16_t mdecB,uint32_t mclkA,uint32_t mclkB); void setPwm(uint8_t val,uint16_t maxcnt,uint8_t bbmL,uint8_t bbmH);// 100MHz/maxcnt+1 void setPwm(TMC_PwmMode val);// pwm mode @@ -695,6 +775,16 @@ friend class TMCDebugBridge; // void ABN_init(); // void AENC_init(); + void handleStateWaitPower(); + void handleStateRunning(); + void handleStateFullCalibration(); + + // Calibration helpers + void applySafeTorque(float torque_cmd); + float getAbsolutePosition(); + float getWrappedError(float target, float actual); + float getFilteredPosition(); + void encoderInit(); void errorCallback(const Error &error, bool cleared); bool pidAutoTune(); @@ -708,17 +798,57 @@ friend class TMCDebugBridge; TMC4671Biquad_conf torqueFilterConf; TMC4671BiquadFilters curFilters; - const float fluxFilterFreq = 350.0; - - // External encoder timer fires interrupts to trigger a new commutation position update + static constexpr float FLUX_FILTER_FREQ = 350.0f; + + // --- Calibration & External Encoder Timing Variables --- + + // Utility: Timer driving the external encoder updater thread. When using an external encoder, + // this timer must continue running unaffected to avoid desynchronizing the encoder. + // Pacing of the calibration is then synchronized to this timer via tick counting. + // Expected Value: Points to &TIM_TMC (typically htim6, configured with TIM_TMC_ARR) or nullptr. + TIM_HandleTypeDef* externalEncoderTimer = #ifdef TIM_TMC + &TIM_TMC; +#else + nullptr; +#endif - TIM_HandleTypeDef* externalEncoderTimer = &TIM_TMC; + // Utility: Dedicated FreeRTOS helper thread to write external encoder positions to the TMC4671 + // register 0x1C asynchronously over SPI, keeping long SPI transfers out of the ISR. + // Expected Value: Valid std::unique_ptr when usingExternalEncoder() is true, or nullptr. std::unique_ptr extEncUpdater = nullptr; + + // Utility: Timer used for pacing the calibration loops when no external encoder is used. + // Shared with TIM_USER (MidiMain) and reconfigured dynamically during calibration. + // Expected Value: Points to &TIM_CALIBRATION (typically htim9) or nullptr if not defined/available. + TIM_HandleTypeDef* calibTimer = +#ifdef TIM_CALIBRATION + &TIM_CALIBRATION; #else - TIM_HandleTypeDef* externalEncoderTimer = nullptr; + nullptr; #endif + + // Utility: Active tick counter incremented inside the external encoder timer (TIM_TMC) ISR. + // Used only when calibration pacing is driven by the external encoder timer. + // Expected Value / Range: Increments from 0 up to (calibTicksTarget - 1). + volatile uint32_t calibTicksCount = 0; + + // Utility: The target tick threshold from TIM_TMC that corresponds to the requested calibration period. + // Set to 0 when tick-based pacing is inactive. + // Expected Value: Typically 1 for fast calibration loops (e.g., 250 us loop / 250 us ARR) + // and 4 for slow calibration loops (e.g., 1000 us loop / 250 us ARR). Value is 0 when inactive. + volatile uint32_t calibTicksTarget = 0; + void setUpExtEncTimer(); + + // Configures and starts the calibration hardware timer with the specified period (in microseconds). + void startCalibTimers(uint32_t period_us); + + // Stops the calibration hardware timer and restores default external encoder timer behavior if needed. + void stopCalibTimers(); + + // Returns the actual calibration period in microseconds based on the active timer pacing source. + uint32_t getActualCalibPeriod(uint32_t target_period_us); }; diff --git a/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h b/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h index bed80e3af..f50aecbf7 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 168 +#define NB_OF_VAR 189 extern const uint16_t VirtAddVarTab[NB_OF_VAR]; // Amount of variables in exportable list -#define NB_EXPORTABLE_ADR 153 +#define NB_EXPORTABLE_ADR 174 extern const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR]; @@ -99,7 +99,6 @@ 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 @@ -120,13 +119,14 @@ uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data) will return 1 if #define ADR_TMC1_FLUX_I 0x32A #define ADR_TMC1_PHIE_OFS 0x32B #define ADR_TMC1_TRQ_FILT 0x32C +#define ADR_TMC1_COGGING_CAL 0x32D +#define ADR_TMC1_COGGING_SCALE 0x32E // AXIS2 #define ADR_AXIS2_CONFIG 0x341 // 0-2 ENC, 3-5 DRV #define ADR_AXIS2_POWER 0x342 #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 @@ -147,13 +147,14 @@ uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data) will return 1 if #define ADR_TMC2_FLUX_I 0x36A #define ADR_TMC2_PHIE_OFS 0x36B #define ADR_TMC2_TRQ_FILT 0x36C +#define ADR_TMC2_COGGING_CAL 0x36D +#define ADR_TMC2_COGGING_SCALE 0x36E // AXIS3 #define ADR_AXIS3_CONFIG 0x381 // 0-2 ENC, 3-5 DRV #define ADR_AXIS3_POWER 0x382 #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 @@ -174,6 +175,8 @@ uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data) will return 1 if #define ADR_TMC3_FLUX_I 0x3AA #define ADR_TMC3_PHIE_OFS 0x3AB #define ADR_TMC3_TRQ_FILT 0x3AC +#define ADR_TMC3_COGGING_CAL 0x3AD +#define ADR_TMC3_COGGING_SCALE 0x3AE // RMD CAN Motor #define ADR_RMD1_DATA1 0x3C0 //0-4 CAN ID #define ADR_RMD1_TORQUE 0x3C1 //Maximum current diff --git a/Firmware/FFBoard/UserExtensions/Src/EncoderBissC.cpp b/Firmware/FFBoard/UserExtensions/Src/EncoderBissC.cpp index 5f36b13b1..e5ca4b42b 100644 --- a/Firmware/FFBoard/UserExtensions/Src/EncoderBissC.cpp +++ b/Firmware/FFBoard/UserExtensions/Src/EncoderBissC.cpp @@ -26,7 +26,7 @@ std::array EncoderBissC::tableCRC6n __attribute__((section (".ccmram EncoderBissC::EncoderBissC() : SPIDevice(ENCODER_SPI_PORT, ENCODER_SPI_PORT.getCsPins()[0]), CommandHandler("bissenc",CLSID_ENCODER_BISS,0), - cpp_freertos::Thread("BISSENC",64,42) { + cpp_freertos::Thread("BISSENC",128,42) { EncoderBissC::inUse = true; diff --git a/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp b/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp index c3b9745a7..ef0453a92 100644 --- a/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp +++ b/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp @@ -2,10 +2,12 @@ * TMC4671.cpp * * Created on: Feb 1, 2020 - * Author: Yannick, Vincent + * Author: Yannick, Vincent */ #include "TMC4671.h" +#include +#include #ifdef TMC4671DRIVER #include "ledEffects.h" #include "voltagesense.h" @@ -93,13 +95,25 @@ const ClassIdentifier TMC4671::getInfo() { void TMC4671::setAddress(uint8_t address){ if (address == 1){ - this->flashAddrs = TMC4671FlashAddrs({ADR_TMC1_MOTCONF, ADR_TMC1_CPR, ADR_TMC1_ENCA, ADR_TMC1_OFFSETFLUX, ADR_TMC1_TORQUE_P, ADR_TMC1_TORQUE_I, ADR_TMC1_FLUX_P, ADR_TMC1_FLUX_I,ADR_TMC1_ADC_I0_OFS,ADR_TMC1_ADC_I1_OFS,ADR_TMC1_ENC_OFFSET,ADR_TMC1_PHIE_OFS,ADR_TMC1_TRQ_FILT}); + this->flashAddrs = TMC4671FlashAddrs({ADR_TMC1_MOTCONF, ADR_TMC1_CPR, ADR_TMC1_ENCA, ADR_TMC1_OFFSETFLUX, ADR_TMC1_TORQUE_P, ADR_TMC1_TORQUE_I, ADR_TMC1_FLUX_P, ADR_TMC1_FLUX_I,ADR_TMC1_ADC_I0_OFS,ADR_TMC1_ADC_I1_OFS,ADR_TMC1_ENC_OFFSET,ADR_TMC1_PHIE_OFS,ADR_TMC1_TRQ_FILT +#ifdef COGGING_TABLE_FLASH_START_ADDRESS + ,ADR_TMC1_COGGING_CAL +#endif + }); }else if (address == 2) { - this->flashAddrs = TMC4671FlashAddrs({ADR_TMC2_MOTCONF, ADR_TMC2_CPR, ADR_TMC2_ENCA, ADR_TMC2_OFFSETFLUX, ADR_TMC2_TORQUE_P, ADR_TMC2_TORQUE_I, ADR_TMC2_FLUX_P, ADR_TMC2_FLUX_I,ADR_TMC2_ADC_I0_OFS,ADR_TMC2_ADC_I1_OFS,ADR_TMC2_ENC_OFFSET,ADR_TMC2_PHIE_OFS,ADR_TMC2_TRQ_FILT}); + this->flashAddrs = TMC4671FlashAddrs({ADR_TMC2_MOTCONF, ADR_TMC2_CPR, ADR_TMC2_ENCA, ADR_TMC2_OFFSETFLUX, ADR_TMC2_TORQUE_P, ADR_TMC2_TORQUE_I, ADR_TMC2_FLUX_P, ADR_TMC2_FLUX_I,ADR_TMC2_ADC_I0_OFS,ADR_TMC2_ADC_I1_OFS,ADR_TMC2_ENC_OFFSET,ADR_TMC2_PHIE_OFS,ADR_TMC2_TRQ_FILT +#ifdef COGGING_TABLE_FLASH_START_ADDRESS + ,ADR_TMC2_COGGING_CAL +#endif + }); }else if (address == 3) { - this->flashAddrs = TMC4671FlashAddrs({ADR_TMC3_MOTCONF, ADR_TMC3_CPR, ADR_TMC3_ENCA, ADR_TMC3_OFFSETFLUX, ADR_TMC3_TORQUE_P, ADR_TMC3_TORQUE_I, ADR_TMC3_FLUX_P, ADR_TMC3_FLUX_I,ADR_TMC3_ADC_I0_OFS,ADR_TMC3_ADC_I1_OFS,ADR_TMC3_ENC_OFFSET,ADR_TMC3_PHIE_OFS,ADR_TMC3_TRQ_FILT}); + this->flashAddrs = TMC4671FlashAddrs({ADR_TMC3_MOTCONF, ADR_TMC3_CPR, ADR_TMC3_ENCA, ADR_TMC3_OFFSETFLUX, ADR_TMC3_TORQUE_P, ADR_TMC3_TORQUE_I, ADR_TMC3_FLUX_P, ADR_TMC3_FLUX_I,ADR_TMC3_ADC_I0_OFS,ADR_TMC3_ADC_I1_OFS,ADR_TMC3_ENC_OFFSET,ADR_TMC3_PHIE_OFS,ADR_TMC3_TRQ_FILT +#ifdef COGGING_TABLE_FLASH_START_ADDRESS + ,ADR_TMC3_COGGING_CAL +#endif + }); } //this->setAxis((char)('W'+address)); } @@ -128,6 +142,14 @@ void TMC4671::saveFlash(){ uint16_t filterval = (torqueFilterConf.params.freq & 0x1fff) | ((uint8_t)(torqueFilterConf.mode) << 13); Flash_Write(flashAddrs.torqueFilter, filterval); +#ifdef COGGING_TABLE_FLASH_START_ADDRESS + // Save cogging state + uint16_t coggingFlash = (cogging_enabled ? 1 : 0); + Flash_Write(flashAddrs.coggingEnable, coggingFlash); + + int16_t scale_int = clip(this->cogging_scale * 10000.0f, -32767, 32767); + Flash_Write(flashAddrs.coggingScale, (uint16_t)scale_int); +#endif } /** @@ -139,6 +161,18 @@ void TMC4671::saveAdcParams(){ adcSettingsStored = true; } +#ifdef COGGING_TABLE_FLASH_START_ADDRESS +void TMC4671::saveCoggingTable(){ + Flash_WriteCoggingTable(this->drv_address - 1, (void*)this->cogging_harmonics); +} + +void TMC4671::clearCoggingTable(){ + memset(cogging_harmonics, 0, sizeof(cogging_harmonics)); + cogging_enabled = false; + saveCoggingTable(); +} +#endif + /** * Restores saved parameters * Call initialize() to apply some of the settings @@ -191,6 +225,24 @@ void TMC4671::restoreFlash(){ torqueFilterConf.mode = static_cast((filterval >> 13) & 0x7); } +#ifdef COGGING_TABLE_FLASH_START_ADDRESS + uint16_t cogging = 0; // Initialize to avoid random stack data + if(Flash_Read(flashAddrs.coggingEnable, &cogging)) { + cogging_enabled = cogging & 0x01; + } else { + cogging_enabled = false; + } + + uint16_t scale_flash = 0; + if(Flash_Read(flashAddrs.coggingScale, &scale_flash)) { + int16_t scale_int = (int16_t)scale_flash; + this->cogging_scale = (float)scale_int / 10000.0f; + } else { + this->cogging_scale = 0.0f; + } + + Flash_ReadCoggingTable(this->drv_address - 1, (int16_t*)this->cogging_harmonics); +#endif } bool TMC4671::hasPower(){ @@ -264,7 +316,7 @@ bool TMC4671::initialize(){ // } // Check if a TMC4671 is active and replies correctly if(!pingDriver()){ - ErrorHandler::addError(communicationError); + ErrorHandler::addError(COMMUNICATION_ERROR); return false; } @@ -307,7 +359,7 @@ bool TMC4671::initialize(){ setAdcOffset(conf.adc_I0_offset, conf.adc_I1_offset); setAdcScale(conf.adc_I0_scale, conf.adc_I1_scale); setTorqueFilter(torqueFilterConf); - setBiquadFlux(TMC4671Biquad(Biquad(BiquadType::lowpass, fluxFilterFreq / getPwmFreq(), 0.7,0.0), true)); // Create flux filter + setBiquadFlux(TMC4671Biquad(Biquad(BiquadType::lowpass, FLUX_FILTER_FREQ / getPwmFreq(), 0.7,0.0), true)); // Create flux filter // Initial adc calibration and check without PWM if power off to get basic offsets. PWM is off! if(!hasPower()){ @@ -471,76 +523,22 @@ void TMC4671::Run(){ break; case TMC_ControlState::waitPower: - { - allowStateChange = false; - pulseClipLed(); // blink led - static uint8_t powerCheckCounter = 0; - // if powered check ADCs and go to encoder calibration - if(!hasPower() || emergency){ - powerCheckCounter = 0; - Delay(250); - break; - } - if(++powerCheckCounter > 5 && !powerInitialized){ - initializeWithPower(); - } - if(powerInitialized){ - allowStateChange = true; - } - Delay(100); + handleStateWaitPower(); break; - } case TMC_ControlState::FullCalibration: - { - fullCalibrationInProgress = true; - /* - * Wait for power (OK) - * Calibrate ADC offsets (OK) - * Measure motor response - * depending on encoder do encoder parameter estimation - * align and store phiE for single phase AENC or indexed ABN enc - * - * If at any point external movement is detected abort - */ - // Wait for Power - while(!hasPower()){ - Delay(100); - } - curFilters.flux.params.enable = false; - setBiquadFlux(curFilters.flux); - // Calibrate ADC - enablePin.set(); - setPwm(TMC_PwmMode::PWM_FOC); // enable foc to calibrate adc - Delay(50); - if(calibrateAdcOffset(500)){ - saveAdcParams(); - }else{ - calibFailCb(); - break; - } - - // Encoder - calibrateEncoder(); - setEncoderType(conf.motconf.enctype); - recalibrationRequired = false; - curFilters.flux.params.enable = true; - setBiquadFlux(curFilters.flux); + handleStateFullCalibration(); break; - } + case TMC_ControlState::Pidautotune: - { - allowStateChange = false; - // Wait for Power - while(!hasPower()){ - Delay(100); - } - pidAutoTune(); - allowStateChange = true; - changeState(laststate,false); - break; - } + handleStatePidAutoTune(); + break; +#ifdef COGGING_TABLE_FLASH_START_ADDRESS + case TMC_ControlState::CoggingCalibration: + handleStateCoggingCalibration(); + break; +#endif case TMC_ControlState::IndexSearch: autohome(); changeState(laststate); @@ -548,35 +546,8 @@ void TMC4671::Run(){ break; case TMC_ControlState::Running: - { - // Check status, Temps, Everything alright? - uint32_t tick = HAL_GetTick(); - if(tick - lastStatTime > 2000){ // Every 2s - lastStatTime = tick; - statusCheck(); - // Get enable input. If tmc does not reply the result will read 0 or 0xffffffff (not possible normally) - uint32_t pins = readReg(0x76); - bool tmc_en = ((pins >> 15) & 0x01) && pins != 0xffffffff; - if(!tmc_en && motorEnabledRequested){ // Hardware emergency. - this->estopTriggered = true; - this->emergencyStop(false); - ErrorHandler::addError(estopError); - //changeState(TMC_ControlState::HardError); - } - - // Temperature sense - if(conf.hwconf.thermistorSettings.temperatureEnabled){ - float temp = getTemp(); - if(temp > conf.hwconf.thermistorSettings.temp_limit){ - changeState(TMC_ControlState::OverTemp); - pulseErrLed(); - } - } - - } - Delay(200); - } - break; + handleStateRunning(); + break; case TMC_ControlState::Shutdown: Delay(100); @@ -585,7 +556,7 @@ void TMC4671::Run(){ bool tmc_en = ((pins >> 15) & 0x01) && pins != 0xffffffff; if(tmc_en){ // Emergency stop reset - ErrorHandler::clearError(estopError); + ErrorHandler::clearError(ESTOP_ERROR); this->estopTriggered = false; // TODO resume correctly changeState(TMC_ControlState::uninitialized,true); } @@ -625,7 +596,6 @@ void TMC4671::Run(){ } if(fullCalibrationInProgress){ - fullCalibrationInProgress = false; Flash_Write(flashAddrs.encA,encodeEncHallMisc()); // Save encoder settings } @@ -635,15 +605,16 @@ void TMC4671::Run(){ default: break; + } // Optional update methods for safety - if(!hasPower() && state != TMC_ControlState::waitPower && initialized && powerInitialized){ // low voltage or overvoltage + if(!hasPower() && state != TMC_ControlState::waitPower && state != TMC_ControlState::CoggingCalibration && initialized && powerInitialized){ // low voltage or overvoltage requestedState = state; - ErrorHandler::addError(lowVoltageError); + ErrorHandler::addError(LOW_VOLTAGE_ERROR); setMotionMode(MotionMode::stop,true); // Disable tmc changeState(TMC_ControlState::waitPower,true); allowStateChange = false; @@ -660,7 +631,6 @@ void TMC4671::Run(){ } } // End while } - void TMC4671::calibrateEncoder(){ if(conf.motconf.enctype == EncoderType_TMC::abn) { estimateABNparams(); @@ -678,105 +648,6 @@ void TMC4671::calibrateEncoder(){ } -/** - * Iterative tuning function for tuning the torque mode PI values - */ -bool TMC4671::pidAutoTune(){ - /** - * Enter phieExt & torque mode - * Zero I, default P - * Ramp up flux P until 50% of target, then lower increments until targetflux_p is reached - * Increase I until oscillation is found. Back off a bit - */ - curFilters.flux.params.enable = false; - setBiquadFlux(curFilters.flux); - PhiE lastphie = getPhiEtype(); - MotionMode lastmode = getMotionMode(); - setPhiE_ext(getPhiE()); - setPhiEtype(PhiE::ext); // Fixed phase - setMotionMode(MotionMode::torque, true); - setFluxTorque(0, 0); - TMC4671PIDConf newpids = curPids; - int16_t targetflux = std::min(this->curLimits.pid_torque_flux,bangInitPower); // Respect limits - int16_t targetflux_p = targetflux * 0.75; - - uint16_t fluxI = 0,fluxP = 100; // Startvalues - writeReg(0x54, fluxI | (fluxP << 16)); - int32_t flux = 0; - setFluxTorque(targetflux, 0); // Start flux step - while(fluxP < 20000){ - writeReg(0x54, fluxI | (fluxP << 16)); // Update P - Delay(50); // Wait a bit. not critical - flux = getActualFlux(); - if(flux > targetflux_p){ - break; - }else if(flux > targetflux * 0.5){ - // Reduce steps when we are close - fluxP+=10; - }else{ - fluxP+=100; - } - } - setFluxTorque(0, 0); - Delay(100); // Let the current settle down - - // Tune I. This is more difficult because we need to take overshoot into account - uint32_t measuretime = 50; // ms to wait per measurement - uint16_t step_i = 64; - fluxI = 100; - flux = 0; - while(fluxI < 20000){ - writeReg(0x54, fluxI | (fluxP << 16)); - uint32_t tick = HAL_GetTick();//micros(); - int32_t peakflux = 0; - setFluxTorque(targetflux, 0); - - while(HAL_GetTick() - tick < measuretime){ // Measure current for this pulse - flux = getActualFlux(); - peakflux = std::max(peakflux, flux); - } - setFluxTorque(0, 0); - uint8_t timeout = 100; // Let the current settle down - while(timeout-- && flux > 10){ - Delay(1); - flux = getActualFlux(); - } - - if(peakflux > (targetflux + ( targetflux * TMC4671_ITUNE_CUTOFF))) // Overshoot target by 4% default - { - fluxI -= step_i; // Revert last step - break; - } - if(peakflux < targetflux*0.95){ // Do larger steps if we don't even reach near the target within the time. - step_i = 100; - }else{ - step_i = 10; - } - fluxI += step_i; - - } - curFilters.flux.params.enable = true; - setBiquadFlux(curFilters.flux); - - if(fluxP && fluxP < 20000 && fluxI && fluxI < 20000){ - newpids.fluxP = fluxP; - newpids.torqueP = fluxP; - newpids.fluxI = fluxI; - newpids.torqueI = fluxI; - }else{ - CommandHandler::broadcastCommandReply(CommandReply("PID Autotune failed",0), (uint32_t)TMC4671_commands::pidautotune, CMDtype::get); - setPhiEtype(lastphie); - setMotionMode(lastmode,true); - return false; - } - - setPids(newpids); // Apply new values - CommandHandler::broadcastCommandReply(CommandReply("PID Autotune success",1), (uint32_t)TMC4671_commands::pidautotune, CMDtype::get); - setPhiEtype(lastphie); - setMotionMode(lastmode,true); - return true; -} - bool TMC4671::autohome(){ // Moves motor to index if(findEncoderIndex(abnconf.posOffsetFromIndex < 0 ? 10 : -10,bangInitPower/2,false,false)){ @@ -871,7 +742,7 @@ bool TMC4671::findEncoderIndex(int32_t speed, uint16_t power,bool offsetPhiM,boo runOpenLoop(0, 0, 0, 10, true); if(!encoderIndexHitFlag){ pulseErrLed(); - ErrorHandler::addError(indexNotHitError); + ErrorHandler::addError(INDEX_NOT_HIT_ERROR); } // If zero count on index write a phiM offset so that phiM is 0 on index and we don't need to change the raw encoder count (possible timing danger) @@ -1486,7 +1357,7 @@ void TMC4671::encoderInit(){ // Check encoder if(!checkEncoder()){ - if(++enc_retry > enc_retry_max){ + if(++enc_retry > ENC_RETRY_MAX){ encoderAligned = false; Error err = Error(ErrorCode::encoderAlignmentFailed,ErrorType::critical,"Encoder alignment failed"); ErrorHandler::addError(err); @@ -1731,7 +1602,7 @@ void TMC4671::emergencyStop(bool reset){ int16_t TMC4671::controlFluxDissipate(){ int32_t vDiff = getIntV() - getExtV(); - if(vDiff > fluxDissipationLimit){ + if(vDiff > FLUX_DISSIPATION_LIMIT){ // Reaches limit at +5v if scaler is 1 return(clip(vDiff * conf.hwconf.fluxDissipationScaler * curLimits.pid_torque_flux * 0.0002,0,curLimits.pid_torque_flux)); } @@ -1745,15 +1616,40 @@ int16_t TMC4671::controlFluxDissipate(){ void TMC4671::turn(int16_t power){ if(!(this->motorReady() && motorEnabledRequested)) return; + int32_t flux = 0; + int32_t totalPower = power; + + // Anticogging id enable in firmware +#ifdef COGGING_TABLE_FLASH_START_ADDRESS + if (cogging_enabled && !this->isCalibrationInProgress()) { + float pos_f = this->getFilteredPosition(); + + // Fourier series compensation: Sum(A_k * sin(k * theta + phi_k)) + float compensation = 0; + float angle_rad = pos_f * 2.0f * PI; + + for (uint8_t i = 0; i < COGGING_HARMONICS_COUNT; i++) { + if (cogging_harmonics[i].amplitude > 0.0f) { + compensation += cogging_harmonics[i].amplitude * arm_sin_f32(angle_rad * cogging_harmonics[i].order + cogging_harmonics[i].phase); + } + } + this->last_anticogging_torque = (int32_t)(this->cogging_scale * compensation); - // Flux offset for field weakening + totalPower += this->last_anticogging_torque; + } else { + this->last_anticogging_torque = 0; + } +#endif - flux = idleFlux-clip(abs(power),0,maxOffsetFlux); - if((this->conf.encoderReversed && conf.motconf.enctype == EncoderType_TMC::ext) || conf.invertForce){ - power = -power; // Encoder does not match + // Inversion if encoder is reversed (ext) or force inversion is requested + if((this->conf.encoderReversed && conf.motconf.enctype == EncoderType_TMC::ext) ^ conf.invertForce){ + totalPower = -totalPower; } + // Flux offset for field weakening + flux = idleFlux-clip(abs(power),0,maxOffsetFlux); + /* * If flux dissipation is on prefer this over the resistor. * Warning: The axis only calls this function when active and if torque changed. @@ -1767,7 +1663,7 @@ void TMC4671::turn(int16_t power){ } } - setFluxTorque(flux, power); + setFluxTorque(flux, clip(totalPower, -32768, 32767)); } /** @@ -1910,6 +1806,7 @@ void TMC4671::setTmcPos(int32_t pos){ int32_t TMC4671::getPos(){ int32_t pos = (int32_t)readReg(0x6B); + this->cached_pos = pos; return pos; } @@ -2089,6 +1986,7 @@ void TMC4671::setFluxTorque(int16_t flux, int16_t torque){ #endif } + void TMC4671::setFluxTorqueFF(int16_t flux, int16_t torque){ if(curMotionMode != MotionMode::torque){ setMotionMode(MotionMode::torque,true); @@ -2503,7 +2401,7 @@ std::pair TMC4671::getActualTorqueFlux(){ uint32_t tfluxa = readReg(0x69); int16_t af = (tfluxa & 0xffff); int16_t at = (tfluxa >> 16); - return std::pair(af,at); + return std::make_pair((int32_t)af, (int32_t)at); } /** @@ -2775,7 +2673,7 @@ void TMC4671::registerCommands(){ registerCommand("tmcHwType", TMC4671_commands::tmcHwType, "Version of TMC board",CMDFLAG_GET | CMDFLAG_SET | CMDFLAG_INFOSTRING); registerCommand("encalign", TMC4671_commands::encalign, "Align encoder",CMDFLAG_GET); registerCommand("poles", TMC4671_commands::poles, "Motor pole pairs",CMDFLAG_GET | CMDFLAG_SET); - registerCommand("acttrq", TMC4671_commands::acttrq, "Measure torque and flux",CMDFLAG_GET); + registerCommand("acttrq", TMC4671_commands::acttrq, "Measure torque, flux, cogging",CMDFLAG_GET); registerCommand("pwmlim", TMC4671_commands::pwmlim, "PWM limit",CMDFLAG_DEBUG | CMDFLAG_GET | CMDFLAG_SET); registerCommand("torqueP", TMC4671_commands::torqueP, "Torque P",CMDFLAG_GET | CMDFLAG_SET); registerCommand("torqueI", TMC4671_commands::torqueI, "Torque I",CMDFLAG_GET | CMDFLAG_SET); @@ -2811,6 +2709,14 @@ void TMC4671::registerCommands(){ registerCommand("pidautotune", TMC4671_commands::pidautotune, "Start PID autoruning",CMDFLAG_GET); registerCommand("fluxbrake", TMC4671_commands::fluxbrake, "Prefer energy dissipation in motor",CMDFLAG_GET | CMDFLAG_SET); registerCommand("pwmfreq", TMC4671_commands::pwmfreq, "Get/set pwm frequency",CMDFLAG_GET | CMDFLAG_SET | CMDFLAG_DEBUG); +#ifdef COGGING_TABLE_FLASH_START_ADDRESS + registerCommand("cogging", TMC4671_commands::cogging, "Get/Set the cogging compensation",CMDFLAG_GET | CMDFLAG_SET); + registerCommand("calibrateCogging", TMC4671_commands::calibrateCogging, "Cogging calibration",CMDFLAG_GET); + registerCommand("coggingTable", TMC4671_commands::coggingTable, "Get the cogging table, or clear table (set 0)",CMDFLAG_GET | CMDFLAG_SET); + registerCommand("coggingScale", TMC4671_commands::coggingScale, "Cogging compensation scale (0-100)",CMDFLAG_GET | CMDFLAG_SET); + registerCommand("coggingSpeedP", TMC4671_commands::coggingSpeedP, "Manual speed loop P gain for cogging",CMDFLAG_GET | CMDFLAG_SET); + registerCommand("coggingSpeedI", TMC4671_commands::coggingSpeedI, "Manual speed loop I gain for cogging",CMDFLAG_GET | CMDFLAG_SET); +#endif } @@ -2850,7 +2756,7 @@ CommandStatus TMC4671::command(const ParsedCommand& cmd,std::vectorconf.motconf.motor_type); - }else if(cmd.type == CMDtype::set && (uint8_t)cmd.type <= (uint8_t)MotorType::BLDC){ + }else if(cmd.type == CMDtype::set && (uint8_t)cmd.val <= (uint8_t)MotorType::BLDC){ this->setMotorType((MotorType)cmd.val, this->conf.motconf.pole_pairs); }else{ std::string rplstr = ""; @@ -2915,7 +2821,11 @@ CommandStatus TMC4671::command(const ParsedCommand& cmd,std::vector current = getActualTorqueFlux(); - replies.emplace_back(current.second,current.first); + std::string replyStr = std::to_string(current.second) + ":" + std::to_string(current.first); +#ifdef COGGING_TABLE_FLASH_START_ADDRESS + replyStr += ":" + std::to_string(this->last_anticogging_torque); +#endif + replies.emplace_back(CommandReply(replyStr, current.second, current.first)); } break; @@ -3124,25 +3034,118 @@ CommandStatus TMC4671::command(const ParsedCommand& cmd,std::vector 0.0f) { + val = (int16_t)cogging_harmonics[n].amplitude; + break; + } + } + + s.append(std::to_string(val)); + + if (s.length() >= 500) { // send data by string cut at 500 items + s.append(")"); + CommandHandler::broadcastCommandReply(CommandReply(s,0), (uint32_t)TMC4671_commands::coggingTable, CMDtype::get); + s.clear(); + s = "item:" + std::to_string(order+1) + ",data:("; + } else { + if (order < COGGING_CALIB_DFT_HARMONICS - 1) { + s.append(","); + } + } + } + + s.append(")"); + CommandHandler::broadcastCommandReply(CommandReply(s,0), (uint32_t)TMC4671_commands::coggingTable, CMDtype::get); + return CommandStatus::NO_REPLY; + } else if(cmd.type == CMDtype::set && cmd.val == 0) { + clearCoggingTable(); + } else { + status = CommandStatus::ERR; + } + break; + + case TMC4671_commands::coggingScale: + if (cmd.type == CMDtype::get) { + replies.emplace_back((int16_t)(this->cogging_scale * 10000.0f)); + } else if (cmd.type == CMDtype::set) { + this->cogging_scale = (float)(int16_t)cmd.val / 10000.0f; + } + break; + + case TMC4671_commands::coggingSpeedP: + handleGetSet(cmd, replies, this->coggingSpeedP); + break; + + case TMC4671_commands::coggingSpeedI: + handleGetSet(cmd, replies, this->coggingSpeedI); + break; +#endif + + default: + return CommandStatus::NOT_FOUND; } return status; } -#ifdef TIM_TMC +#if defined(TIM_TMC) || defined(TIM_CALIBRATION) void TMC4671::timerElapsed(TIM_HandleTypeDef* htim){ - if(htim != this->externalEncoderTimer){ +#ifdef TIM_CALIBRATION + // If calibration timer triggered the interrupt, notify the waiting calibration loop thread. + if(this->calibTimer != nullptr && htim == this->calibTimer){ + this->NotifyFromISR(); return; } - // Read encoder and send to tmc - if(usingExternalEncoder() && externalEncoderAllowed() && this->conf.motconf.phiEsource == PhiE::extEncoder && extEncUpdater != nullptr){ - //setPhiE_ext(getPhiEfromExternalEncoder()); - // Signal phiE update - extEncUpdater->updateFromIsr(); // Use task so that the update is not being done inside an ISR +#endif +#ifdef TIM_TMC + if(htim == this->externalEncoderTimer){ + // Read encoder and send to tmc + if(usingExternalEncoder() && externalEncoderAllowed() && this->conf.motconf.phiEsource == PhiE::extEncoder && extEncUpdater != nullptr){ + //setPhiE_ext(getPhiEfromExternalEncoder()); + // Signal phiE update + extEncUpdater->updateFromIsr(); // Use task so that the update is not being done inside an ISR + } + + // If we are using the external encoder timer to pace calibration + if (usingExternalEncoder() && this->calibTicksTarget > 0) { + this->calibTicksCount = this->calibTicksCount + 1; + if (this->calibTicksCount >= this->calibTicksTarget) { + this->calibTicksCount = 0; + this->NotifyFromISR(); + } + } } +#endif } #endif @@ -3159,6 +3162,82 @@ void TMC4671::setUpExtEncTimer(){ #endif } +/** + * Configures and starts the calibration hardware timer, or sets up tick counting + * using the external encoder timer if in use, to avoid desynchronization. + */ +void TMC4671::startCalibTimers(uint32_t period_us) { +#ifdef TIM_TMC + // If an external encoder is used, we pace the calibration using the existing encoder timer + // to avoid stopping, reconfiguring, or desynchronizing it. + if (usingExternalEncoder() && this->externalEncoderTimer != nullptr) { + uint32_t tmc_period = this->externalEncoderTimer->Instance->ARR; + if (tmc_period == 0) { + tmc_period = TIM_TMC_ARR; + } + this->calibTicksTarget = period_us / tmc_period; + if (this->calibTicksTarget == 0) { + this->calibTicksTarget = 1; + } + this->calibTicksCount = 0; + } else +#endif + { + this->calibTicksTarget = 0; + this->calibTicksCount = 0; +#ifdef TIM_CALIBRATION + if (this->calibTimer != nullptr) { + // Stop the timer before changing configurations + HAL_TIM_Base_Stop_IT(this->calibTimer); + // Configure the prescaler to count in microseconds (1 MHz frequency) + this->calibTimer->Instance->PSC = (SystemCoreClock / 1000000) - 1; + // Auto-reload register defines the period in microseconds + this->calibTimer->Instance->ARR = period_us; + // Pre-charge counter to trigger the first interrupt quickly + this->calibTimer->Instance->CNT = period_us - 20; + this->calibTimer->Instance->CR1 = 1; + HAL_TIM_Base_Start_IT(this->calibTimer); + } +#endif + } + // Clear any pending thread notifications before starting wait loops + this->WaitForNotification(0); +} + +/** + * Stops the calibration hardware timer and resets the calibration tick target. + */ +void TMC4671::stopCalibTimers() { + this->calibTicksTarget = 0; + this->calibTicksCount = 0; +#ifdef TIM_CALIBRATION + if (this->calibTimer != nullptr) { + HAL_TIM_Base_Stop_IT(this->calibTimer); + } +#endif +} + +/** + * Returns the actual calibration period in microseconds based on the active timer pacing source. + */ +uint32_t TMC4671::getActualCalibPeriod(uint32_t target_period_us) { +#ifdef TIM_TMC + // If pacing calibration using TIM_TMC, the actual period is a multiple of TIM_TMC's ARR period. + if (usingExternalEncoder() && this->externalEncoderTimer != nullptr) { + uint32_t tmc_period = this->externalEncoderTimer->Instance->ARR; + if (tmc_period == 0) { + tmc_period = TIM_TMC_ARR; + } + uint32_t ticks = target_period_us / tmc_period; + if (ticks == 0) { + ticks = 1; + } + return tmc_period * ticks; + } +#endif + return target_period_us; +} + /** * Medium priority task to update external encoders */ @@ -3187,4 +3266,1040 @@ void TMC4671::errorCallback(const Error &error, bool cleared){ this->changeState(TMC_ControlState::HardError, true); } } + +bool TMC4671::isCalibrationInProgress() { + return (state == TMC_ControlState::FullCalibration || +#ifdef COGGING_TABLE_FLASH_START_ADDRESS + state == TMC_ControlState::CoggingCalibration || +#endif + state == TMC_ControlState::Pidautotune || + state == TMC_ControlState::IndexSearch || + state == TMC_ControlState::EncoderInit || + state == TMC_ControlState::ExternalEncoderInit); +} + +void TMC4671::handleStateWaitPower() { + allowStateChange = false; + pulseClipLed(); // blink led + + if (!hasPower() || emergency) { + this->powerCheckCounter = 0; + // Special case: Allow CoggingCalibration simulation even without power + if (requestedState == TMC_ControlState::CoggingCalibration) { + allowStateChange = true; + } + Delay(250); + return; + } + + if (++this->powerCheckCounter > 5) { + if (!powerInitialized) { + initializeWithPower(); + } + allowStateChange = true; + + // If a calibration was pending power, go there now + if (this->postPowerState != TMC_ControlState::NONE) { + changeState(this->postPowerState); + this->postPowerState = TMC_ControlState::NONE; + } else if (encoderAligned) { + // Normal flow if encoder is already aligned + changeState(requestedState); + } + } + Delay(100); +} + +void TMC4671::handleStateRunning() { + + // Check status, Temps, Everything alright? + uint32_t tick = HAL_GetTick(); + if (tick - lastStatTime > 2000) { // Every 2s + lastStatTime = tick; + statusCheck(); + // Get enable input. If tmc does not reply the result will read 0 or 0xffffffff (not possible normally) + uint32_t pins = readReg(0x76); + bool tmc_en = ((pins >> 15) & 0x01) && pins != 0xffffffff; + if (!tmc_en && motorEnabledRequested) { // Hardware emergency. + this->estopTriggered = true; + this->emergencyStop(false); + ErrorHandler::addError(ESTOP_ERROR); + } + + // Temperature sense + if (conf.hwconf.thermistorSettings.temperatureEnabled) { + float temp = getTemp(); + if (temp > conf.hwconf.thermistorSettings.temp_limit) { + changeState(TMC_ControlState::OverTemp); + pulseErrLed(); + } + } + } +} + +void TMC4671::handleStateFullCalibration() { + if (!hasPower()) { + this->postPowerState = TMC_ControlState::FullCalibration; + changeState(TMC_ControlState::waitPower); + return; + } + + fullCalibrationInProgress = true; + curFilters.flux.params.enable = false; + setBiquadFlux(curFilters.flux); + + // Calibrate ADC + enablePin.set(); + setPwm(TMC_PwmMode::PWM_FOC); // enable foc to calibrate adc + Delay(50); + if (calibrateAdcOffset(500)) { + saveAdcParams(); + } else { + calibFailCb(); + return; + } + + // Encoder + calibrateEncoder(); + setEncoderType(conf.motconf.enctype); + recalibrationRequired = false; + curFilters.flux.params.enable = true; + setBiquadFlux(curFilters.flux); +} + +#ifdef COGGING_TABLE_FLASH_START_ADDRESS +// Helper: Apply torque with zero flux for pure cogging measurement +void TMC4671::applySafeTorque(float torque_cmd) { + float totalPower = torque_cmd; + + // Respect inversion settings + if ((this->conf.encoderReversed && conf.motconf.enctype == EncoderType_TMC::ext) ^ conf.invertForce) { + totalPower = -totalPower; + } + + int16_t pwr = (int16_t)clip(totalPower, -32768, 32767); + // Use zero flux (Id = 0) to ensure measured current is 100% dedicated to torque (Iq) + setFluxTorque(0, pwr); +} + +// Helper: Calculate wrapped error between 0 and 1 turn +float TMC4671::getWrappedError(float target, float actual) { + target = target - floorf(target); + actual = actual - floorf(actual); + float err = target - actual; + if (err > 0.5f) err -= 1.0f; + if (err < -0.5f) err += 1.0f; + return err; +} + +float TMC4671::getAbsolutePosition() { + Encoder* activeEnc = usingExternalEncoder() ? drvEncoder.get() : static_cast(this); + return activeEnc->getPos_f(); +} + +float TMC4671::getFilteredPosition() { + Encoder* activeEnc = usingExternalEncoder() ? drvEncoder.get() : static_cast(this); + + int32_t cpr = activeEnc->getCpr(); + if (cpr == 0) return 0.0f; + + // Integer modulo BEFORE float conversion guarantees zero precision loss + // By ignoring full turns, we maintain 100% of the encoder's fractional resolution forever. + int32_t remainder = activeEnc->getPos() % cpr; + if (remainder < 0) remainder += cpr; // Mathematical modulo (always positive) + + return (float)remainder / (float)cpr; +} + +/** + * Calculates a detent torque compensation map (anti-cogging) for the motor. + * This function is now ATOMIC: it blocks the TMC thread to perform high-frequency sampling. + */ +void TMC4671::handleStateCoggingCalibration() { + + auto broadcastCalibLog = [this](int val, const char* format, ...) { + char buf[128]; + buf[0] = '('; + buf[1] = '\"'; + va_list args; + va_start(args, format); + int len = vsnprintf(buf + 2, sizeof(buf) - 16, format, args); + va_end(args); + if (len < 0) return; + snprintf(buf + 2 + len, sizeof(buf) - len - 2, "\",%d)", val); + CommandHandler::broadcastCommandReply(CommandReply(std::string(buf)), (uint32_t)TMC4671_commands::calibrateCogging, CMDtype::get); + }; + const char* errorMessage = nullptr; + float* iq_acc_cos = nullptr; + float* iq_acc_sin = nullptr; +#ifdef COGGING_CALIB_ENABLE_ID_DIAG + float* id_acc_cos = nullptr; + float* id_acc_sin = nullptr; +#endif + + prevCalibMode = getMotionMode(); + TMC4671PIDConf prevPids = curPids; + TMC4671PIDConf calibPids = curPids; + + // --- TUNABLE CALIBRATION CONSTANTS --- + const uint32_t REVOLUTION_TIME_MS = COGGING_CALIB_TIME_PER_REV_S * 1500; + const uint32_t BREAKOUT_STEP_MS = 50; + const uint32_t SYSID_J_PULSE_MS = 150; + const uint32_t SYSID_B_DURATION_MS = 2000; + const uint32_t VAL_TOTAL_DURATION_MS = 2500; + const uint32_t COGGING_WARMUP_MS = 1500; + + arm_pid_instance_f32 pid_soft; + char dbg_buf[128]; + float max_test_torque = bangInitPower > 0 ? (float)bangInitPower * 0.8f : 2000.0f; + + broadcastCalibLog(0, "Starting Cogging Calibration: Continuous DFT..."); + + if (this->getCpr() == 0) { + errorMessage = "Abort: CPR is 0"; + goto cleanup; + } + + allowStateChange = false; + + // Reset cogging table to prevent old harmonics from applying during calibration + clearCoggingTable(); + + // Set temporary robust PIDs for velocity control during calibration + calibPids.velocityP = 1000; + calibPids.velocityI = 100; + setPids(calibPids); + + // Disable flux filter during calibration (match FullCalibration logic) + curFilters.flux.params.enable = false; + setBiquadFlux(curFilters.flux); + + // 1. ALLOCATE ACCUMULATORS + iq_acc_cos = (float*)pvPortMalloc(COGGING_CALIB_DFT_HARMONICS * sizeof(float)); + iq_acc_sin = (float*)pvPortMalloc(COGGING_CALIB_DFT_HARMONICS * sizeof(float)); +#ifdef COGGING_CALIB_ENABLE_ID_DIAG + id_acc_cos = (float*)pvPortMalloc(COGGING_CALIB_DFT_HARMONICS * sizeof(float)); + id_acc_sin = (float*)pvPortMalloc(COGGING_CALIB_DFT_HARMONICS * sizeof(float)); +#endif + + if(!iq_acc_cos || !iq_acc_sin +#ifdef COGGING_CALIB_ENABLE_ID_DIAG + || !id_acc_cos || !id_acc_sin +#endif + ) { + errorMessage = "Abort: Memory fail (Heap)"; + goto cleanup; + } + + memset(iq_acc_cos, 0, COGGING_CALIB_DFT_HARMONICS * sizeof(float)); + memset(iq_acc_sin, 0, COGGING_CALIB_DFT_HARMONICS * sizeof(float)); +#ifdef COGGING_CALIB_ENABLE_ID_DIAG + memset(id_acc_cos, 0, COGGING_CALIB_DFT_HARMONICS * sizeof(float)); + memset(id_acc_sin, 0, COGGING_CALIB_DFT_HARMONICS * sizeof(float)); +#endif + + // 2. CALIBRATION PROCESS AND ANALYSIS + { + uint32_t total_samples = 0; + float starting_torque = bangInitPower > 0 ? (float)bangInitPower * 0.15f : 300.0f; + float tuning_torque = starting_torque; + float dynamic_friction = 0.0f; + + uint32_t enc_cpr = this->getCpr(); + uint32_t enc_decimation_ratio = 1; + float resolution_penalty = 1.0f; + float calib_rpm = 60.0f / (float)COGGING_CALIB_TIME_PER_REV_S; + const char* enc_perf = "High-Res"; + + if (!hasPower()) { + // --- DEBUG SIMULATION MODE --- + broadcastCalibLog(0, "Simulating acquisition (No power detected)..."); + total_samples = 2000; + for (uint32_t i = 0; i < total_samples; i++) { + float pos_f = (float)i / (float)total_samples; + float angle_rad = pos_f * 2.0f * PI; + float iq = 1500.0f * arm_sin_f32(angle_rad * 13.0f) + 800.0f * arm_sin_f32(angle_rad * 36.0f + (PI/2.0f)); +#ifdef COGGING_CALIB_ENABLE_ID_DIAG + float id = 100.0f; +#endif + float s1, c1; + arm_sin_cos_f32(pos_f * 360.0f, &s1, &c1); + + float cur_s = s1, cur_c = c1; + for (int k = 1; k < COGGING_CALIB_DFT_HARMONICS; k++) { + iq_acc_cos[k] += (iq * cur_c); + iq_acc_sin[k] += (iq * cur_s); +#ifdef COGGING_CALIB_ENABLE_ID_DIAG + id_acc_cos[k] += (id * cur_c); + id_acc_sin[k] += (id * cur_s); +#endif + float next_c = cur_c * c1 - cur_s * s1; + float next_s = cur_c * s1 + cur_s * c1; + cur_c = next_c; cur_s = next_s; + } + if(i % 100 == 0) refreshWatchdog(); + } + } else { + float J = 0.0f; + float B = 0.0f; + uint32_t next_tick = 0; + uint32_t act_period = 0; + float dt_sec = 0.0f; + // --- REAL ACQUISITION SETUP --- + startMotor(); + setMotionMode(MotionMode::torque, true); + + // --- STEP 1/4: ENCODER IDENTIFICATION --- + broadcastCalibLog(0, "STEP 1/4: Encoder Identification..."); + + if (enc_cpr > 0 && enc_cpr < 20000) { + enc_decimation_ratio = 8; // PID runs at 500 Hz + resolution_penalty = 0.2f; // Heavily damp the proportional gain + calib_rpm = 12.0f; // Spin faster to accumulate enough ticks between loops + enc_perf = "Low-Res"; + } else if (enc_cpr >= 20000 && enc_cpr < 50000) { + enc_decimation_ratio = 4; // PID runs at 1 kHz + resolution_penalty = 0.5f; // Mildly damp the proportional gain + calib_rpm = 6.0f; // Slightly faster calibration + enc_perf = "Medium-Res"; + } + + broadcastCalibLog(0, "Encoder: %s (CPR: %lu). Decimation: %lu, Pen: %.2f", enc_perf, enc_cpr, enc_decimation_ratio, resolution_penalty); + + // --- STEP 2/4: SYSTEM IDENTIFICATION (J & B) --- + broadcastCalibLog(0, "STEP 2/4: Physical System Identification..."); + + float start_pos = getAbsolutePosition(); + bool friction_broken = false; + + if (coggingSpeedP == 0.0f && coggingSpeedI == 0.0f) { + // Step 1.1: Break static friction + broadcastCalibLog(0, "Step 1.1: Breaking static friction..."); + while (!friction_broken && tuning_torque < max_test_torque && !emergency) { + applySafeTorque(tuning_torque); + Delay(BREAKOUT_STEP_MS); + if (fabs(getWrappedError(start_pos, getAbsolutePosition())) > 0.005f) { + friction_broken = true; + tuning_torque *= 1.1f; + } else { + tuning_torque += 100.0f; + } + } + applySafeTorque(0); + Delay(500); + + // Step 1.2: Measure Inertia (J) using a precise torque pulse + broadcastCalibLog(0, "Step 1.2: Measuring Mechanical Inertia (J)..."); + float j_torque = tuning_torque; + start_pos = getAbsolutePosition(); + uint32_t j_start_us = micros(); + uint32_t j_target_us = j_start_us + (SYSID_J_PULSE_MS * 1000); + + applySafeTorque(j_torque); + // Strict timing loop: Busy-wait to guarantee precise pulse duration + while (((micros() - j_target_us) & 0x80000000) && !emergency) { + refreshWatchdog(); + } + uint32_t j_end_us = micros(); + float end_pos = getAbsolutePosition(); + applySafeTorque(0); + + float dt_j = (float)(j_end_us - j_start_us) / 1000000.0f; + float d_pos_turns = fabs(getWrappedError(start_pos, end_pos)); + float d_pos_rad = d_pos_turns * 2.0f * PI; + + if (d_pos_rad < 0.001f) { + errorMessage = "Abort: Motor did not move during inertia measurement"; + goto cleanup; + } + + // Physics Formula: J = (Torque * dt^2) / (2 * delta_pos_rad) + // Scale by 100 to map physical J to the abstract TMC integer units + J = ((j_torque * dt_j * dt_j) / (2.0f * d_pos_rad)) * 100.0f; + Delay(500); + + // Step 1.3: Measure Viscous Friction (B) + broadcastCalibLog(0, "Step 1.3: Measuring Viscous Friction (B)..."); + float test_rpm = 30.0f; + float b_target_vel_turns = test_rpm / 60.0f; + float b_target_vel_rad = b_target_vel_turns * 2.0f * PI; + float b_pos = getAbsolutePosition(); + float b_sum_torque = 0.0f; + uint32_t b_samples = 0; + uint32_t b_start = HAL_GetTick(); + next_tick = micros(); + + float kp_vel = j_torque * 5.0f; + act_period = getActualCalibPeriod(1000); + dt_sec = (float)act_period / 1000000.0f; + startCalibTimers(1000); + while (HAL_GetTick() - b_start < SYSID_B_DURATION_MS && !emergency) { + next_tick += act_period; + b_pos += b_target_vel_turns * dt_sec; + float err = b_pos - getAbsolutePosition(); + float cmd = err * kp_vel; + cmd = clip(cmd, -max_test_torque, max_test_torque); + applySafeTorque(cmd); + + // WARMUP: Only integrate torque after half the duration to ensure steady-state velocity + if (HAL_GetTick() - b_start > (SYSID_B_DURATION_MS / 2)) { + b_sum_torque += cmd; + b_samples++; + } + refreshWatchdog(); +#ifdef TIM_CALIBRATION + // Yield thread via timer interrupt notifications to avoid busy-waiting, falling back if timer is disabled + if (this->calibTimer != nullptr) { + this->WaitForNotification(); + } else +#endif + { + while ((micros() - next_tick) & 0x80000000) {} + } + } + stopCalibTimers(); + applySafeTorque(0); + // Scale by 100 to map physical B to the abstract TMC integer units + // Subtract Coulomb friction (breakout torque) to isolate true viscous damping + dynamic_friction = b_sum_torque / (float)b_samples; + B = (dynamic_friction / b_target_vel_rad) * 100.0f; + B = clip(B, 0.0f, 100000.0f); // Ensure B is positive + Delay(500); + + // Step 1.4: IMC (Internal Model Control) Pole Placement Calculation + + /* * 1. Continuous Bandwidth (f_bw) Calculation + * Instead of hardcoded steps, we linearly degrade the bandwidth based on inertia (J). + * - Low inertia (e.g., K52G, J ~ 100): High bandwidth (clamped to 15.0 Hz) for maximum responsiveness. + * - High inertia (e.g., Mige, J ~ 630): Lower bandwidth (~13.5 Hz) to prevent Kp saturation and resonance. + * - Extreme/Error inertia (J > 2000): Safe mode bandwidth (clamped to 6.0 Hz) for stability. + */ + float f_bw = clip(16.5f - (0.0047f * J), 6.0f, 15.0f); + + /* * 2. Continuous Integral Scale (ki_scale) Calculation + * NOTE: ki_scale is divided by freq_khz because the main loops run at freq_khz instead of 1 kHz. + */ + float freq_khz = 1000.0f / (float)TIM_TMC_ARR; + float ki_scale = clip(0.3f / J, 0.0002f, 0.001f) / freq_khz; + + float wn = 2.0f * PI * f_bw; + + pid_soft.Kp = 2.0f * 1.0f * wn * J - B; + pid_soft.Ki = wn * wn * J * ki_scale; + pid_soft.Kd = 0.0f; + + // Higher Kp limit allowed at 5kHz due to lower latency, scale by resolution + pid_soft.Kp = pid_soft.Kp * resolution_penalty; + pid_soft.Kp = clip(pid_soft.Kp, 50.0f, 250000.0f); + pid_soft.Ki = clip(pid_soft.Ki, 1.0f, 100000.0f); + + broadcastCalibLog(0, "System Identified -> J:%.0f B:%.0f (torque %.2f, range (%.2f..%.2f)) | Gains -> Kp:%.0f Ki:%.0f", J, B, tuning_torque, starting_torque, max_test_torque, pid_soft.Kp, pid_soft.Ki); + } else { + pid_soft.Kp = coggingSpeedP; + pid_soft.Ki = coggingSpeedI; + pid_soft.Kd = 0.0f; + + broadcastCalibLog(0, "Manual PID Override -> Gains -> Kp:%.0f Ki:%.0f (torque %.2f, range (%.2f..%.2f)", pid_soft.Kp, pid_soft.Ki, tuning_torque, starting_torque, max_test_torque); + } + + arm_pid_init_f32(&pid_soft, 1); + + // --- 1.5 FINAL SANITY CHECK ROTATION --- + broadcastCalibLog(0, "Final validation rotation..."); + + float val_target_pos = getAbsolutePosition(); + float max_err_deg = 0.0f; + uint32_t val_start = HAL_GetTick(); + next_tick = micros(); + + act_period = getActualCalibPeriod(1000); + dt_sec = (float)act_period / 1000000.0f; + + uint32_t enc_decimation_counter = 0; + float iq_cmd = 0.0f; + + startCalibTimers(1000); + while (HAL_GetTick() - val_start < VAL_TOTAL_DURATION_MS && !emergency) { + next_tick += act_period; + val_target_pos += (calib_rpm / 60.0f) * dt_sec; + + // Execute PID calculations only every N cycles to avoid quantization noise + if (enc_decimation_counter % enc_decimation_ratio == 0) { + float err = val_target_pos - getAbsolutePosition(); + + // Calculate base torque from tuned PID + float iq_pid = arm_pid_f32(&pid_soft, err); + + // FRICTION FEED-FORWARD: take the dynamic friction read at step 1.3 + float iq_ff = dynamic_friction; + + iq_cmd = iq_pid + iq_ff; + iq_cmd = clip(iq_cmd, -max_test_torque, max_test_torque); + applySafeTorque(iq_cmd); + + if (HAL_GetTick() - val_start > COGGING_WARMUP_MS) { + if (fabs(err) * 360.0f > max_err_deg) max_err_deg = fabs(err) * 360.0f; + } + } + enc_decimation_counter++; + refreshWatchdog(); +#ifdef TIM_CALIBRATION + // Yield thread via timer interrupt notifications to avoid busy-waiting, falling back if timer is disabled + if (this->calibTimer != nullptr) { + this->WaitForNotification(); + } else +#endif + { + while ((micros() - next_tick) & 0x80000000) {} + } + } + stopCalibTimers(); + applySafeTorque(0); + + if (max_err_deg > 5.0f) { + broadcastCalibLog(0, "Abort: Stability check failed (%.2f deg > 5.0 deg)", max_err_deg); + errorMessage = dbg_buf; + goto cleanup; + } + Delay(250); + + // --- STEP 3/4: ACQUISITION DFT AT CONSTANT SPEED --- + broadcastCalibLog(0, "STEP 3/4: Acquisition DFT at constant speed..."); + int8_t dirs[2] = {1, -1}; + + for (int8_t p : dirs) { + broadcastCalibLog(0, p == 1 ? "Forward integration..." : "Backward integration..."); + + float target_rpm = (p == 1) ? calib_rpm : -calib_rpm; + float target_pos_f = getFilteredPosition(); + float integrated_distance = 0.0f; // Track total integrated displacement (0.0 to 1.0) + + // Statistical trackers + float max_iq_cmd_used = 0.0f; + float max_err_seen = 0.0f; + + arm_pid_init_f32(&pid_soft, 1); + + uint32_t waitStart = HAL_GetTick(); + while(HAL_GetTick() - waitStart < 1000 && !emergency && hasPower()) { + refreshWatchdog(); + Delay(10); + } + + if (!emergency && hasPower()) { + calibStartTime = HAL_GetTick(); + uint32_t period_us = getActualCalibPeriod(TIM_TMC_ARR); + uint32_t next_tick = micros(); + float dt_sec = (float)period_us / 1000000.0f; + + // Variables for acceleration tracking + float prev_actual_pos_f = getFilteredPosition(); + float prev_vel_turns = target_rpm / 60.0f; + + uint32_t enc_decimation_counter = 0; + float iq_cmd = 0.0f; + float iq_inertia = 0.0f; + + startCalibTimers(TIM_TMC_ARR); + while (HAL_GetTick() - calibStartTime < REVOLUTION_TIME_MS && !emergency && hasPower()) { + next_tick += period_us; + + float step = (target_rpm / 60.0f) * dt_sec; + target_pos_f += step; + if (target_pos_f >= 1.0f) target_pos_f -= 1.0f; + if (target_pos_f < 0.0f) target_pos_f += 1.0f; + + float actual_pos_f = getFilteredPosition(); + + // Execute PID and acceleration calculations only every N cycles + // This ensures lower-res encoders have accumulated enough ticks to measure delta_pos accurately + if (enc_decimation_counter % enc_decimation_ratio == 0) { + float error = getWrappedError(target_pos_f, actual_pos_f); + float iq_pid = arm_pid_f32(&pid_soft, error); + + // --- REAL-TIME ACCELERATION TRACKING --- + float delta_pos = getWrappedError(prev_actual_pos_f, actual_pos_f); + float dt_decimated = dt_sec * enc_decimation_ratio; + float current_vel_turns = delta_pos / dt_decimated; + // Accel in rad/s^2 + float current_accel_rad = ((current_vel_turns - prev_vel_turns) / dt_decimated) * 2.0f * PI; + + prev_actual_pos_f = actual_pos_f; + prev_vel_turns = current_vel_turns; + + // --- INERTIA FEED-FORWARD --- + // J was calculated as physical_J * 100 + iq_inertia = (J / 100.0f) * current_accel_rad; + + // FRICTION FEED-FORWARD: Pre-compensate friction so PID only fights cogging + float iq_ff = (target_rpm > 0) ? dynamic_friction : -dynamic_friction; + iq_cmd = iq_pid + iq_ff; + + // Tracking stats (Ignore startup transient) + if (HAL_GetTick() - calibStartTime > COGGING_WARMUP_MS) { + if (fabs(error) > max_err_seen) max_err_seen = fabs(error); + if (fabs(iq_cmd) > max_iq_cmd_used) max_iq_cmd_used = fabs(iq_cmd); + } + + iq_cmd = clip(iq_cmd, -max_test_torque, max_test_torque); + applySafeTorque(iq_cmd); + } + + enc_decimation_counter++; + + // --- DISPLACEMENT-BASED ACQUISITION SECURITY --- + // Only integrate into DFT if we haven't completed one full revolution (360°) + // Wait at least COGGING_WARMUP_MS for the PID to stabilize the speed before starting data collection + if (integrated_distance < 1.0f && (HAL_GetTick() - calibStartTime > COGGING_WARMUP_MS)) { + // Reconstruct "pure" cogging by removing inertia artifacts from the PID command + float iq = iq_cmd - iq_inertia; +#ifdef COGGING_CALIB_ENABLE_ID_DIAG + float id = (float)getActualFlux(); +#endif + float s1, c1; + arm_sin_cos_f32(actual_pos_f * 360.0f, &s1, &c1); + + float cur_s = s1, cur_c = c1; + for (int k = 1; k < COGGING_CALIB_DFT_HARMONICS; k++) { + iq_acc_cos[k] += (iq * cur_c); + iq_acc_sin[k] += (iq * cur_s); +#ifdef COGGING_CALIB_ENABLE_ID_DIAG + id_acc_cos[k] += (id * cur_c); + id_acc_sin[k] += (id * cur_s); +#endif + float next_c = cur_c * c1 - cur_s * s1; + float next_s = cur_c * s1 + cur_s * c1; + cur_c = next_c; cur_s = next_s; + } + total_samples++; + integrated_distance += fabs(step); + } + + refreshWatchdog(); +#ifdef TIM_CALIBRATION + // Yield thread via timer interrupt notifications to avoid busy-waiting, falling back if timer is disabled + if (this->calibTimer != nullptr) { + this->WaitForNotification(); + } else +#endif + { + while ((micros() - next_tick) & 0x80000000) { } + } + } + stopCalibTimers(); + } + applySafeTorque(0); + + // Summary message for the acquisition pass + snprintf(dbg_buf, sizeof(dbg_buf), "Acq %s Done -> Dist:%.2f MaxErr: %.2f deg", + (p == 1) ? "FWD" : "BWD", integrated_distance, (max_err_seen * 360.0f)); + } + } + + // 3. POST-PROCESSING + if (!emergency && (total_samples > 0)) { + broadcastCalibLog(0, "STEP 4/4: Post-Processing & Harmonics Extraction..."); + + float norm = 2.0f / (float)total_samples; + struct TempHarmonic { float32_t mag; uint16_t k; float32_t phase; }; + TempHarmonic* candidates = (TempHarmonic*)pvPortMalloc(COGGING_CALIB_DFT_HARMONICS * sizeof(TempHarmonic)); + + if (candidates) { + for (int k = 1; k < COGGING_CALIB_DFT_HARMONICS; k++) { + float re = iq_acc_cos[k] * norm; + float im = iq_acc_sin[k] * norm; + candidates[k].mag = sqrtf(re*re + im*im); + candidates[k].phase = atan2f(re, im); + candidates[k].k = k; + } + + // Point 2: Eccentricity + if (candidates[1].mag > 5000.0f) { + broadcastCalibLog(0, "Warning: High motor eccentricity detected"); + } + + // Point 4: Cogging extraction + memset(cogging_harmonics, 0, sizeof(cogging_harmonics)); + for (int n = 0; n < COGGING_HARMONICS_COUNT; n++) { + float max_m = 0; int best_idx = -1; + for (int k = 11; k < COGGING_CALIB_DFT_HARMONICS; k++) { + bool taken = false; + for (int j=0; j max_m) { max_m = candidates[k].mag; best_idx = k; } + } + if (best_idx != -1) { + cogging_harmonics[n].order = candidates[best_idx].k; + cogging_harmonics[n].amplitude = candidates[best_idx].mag; + cogging_harmonics[n].phase = candidates[best_idx].phase; + } + } + +#ifdef COGGING_CALIB_ENABLE_ID_DIAG + // Point 1: Electrical check (Id axis) + float h3_id = sqrtf(powf(id_acc_cos[3]*norm,2) + powf(id_acc_sin[3]*norm,2)); + float h6_id = sqrtf(powf(id_acc_cos[6]*norm,2) + powf(id_acc_sin[6]*norm,2)); + if (h3_id > 500.0f || h6_id > 500.0f) { + broadcastCalibLog(0, "Warning: Potential electrical phase imbalance"); + } +#endif + vPortFree(candidates); + saveCoggingTable(); + broadcastCalibLog(0, "Cogging calibration successful"); + + // --- 3.5 SCALE CALIBRATION (Dichotomy) --- + // Get the raw absolute position (no modulo 1.0 wrapping) + float actual_pos_f = getAbsolutePosition(); + float target_rpm = (actual_pos_f > 0.0f) ? -calib_rpm : calib_rpm; + + int main_h = -1; + float max_amp = 0; + for(int n=0; n max_amp){ + max_amp = cogging_harmonics[n].amplitude; + main_h = n; + } + } + + if (main_h != -1) { + broadcastCalibLog(0, "STEP 4.5/4: Fine-tuning Cogging Scale (Main harmonic: %d, Amplitude: %.1f)", cogging_harmonics[main_h].order, cogging_harmonics[main_h].amplitude); + + float currentTestScale = 0.0f; // Scale being tested in the current iteration + float scaleZero = 0.0f; // S0: Scale at iteration 0 (0.0) + float errorAtScaleZero = 0.0f; // E0: Measured error at scale 0.0 + float scaleOne = 1.0f; // S1: Scale at iteration 1 (1.0) + float errorAtScaleOne = 0.0f; // E1: Measured error at scale 1.0 + + // gradientLearningRate is dynamically calculated as the inverse slope of the secant line + // This acts as a perfect proportional gain for the gradient descent phase + float gradientLearningRate = 0.0f; + + // Stores the dot product error from the previous iteration to calculate the next step + float previousResidualError = 0.0f; + + // Safeguard variables to remember the absolute best result across all iterations + float optimalScaleFound = 0.0f; + float lowestAbsoluteError = 9999999.0f; + + arm_pid_init_f32(&pid_soft, 1); + + for (int iter = 0; iter < 8; iter++) { + if (iter == 0) { + // Phase 1 - Step 1: Measure at 0% compensation + currentTestScale = 0.0f; + scaleZero = currentTestScale; + } else if (iter == 1) { + // Phase 1 - Step 2: Measure at 100% compensation + currentTestScale = 1.0f; + scaleOne = currentTestScale; + } else if (iter == 2) { + // Phase 1 - Step 3: Calculate the Secant line to jump to the theoretical optimal target + if (fabs(errorAtScaleOne - errorAtScaleZero) > 1.0f) { + gradientLearningRate = fabs((scaleOne - scaleZero) / (errorAtScaleOne - errorAtScaleZero)); + } else { + gradientLearningRate = 0.0f; // Fallback if no gradient (flat line) + } + currentTestScale = scaleOne + errorAtScaleOne * gradientLearningRate; + } else { + // Phase 2: Fine-tune using Gradient Descent + // The learning rate is halved (* 0.5f) to ensure stability and prevent oscillations + currentTestScale = currentTestScale + previousResidualError * (gradientLearningRate * 0.5f); + } + + currentTestScale = clip(currentTestScale, -2.0f, 2.0f); + this->cogging_scale = currentTestScale; + + float target_pos_move = actual_pos_f; + float segment_dist = 0.0f; + + // Calculate distance to cover an exact integer number of cogging periods (at least ~0.3 turns) + // This prevents "spectral leakage" which destroys the dot product calculation. + float periods = ceilf(0.3f * (float)cogging_harmonics[main_h].order); + float segment_target = periods / (float)cogging_harmonics[main_h].order; + + float acc_re = 0, acc_im = 0; + uint32_t samples = 0; + + uint32_t act_period = getActualCalibPeriod(1000); + float dt_sec = (float)act_period / 1000000.0f; + uint32_t next_tick = micros(); + uint32_t iter_start_tick = HAL_GetTick(); + + uint32_t enc_decimation_counter = 0; + float iq_cmd = 0.0f; + + startCalibTimers(1000); + while (segment_dist < segment_target && !emergency && hasPower()) { + next_tick += act_period; + float step = (target_rpm / 60.0f) * dt_sec; + target_pos_move += step; + actual_pos_f = getAbsolutePosition(); + + // Execute PID every N cycles for noise resilience + if (enc_decimation_counter % enc_decimation_ratio == 0) { + float error = target_pos_move - actual_pos_f; + + float iq_pid = arm_pid_f32(&pid_soft, error); + float iq_ff = (target_rpm > 0) ? dynamic_friction : -dynamic_friction; + iq_cmd = iq_pid + iq_ff; + + float angle_rad = actual_pos_f * 2.0f * PI; + float compensation = 0; + for (uint8_t i = 0; i < COGGING_HARMONICS_COUNT; i++) { + if (cogging_harmonics[i].amplitude > 0.0f) { + compensation += cogging_harmonics[i].amplitude * arm_sin_f32(angle_rad * cogging_harmonics[i].order + cogging_harmonics[i].phase); + } + } + applySafeTorque(iq_cmd + (this->cogging_scale * compensation)); + } + enc_decimation_counter++; + + // Only accumulate data after 200ms of warmup to let PID stabilize on the ramp + if (HAL_GetTick() - iter_start_tick > 200) { + float s, c; + arm_sin_cos_f32(actual_pos_f * 360.0f * cogging_harmonics[main_h].order, &s, &c); + acc_re += iq_cmd * c; + acc_im += iq_cmd * s; + + samples++; + segment_dist += fabs(step); + } + + refreshWatchdog(); +#ifdef TIM_CALIBRATION + // Yield thread via timer interrupt notifications to avoid busy-waiting, falling back if timer is disabled + if (this->calibTimer != nullptr) { + this->WaitForNotification(); + } else +#endif + { + while ((micros() - next_tick) & 0x80000000) {} + } + } + stopCalibTimers(); + + float re = acc_re / (float)samples; + float im = acc_im / (float)samples; + // Dot product between residual PID torque and the expected cogging shape. + // If dot > 0, the PID is still "fighting" cogging in its original phase -> Under-compensated. + // If dot < 0, the PID is fighting the compensation itself -> Over-compensated. + float dot = re * sinf(cogging_harmonics[main_h].phase) + im * cosf(cogging_harmonics[main_h].phase); + previousResidualError = dot; + + // Safeguard: Record the scale if it produces the lowest absolute error seen so far + if (fabs(dot) < lowestAbsoluteError) { + lowestAbsoluteError = fabs(dot); + optimalScaleFound = currentTestScale; + } + + if (iter == 0) errorAtScaleZero = dot; + if (iter == 1) errorAtScaleOne = dot; + + broadcastCalibLog(0, "Iteration %d/8: Scale %.4f -> Error %.2f Iq counts", iter + 1, currentTestScale, dot); + } + + // Apply the best scale found during the calibration + this->cogging_scale = optimalScaleFound; + this->cogging_enabled = true; + + broadcastCalibLog(0, "Optimal cogging scale identified: %.4f (Err: %.2f)", optimalScaleFound, lowestAbsoluteError); + } + + // --- 4. RETURN TO CENTER (Unwinding multi-turn) --- + + // Get current position after fine-tuning + actual_pos_f = getAbsolutePosition(); + + broadcastCalibLog(0, "Return to center: Start pos = %.3f turns", actual_pos_f); + + // USE THE FULL TUNED GAINS + arm_pid_init_f32(&pid_soft, 1); + + // removed initial torque warmup delay to prevent transients + + float target_pos_f = actual_pos_f; + uint32_t period_us = getActualCalibPeriod(TIM_TMC_ARR); + target_rpm = (actual_pos_f > 0.0f) ? -calib_rpm : calib_rpm; + + // Dynamic timeout based on the distance to cover + float distance_turns = fabs(actual_pos_f); + // (distance_turns * 60 sec/min * 1000 ms/sec) / calib_rpm + uint32_t dynamic_timeout_ms = (uint32_t)((distance_turns * 60000.0f) / calib_rpm) + 5000; + + uint32_t return_start = HAL_GetTick(); + uint32_t next_tick = micros(); + startCalibTimers(TIM_TMC_ARR); + while (fabs(actual_pos_f) > 0.005f && !emergency && hasPower()) { + next_tick += period_us; + + // Absolute ramp generation towards 0.0 + target_pos_f += (target_rpm / 60.0f) * ((float)period_us / 1000000.0f); + + // Clamp at zero + if (target_rpm < 0 && target_pos_f < 0.0f) target_pos_f = 0.0f; + if (target_rpm > 0 && target_pos_f > 0.0f) target_pos_f = 0.0f; + + // Read the raw absolute position + actual_pos_f = getAbsolutePosition(); + + // Get raw error towards 0 + float error = target_pos_f - actual_pos_f; + + // Calculate torque via the CMSIS PID + float iq_pid = arm_pid_f32(&pid_soft, error); + float iq_ff = (target_rpm > 0) ? dynamic_friction : -dynamic_friction; + float iq_cmd = iq_pid + iq_ff; + + // Use full test torque limit (standard for all phases) + iq_cmd = clip(iq_cmd, -max_test_torque, max_test_torque); + applySafeTorque(iq_cmd); + + + refreshWatchdog(); + if (HAL_GetTick() - return_start > dynamic_timeout_ms) { + broadcastCalibLog(0, "Return to center timeout"); + break; // safety timeout + } +#ifdef TIM_CALIBRATION + // Yield thread via timer interrupt notifications to avoid busy-waiting, falling back if timer is disabled + if (this->calibTimer != nullptr) { + this->WaitForNotification(); + } else +#endif + { + while ((micros() - next_tick) & 0x80000000) { } + } + } + stopCalibTimers(); + applySafeTorque(0); + broadcastCalibLog(0, "Centering done. Re-aligning encoder..."); + + goto cleanup; + } else { + errorMessage = "Abort: Memory fail (candidates)"; + } + } + } + +cleanup: + if (errorMessage) { + broadcastCalibLog(1, errorMessage); + } else { + broadcastCalibLog(1, "Cogging detection finished"); + } + + setTargetVelocity(0); // Ensure motor stops + + if(iq_acc_cos) vPortFree(iq_acc_cos); + if(iq_acc_sin) vPortFree(iq_acc_sin); +#ifdef COGGING_CALIB_ENABLE_ID_DIAG + if(id_acc_cos) vPortFree(id_acc_cos); + if(id_acc_sin) vPortFree(id_acc_sin); +#endif + + // Restore hardware state (match FullCalibration logic) + setPids(prevPids); + curFilters.flux.params.enable = true; + setBiquadFlux(curFilters.flux); + setMotionMode(prevCalibMode, true); + + allowStateChange = true; + // Transition to a stable state instead of potentially jumping to uninitialized + if (errorMessage) { + changeState(hasPower() ? TMC_ControlState::Running : TMC_ControlState::waitPower); + } else { + changeState(TMC_ControlState::EncoderInit); + } +} +#endif + +/** + * Iterative tuning function for tuning the torque mode PI values (Asynchronous version) + */ +void TMC4671::handleStatePidAutoTune() { + if (!hasPower() || emergency) { + pidTuneState = PidTuneState::Init; + this->postPowerState = TMC_ControlState::Pidautotune; + changeState(TMC_ControlState::waitPower); + return; + } + + // 1. Initial Setup + allowStateChange = false; + curFilters.flux.params.enable = false; + setBiquadFlux(curFilters.flux); + lastPidTunePhiE = getPhiEtype(); + lastPidTuneMode = getMotionMode(); + setPhiE_ext(getPhiE()); + setPhiEtype(PhiE::ext); // Fixed phase for testing flux response + setMotionMode(MotionMode::torque, true); + setFluxTorque(0, 0); + + int16_t targetflux = std::min(this->curLimits.pid_torque_flux, bangInitPower); + int16_t targetflux_p = targetflux * 0.75; + + tuneFluxI = 0; + tuneFluxP = 100; + writeReg(0x54, tuneFluxI | (tuneFluxP << 16)); + + // 2. Atomic P-Gain Tuning (Ramp) + while (tuneFluxP < 20000 && !emergency && hasPower()) { + setFluxTorque(targetflux, 0); + Delay(50); // Stabilization delay + int32_t flux = getActualFlux(); + if (flux > targetflux_p) break; + + // Adaptive steps + tuneFluxP += (flux > targetflux * 0.5) ? 50 : 250; + writeReg(0x54, tuneFluxI | (tuneFluxP << 16)); + } + + // 3. Atomic I-Gain Tuning (Overshoot detection) + tuneFluxI = 100; + while (tuneFluxI < 20000 && !emergency && hasPower()) { + setFluxTorque(0, 0); + Delay(100); // Settle current to zero + + tuneMeasurePeak = 0; + writeReg(0x54, tuneFluxI | (tuneFluxP << 16)); + setFluxTorque(targetflux, 0); // Pulse + + // Ultra-fast sampling loop for peak detection (Atomic) + uint32_t start = HAL_GetTick(); + while(HAL_GetTick() - start < 50) { + tuneMeasurePeak = std::max(tuneMeasurePeak, getActualFlux()); + } + + // Check for overshoot target (default 4%) + if (tuneMeasurePeak > (targetflux + (targetflux * TMC4671_ITUNE_CUTOFF))) { + if (tuneMeasurePeak > targetflux) tuneFluxI -= (tuneFluxI > 20) ? 20 : 0; // Back off + break; + } + + tuneFluxI += (tuneMeasurePeak < targetflux * 0.95) ? 150 : 25; + } + + // 4. Finalization & Cleanup + setFluxTorque(0, 0); + curFilters.flux.params.enable = true; + setBiquadFlux(curFilters.flux); + + if (tuneFluxP < 20000 && tuneFluxI < 20000 && hasPower() && !emergency) { + pidTuneNewPids = curPids; + pidTuneNewPids.fluxP = tuneFluxP; + pidTuneNewPids.torqueP = tuneFluxP; + pidTuneNewPids.fluxI = tuneFluxI; + pidTuneNewPids.torqueI = tuneFluxI; + setPids(pidTuneNewPids); + CommandHandler::broadcastCommandReply(CommandReply("PID Autotune successful", 1), (uint32_t)TMC4671_commands::pidautotune, CMDtype::get); + } else { + CommandHandler::broadcastCommandReply(CommandReply("PID Autotune failed or interrupted", 0), (uint32_t)TMC4671_commands::pidautotune, CMDtype::get); + } + + setPhiEtype(lastPidTunePhiE); + setMotionMode(lastPidTuneMode, true); + allowStateChange = true; + pidTuneState = PidTuneState::Init; + changeState(laststate, false); +} + #endif diff --git a/Firmware/FFBoard/UserExtensions/Src/eeprom_addresses.c b/Firmware/FFBoard/UserExtensions/Src/eeprom_addresses.c index 2a0e83a21..5b215fb51 100644 --- a/Firmware/FFBoard/UserExtensions/Src/eeprom_addresses.c +++ b/Firmware/FFBoard/UserExtensions/Src/eeprom_addresses.c @@ -81,7 +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 @@ -102,13 +102,15 @@ const uint16_t VirtAddVarTab[NB_OF_VAR] = ADR_TMC1_FLUX_I, ADR_TMC1_PHIE_OFS, ADR_TMC1_TRQ_FILT, + ADR_TMC1_COGGING_CAL, + ADR_TMC1_COGGING_SCALE, // AXIS2 ADR_AXIS2_CONFIG, // 0-2 ENC, 3-5 DRV ADR_AXIS2_POWER, 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 @@ -129,13 +131,15 @@ const uint16_t VirtAddVarTab[NB_OF_VAR] = ADR_TMC2_FLUX_I, ADR_TMC2_PHIE_OFS, ADR_TMC2_TRQ_FILT, + ADR_TMC2_COGGING_CAL, + ADR_TMC2_COGGING_SCALE, // AXIS3 ADR_AXIS3_CONFIG, // 0-2 ENC, 3-5 DRV ADR_AXIS3_POWER, 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 @@ -156,6 +160,8 @@ const uint16_t VirtAddVarTab[NB_OF_VAR] = ADR_TMC3_FLUX_I, ADR_TMC3_PHIE_OFS, ADR_TMC3_TRQ_FILT, + ADR_TMC3_COGGING_CAL, + ADR_TMC3_COGGING_SCALE, // RMD CAN Motor ADR_RMD1_DATA1, //0-4 CAN ID ADR_RMD1_TORQUE, //Maximum current @@ -285,7 +291,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,13 +312,15 @@ const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR] = ADR_TMC1_FLUX_I, // ADR_TMC1_PHIE_OFS, ADR_TMC1_TRQ_FILT, + ADR_TMC1_COGGING_CAL, + ADR_TMC1_COGGING_SCALE, // AXIS2 ADR_AXIS2_CONFIG, // 0-2 ENC, 3-5 DRV ADR_AXIS2_POWER, 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 @@ -333,13 +341,15 @@ const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR] = ADR_TMC2_FLUX_I, // ADR_TMC2_PHIE_OFS, ADR_TMC2_TRQ_FILT, + ADR_TMC2_COGGING_CAL, + ADR_TMC2_COGGING_SCALE, // AXIS3 ADR_AXIS3_CONFIG, // 0-2 ENC, 3-5 DRV ADR_AXIS3_POWER, 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 @@ -360,6 +370,8 @@ const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR] = ADR_TMC3_FLUX_I, // ADR_TMC3_PHIE_OFS, ADR_TMC3_TRQ_FILT, + ADR_TMC3_COGGING_CAL, + ADR_TMC3_COGGING_SCALE, // RMD CAN Motor ADR_RMD1_DATA1, //0-4 CAN ID ADR_RMD1_TORQUE, //Maximum current @@ -418,4 +430,4 @@ const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR] = ADR_ADS111X_MAX_2, ADR_ADS111X_MIN_3, ADR_ADS111X_MAX_3, -}; \ No newline at end of file +}; diff --git a/Firmware/Targets/F407VG/.settings/stm32cubeide.project.prefs b/Firmware/Targets/F407VG/.settings/stm32cubeide.project.prefs index 7272f8f0e..50866d954 100644 --- a/Firmware/Targets/F407VG/.settings/stm32cubeide.project.prefs +++ b/Firmware/Targets/F407VG/.settings/stm32cubeide.project.prefs @@ -1,4 +1,4 @@ -2F62501ED4689FB349E356AB974DBE57=2E846C232C86F8091E1D1C41786923FD -8DF89ED150041C4CBC7CB9A9CAA90856=2E846C232C86F8091E1D1C41786923FD +2F62501ED4689FB349E356AB974DBE57=0F7D026DB66270783A3B36F955010E06 +8DF89ED150041C4CBC7CB9A9CAA90856=0F7D026DB66270783A3B36F955010E06 DC22A860405A8BF2F2C095E5B6529F12=22CA254BACDE848B9B8194C2EFC7A52E eclipse.preferences.version=1 diff --git a/Firmware/Targets/F407VG/Core/Inc/FreeRTOSConfig.h b/Firmware/Targets/F407VG/Core/Inc/FreeRTOSConfig.h index 3d718b1e3..30419ae0c 100644 --- a/Firmware/Targets/F407VG/Core/Inc/FreeRTOSConfig.h +++ b/Firmware/Targets/F407VG/Core/Inc/FreeRTOSConfig.h @@ -72,7 +72,7 @@ #define configTICK_RATE_HZ ((TickType_t)1000) #define configMAX_PRIORITIES ( 56 ) #define configMINIMAL_STACK_SIZE ((uint16_t)64) -#define configTOTAL_HEAP_SIZE ((size_t)0xffff) +#define configTOTAL_HEAP_SIZE ((size_t)(78 * 1024)) #define configMAX_TASK_NAME_LEN ( 16 ) #define configGENERATE_RUN_TIME_STATS 1 #define configUSE_TRACE_FACILITY 1 diff --git a/Firmware/Targets/F407VG/Core/Inc/target_constants.h b/Firmware/Targets/F407VG/Core/Inc/target_constants.h index 0cc62e9f8..e4daae48c 100644 --- a/Firmware/Targets/F407VG/Core/Inc/target_constants.h +++ b/Firmware/Targets/F407VG/Core/Inc/target_constants.h @@ -66,7 +66,10 @@ #define TIM_PWM htim1 #define TIM_MICROS_HALTICK htim7 // Micros timer MUST be reset by hal tick timer or isr to count microseconds since last tick -#define TIM_USER htim9 // Timer with full core clock speed available for the mainclass +// htim9 is shared: used as TIM_USER (e.g. MidiMain) and TIM_CALIBRATION (TMC4671). +// As these two features are mutually exclusive at runtime, sharing htim9 is safe. +#define TIM_USER htim9 +#define TIM_CALIBRATION htim9 #define TIM_TMC htim6 // Timer running at half clock speed #define TIM_TMC_BCLK SystemCoreClock / 2 #define TIM_TMC_ARR 250 // 4khz @@ -133,6 +136,12 @@ extern CAN_HandleTypeDef hcan1; #define EEPROM_START_ADDRESS ((uint32_t)0x08004000) /* EEPROM emulation start address: from sector1*/ #define PAGE_SIZE (uint32_t)0x4000 /* Page size = 16KByte */ +// Cogging tables in flash +#define COGGING_HARMONICS_COUNT 20 +#define COGGING_TABLE_FLASH_START_ADDRESS ((uint32_t)0x08010000) +#define COGGING_TABLE_SIZE (COGGING_HARMONICS_COUNT * 12) +#define MAX_COGGING_TABLES 3 +#define COGGING_TABLE_FLASH_SECTOR FLASH_SECTOR_4 // System // BKPSRAM positions diff --git a/Firmware/Targets/F407VG/STM32F407VGTX_FLASH.ld b/Firmware/Targets/F407VG/STM32F407VGTX_FLASH.ld index 96b7d0b28..64b120950 100644 --- a/Firmware/Targets/F407VG/STM32F407VGTX_FLASH.ld +++ b/Firmware/Targets/F407VG/STM32F407VGTX_FLASH.ld @@ -42,11 +42,13 @@ MEMORY RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K FLASH_BOOT (rx) : ORIGIN = 0x8000000, LENGTH = 16k - + EMULATED_EEPROM (rwx) : ORIGIN = ORIGIN(FLASH_BOOT) + LENGTH(FLASH_BOOT), LENGTH = 32k - FLASH (rx) : ORIGIN = ORIGIN(EMULATED_EEPROM) + LENGTH(EMULATED_EEPROM), - LENGTH = 1024k - LENGTH(FLASH_BOOT) - LENGTH(EMULATED_EEPROM) + COGGING_TABLES (rwx) : ORIGIN = 0x08010000, /* Sector 4 (64k) */ + LENGTH = 64k + FLASH (rx) : ORIGIN = 0x08020000, /* Sectors 5-11 (7x128k) */ + LENGTH = 896k } /* Sections */ diff --git a/Firmware/Targets/F407VG_DISCO/Core/Inc/FreeRTOSConfig.h b/Firmware/Targets/F407VG_DISCO/Core/Inc/FreeRTOSConfig.h index 407cbc405..3d2aabc4c 100644 --- a/Firmware/Targets/F407VG_DISCO/Core/Inc/FreeRTOSConfig.h +++ b/Firmware/Targets/F407VG_DISCO/Core/Inc/FreeRTOSConfig.h @@ -68,7 +68,7 @@ #define configTICK_RATE_HZ ((TickType_t)1000) #define configMAX_PRIORITIES ( 56 ) #define configMINIMAL_STACK_SIZE ((uint16_t)128) -#define configTOTAL_HEAP_SIZE ((size_t)0xffff) +#define configTOTAL_HEAP_SIZE ((size_t)(78 * 1024)) #define configMAX_TASK_NAME_LEN ( 16 ) #define configUSE_TRACE_FACILITY 1 #define configUSE_16_BIT_TICKS 0 diff --git a/Firmware/Targets/F407VG_DISCO/Core/Inc/target_constants.h b/Firmware/Targets/F407VG_DISCO/Core/Inc/target_constants.h index f083732d7..90a18c0f5 100644 --- a/Firmware/Targets/F407VG_DISCO/Core/Inc/target_constants.h +++ b/Firmware/Targets/F407VG_DISCO/Core/Inc/target_constants.h @@ -64,7 +64,10 @@ #define TIM_PWM htim1 #define TIM_MICROS_HALTICK htim7 // Micros timer MUST be reset by hal tick timer or isr to count microseconds since last tick -#define TIM_USER htim9 // Timer with full core clock speed available for the mainclass +// htim9 is shared: used as TIM_USER (e.g. MidiMain) and TIM_CALIBRATION (TMC4671). +// As these two features are mutually exclusive at runtime, sharing htim9 is safe. +#define TIM_USER htim9 +#define TIM_CALIBRATION htim9 #define TIM_TMC htim6 // Timer running at half clock speed extern UART_HandleTypeDef huart1; @@ -131,4 +134,11 @@ extern CAN_HandleTypeDef hcan1; // System #define CCRAM_SEC ".ccmram" +// Cogging tables in flash +#define COGGING_HARMONICS_COUNT 20 +#define COGGING_TABLE_FLASH_START_ADDRESS ((uint32_t)0x08010000) +#define COGGING_TABLE_SIZE (COGGING_HARMONICS_COUNT * 12) +#define MAX_COGGING_TABLES 3 +#define COGGING_TABLE_FLASH_SECTOR FLASH_SECTOR_4 + #endif /* INC_TARGET_CONSTANTS_H_ */ diff --git a/Firmware/Targets/F407VG_DISCO/STM32F407VGTX_FLASH.ld b/Firmware/Targets/F407VG_DISCO/STM32F407VGTX_FLASH.ld index 96b7d0b28..188bd581b 100644 --- a/Firmware/Targets/F407VG_DISCO/STM32F407VGTX_FLASH.ld +++ b/Firmware/Targets/F407VG_DISCO/STM32F407VGTX_FLASH.ld @@ -45,8 +45,10 @@ MEMORY EMULATED_EEPROM (rwx) : ORIGIN = ORIGIN(FLASH_BOOT) + LENGTH(FLASH_BOOT), LENGTH = 32k - FLASH (rx) : ORIGIN = ORIGIN(EMULATED_EEPROM) + LENGTH(EMULATED_EEPROM), - LENGTH = 1024k - LENGTH(FLASH_BOOT) - LENGTH(EMULATED_EEPROM) + COGGING_TABLES (rwx) : ORIGIN = 0x08010000, /* Sector 4 (64k) */ + LENGTH = 64k + FLASH (rx) : ORIGIN = 0x08020000, /* Sectors 5-11 (7x128k) */ + LENGTH = 896k } /* Sections */ diff --git a/Firmware/Targets/F411RE/Core/Inc/FreeRTOSConfig.h b/Firmware/Targets/F411RE/Core/Inc/FreeRTOSConfig.h index 7323076ca..b9641d019 100644 --- a/Firmware/Targets/F411RE/Core/Inc/FreeRTOSConfig.h +++ b/Firmware/Targets/F411RE/Core/Inc/FreeRTOSConfig.h @@ -68,7 +68,7 @@ #define configTICK_RATE_HZ ((TickType_t)1000) #define configMAX_PRIORITIES ( 56 ) #define configMINIMAL_STACK_SIZE ((uint16_t)128) -#define configTOTAL_HEAP_SIZE ((size_t)0xffff) +#define configTOTAL_HEAP_SIZE ((size_t)(78 * 1024)) #define configMAX_TASK_NAME_LEN ( 16 ) #define configUSE_TRACE_FACILITY 1 #define configUSE_16_BIT_TICKS 0 diff --git a/Firmware/Targets/F411RE/Core/Inc/target_constants.h b/Firmware/Targets/F411RE/Core/Inc/target_constants.h index aa19ee72e..decbf0244 100644 --- a/Firmware/Targets/F411RE/Core/Inc/target_constants.h +++ b/Firmware/Targets/F411RE/Core/Inc/target_constants.h @@ -52,7 +52,10 @@ #define TIM_ENC htim3 // Timer 3 is used by the encoder. +// htim9 is shared: used as TIM_USER (e.g. MidiMain) and TIM_CALIBRATION (TMC4671). +// As these two features are mutually exclusive at runtime, sharing htim9 is safe. #define TIM_USER htim9 +#define TIM_CALIBRATION htim9 #define TIM_TMC htim2 #define TIM_TMC_BCLK SystemCoreClock @@ -98,5 +101,12 @@ extern SPI_HandleTypeDef HSPI2; #define EEPROM_START_ADDRESS ((uint32_t)0x08004000) /* EEPROM emulation start address: from sector1*/ #define PAGE_SIZE (uint32_t)0x4000 /* Page size = 16KByte */ +// Cogging tables in flash +#define COGGING_HARMONICS_COUNT 20 +#define COGGING_TABLE_FLASH_START_ADDRESS ((uint32_t)0x08010000) +#define COGGING_TABLE_SIZE (COGGING_HARMONICS_COUNT * 12) +#define MAX_COGGING_TABLES 3 +#define COGGING_TABLE_FLASH_SECTOR FLASH_SECTOR_4 + #define CCRAM_SEC ".data" // Has no ccmram #endif /* INC_TARGET_CONSTANTS_H_ */ diff --git a/Firmware/Targets/F411RE/STM32F411RETX_FLASH.ld b/Firmware/Targets/F411RE/STM32F411RETX_FLASH.ld index c0ea5fd8a..79e21459f 100644 --- a/Firmware/Targets/F411RE/STM32F411RETX_FLASH.ld +++ b/Firmware/Targets/F411RE/STM32F411RETX_FLASH.ld @@ -66,8 +66,10 @@ MEMORY EMULATED_EEPROM (rwx) : ORIGIN = ORIGIN(FLASH_BOOT) + LENGTH(FLASH_BOOT), LENGTH = 32k - FLASH (rx) : ORIGIN = ORIGIN(EMULATED_EEPROM) + LENGTH(EMULATED_EEPROM), - LENGTH = 512k - LENGTH(FLASH_BOOT) - LENGTH(EMULATED_EEPROM) + COGGING_TABLES (rwx) : ORIGIN = 0x08010000, /* Sector 4 (64k) */ + LENGTH = 64k + FLASH (rx) : ORIGIN = 0x08020000, /* Sectors 5-7 (3x128k) */ + LENGTH = 384k RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K } diff --git a/Firmware/scripts/memory_map.csv b/Firmware/scripts/memory_map.csv index 822ba9c93..6326b0f94 100644 --- a/Firmware/scripts/memory_map.csv +++ b/Firmware/scripts/memory_map.csv @@ -84,6 +84,8 @@ Section comment,Name,Value,Comment on key,VirtAddVarTab,exportableFlashAddresses ,ADR_TMC1_FLUX_I,0x32A,,1,1 ,ADR_TMC1_PHIE_OFS,0x32B,,1, ,ADR_TMC1_TRQ_FILT,0x32C,,1,1 +,ADR_TMC1_COGGING_CAL,0x32D,,1,1 +,ADR_TMC1_COGGING_SCALE,0x32E,,1,1 // AXIS2,,,,, ,ADR_AXIS2_CONFIG,0x341,"// 0-2 ENC, 3-5 DRV",1,1 ,ADR_AXIS2_POWER,0x342,,1,1 @@ -117,6 +119,8 @@ Section comment,Name,Value,Comment on key,VirtAddVarTab,exportableFlashAddresses ,ADR_TMC2_FLUX_I,0x36A,,1,1 ,ADR_TMC2_PHIE_OFS,0x36B,,1, ,ADR_TMC2_TRQ_FILT,0x36C,,1,1 +,ADR_TMC2_COGGING_CAL,0x36D,,1,1 +,ADR_TMC2_COGGING_SCALE,0x36E,,1,1 // AXIS3,,,,, ,ADR_AXIS3_CONFIG,0x381,"// 0-2 ENC, 3-5 DRV",1,1 ,ADR_AXIS3_POWER,0x382,,1,1 @@ -146,6 +150,8 @@ Section comment,Name,Value,Comment on key,VirtAddVarTab,exportableFlashAddresses ,ADR_TMC3_FLUX_I,0x3AA,,1,1 ,ADR_TMC3_PHIE_OFS,0x3AB,,1, ,ADR_TMC3_TRQ_FILT,0x3AC,,1,1 +,ADR_TMC3_COGGING_CAL,0x3AD,,1,1 +,ADR_TMC3_COGGING_SCALE,0x3AE,,1,1 // RMD CAN Motor,,,,, ,ADR_RMD1_DATA1,0x3C0,//0-4 CAN ID,1,1 ,ADR_RMD1_TORQUE,0x3C1,//Maximum current,1,1 diff --git a/doc/TMC4671.md b/doc/TMC4671.md new file mode 100644 index 000000000..2d320cf65 --- /dev/null +++ b/doc/TMC4671.md @@ -0,0 +1,110 @@ +# TMC4671 Class State Machine Documentation + +This document describes the operation of the asynchronous state machine for the `TMC4671` class, responsible for controlling the Trinamic TMC4671 motor driver. + +## 1. Possible States + +The `TMC_ControlState` enum defines the primary states of the machine: + +```cpp +enum class TMC_ControlState : uint32_t { + uninitialized, // Initial startup and hardware detection + waitPower, // Waiting for VM (Motor Voltage) to be stable + Shutdown, // Motor stopped, PWM off + Running, // Main operational state (Torque/Velocity/Position) + EncoderInit, // Internal encoder alignment sequence + EncoderFinished, // Encoder ready, waiting for start + HardError, // Fatal error, requires reset + OverTemp, // Thermal protection triggered + IndexSearch, // Searching for encoder index pulse + FullCalibration, // Complete commissioning routine + ExternalEncoderInit, // External encoder setup + Pidautotune, // torque PID auto-tuning + CoggingCalibration, // Anti-cogging map generation + SlewRateCalibration, // Maximum current ramp measurement + NONE // Placeholder for recovery logic +}; +``` + +## 2. State Control Variables + +- `TMC_ControlState state`: The current active state. +- `TMC_ControlState laststate`: Saves the previous state to allow returning after temporary routines (e.g., calibrations). +- `TMC_ControlState requestedState`: The next intended state, applied in the next loop tick. +- `TMC_ControlState postPowerState`: Stores a calibration state to be resumed automatically after a power loss/recovery cycle. +- `bool allowStateChange`: A lock that prevents external state transitions during critical internal procedures. + +## 3. Asynchronous Transition Mechanism + +The `changeState(TMC_ControlState newState, bool force = false)` method manages transitions: +- **`force = true`**: Immediate state change (`state = newState`). Used for critical failures or internal state machine logic. +- **`force = false`**: Requests a transition by setting `requestedState`. The change is applied at the beginning of the next `Run()` iteration if `allowStateChange` is `true`. + +## 4. Detailed Description of States and Transitions + +The logic is executed within the `Run()` thread loop. To maintain responsiveness and prevent stack overflows, the state machine is strictly non-blocking. + +--- + +### **State: `uninitialized`** +- **Role**: Hardware discovery. +- **Actions**: Pings the driver and calls `initialize()` to set PWM frequency, motor type, and basic registers. +- **Transition**: ➡️ `waitPower` on success. + +--- + +### **State: `waitPower`** +- **Role**: Monitors supply voltage (VM). +- **Actions**: + 1. Checks `hasPower()`. + 2. Once stable (5 consecutive successful checks), calls `initializeWithPower()` for ADC calibration. +- **Post-Power Recovery**: If `postPowerState` is not `NONE`, the machine automatically transitions to that state (e.g., resuming a Cogging calibration that was interrupted by a power cut). + +--- + +### **State: `Running`** +- **Role**: Main control loop. +- **Actions**: Applies real-time torque, velocity, or position. Monitors temperature and driver status flags. +- **Anti-Cogging**: Applies real-time torque compensation from the Flash-stored map at ~1kHz. + +--- + +### **Calibration Sub-State Machines** + +To avoid blocking the 1KB TMC thread, long calibrations use internal sub-states: + +#### **Cogging Calibration (`CoggingState`)** +- **Mechanism**: Iterates through `ForwardWait`, `ForwardMeasure`, `BackwardWait`, `BackwardMeasure`, and `Compute`. +- **Memory Management**: Large calibration arrays (~34KB) are allocated on the **heap** using `std::make_unique()` only when the calibration starts, and are freed immediately after completion to prevent stack overflow. + +#### **PID Auto-Tune (`PidTuneState`)** +- **Mechanism**: Iterates through `RampFluxP`, `TuneFluxI_Pulse`, and `TuneFluxI_Measure`. +- **Asynchronicity**: Replaces `while` loops with tick-based measurements. If power is lost during tuning, it resets to `waitPower` and marks `postPowerState` for resumption. + +--- + +## 5. Memory and Stack Vigilance + +- **Stack Limit**: The TMC thread stack is limited (`TMC_THREAD_MEM = 256` words / 1KB). +- **Heap Allocation**: Any large data structure (like the `CoggingCalibData` struct) **must** be allocated on the heap. +- **Safety**: Always initialize local variables (e.g., when reading from Flash) to prevent erratic behavior from uninitialized memory. + +--- + +## 6. FreeRTOS Safety and Critical Sections + +There is a strict orchestration requirement for hardware initialization to avoid FreeRTOS crashes: + +1. **No Blocking in Critical Sections**: Calls like `xSemaphoreTake` (used in SPI transfers), `vTaskDelay`, or `Task Creation` must **never** occur inside a `cpp_freertos::CriticalSection`. +2. **Driver Instantiation Pattern**: In `Axis::setDrvType`, the `TMC4671` object is instantiated **outside** the critical section. This allows its constructor to safely use SPI/Semaphores to read Flash and configure registers. +3. **The Swap**: The critical section is used only for the micro-second duration required to swap the `std::unique_ptr` driver pointers and update the slew rate values. +4. **Deferred Cleanup**: Old driver objects are deleted **after** exiting the critical section to ensure their destructors do not block while interrupts are disabled. + +--- + +## 7. Interaction with User Commands + +User commands (`calibrate`, `cogging`, `pidautotune`) trigger a `changeState()` request. +1. The command sets `allowStateChange = false` to lock the machine. +2. The `Run()` loop processes the calibration sub-states tick-by-tick. +3. Upon completion (Success or Fail), `allowStateChange` is set to `true`, and the machine returns to `laststate` (usually `Running` or `Shutdown`). diff --git a/doc/TMC4671.pdf b/doc/TMC4671.pdf new file mode 100644 index 000000000..8af32a2fd Binary files /dev/null and b/doc/TMC4671.pdf differ diff --git a/doc/cogging_analysis.md b/doc/cogging_analysis.md new file mode 100644 index 000000000..5205e0414 --- /dev/null +++ b/doc/cogging_analysis.md @@ -0,0 +1,184 @@ +# Anti-Cogging and Motor Diagnostic Analysis (Continuous DFT-128 Method) + +This document describes the implementation of the harmonic-based anti-cogging compensation and motor diagnostic system for the OpenFFBoard (TMC4671). + +## 1. Process Overview + +The system uses a **Continuous Discrete Fourier Transform (DFT)** integration. It eliminates lookup tables and fixed-size buffers, making it memory-safe for all supported microcontrollers (F407 and F411). + +1. **Software PID & System Identification**: Before acquisition, the system performs a deterministic identification of the motor's physical parameters to calculate ideal PID gains (using CMSIS-DSP). It avoids heuristic trial-and-error by measuring the actual hardware: + * **Encoder Identification**: Audits the encoder resolution (`enc_cpr`) to mathematically decimate the PID execution rate (e.g., down to 1kHz or 500Hz) for low-resolution encoders. This allows enough physical ticks to accumulate, guaranteeing a stable derivative signal for inertia feedforward without amplifying pure quantization noise. The system logs the determined encoder performance class (Low/Medium/High-Res) and chosen constants. + * **Static Friction (Breakout)**: Identifies the minimum torque required to overcome stiction. + * **Mechanical Inertia ($J$)**: Measures angular acceleration ($\alpha$) resulting from a constant torque pulse ($\tau$). $J = \tau / \alpha$. + * **Viscous Friction ($B$)**: Measures the steady-state torque required to maintain a constant moderate speed ($\omega$). $B = \tau / \omega$. + * **Deterministic Gain Calculation (Pole Placement / IMC)**: Instead of Ziegler-Nichols, the system uses exact physics-based formulas to guarantee a **critically damped response** ($\zeta = 1.0$) at a target control bandwidth ($f_{bw}$). + * **Bandwidth ($f_{bw}$)**: Linearly degraded based on inertia ($J$). Low inertia gives high bandwidth (up to 15Hz), high inertia gives lower bandwidth (down to 6Hz) to prevent $K_p$ saturation. Formula: $f_{bw} = 16.5 - 0.0047 \cdot J$. + * **Integral Scale ($ki_{scale}$)**: Inversely proportional to $J$ ($0.3 / J$) and scaled for the loop frequency in kHz ($/ freq\_khz$) to prevent integral windup on heavy motors. The frequency `freq_khz` is dynamically calculated from `TIM_TMC_ARR` (e.g. 4.0 for 4kHz, 5.0 for 5kHz). + * $K_p = 2 \cdot \zeta \cdot (2\pi \cdot f_{bw}) \cdot J - B$ (Clamped up to 250,000 for high-frequency stiffness). + * $K_i = (2\pi \cdot f_{bw})^2 \cdot J \cdot ki_{scale}$ + * $K_d = 0$ (Forced to zero for maximum stability at low speed). + * **Manual PID Override**: If either `coggingSpeedP` or `coggingSpeedI` is set to a non-zero value, the physical identification phase (Breakout, Inertia $J$, Viscous Friction $B$, and IMC Calculation) is entirely bypassed. The software PID directly uses these manual overrides for $K_p$ and $K_i$. If both are `0.0f` (default), the automatic identification is executed. + * **Validation Rotation**: A single 2.5s test rotation at **calib_rpm** is performed to verify tracking stability. If the error exceeds 5.0°, calibration aborts. + +2. **Torque Response Capture (Deterministic Dual-Pass Acquisition)**: The motor rotates at a constant speed defined by **calib_rpm** (default: 7.5 RPM) in both directions. + * **High-Fidelity Integration**: Acquisition is strictly clocked at the configured `TIM_TMC` frequency (e.g., 4kHz / 250µs or 5kHz / 200µs). The system waits for a stabilization period (`COGGING_WARMUP_MS`) and then integrates the torque over exactly 360° of displacement (`integrated_distance < 1.0f`). + * **Real-Time Inertia Feedforward**: At the active loop frequency, the PID must output torque to overcome even microscopic accelerations. The system calculates real-time angular acceleration ($\alpha$) and dynamically subtracts the inertial energy ($iq\_inertia = J \cdot \alpha$) from the measured torque. The DFT only processes the "pure" magnetic cogging signature, eliminating phase lag and distortion. + * **Torque Inversion & Safety**: The system supports `conf.invertForce` during torque application via `applySafeTorque`, preventing positive feedback loops in various hardware configurations. + * **Continuous Mathematical Wrapping**: The system uses `floorf`-based modulo arithmetic for position wrapping ($pos - \lfloor pos \rfloor$) on both target and actual positions, ensuring robust tracking across multiple revolutions. + * **Complex Rotation Optimization**: Uses recursive complex multiplication ($e^{i(k+1)\theta} = e^{ik\theta} \cdot e^{i\theta}$) to calculate 128 harmonics with only one trigonometric call per sample. + +3. **Scale Calibration (Hybrid Secant + Gradient Descent)**: Fine-tuning of the compensation amplitude (`cogging_scale`). + * **Phase 1 - Secant Approximation**: Two initial measurements are taken at 0% and 100% compensation. Using the resulting residual errors, a secant line is calculated to mathematically jump to the theoretical optimal scale. The inverse slope of this secant line perfectly represents the system's proportional gain regardless of motor torque or units. + * **Phase 2 - Gradient Descent Refinement**: For the remaining iterations (up to 8 total), the system uses the gain extracted from the Secant phase (halved for stability) as a proportional controller. This dynamically refines the scale to compensate for any physical non-linearities, driving the error to the absolute measurement noise floor. + * **Absolute Best Safeguard**: A safeguard mechanism records the scale that yielded the lowest absolute residual error across all 8 iterations and strictly applies this maximum-precision value at the end. + * **Spectral Leakage Prevention**: Each test iteration calculates an exact integer number of periods for the main harmonic (minimum 0.3 turns). This prevents fractional-period integration, which would destroy the dot product phase correlation. + +4. **Post-Acquisition Homing & Re-alignment**: After successful analysis, the system ensures the motor returns to a known hardware state. + * **Multi-turn Unwinding**: Using the full tuned PID gains at the aligned frequency (4kHz or 5kHz), the motor performs a controlled absolute position ramp towards `0.0`. This "unwinds" any revolutions accumulated during the acquisition or retry phases. + * **Encoder Re-Zeroing**: Upon reaching the origin, the driver is automatically switched to the `EncoderInit` state. This triggers a hardware re-alignment and resets the encoder position registers. + +## 2. Technical Specifications + +### Memory Efficiency (The Zero-Buffer Goal) +* **Accumulators**: Uses 128 pairs of `float` (32-bit) values to leverage the hardware FPU. +* **Peak RAM**: Approximately **2 KB** on the FreeRTOS heap during calibration. +* **Permanent RAM**: Only **240 bytes** (20 saved harmonics). +* **Compatibility**: Fully portable between F407 and F411. + +### Internal Tuning Constants +* `MAX_TOLERANCE_DEG` (3.0°): Absolute maximum error limit allowed during DFT. +* `VAL_TOTAL_DURATION_MS` (2500ms): Duration of the PID validation check. +* `COGGING_WARMUP_MS` (1500ms): Stabilization window ignored during measurement and integration. + +## 3. Calibration Phase Summary (Steps & Timings) + +The following table summarizes the sequence of operations, their durations, and the torque strategies used to ensure stability. + +| Phase | Sub-Step | Duration | Torque / Control Strategy | Goal | +| :--- | :--- | :--- | :--- | :--- | +| **1. Encoder ID** | Profiling | - | Mathematical Decimation | Audit hardware resolution | +| **2. SysID** | Breakout | Variable | Ramp-up | Measure static friction | +| | Inertia ($J$) | 150ms | Constant Pulse | Measure rotor mass | +| | Friction ($B$) | 2000ms | Velocity P-Loop | Measure dynamic drag | +| **3. Validation** | Sanity Check | 2500ms | PID Control (Calculated Gains) | Verify tracking stability | +| **4. Acquisition** | Setup | 1000ms | Zero Torque | Settle motor at rest | +| | DFT Integration | ~12.0s (Max) | PID Control (Wait 1500ms before DFT) @ Aligned Rate (e.g. 4kHz/5kHz) | Capture exactly 360° of pure cogging (inertia subtracted) | +| **4.5. Scaling**| Hybrid Secant/Gradient | ~15.0s | PID Control @ Aligned Rate (e.g. 4kHz/5kHz) | Mathematical approximation and proportional refinement of `cogging_scale` | +| **5. Return** | Centering | Variable | PID Control (Absolute Ramp to 0.0) @ Aligned Rate (e.g. 4kHz/5kHz) | Unwind motor revolutions | +| | Final Align | - | `EncoderInit` State | Reset hardware alignment | + +### Encoder Profiling Constants +The calibration speed (`calib_rpm`) and PID execution rate are precalculated automatically based on the hardware resolution. This matrix ensures that even 12-bit magnetic encoders can achieve successful calibration without violent torque oscillations: + +| Encoder Class | CPR Range | PID Rate (Decimation) | Kp Penalty | Calibration Speed | Justification | +| :--- | :--- | :--- | :--- | :--- | :--- | +| **High-Res** | > 50,000 | 4kHz/5kHz (Ratio: 1) | 1.0x | 7.5 RPM | Infinite resolution allows full-speed loop and purest gain calculation | +| **Medium-Res** | 20,000 - 50,000 | 1kHz (Ratio: 4) | 0.5x | 6.0 RPM | Lower execution rate requires slight gain dampening for Nyquist stability | +| **Low-Res** | < 20,000 | 500Hz (Ratio: 8) | 0.2x | 12.0 RPM | Heavy dampening and faster spin needed to accumulate enough ticks per loop | + +### Configuration Macros +* `COGGING_CALIB_LUT_RESOLUTION`: Standardized at 2880 points for communication protocol compatibility. +* `COGGING_CALIB_TIME_PER_REV_S`: Time in seconds to complete one revolution (Default: 8s). +* `COGGING_CALIB_DFT_HARMONICS`: Number of analyzed harmonics (128). +* `COGGING_CALIB_ENABLE_ID_DIAG`: Macro to enable electrical phase analysis on the Id axis. + +## 4. Design Choices & Timing Analysis + +### Why 8 Seconds per Revolution? +This speed (**7.5 RPM**) is carefully selected as a "Physical Sweet Spot": +* **Avoiding Stick-Slip**: It is fast enough to ensure the motor remains in the "viscous friction" regime, avoiding the jerky "stick-slip" motion caused by static friction. +* **Negligible Dynamics**: It is slow enough that Back-EMF and gross phase lag are negligible. + +### Why 4kHz/5kHz (High-Frequency) Sampling & Control? +The system is synchronized directly to the target configuration's `TIM_TMC` rate (typically 4kHz / 250µs or 5kHz / 200µs) for both the PID and DFT to achieve robotic-grade force control: +* **Dead-Time Reduction**: A 4kHz/5kHz loop dramatically reduces phase latency between position reading and torque application. This allows the PID proportional gain ($K_p$) to be pushed significantly higher without oscillating, resulting in a much "stiffer" and more accurate tracking of the 7.5 RPM target. +* **High-Resolution Accumulation**: Running at this speed provides a dense data set for the continuous Fourier Transform, naturally filtering out high-frequency electrical noise and SPI jitter. +* **Inertial Disaggregation**: At 4kHz/5kHz, the discrete derivative of velocity (acceleration) is highly responsive. This enables the calculation of real-time inertial torque ($J \cdot \alpha$), which is subtracted from the PID output before feeding the DFT. This prevents the algorithm from mistakenly compensating for the motor's own mass. +* **SPI Feasibility**: A standard TMC4671 SPI read takes ~6µs. At 4kHz/5kHz, communication overhead remains negligible (~3% CPU time), safely allowing the STM32F407 to process the complex math without RTOS deadline violations. +* **Dynamic Math Scaling**: All formulas dynamically adapt to the active period (`period_us` read from `TIM_TMC_ARR`). The integration timestep is set via $dt\_sec = period\_us / 1000000.0f$. The continuous integral gain scale $ki\_scale$ is scaled by dividing by the loop frequency in kHz ($freq\_khz = 1000.0f / TIM\_TMC\_ARR$) to prevent windup and guarantee stability regardless of the target's timer speed. + +## 5. Diagnostics & Engineering Points + +### Point 1: Electrical Diagnostic (`COGGING_CALIB_ENABLE_ID_DIAG`) +* Analyzes the **Id axis** (Flux). +* In a healthy motor, the flux remains DC. Significant energy in **H3** or **H6** indicates a phase imbalance or partial winding short. + +### Point 2: Mechanical Diagnostic (Eccentricity) +* Analyzes the **H1** magnitude on the Iq axis. +* A high H1 magnitude signals rotor eccentricity, a bent shaft, or encoder misalignment. + +### Point 3: Harmonic Anti-Cogging +* The system scans all 128 harmonics. +* It selects the **Top 20 peaks** with an **Order > 10**. +* This captures the high-frequency magnetic "detent" while ignoring the low-frequency gravitational imbalance of asymmetric steering wheels. + +## 6. Real-Time Compensation Formula + +Implemented in the `turn()` method for zero latency: + +$$T_{comp}(\theta) = \sum_{n=1}^{20} A_n \cdot \sin(Order_n \cdot \theta + \Phi_n)$$ +$$T_{final} = T_{requested} + \left( Scale \cdot T_{comp}(\theta) \right)$$ + +## 7. Timer, Pacing and Parameter Reference + +For developers modifying the driver or timing subsystem, here is a detailed reference of the variables controlling calibration pacing, external encoder synchronization, and override parameters: + +### `externalEncoderTimer` +* **Type**: `TIM_HandleTypeDef*` +* **Utility**: Points to the hardware timer used to drive the external encoder updater thread (`TIM_TMC`, typically `htim6`). When an external encoder is active, this timer *must* continue running without being stopped or reconfigured to prevent interrupting the position feedback loop. In this scenario, the calibration logic uses this timer as a synchronous pacing clock. +* **Expected Value**: `&TIM_TMC` (configured with `TIM_TMC_ARR` defining the tick period in microseconds) or `nullptr` if `TIM_TMC` is not defined. + +### `calibTimer` +* **Type**: `TIM_HandleTypeDef*` +* **Utility**: Points to the hardware timer used for pacing the calibration thread (`TIM_CALIBRATION`, typically `htim9`) when *no* external encoder is used. In this case, `calibTimer` can be reconfigured dynamically to generate interrupts at the exact requested period (e.g., 200/250 microseconds for fast calibration loops, 1000 microseconds for slow calibration loops). +* **Expected Value**: `&TIM_CALIBRATION` or `nullptr` if `TIM_CALIBRATION` is not defined. + +### `calibTicksCount` +* **Type**: `volatile uint32_t` +* **Utility**: Active tick accumulator incremented within the `TIM_TMC` ISR. Used only when pacing the calibration via the external encoder timer (i.e. `usingExternalEncoder()` is true). +* **Expected Value / Range**: Increments from 0 up to (calibTicksTarget - 1). Resets to 0 once `calibTicksTarget` is reached. + +### `calibTicksTarget` +* **Type**: `volatile uint32_t` +* **Utility**: The threshold number of ticks from the `externalEncoderTimer` needed to match the requested calibration period. For instance, if `TIM_TMC_ARR` is 250 (4 kHz) and a 1000 us (1 kHz) loop step is requested, `calibTicksTarget` is set to `1000 / 250 = 4`. A value of `0` indicates that tick-based pacing is disabled (e.g., calibration is idle or using `calibTimer`). +* **Expected Value**: Typically `1` for fast loops, `4` for slow loops (assuming a 250 us timer ARR), or `0` when inactive. + +### `extEncUpdater` +* **Type**: `std::unique_ptr` +* **Utility**: Manages a dedicated FreeRTOS helper thread that asynchronously writes the external encoder position to the TMC4671 register `0x1C` via SPI. This thread is notified from the timer ISR to keep SPI writes out of the interrupt context. +* **Expected Value**: Valid `std::unique_ptr` instance when an external encoder is configured, or `nullptr` otherwise. + +### `enc_decimation_ratio` +* **Type**: `uint32_t` +* **Utility**: An execution rate divider that decouples the mathematical PID loop from the physical hardware timer. If the SPI loop runs at 4kHz and the ratio is 4, the PID is only evaluated at 1kHz. This is strictly required for encoders with low CPR to prevent severe quantization noise in the acceleration derivative. +* **Expected Value**: `1`, `4`, or `8` depending on the initial CPR audit. + +### `resolution_penalty` +* **Type**: `float` +* **Utility**: A multiplier applied to the critically damped proportional gain ($K_p$). Running a control loop at a lower decimated rate (e.g. 500Hz instead of 4kHz) reduces the phase margin, making the motor prone to violent high-frequency oscillations if the original stiff 4kHz gain is applied. The penalty mathematically relaxes the stiffness. +* **Expected Value**: `1.0f` (High-Res), `0.5f` (Med-Res), or `0.2f` (Low-Res). + +### `coggingSpeedP` & `coggingSpeedI` +* **Type**: `float` +* **Utility**: Optional manual speed loop Proportional ($K_p$) and Integral ($K_i$) gains. If either parameter is non-zero, the system identification steps (measuring $J$, $B$, static friction, and IMC calculation) are bypassed, and these gains are directly injected. +* **Expected Value**: `0.0f` (default, to use auto-identification) or manual PID constants. + +## 8. Encoder Precision and Architecture + +### Float Mantissa Truncation (The 24-bit Limit) +The system requires absolute position tracking across multiple turns for robust calibration and FFB effects. However, storing absolute position directly in a 32-bit `float` introduces severe precision loss. A standard IEEE 754 32-bit float only provides 24 bits of mantissa. High-resolution encoders (e.g., 22-bit BISS-C) inherently use most of this mantissa just for a single fraction of a turn. Accumulating full turns (e.g., `1000.xxxx`) quickly pushes the fractional data out of the 24-bit window, truncating the lower bits of the encoder. This results in devastating quantization noise for the PID and severe phase-drift in the anti-cogging Fourier series over long sessions. + +**The Fix**: +1. **Mathematical Protection in `Encoder::getPos_f()`**: Instead of casting the full 32-bit position integer to float, the system separates the turns from the fraction via integer arithmetic: `turns = pos / cpr; remainder = pos % cpr`. The float is only constructed at the very end (`turns + remainder / cpr`), preserving maximum fractional precision before the hardware limits apply. +2. **Infinite Precision in `getFilteredPosition()`**: For anti-cogging and harmonic calculations, full turns are mathematically irrelevant. `getFilteredPosition()` performs a strict integer modulo `pos % cpr` *before* any float conversion. This guarantees 100% of the encoder's raw resolution is maintained indefinitely, regardless of how many turns the motor has completed. This protected pure-fractional position is fed directly to the `turn()` method for the Fourier compensation phase (`arm_sin_f32`). + +### Polymorphic Encoder Routing (`activeEnc`) +The firmware supports both internal (TMC4671 native pins) and external (STM32 pins) encoders. Rather than relying on redundant and prone-to-error `if/else` checks, the system unifies encoder access using a polymorphic pointer: +```cpp +Encoder* activeEnc = usingExternalEncoder() ? drvEncoder : this; +``` +Because both `TMC4671` (the driver itself, `this`) and external drivers (e.g., `EncoderBissC`) inherit from the base `Encoder` class, calling `activeEnc->getPos_f()` securely routes the execution to the active hardware while automatically inheriting the mantissa protection logic. Note that the `usingExternalEncoder()` inline function intrinsically validates the `drvEncoder != nullptr` condition, ensuring robust memory safety without redundant checks. + +### Low-Resolution Encoder Support (PID Decimation) +For low-resolution encoders (e.g., < 4000 CPR), the 4kHz/5kHz control loop is too fast: the encoder may not physically tick between two consecutive PID cycles, producing a discrete `delta_pos` of zero, followed by a massive spike. This aggressively amplifies noise in the derivative and acceleration terms (Inertia Feedforward). +**The Fix**: An `enc_decimation_ratio` dynamically scales the PID execution rate based on the encoder's physical CPR. High-res encoders run at the full 4kHz loop, while low-res encoders execute the PID calculation every N cycles (e.g., 500Hz). This allows enough physical time for the encoder to accumulate ticks, creating a smooth, high-fidelity `delta_pos` signal for the Inertia feedforward, while the main loop continues at maximum frequency to keep the TMC4671 SPI watchdogs satisfied. diff --git a/doc/memory_analysis.md b/doc/memory_analysis.md new file mode 100644 index 000000000..e37ef5afc --- /dev/null +++ b/doc/memory_analysis.md @@ -0,0 +1,48 @@ +# OpenFFBoard Memory Analysis (STM32F407VG & STM32F411RE) + +## Overview +This document provides a systematic analysis of the memory footprint of core OpenFFBoard classes, assuming a maximum configuration of **2 axes**. It highlights risks related to FreeRTOS heap exhaustion, heap fragmentation, and thread stack overflows, and details the implemented 78KB heap strategy to ensure system stability. + +## Target Architecture Context + +Both the **STM32F407VG** and **STM32F411RE** share the same primary memory constraint: **128KB of general SRAM**. +To maintain architectural consistency across targets, the FreeRTOS heap is centrally managed within this 128KB limit, without relying on the F407's isolated CCMRAM. + +## Memory Consumption Profile (Verified 2-Axis Setup) + +| Component / Thread | Permanent Memory (Heap) | Thread Stack Allocation | Temporary Spikes (Heap) | Overflow Risks & Observations | +| :--- | :--- | :--- | :--- | :--- | +| **TMC4671** (x2 axes) | **~12.6 KB** (2x `data_cogging`) | **3.0 KB** (2x 384-word threads) | **17.3 KB** (Calibration) | `data_cogging` (5.7KB per axis) is permanent. `CoggingCalibData` (17.3KB) is dynamically allocated *only* during calibration. | +| **Axes & HidFFB** | **~13.0 KB** (Effects Array) | **0.5 KB** (FFB Main Thread) | **~5.1 KB** (Biquad Filters) | Array of 40 `FFB_Effect` is large (~12KB). Dynamic Biquad allocation is necessary for render quality. | +| **Command Interfaces** | ~1.0 KB | **4.3 KB** (CMD Main + CDC) | **~5.8 KB** (via `help`) | **[FIXED]** `help` command consumes ~5.8KB peak. Stack rebalancing recovered 1.7KB for the heap. | +| **BISS-C Encoder** | < 1.0 KB | **0.5 KB** (128-word thread) | - | **[CRITICAL FIX]** Original 64-word stack had only 60 bytes of headroom. Increased to 128 words. | + +**Total Estimated Permanent Heap Usage (Base + Stacks):** ~42.0 KB. +**Total Available Heap:** **78.0 KB** (`configTOTAL_HEAP_SIZE`). +**Idle Heap Free:** ~36.0 KB. + +### Peak Stress Scenarios +1. **Help Command:** Consumes ~5.8KB. Remaining heap: ~30.2KB. (Status: **STABLE**) +2. **Cogging Calibration:** Consumes **17.3KB**. Remaining heap: ~18.7KB. (Status: **SAFE**) + - *Note: Minimum ever free heap (High Water Mark) observed during Stress: ~18.3KB.* + +## Optimized Stack Strategy (Rebalancing) + +Empirical analysis using `sys.tasklist` (Stack High Water Mark) allowed for precision rebalancing, recovering **1.7KB** of heap while securing critical threads: + +1. **BISSENC:** Increased from 64 to **128 words** (Prevented imminent stack overflow). +2. **CDCCMD:** Reduced from 512 to **384 words** (Recovered 512 bytes). +3. **TMC Thread:** Reduced from 512 to **384 words** (Recovered 1024 bytes for 2 axes). +4. **CMD_MAIN:** Reduced from 700 to **600 words** (Recovered 400 bytes). + +## Linker & SRAM Constraints + +The FreeRTOS heap was finalized at **78KB** to accommodate the 21.6KB SRAM overflow initially encountered at 100KB. +- **Total SRAM:** 128KB. +- **FreeRTOS Heap:** 78KB. +- **Static `.bss` / `.data`:** ~49KB. +- **Result:** ~127KB used. Fits within the 128KB hardware limit with minimal overhead for the system stack and interrupts. + +## Recommendations +- **Avoid concurrent calibration:** Do not run Cogging calibration while generating massive help strings to maintain the 18KB safety buffer. +- **Monitor BISSENC:** If new features are added to the BISS-C driver, re-verify the 128-word stack headroom.