Velocity calculation update

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/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;

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
@@ -435,7 +439,7 @@ void Axis::setupTMC4671()
 	tmclimits.pid_torque_flux = getPower();
 	drv->setLimits(tmclimits);
 	//drv->setBiquadTorque(TMC4671Biquad(tmcbq_500hz_07q_25k));
-	
+
 
 	// Enable driver
 
@@ -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);
@@ -652,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;
+	}
 }
 
 
@@ -939,14 +963,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 +1048,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_ */