飞特电机增加自动重发功能

components/mini_servo_motor/feite_servo_motor.cpp

@@ -536,6 +536,17 @@ bool FeiTeServoMotor::read_regs(uint8_t id, uint8_t add, uint8_t* data, uint8_t
   return true;
 }
 bool FeiTeServoMotor::tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, int32_t overtimems) {
+  for (int i = 0; i < 3; i++) {
+    bool suc = _tx_and_rx(tx, txdatalen, rx, expectrxsize, overtimems);
+    if (suc) {
+      return true;
+    }
+    osDelay(1);
+  }
+  return false;
+}
+
+bool FeiTeServoMotor::_tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, int32_t overtimems) {
   uint32_t enter_ticket = HAL_GetTick();
   dumphex("tx:", tx, txdatalen);
 #if 1

components/mini_servo_motor/feite_servo_motor.hpp

@@ -198,6 +198,7 @@ class FeiTeServoMotor {
   bool read_regs(uint8_t id, uint8_t add, uint8_t* data, uint8_t len);
 
  private:
+  bool    _tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, int32_t overtimems);
   bool    tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, int32_t overtimems);
   uint8_t checksum(uint8_t* data, uint8_t len);
   uint8_t checksum_packet(uint8_t* data, uint8_t len);

components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp

@@ -106,7 +106,17 @@ int32_t MiniServoCtrlModule::mini_servo_move_to(int32_t pos3600) {
         {
           int32_t velocity = m_cfg.limit_velocity;
 
-          m_bus->runInMode0(m_idinbus, m_cfg.limit_torque, m_cfg.limit_velocity, pos3600);
+          bool callsuc = false;
+          for (size_t i = 0; i < 3; i++) {
+            callsuc = m_bus->runInMode0(m_idinbus, m_cfg.limit_torque, m_cfg.limit_velocity, pos3600);
+            if (!callsuc) continue;
+            break;
+          }
+
+          if (!callsuc) {
+            creg.module_errorcode = err::ksubdevice_overtime;
+            return;
+          }
 
           int32_t moveflag = 0;
           int32_t nowpos   = 0;
@@ -125,10 +135,12 @@ int32_t MiniServoCtrlModule::mini_servo_move_to(int32_t pos3600) {
           }
         }
 
-        after_motor_move();
         ZLOGI(TAG, "move to pos %d", pos3600);
       },
-      [this]() { inter_mini_servo_stop(0); });
+      [this]() {
+        after_motor_move();
+        inter_mini_servo_stop(0);
+      });
 
   return 0;
 }
@@ -174,8 +186,8 @@ int32_t MiniServoCtrlModule::mini_servo_read_io_state(int32_t ioindex, int32_t *
 }
 
 void MiniServoCtrlModule::befor_motor_move() {
-  creg.module_status      = 1;
-  creg.module_errorcode     = 0;
+  creg.module_status    = 1;
+  creg.module_errorcode = 0;
 }
 void MiniServoCtrlModule::after_motor_move() {
   if (creg.module_errorcode != 0) {