Adding magntek encoder speed selection

CHANGELOG.md

@@ -1,8 +1,6 @@
 ### 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
+- Added "reg" and "save" commands to MagnTek encoder. Allows programming MT6835 encoders (debug=1 mode required!)
 
 ### Changes in 1.16:
 
@@ -19,4 +17,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
+- 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)

Firmware/FFBoard/Inc/CAN.h

@@ -11,6 +11,7 @@
 #ifdef CANBUS
 //#include "CanHandler.h"
 #include "main.h"
+#include <algorithm>
 #include <vector>
 #include "semaphore.hpp"
 #include <GPIOPin.h>

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

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<T>::value) {
+   if (!std::is_standard_layout<T>::value && !std::is_trivial<T>::value) {
       *ok = false;
       return data;
    }

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,21 +397,21 @@ 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<MotorDriver>(drv);
 	this->conf.drvtype = drvtype;
 
 	// Pass encoder to driver again
 	if(!this->drv->hasIntegratedEncoder()){
 		this->drv->setEncoder(this->enc);
 	}
 #ifdef TMC4671DRIVER
-	if (dynamic_cast<TMC4671 *>(drv))
+	if (dynamic_cast<TMC4671 *>(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<CommandReply>&
 		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<CommandReply>&
 		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<TMC4671 *>(this->drv.get()); // Special case for TMC. Get the actual encoder resolution

Firmware/FFBoard/Src/CommandHandler.cpp

@@ -12,6 +12,7 @@
 #include "CDCcomm.h"
 //#include <set>
 #include "ChoosableClass.h"
+#include <algorithm>
 
 //std::vector<CommandHandler*> CommandHandler::cmdHandlers;
 //std::set<uint16_t> CommandHandler::cmdHandlerIDs;

Firmware/FFBoard/Src/ErrorHandler.cpp

@@ -10,6 +10,7 @@
 #include "FFBoardMain.h"
 #include "cppmain.h"
 #include "critical.hpp"
+#include <algorithm>
 #include <span>
 
 std::vector<ErrorHandler*> ErrorHandler::errorHandlers;

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,reg,save
 	};
 	enum class MtEncoderSPI_mode : uint8_t{
 		mt6825,mt6835
 	};
+	const std::array<float,3> spispeeds = {10e6,5e6,2.5e6}; // Target speeds. Must double each entry
 public:
 	MtEncoderSPI();
 	virtual ~MtEncoderSPI();
@@ -57,10 +58,13 @@ 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);
 	void spiTxRxCompleted(SPIPort* port);
+	bool saveEeprom();
 
 
 	bool nomag = false; // Magnet lost in last report
@@ -85,6 +89,8 @@ class MtEncoderSPI: public Encoder, public SPIDevice, public PersistentStorage,
 
 	static std::array<uint8_t,256> tableCRC;
 	const uint8_t POLY = 0x07;
+
+	uint8_t spiSpeedPreset = 0;
 };
 
 #endif /* USEREXTENSIONS_SRC_MTENCODERSPI_H_ */

Firmware/FFBoard/UserExtensions/Src/MtEncoderSPI.cpp

@@ -23,7 +23,7 @@ std::array<uint8_t,256> 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,14 +32,17 @@ 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();
 	registerCommand("cs", MtEncoderSPI_commands::cspin, "CS pin",CMDFLAG_GET | CMDFLAG_SET);
 	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);
+	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();
 }
 
@@ -51,14 +54,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<MtEncoderSPI_mode>(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);
@@ -114,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){
@@ -134,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;
 }
@@ -154,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);
@@ -262,6 +281,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<uint8_t,uint8_t>(preset,0,spispeeds.size());
+	this->spiConfig.peripheral.BaudRatePrescaler = spiPort.getClosestPrescaler(spispeeds[spiSpeedPreset]).first;
+	initSPI();
+}
+
 
 CommandStatus MtEncoderSPI::command(const ParsedCommand& cmd,std::vector<CommandReply>& replies){
 	switch(static_cast<MtEncoderSPI_commands>(cmd.cmdId)){
@@ -298,6 +326,40 @@ CommandStatus MtEncoderSPI::command(const ParsedCommand& cmd,std::vector<Command
 			return CommandStatus::ERR;
 		}
 		break;
+
+	case MtEncoderSPI_commands::speed:
+	{
+		if(cmd.type == CMDtype::get){
+			replies.emplace_back(spiSpeedPreset);
+		}else if(cmd.type == CMDtype::set){
+			setSpiSpeed(cmd.val);
+		}else if(cmd.type == CMDtype::info){
+			for(uint8_t i = 0; i<spispeeds.size();i++){
+				replies.emplace_back(std::to_string(this->spiPort.getClosestPrescaler(spispeeds[i]).second)  + ":" + std::to_string(i)+"\n");
+			}
+		}else{
+			return CommandStatus::ERR;
+		}
+		break;
+	}
+	case MtEncoderSPI_commands::reg:
+	{
+		if(cmd.type == CMDtype::getat){
+			replies.emplace_back(readSpi(cmd.adr));
+		}else if(cmd.type == CMDtype::setat){
+			writeSpi(cmd.adr, cmd.val);
+		}else{
+			return CommandStatus::ERR;
+		}
+		break;
+	}
+	case MtEncoderSPI_commands::save:
+	{
+		if(cmd.type == CMDtype::get){
+			replies.emplace_back(saveEeprom() ? 1 : 0);
+		}
+		break;
+	}
 	default:
 		return CommandStatus::NOT_FOUND;
 	}