From e23abe82ced7340f14e1d43ef55e44d33b4435c3 Mon Sep 17 00:00:00 2001 From: Lucas Jansson Date: Mon, 20 Oct 2025 13:00:31 +0200 Subject: [PATCH 1/8] Include the algorithm library where needed Fixes build with gcc 14+ --- Firmware/FFBoard/Inc/CAN.h | 1 + Firmware/FFBoard/Src/CommandHandler.cpp | 1 + Firmware/FFBoard/Src/ErrorHandler.cpp | 1 + 3 files changed, 3 insertions(+) diff --git a/Firmware/FFBoard/Inc/CAN.h b/Firmware/FFBoard/Inc/CAN.h index d58a31841..06357e6ed 100644 --- a/Firmware/FFBoard/Inc/CAN.h +++ b/Firmware/FFBoard/Inc/CAN.h @@ -11,6 +11,7 @@ #ifdef CANBUS //#include "CanHandler.h" #include "main.h" +#include #include #include "semaphore.hpp" #include diff --git a/Firmware/FFBoard/Src/CommandHandler.cpp b/Firmware/FFBoard/Src/CommandHandler.cpp index ae7c3fcbc..1c488a832 100644 --- a/Firmware/FFBoard/Src/CommandHandler.cpp +++ b/Firmware/FFBoard/Src/CommandHandler.cpp @@ -12,6 +12,7 @@ #include "CDCcomm.h" //#include #include "ChoosableClass.h" +#include //std::vector CommandHandler::cmdHandlers; //std::set CommandHandler::cmdHandlerIDs; diff --git a/Firmware/FFBoard/Src/ErrorHandler.cpp b/Firmware/FFBoard/Src/ErrorHandler.cpp index 94f6eaf22..dc7ff60e4 100644 --- a/Firmware/FFBoard/Src/ErrorHandler.cpp +++ b/Firmware/FFBoard/Src/ErrorHandler.cpp @@ -10,6 +10,7 @@ #include "FFBoardMain.h" #include "cppmain.h" #include "critical.hpp" +#include #include std::vector ErrorHandler::errorHandlers; From 07ee4c61d445b4634348cb9be8d3ca528dce469a Mon Sep 17 00:00:00 2001 From: Lucas Jansson Date: Mon, 20 Oct 2025 14:07:01 +0200 Subject: [PATCH 2/8] Fix deprecation warning in ringbufferwrapper --- Firmware/FFBoard/Inc/ringbufferwrapper.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/FFBoard/Inc/ringbufferwrapper.h b/Firmware/FFBoard/Inc/ringbufferwrapper.h index 2ba46b8c8..99eef8caf 100644 --- a/Firmware/FFBoard/Inc/ringbufferwrapper.h +++ b/Firmware/FFBoard/Inc/ringbufferwrapper.h @@ -57,7 +57,7 @@ T RingBufferWrapper::peek_as(bool* ok) noexcept T data; // Only POD types can be trivially copied from // the ring buffer. - if (!std::is_pod::value) { + if (!std::is_standard_layout::value && !std::is_trivial::value) { *ok = false; return data; } From bc63e6a40d2195cf288316312fc19c07b68f3e9d Mon Sep 17 00:00:00 2001 From: Lucas Jansson Date: Mon, 20 Oct 2025 13:04:23 +0200 Subject: [PATCH 3/8] Makefile cleanup --- Firmware/Makefile | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/Firmware/Makefile b/Firmware/Makefile index 4c50b75b1..b5e4fb33c 100644 --- a/Firmware/Makefile +++ b/Firmware/Makefile @@ -26,7 +26,7 @@ BUILD_DIR = build ###################################### # Choose your board mcu F407VG or F411RE -MCU_TARGET=F407VG +MCU_TARGET ?= F407VG # The directory of mcu target TARGET_DIR = Targets/$(MCU_TARGET) @@ -36,7 +36,7 @@ TARGET_DIR = Targets/$(MCU_TARGET) TARGET = OpenFFBoard_$(MCU_TARGET) # Output directory for hex/bin files -OUTPUT_DIR = $(BUILD_DIR) +OUTPUT_DIR ?= $(BUILD_DIR) # C sources C_SOURCES = $(wildcard $(TARGET_DIR)/Core/Src/*.c) @@ -229,15 +229,15 @@ $(BUILD_DIR)/$(TARGET).elf: $(OBJECTS) Makefile $(OUTPUT_DIR)/%.hex: $(BUILD_DIR)/%.elf | $(OUTPUT_DIR) $(HEX) $< $@ - + $(OUTPUT_DIR)/%.bin: $(BUILD_DIR)/%.elf | $(OUTPUT_DIR) - $(BIN) $< $@ - + $(BIN) $< $@ + $(BUILD_DIR): - -mkdir $@ - + -mkdir -p $@ + $(OUTPUT_DIR): $(BUILD_DIR) - -mkdir $@ + -mkdir -p $@ ####################################### # clean up @@ -256,4 +256,4 @@ upload: $(BUILD_DIR)/$(TARGET).bin ####################################### -include $(wildcard $(BUILD_DIR)/*.d) -# *** EOF *** \ No newline at end of file +# *** EOF *** From a7087358f4b325052e8d1ef07f8c2a8e01639cf3 Mon Sep 17 00:00:00 2001 From: Yannick Richter Date: Tue, 11 Nov 2025 11:36:02 +0100 Subject: [PATCH 4/8] More encoder validity checks --- Firmware/FFBoard/Src/Axis.cpp | 32 ++++++++++++++++++-------------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/Firmware/FFBoard/Src/Axis.cpp b/Firmware/FFBoard/Src/Axis.cpp index f5394a296..2a61616b4 100644 --- a/Firmware/FFBoard/Src/Axis.cpp +++ b/Firmware/FFBoard/Src/Axis.cpp @@ -13,6 +13,7 @@ #include "ODriveCAN.h" #include "MotorSimplemotion.h" #include "RmdMotorCAN.h" +#include "critical.hpp" ////////////////////////////////////////////// /* @@ -282,8 +283,9 @@ uint8_t Axis::getEncType(){ void Axis::setPos(uint16_t val) { startForceFadeIn(0.25,0.5); - if(this->drv != nullptr){ - drv->getEncoder()->setPos(val); + Encoder* enc_p = getEncoder(); + if(enc_p != nullptr){ + enc_p->setPos(val); } } @@ -292,6 +294,7 @@ MotorDriver* Axis::getDriver(){ } Encoder* Axis::getEncoder(){ + if(!drv) return nullptr; return drv->getEncoder(); } @@ -306,7 +309,7 @@ void Axis::prepareForUpdate(){ //if (!drv->motorReady()) return; - float angle = getEncAngle(this->drv->getEncoder()); + float angle = getEncAngle(getEncoder()); // Scale encoder value to set rotation range // Update a change of range only when new range is within valid range @@ -394,13 +397,13 @@ void Axis::setDrvType(uint8_t drvtype) { return; } - this->drv.reset(nullptr); - MotorDriver* drv = drv_chooser.Create((uint16_t)drvtype); + cpp_freertos::CriticalSection::Enter(); + this->drv.reset(drv_chooser.Create((uint16_t)drvtype)); if (drv == nullptr) { + cpp_freertos::CriticalSection::Exit(); return; } - this->drv = std::unique_ptr(drv); this->conf.drvtype = drvtype; // Pass encoder to driver again @@ -408,7 +411,7 @@ void Axis::setDrvType(uint8_t drvtype) this->drv->setEncoder(this->enc); } #ifdef TMC4671DRIVER - if (dynamic_cast(drv)) + if (dynamic_cast(drv.get())) { setupTMC4671(); } @@ -422,6 +425,7 @@ void Axis::setDrvType(uint8_t drvtype) { drv->startMotor(); } + cpp_freertos::CriticalSection::Exit(); } #ifdef TMC4671DRIVER @@ -461,7 +465,7 @@ void Axis::setEncType(uint8_t enctype) this->conf.enctype = 0; // None encoder } - float angle = getEncAngle(this->drv->getEncoder()); + float angle = getEncAngle(this->getEncoder()); //int32_t scaledEnc = scaleEncValue(angle, degreesOfRotation); // reset metrics this->resetMetrics(angle); @@ -939,14 +943,14 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector& break; case Axis_commands::pos: - if (cmd.type == CMDtype::get && this->drv->getEncoder() != nullptr) + if (cmd.type == CMDtype::get && getEncoder() != nullptr) { - int32_t pos = this->drv->getEncoder()->getPos(); + int32_t pos = getEncoder()->getPos(); replies.emplace_back(isInverted() ? -pos : pos); } - else if (cmd.type == CMDtype::set && this->drv->getEncoder() != nullptr) + else if (cmd.type == CMDtype::set && getEncoder() != nullptr) { - this->drv->getEncoder()->setPos(isInverted() ? -cmd.val : cmd.val); + getEncoder()->setPos(isInverted() ? -cmd.val : cmd.val); } else { @@ -1024,8 +1028,8 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector& if (cmd.type == CMDtype::get) { uint32_t cpr = 0; - if(this->drv->getEncoder() != nullptr){ - cpr = this->drv->getEncoder()->getCpr(); + if(this->getEncoder() != nullptr){ + cpr = this->getEncoder()->getCpr(); } //#ifdef TMC4671DRIVER // CPR should be consistent with position. Maybe change TMC to prescale to encoder count or correct readout in UI // TMC4671 *tmcdrv = dynamic_cast(this->drv.get()); // Special case for TMC. Get the actual encoder resolution From 7e0e29b8231ccee4c5860e6a3d539860dfa49374 Mon Sep 17 00:00:00 2001 From: Yannick Richter Date: Mon, 17 Nov 2025 13:47:21 +0100 Subject: [PATCH 5/8] Added magntek spi speed command --- CHANGELOG.md | 11 +++--- Firmware/FFBoard/Inc/constants.h | 2 +- .../FFBoard/UserExtensions/Inc/MtEncoderSPI.h | 7 +++- .../UserExtensions/Src/MtEncoderSPI.cpp | 36 ++++++++++++++++--- 4 files changed, 45 insertions(+), 11 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 75d2ef41b..6dbb43d3c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,8 +1,5 @@ ### Changes this version: -- Reformatted USB serial string as hex and added command to request UID as hex string -- Added device name to USB Product name -- Added support for F407 OTP section -- Added support for MagnTek MT6835 via SPI (SPI3 port, MagnTek encoder class) +- Added SPI speed selector to MagnTek encoders ### Changes in 1.16: @@ -19,4 +16,8 @@ Internal changes: - Using analog VREF for voltage sensing (better accuracy with unstable 3.3V) - Added chip temperature readout - Added remote CAN button/analog source mainclass -- Added exponential torque postprocessing for game effects \ No newline at end of file +- Added exponential torque postprocessing for game effects +- Reformatted USB serial string as hex and added command to request UID as hex string +- Added device name to USB Product name +- Added support for F407 OTP section +- Added support for MagnTek MT6835 via SPI (SPI3 port, MagnTek encoder class) \ No newline at end of file diff --git a/Firmware/FFBoard/Inc/constants.h b/Firmware/FFBoard/Inc/constants.h index 8f6724a9e..e18918af6 100644 --- a/Firmware/FFBoard/Inc/constants.h +++ b/Firmware/FFBoard/Inc/constants.h @@ -8,7 +8,7 @@ * For more settings see target_constants.h in a target specific folder */ -static const uint8_t SW_VERSION_INT[3] = {1,16,5}; // Version as array. 8 bit each! +static const uint8_t SW_VERSION_INT[3] = {1,16,6}; // Version as array. 8 bit each! #ifndef MAX_AXIS #define MAX_AXIS 2 // ONLY USE 2 for now else screws HID Reports #endif diff --git a/Firmware/FFBoard/UserExtensions/Inc/MtEncoderSPI.h b/Firmware/FFBoard/UserExtensions/Inc/MtEncoderSPI.h index 381636625..f9a932776 100644 --- a/Firmware/FFBoard/UserExtensions/Inc/MtEncoderSPI.h +++ b/Firmware/FFBoard/UserExtensions/Inc/MtEncoderSPI.h @@ -19,11 +19,12 @@ class MtEncoderSPI: public Encoder, public SPIDevice, public PersistentStorage, public CommandHandler,cpp_freertos::Thread{ enum class MtEncoderSPI_commands : uint32_t{ - cspin,pos,errors,mode + cspin,pos,errors,mode,speed }; enum class MtEncoderSPI_mode : uint8_t{ mt6825,mt6835 }; + const std::array spispeeds = {10e6,5e6,2.5e6}; // Target speeds. Must double each entry public: MtEncoderSPI(); virtual ~MtEncoderSPI(); @@ -57,6 +58,8 @@ class MtEncoderSPI: public Encoder, public SPIDevice, public PersistentStorage, //bool useDMA = false; // if true uses DMA for angle updates instead of polling SPI. TODO when used with tmc external encoder using DMA will hang the interrupt randomly + void setSpiSpeed(uint8_t preset); + private: uint8_t readSpi(uint16_t addr); void writeSpi(uint16_t addr,uint8_t data); @@ -85,6 +88,8 @@ class MtEncoderSPI: public Encoder, public SPIDevice, public PersistentStorage, static std::array tableCRC; const uint8_t POLY = 0x07; + + uint8_t spiSpeedPreset = 0; }; #endif /* USEREXTENSIONS_SRC_MTENCODERSPI_H_ */ diff --git a/Firmware/FFBoard/UserExtensions/Src/MtEncoderSPI.cpp b/Firmware/FFBoard/UserExtensions/Src/MtEncoderSPI.cpp index b0ef5bf13..e4868a899 100644 --- a/Firmware/FFBoard/UserExtensions/Src/MtEncoderSPI.cpp +++ b/Firmware/FFBoard/UserExtensions/Src/MtEncoderSPI.cpp @@ -23,7 +23,7 @@ std::array MtEncoderSPI::tableCRC __attribute__((section (".ccmram" MtEncoderSPI::MtEncoderSPI() : SPIDevice(ENCODER_SPI_PORT,ENCODER_SPI_PORT.getFreeCsPins()[0]), CommandHandler("mtenc",CLSID_ENCODER_MTSPI,0),cpp_freertos::Thread("MTENC",256,42) { MtEncoderSPI::inUse = true; - this->spiConfig.peripheral.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_4; // 4 = 10MHz 8 = 5MHz + this->spiConfig.peripheral.BaudRatePrescaler = spiPort.getClosestPrescaler(10e6).first; // 10MHz max this->spiConfig.peripheral.FirstBit = SPI_FIRSTBIT_MSB; this->spiConfig.peripheral.CLKPhase = SPI_PHASE_2EDGE; this->spiConfig.peripheral.CLKPolarity = SPI_POLARITY_HIGH; @@ -32,7 +32,7 @@ MtEncoderSPI::MtEncoderSPI() : SPIDevice(ENCODER_SPI_PORT,ENCODER_SPI_PORT.getFr //Init CRC-8 table makeCrcTable(tableCRC,POLY,8); // Mt6825, Poly X8+X2+X (+1) - restoreFlash(); + restoreFlash(); // Also configures SPI port spiPort.reserveCsPin(this->spiConfig.cs); CommandHandler::registerCommands(); @@ -40,6 +40,7 @@ MtEncoderSPI::MtEncoderSPI() : SPIDevice(ENCODER_SPI_PORT,ENCODER_SPI_PORT.getFr registerCommand("pos", MtEncoderSPI_commands::pos, "Position",CMDFLAG_GET | CMDFLAG_SET); registerCommand("errors", MtEncoderSPI_commands::errors, "Parity error count",CMDFLAG_GET); registerCommand("mode", MtEncoderSPI_commands::mode, "Encoder mode (MT6825=0;MT6835=1)",CMDFLAG_GET | CMDFLAG_SET | CMDFLAG_INFOSTRING); + registerCommand("speed", MtEncoderSPI_commands::speed, "SPI speed preset",CMDFLAG_GET | CMDFLAG_SET | CMDFLAG_INFOSTRING); this->Start(); } @@ -51,14 +52,16 @@ MtEncoderSPI::~MtEncoderSPI() { void MtEncoderSPI::restoreFlash(){ uint16_t conf_int = Flash_ReadDefault(ADR_MTENC_CONF1, 0); offset = Flash_ReadDefault(ADR_MTENC_OFS, 0) << 2; - uint8_t cspin = conf_int & 0xF; + uint8_t cspin = conf_int & 0x3; MtEncoderSPI_mode mode = static_cast(conf_int >> 8); setMode(mode); setCsPin(cspin); + setSpiSpeed((conf_int >> 2) & 0x3); } void MtEncoderSPI::saveFlash(){ - uint16_t conf_int = this->cspin & 0xF; + uint16_t conf_int = this->cspin & 0x3; + conf_int |= (this->spiSpeedPreset & 0x3) << 2; conf_int |= ((uint8_t)mode & 0xf) << 8; Flash_Write(ADR_MTENC_CONF1, conf_int); Flash_Write(ADR_MTENC_OFS, offset >> 2); @@ -262,6 +265,15 @@ void MtEncoderSPI::setMode(MtEncoderSPI::MtEncoderSPI_mode mode){ this->overspeed = false; } +void MtEncoderSPI::setSpiSpeed(uint8_t preset){ + if(preset == spiSpeedPreset){ + return; // Ignore if no change + } + spiSpeedPreset = clip(preset,0,spispeeds.size()); + this->spiConfig.peripheral.BaudRatePrescaler = spiPort.getClosestPrescaler(spispeeds[spiSpeedPreset]).first; + initSPI(); +} + CommandStatus MtEncoderSPI::command(const ParsedCommand& cmd,std::vector& replies){ switch(static_cast(cmd.cmdId)){ @@ -298,6 +310,22 @@ CommandStatus MtEncoderSPI::command(const ParsedCommand& cmd,std::vectorspiPort.getClosestPrescaler(spispeeds[i]).second) + ":" + std::to_string(i)+"\n"); + } + }else{ + return CommandStatus::ERR; + } + break; + } default: return CommandStatus::NOT_FOUND; } From f60a319905629f96eee95ece17816a35e69f83fb Mon Sep 17 00:00:00 2001 From: Yannick Richter Date: Mon, 17 Nov 2025 14:47:47 +0100 Subject: [PATCH 6/8] Configurator update --- Configurator | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Configurator b/Configurator index a3bc7f11a..9eee2b5ff 160000 --- a/Configurator +++ b/Configurator @@ -1 +1 @@ -Subproject commit a3bc7f11aeb04853ae8bbc1446b6761d2a739588 +Subproject commit 9eee2b5ffe44f2dec1c6d2eae4a0ad987d53b5c8 From dd3010cba49df8cc8ab5676592c8c4703d25b67e Mon Sep 17 00:00:00 2001 From: Yannick Richter Date: Mon, 17 Nov 2025 17:32:30 +0100 Subject: [PATCH 7/8] Added reg and save debug commands to mtenc --- CHANGELOG.md | 1 + .../FFBoard/UserExtensions/Inc/MtEncoderSPI.h | 3 +- .../UserExtensions/Src/MtEncoderSPI.cpp | 42 +++++++++++++++++-- 3 files changed, 41 insertions(+), 5 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 6dbb43d3c..7d6fa0308 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,6 @@ ### Changes this version: - Added SPI speed selector to MagnTek encoders +- Added "reg" and "save" commands to MagnTek encoder. Allows programming MT6835 encoders (debug=1 mode required!) ### Changes in 1.16: diff --git a/Firmware/FFBoard/UserExtensions/Inc/MtEncoderSPI.h b/Firmware/FFBoard/UserExtensions/Inc/MtEncoderSPI.h index f9a932776..981dea2de 100644 --- a/Firmware/FFBoard/UserExtensions/Inc/MtEncoderSPI.h +++ b/Firmware/FFBoard/UserExtensions/Inc/MtEncoderSPI.h @@ -19,7 +19,7 @@ class MtEncoderSPI: public Encoder, public SPIDevice, public PersistentStorage, public CommandHandler,cpp_freertos::Thread{ enum class MtEncoderSPI_commands : uint32_t{ - cspin,pos,errors,mode,speed + cspin,pos,errors,mode,speed,reg,save }; enum class MtEncoderSPI_mode : uint8_t{ mt6825,mt6835 @@ -64,6 +64,7 @@ class MtEncoderSPI: public Encoder, public SPIDevice, public PersistentStorage, uint8_t readSpi(uint16_t addr); void writeSpi(uint16_t addr,uint8_t data); void spiTxRxCompleted(SPIPort* port); + bool saveEeprom(); bool nomag = false; // Magnet lost in last report diff --git a/Firmware/FFBoard/UserExtensions/Src/MtEncoderSPI.cpp b/Firmware/FFBoard/UserExtensions/Src/MtEncoderSPI.cpp index e4868a899..54148bad5 100644 --- a/Firmware/FFBoard/UserExtensions/Src/MtEncoderSPI.cpp +++ b/Firmware/FFBoard/UserExtensions/Src/MtEncoderSPI.cpp @@ -41,6 +41,8 @@ MtEncoderSPI::MtEncoderSPI() : SPIDevice(ENCODER_SPI_PORT,ENCODER_SPI_PORT.getFr registerCommand("errors", MtEncoderSPI_commands::errors, "Parity error count",CMDFLAG_GET); registerCommand("mode", MtEncoderSPI_commands::mode, "Encoder mode (MT6825=0;MT6835=1)",CMDFLAG_GET | CMDFLAG_SET | CMDFLAG_INFOSTRING); registerCommand("speed", MtEncoderSPI_commands::speed, "SPI speed preset",CMDFLAG_GET | CMDFLAG_SET | CMDFLAG_INFOSTRING); + registerCommand("reg", MtEncoderSPI_commands::reg, "Read/Write register",CMDFLAG_GETADR | CMDFLAG_SETADR | CMDFLAG_DEBUG); + registerCommand("save", MtEncoderSPI_commands::save, "Save to memory",CMDFLAG_GET | CMDFLAG_DEBUG); this->Start(); } @@ -117,13 +119,13 @@ uint8_t MtEncoderSPI::readSpi(uint16_t addr){ uint8_t txbuf[2] = {(uint8_t)(addr | 0x80),0}; uint8_t rxbuf[2] = {0,0}; spiPort.transmitReceive(txbuf, rxbuf, 2, this,100); + return rxbuf[1]; }else if(mode == MtEncoderSPI_mode::mt6835){ uint8_t txbuf[3] = {(uint8_t)((addr & 0xf00) | 0x30),(uint8_t)(addr & 0xff),0}; uint8_t rxbuf[3] = {0,0,0}; spiPort.transmitReceive(txbuf, rxbuf, 3, this,100); + return rxbuf[2]; } - - return rxbuf[1]; } void MtEncoderSPI::writeSpi(uint16_t addr,uint8_t data){ @@ -137,6 +139,22 @@ void MtEncoderSPI::writeSpi(uint16_t addr,uint8_t data){ } +/** + * Saves current configuration to permanent storage + */ +bool MtEncoderSPI::saveEeprom(){ + if(mode != MtEncoderSPI_mode::mt6835){ + return false; + } + if(mode == MtEncoderSPI_mode::mt6835){ + uint8_t txbuf[3] = {0xC0,0x00,0x00}; + uint8_t rxbuf[3] = {0,0,0}; + spiPort.transmitReceive(txbuf, rxbuf, 3, this,100); + return rxbuf[2] == 0x55; + } + return false; +} + void MtEncoderSPI::setPos(int32_t pos){ offset = curPos - pos; } @@ -157,8 +175,6 @@ void MtEncoderSPI::spiTxRxCompleted(SPIPort* port){ */ void MtEncoderSPI::updateAngleStatus(){ - - if(mode == MtEncoderSPI_mode::mt6825){ uint8_t txbufNew[5] = {0x03 | 0x80,0,0,0,0}; memcpy(this->txbuf,txbufNew,5); @@ -326,6 +342,24 @@ CommandStatus MtEncoderSPI::command(const ParsedCommand& cmd,std::vector Date: Fri, 21 Nov 2025 18:56:01 -0500 Subject: [PATCH 8/8] velocity calc update Use time between encoder ticks to better estimate low speeds. Calculate an upper bound on velocity based on encoder resolution and time since last tick and use that to update the current velocity estimate when no ticks are observed. --- Firmware/FFBoard/Inc/Axis.h | 1 + Firmware/FFBoard/Src/Axis.cpp | 34 +++++++++++++++++++++++++++------- 2 files changed, 28 insertions(+), 7 deletions(-) diff --git a/Firmware/FFBoard/Inc/Axis.h b/Firmware/FFBoard/Inc/Axis.h index 05e87fdc9..bed8e6eb1 100644 --- a/Firmware/FFBoard/Inc/Axis.h +++ b/Firmware/FFBoard/Inc/Axis.h @@ -239,6 +239,7 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle int32_t effectTorque = 0; int32_t axisEffectTorque = 0; uint8_t fx_ratio_i = 204; // Reduce effects to a certain ratio of the total power to have a margin for the endstop. 80% = 204 + uint16_t timeSincePosChange = 1; uint16_t power = 5000; float torqueScaler = 0; // power * fx_ratio as a ratio between 0 & 1 float effect_margin_scaler = 0; diff --git a/Firmware/FFBoard/Src/Axis.cpp b/Firmware/FFBoard/Src/Axis.cpp index 2a61616b4..c700caf96 100644 --- a/Firmware/FFBoard/Src/Axis.cpp +++ b/Firmware/FFBoard/Src/Axis.cpp @@ -439,7 +439,7 @@ void Axis::setupTMC4671() tmclimits.pid_torque_flux = getPower(); drv->setLimits(tmclimits); //drv->setBiquadTorque(TMC4671Biquad(tmcbq_500hz_07q_25k)); - + // Enable driver @@ -656,12 +656,32 @@ void Axis::updateMetrics(float new_pos) { // pos is degrees std::tie(metric.current.pos,metric.current.pos_f) = scaleEncValue(new_pos, degreesOfRotation); - // compute speed and accel from raw instant speed normalized - float currentSpeed = (new_pos - metric.previous.posDegrees) * 1000.0; // deg/s - metric.current.speed = speedFilter.process(currentSpeed); - metric.current.accel = accelFilter.process((currentSpeed - _lastSpeed))* 1000.0; // deg/s/s - _lastSpeed = currentSpeed; - + // compute speed and accel from time between enocder changes + const float pos_change = new_pos - metric.previous.posDegrees; + timeSincePosChange += 1; + const float tick_rate = 1000.0f; + const uint16_t update_timeout = 1000; // 1 second + const float update_time_inv = (tick_rate/timeSincePosChange); + if ((fabsf(pos_change) > 1e-8f)) { + const float currentSpeed = pos_change * update_time_inv; // deg/s + metric.current.speed = speedFilter.process(currentSpeed); + metric.current.accel = accelFilter.process((currentSpeed - _lastSpeed) * tick_rate); // deg/s/s + _lastSpeed = currentSpeed; + timeSincePosChange = 0; + } else if (timeSincePosChange >= update_timeout) { + metric.current.speed = speedFilter.process(0.0f); + metric.current.accel = accelFilter.process(0.0f); + } else { + const float speed_dir = copysign(1.0f,metric.previous.speed); + // Encoder* enc = getEncoder(); + const uint32_t cpr = 10000;//enc->getCpr(); + const float angle_inc = 360.0f / cpr; // deg + const float speed_bound = angle_inc/update_time_inv; // deg/sec + const float speed_est = speed_dir * std::min(fabsf(metric.previous.speed), speed_bound); // deg/sec + metric.current.speed = speedFilter.process(speed_est); + metric.current.accel = accelFilter.process((speed_est - _lastSpeed) * tick_rate); // deg/s/s + _lastSpeed = speed_est; + } }