fix some bug

components/mini_servo_motor/feite_servo_motor.cpp

@@ -14,11 +14,13 @@ using namespace feite;
   }
 
 static void dumphex(char* tag, uint8_t* data, uint8_t len) {
+#if 0
   printf("%s:", tag);
   for (int i = 0; i < len; i++) {
     printf("%02x ", data[i]);
   }
   printf("\n");
+#endif
 }
 void FeiTeServoMotor::initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hdma_rx, DMA_HandleTypeDef* hdma_tx) {
   m_uart    = uart;
@@ -268,6 +270,7 @@ bool FeiTeServoMotor::write_s16(uint8_t id, feite::reg_add_e add, uint8_t signbi
 }
 bool FeiTeServoMotor::write_reg(uint8_t id, bool async, uint8_t add, uint8_t* data, uint8_t len) {  //
   // ZLOGI(TAG, "write_reg id:%d add:%d len:%d", id, add, len);
+
   cmd_header_t*     cmd_header     = (cmd_header_t*)m_txbuf;
   receipt_header_t* receipt_header = (receipt_header_t*)m_rxbuf;
   cmd_header->header               = 0xffff;
@@ -280,6 +283,9 @@ bool FeiTeServoMotor::write_reg(uint8_t id, bool async, uint8_t add, uint8_t* da
   int txpacketlen = sizeof(cmd_header_t) + 1 + len + 1;
   int rxpacketlen = sizeof(receipt_header_t) + 1;
 
+  ZASSERT(txpacketlen < sizeof(m_txbuf));
+  ZASSERT(rxpacketlen < sizeof(m_rxbuf));
+
   uint8_t checksum         = checksum_packet((uint8_t*)cmd_header, txpacketlen);
   m_txbuf[txpacketlen - 1] = checksum;
   if (!tx_and_rx(m_txbuf, txpacketlen, m_rxbuf, rxpacketlen, OVERTIME)) {
@@ -307,6 +313,9 @@ bool FeiTeServoMotor::read_reg(uint8_t id, uint8_t add, uint8_t* data, uint8_t l
   int rxpacketlen = sizeof(receipt_header_t) + 1 + len;
 
   uint8_t checksum = checksum_packet((uint8_t*)cmd_header, txpacketlen);
+  ZASSERT(txpacketlen > 0);
+  ZASSERT(txpacketlen < sizeof(m_txbuf));
+  ZASSERT(rxpacketlen < sizeof(m_rxbuf));
 
   m_txbuf[txpacketlen - 1] = checksum;
   if (!tx_and_rx(m_txbuf, txpacketlen, m_rxbuf, rxpacketlen, OVERTIME)) {
@@ -323,6 +332,7 @@ bool FeiTeServoMotor::tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uin
   uint32_t enter_ticket = HAL_GetTick();
   dumphex("tx:", tx, txdatalen);
 
+#if 1
   HAL_UART_Transmit(m_uart, tx, txdatalen, 1000);
   HAL_UART_Receive_DMA(m_uart, (uint8_t*)rx, expectrxsize);
 
@@ -348,6 +358,7 @@ bool FeiTeServoMotor::tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uin
     return false;
   }
   return true;
+#endif
 }
 
 bool FeiTeServoMotor::readversion(uint8_t id, uint8_t& mainversion, uint8_t& subversion, uint8_t& miniserv_mainversion, uint8_t& miniserv_subversion) {

components/mini_servo_motor/feite_servo_motor.hpp

@@ -122,8 +122,8 @@ class FeiTeServoMotor {
   DMA_HandleTypeDef*  m_hdma_rx;
   DMA_HandleTypeDef*  m_hdma_tx;
 
-  uint8_t m_txbuf[128] = {0};
-  uint8_t m_rxbuf[128] = {0};
+  uint8_t m_txbuf[256] = {0};
+  uint8_t m_rxbuf[256] = {0};
 
  public:
   void initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hdma_rx, DMA_HandleTypeDef* hdma_tx);

components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp

@@ -10,7 +10,11 @@ static void limit_input(s32& input, s32 min, s32 max) {
   if (input < min) input = min;
 }
 
-void MiniRobotCtrlModule::initialize(FeiTeServoMotor* bus, uint8_t id) { m_bus->write_u8(m_id, feite::kRegServoRunMode, feite::kServoMode); }
+void MiniRobotCtrlModule::initialize(FeiTeServoMotor* bus, uint8_t id) {
+  m_bus = bus;
+  m_id  = id;
+  m_bus->write_u8(m_id, feite::kRegServoRunMode, feite::kServoMode);
+}
 
 int32_t MiniRobotCtrlModule::enable(u8 enable) {
   bool suc = m_bus->write_u8(m_id, feite::reg_add_e::kRegServoTorqueSwitch, enable);