修复TMC5160编码器数值异常问题

components/step_motor_ctrl_module/tmc51x0_motor.cpp

@@ -662,40 +662,56 @@ int32_t TMC51X0Motor::motor_set_enc_resolution(int32_t enc_resolution) {
    */
 
   int32_t enc_resolution_tmp   = enc_resolution * 4;
-  int32_t enc_const_integral   = 0;
-  int32_t enc_const_fractional = 0;
+  int16_t enc_const_integral   = 0;
+  int16_t enc_const_fractional = 0;
 
   switch (abs(enc_resolution_tmp)) {
     case 1000:
       enc_const_integral   = 51;
-      enc_const_fractional = 2;
+      enc_const_fractional = 0.2 * 10000;
       break;
     case 1024:
       enc_const_integral   = 50;
-      enc_const_fractional = 0;
+      enc_const_fractional = 0 * 10000;
       break;
     case 4000:
       enc_const_integral   = 12;
-      enc_const_fractional = 8;
+      enc_const_fractional = 0.8 * 10000;
       break;
     case 4096:
       enc_const_integral   = 12;
-      enc_const_fractional = 5;
+      enc_const_fractional = 0.5 * 10000;
       break;
     case 16384:
       enc_const_integral   = 3;
-      enc_const_fractional = 125;
+      enc_const_fractional = 0.125 * 10000;
       break;
     default:
       return err::kparam_out_of_range;
       break;
   }
 
-  if (enc_resolution_tmp < 0) {
-    enc_const_integral = -enc_const_integral;
-  }
+  // if (enc_resolution_tmp < 0) {
+  //   enc_const_integral = -enc_const_integral;
+  // }
   m_motor_enc_resolution = enc_resolution;
-  int32_t setval         = enc_const_integral * (2 ^ 16) + enc_const_fractional;
+  uint32_t setval        = 0;
+  uint8_t* psetval       = (uint8_t*)&setval;
+
+  // if (enc_resolution_tmp < 0) {
+  //   enc_const_integral = -enc_const_integral;
+  // }
+
+  // if (enc_resolution_tmp < 0) {
+  //   enc_const_fractional = enc_const_fractional;
+  // }
+
+  memcpy(psetval + 2, &enc_const_integral, 2);
+  memcpy(psetval, &enc_const_fractional, 2);
+
+  // int32_t setval = (enc_const_integral << 16) + enc_const_fractional * 10000;
+  // ZLOGI(TAG, "setval:%d", setval);
+  m_stepM1->writeInt(TMC5130_ENCMODE, 0x1 << 10);
   m_stepM1->writeInt(TMC5130_ENC_CONST, setval);
   return 0;
 }
@@ -716,7 +732,14 @@ int32_t TMC51X0Motor::motor_read_enc_val(int32_t* enc_val) {
   if (m_motor_enc_resolution == 0) {
     return motor_read_pos(enc_val);
   }
+  // ZLOGI(TAG, "%d , %d", m_stepM1->getENCVAL(), m_stepM1->getXACTUAL());
+  // m_stepM1->setENCVAL(0);
   inverse_kinematics(m_stepM1->getENCVAL(), *enc_val);
+
+  if (m_motor_enc_resolution < 0) {
+    *enc_val = -*enc_val;
+  }
+
   return 0;
 }
 

components/subcanmodule/zcancmder_subboard_initer.cpp

@@ -93,6 +93,8 @@ void ZCancmderSubboardIniter::loop() {
     OSDefaultSchduler::getInstance()->loop();
     zcanCmder.loop();
     // cmder.schedule();
+    // uint8_t table[20] = {1, 23, 4, 5, 6, 7, 8, 9};
+    // zcanCmder.sendPacket(table, 20);
   }
 }
 #endif

components/tmc/ic/ztmc5130.cpp

@@ -259,6 +259,12 @@ void TMC5130::writeInt(uint8_t address, int32_t value) {
   CriticalContext cc;
   writeDatagram(address, BYTE(value, 3), BYTE(value, 2), BYTE(value, 1), BYTE(value, 0));
 }
+  void    TMC5130::writeInt(uint8_t address, int32_t mask, int32_t shift, int32_t value){
+    CriticalContext cc;
+    writeInt(address, FIELD_SET(readInt(address), mask, shift, value));
+  
+  }
+
 int32_t TMC5130::readInt(uint8_t address) {
   CriticalContext cc;
   address = TMC_ADDRESS(address);

components/tmc/ic/ztmc5130.hpp

@@ -88,7 +88,7 @@ class TMC5130 : public IStepperMotor {
   const uint8_t *m_registerAccessTable;
   const int32_t *m_defaultRegisterResetState;
 
-  int32_t m_scale              = 10000;
+  int32_t m_scale              = 51200;
   int32_t m_scale_deceleration = 1;
 
   mres_type_t m_MRES           = kmres_256;
@@ -156,6 +156,7 @@ class TMC5130 : public IStepperMotor {
   void    readWriteArray(uint8_t *data, size_t length);
   void    writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4);
   void    writeInt(uint8_t address, int32_t value);
+  void    writeInt(uint8_t address, int32_t mask, int32_t shift, int32_t value);
   int32_t readInt(uint8_t address);
 
   void setNoAccLimit(bool enable) override;

components/zcancmder/zcanreceiver.cpp

@@ -305,6 +305,9 @@ void ZCanCmder::STM32_HAL_onCAN_RxFifo0MsgPending(CAN_HandleTypeDef *canHandle)
     uint8_t            nframe  = (pHeader.ExtId & 0xFF00) >> 8;
     uint8_t            frameId = (pHeader.ExtId & 0x00FF);
     CanPacketRxBuffer *rxbuf   = &m_canPacketRxBuffer[0];
+    // ZLOGI(TAG, "rx packet %d %d %d", from, nframe, frameId);
+
+    //在中断中打印，代码会卡死
     if (from != rxbuf->id) {
       // 目前只接收来自主机的消息
       continue;
@@ -327,6 +330,7 @@ void ZCanCmder::STM32_HAL_onCAN_RxFifo0MsgPending(CAN_HandleTypeDef *canHandle)
       rxbuf->m_canPacketNum++;
     }
     if (nframe == frameId + 1) {
+      // ZLOGI(TAG, "rx packet %d", rxbuf->m_canPacketNum);
       rxbuf->dataIsReady = true;
     }
   }
@@ -342,6 +346,7 @@ void ZCanCmder::STM32_HAL_onCAN_Error(CAN_HandleTypeDef *canHandle) {
 void ZCanCmder::loop() {
   CanPacketRxBuffer *rxbuf = &m_canPacketRxBuffer[0];
   if (rxbuf->dataIsReady) {
+    // ZLOGI(TAG, "rx packet %d", rxbuf->m_canPacketNum);
     int dataoff = 0;
     for (size_t i = 0; i < rxbuf->m_canPacketNum; i++) {
       memcpy(rxbuf->rxdata + dataoff, rxbuf->m_canPacket[i].aData, rxbuf->m_canPacket[i].dlc);

components/zcancmder/zcanreceiver.hpp

@@ -76,6 +76,7 @@ class ZCanCmder : public ZCanIRQListener, public IZCanCmder {
   virtual int32_t sendAck(zcr_cmd_header_t *rx_cmd_header, int32_t *ackvar, int32_t nack) override;
   virtual int32_t sendErrorAck(zcr_cmd_header_t *rx_cmd_header, int32_t errorcode) override;
   virtual int32_t triggerEvent(zcr_cmd_header_t *cmd_header, uint8_t *data, int32_t len) override;
+  void sendPacket(uint8_t *packet, size_t len);
 
   void loop();
 
@@ -84,7 +85,6 @@ class ZCanCmder : public ZCanIRQListener, public IZCanCmder {
   virtual void STM32_HAL_onCAN_Error(CAN_HandleTypeDef *can);
 
  private:
-  void sendPacket(uint8_t *packet, size_t len);
   bool sendPacketSub(int npacket, int packetIndex, uint8_t *packet, size_t len, int overtimems);
 
   HAL_StatusTypeDef initializeFilter();