tmc51x0 add refreshcfg

tmcdriver/tmc/tmc_reg_cache.hpp

@@ -0,0 +1,83 @@
+#pragma once
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <functional>
+
+//
+namespace iflytop {
+using namespace std;
+class TMCReg {
+ private:
+  /* data */
+ public:
+  uint8_t flag = 0;
+  uint8_t reg  = 0;
+  uint8_t val  = 0;
+
+  void setActiveFlag() { flag = flag | 0x01; }
+  bool isActive() { return flag & 0x01; }
+
+  void setUpdateFlag() { flag = flag | 0x02; }
+  bool isUpdate() { return flag & 0x02; }
+};
+
+class TMCRegCache {
+ public:
+  TMCReg cache[50];
+
+ public:
+  void foreach (function<void(TMCReg* item)> fn) {
+    for (size_t i = 0; i < sizeof(cache) / sizeof(cache[0]); i++) {
+      if (cache[i].isActive() && cache[i].isUpdate()) {
+        fn(&cache[i]);
+      }
+    }
+  }
+
+  bool activeReg(uint8_t reg) {
+    TMCReg* item = findReg(reg);
+    if (item) return true;
+
+    item = findPassive();
+    if (item) {
+      item->reg = reg;
+      item->setActiveFlag();
+      return true;
+    }
+
+    return false;
+  }
+
+  void setReg(uint8_t reg, uint8_t val) {
+    TMCReg* item = findReg(reg);
+    if (item) {
+      item->val = val;
+      item->setUpdateFlag();
+    }
+  }
+
+ private:
+  TMCReg* findPassive() {
+    for (size_t i = 0; i < sizeof(cache) / sizeof(cache[0]); i++) {
+      if (!cache[i].isActive()) {
+        return &cache[i];
+      }
+    }
+    return nullptr;
+  }
+
+  TMCReg* findReg(uint8_t reg) {
+    for (size_t i = 0; i < sizeof(cache) / sizeof(cache[0]); i++) {
+      if (cache[i].isActive() && cache[i].reg == reg) {
+        return &cache[i];
+      }
+    }
+    return nullptr;
+  }
+};
+
+}  // namespace iflytop

tmcdriver/tmc51x0/tmc51x0.cpp

@@ -28,7 +28,9 @@ static const uint8_t tmc5130_defaultRegisterAccess[TMC5130_REGISTER_COUNT] = {
     0x42, 0x01, 0x02, 0x01, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____   // 0x70 - 0x7F
 };
 #endif
-
+#define TMC5160_GLOBAL_SCALER       0x0B
+#define TMC5160_GLOBAL_SCALER_MASK  0xFF
+#define TMC5160_GLOBAL_SCALER_SHIFT 0
 /***********************************************************************************************************************
  *                                                      工具�?                                                         *
  ***********************************************************************************************************************/
@@ -57,6 +59,25 @@ void TMC51X0::initialize(int mid, TMC51X0Cfg cfg) {
 
   stop();
 
+  ZASSERT(m_cache.activeReg(TMC5130_CHOPCONF));
+  ZASSERT(m_cache.activeReg(TMC5130_PWMCONF));
+  ZASSERT(m_cache.activeReg(TMC5130_GCONF));
+  ZASSERT(m_cache.activeReg(TMC5130_IHOLD_IRUN));
+  ZASSERT(m_cache.activeReg(TMC5160_GLOBAL_SCALER));
+
+  ZASSERT(m_cache.activeReg(TMC5130_VMAX));
+  ZASSERT(m_cache.activeReg(TMC5130_VSTART));
+  ZASSERT(m_cache.activeReg(TMC5130_A1));
+  ZASSERT(m_cache.activeReg(TMC5130_AMAX));
+  ZASSERT(m_cache.activeReg(TMC5130_V1));
+  ZASSERT(m_cache.activeReg(TMC5130_DMAX));
+  ZASSERT(m_cache.activeReg(TMC5130_D1));
+  ZASSERT(m_cache.activeReg(TMC5130_VSTOP));
+  ZASSERT(m_cache.activeReg(TMC5130_TZEROWAIT));
+  ZASSERT(m_cache.activeReg(TMC5130_ENCMODE));
+  ZASSERT(m_cache.activeReg(TMC5130_ENC_CONST));
+  ZASSERT(m_cache.activeReg(TMC5130_XENC));
+
   writeInt(TMC5130_PWMCONF, 0x000500C8);
   writeInt(TMC5130_CHOPCONF, 0x000100c3);
   writeInt(TMC5130_IHOLD_IRUN, 0x00051A00);
@@ -67,11 +88,11 @@ void TMC51X0::initialize(int mid, TMC51X0Cfg cfg) {
   writeInt(TMC5130_TZEROWAIT, 0);
 
   setVstart(100);
-  setA1(30);
-  setAmax(50);
+  setA1(15);
+  setAmax(15);
   setV1(500);
-  setDmax(50);
-  setD1(30);
+  setDmax(15);
+  setD1(15);
   setVstop(100);
   setTzerowait(100);
 
@@ -79,6 +100,13 @@ void TMC51X0::initialize(int mid, TMC51X0Cfg cfg) {
   enable(false);
 }
 
+void TMC51X0::refreshcfg() {
+  m_cache.foreach ([this](TMCReg *item) {
+    ZLOGI(TAG, "refreshcfg %x,%x", item->reg, item->val);
+    writeInt(item->reg, item->val);
+  });
+}
+
 /*******************************************************************************
  *                             驱动器寄存器读写方法                              *
  *******************************************************************************/
@@ -95,6 +123,7 @@ void TMC51X0::writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3,
   readWriteArray(&data[0], 5);
 }
 void TMC51X0::writeInt(uint8_t address, int32_t value) {  //
+  m_cache.setReg(address, value);
   writeDatagram(address, BYTE(value, 3), BYTE(value, 2), BYTE(value, 1), BYTE(value, 0));
 }
 
@@ -206,6 +235,10 @@ void TMC51X0::moveTo(int32_t position, uint32_t velocityMax) {
 void TMC51X0::moveToEnd(int32_t direction, uint32_t velocityMax) {
   zlock_guard lkg(m_mutex);
 
+  // @WARNING:
+  //    the motor will stop running(1000RPM) for about 10 minutes
+  // If you really need to use this feature, reset XACTUAL every 30s
+  setXACTUAL(0);
   if (direction >= 0) {
     writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION);
     writeInt(TMC5130_VMAX, to_motor_vel(velocityMax));
@@ -321,9 +354,6 @@ void TMC51X0::setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) {
   writeInt(TMC5130_IHOLD_IRUN, (iholddelay << TMC5130_IHOLDDELAY_SHIFT) | (irun << TMC5130_IRUN_SHIFT) | (ihold << TMC5130_IHOLD_SHIFT));
 }
 void TMC51X0::setGlobalScale(uint8_t globalscale) {
-#define TMC5160_GLOBAL_SCALER       0x0B
-#define TMC5160_GLOBAL_SCALER_MASK  0xFF
-#define TMC5160_GLOBAL_SCALER_SHIFT 0
   zlock_guard lkg(m_mutex);
   if (isTMC5130()) {
     return;

tmcdriver/tmc51x0/tmc51x0.hpp

@@ -7,6 +7,7 @@
 
 //
 #include "../tmc/tmc_type.h"
+#include "../tmc/tmc_reg_cache.hpp"
 #include "reg/TMC5130_Type.h"
 #include "stm32basic/stm32basic.hpp"
 namespace iflytop {
@@ -24,9 +25,11 @@ class TMC51X0Cfg {
   TMC51X0Cfg() {}
 };
 
+
 class TMC51X0 {
  protected:
-  TMC51X0Cfg m_cfg;
+  TMC51X0Cfg  m_cfg;
+  TMCRegCache m_cache;
 
   SPI_HandleTypeDef *m_hspi = NULL;
   ZGPIO              m_csnpin;
@@ -56,6 +59,7 @@ class TMC51X0 {
  public:
   bool ping();
   void enable(bool enable);
+  void refreshcfg();
 
   void setdriErr(bool val) { m_driErr = val; }
   bool getdriErr() { return m_driErr; }