step motor 45 添加零点过滤逻辑

components/mini_servo_motor/feite_servo_motor.cpp

@@ -65,8 +65,9 @@ bool FeiTeServoMotor::getServoCalibration(uint8_t id, int16_t& poscalibration) {
 }
 
 run_mode_e FeiTeServoMotor::getmode(uint8_t id) {
-  uint8_t data = 0;
-  bool    suc  = read_u8(id, kRegServoRunMode, data);
+  zlock_guard l(m_mutex);
+  uint8_t     data = 0;
+  bool        suc  = read_u8(id, kRegServoRunMode, data);
   if (suc) {
     return (run_mode_e)data;
   } else {
@@ -205,6 +206,8 @@ void FeiTeServoMotor::dump_detailed_status(detailed_status_t* detailed_status) {
 bool FeiTeServoMotor::getMoveFlag(uint8_t id, uint8_t& moveflag) { return read_u8(id, kRegServoMoveFlag, moveflag); }
 
 bool FeiTeServoMotor::reCalibration(int id, int16_t pos) {
+  zlock_guard l(m_mutex);
+
   if (pos < 0 || pos > 4095) {
     ZLOGE(TAG, "reCalibration pos:%d out of range", pos);
     return false;
@@ -308,8 +311,8 @@ 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));
+  ZASSERT((size_t)txpacketlen < sizeof(m_txbuf));
+  ZASSERT((size_t)rxpacketlen < sizeof(m_rxbuf));
 
   uint8_t checksum         = checksum_packet((uint8_t*)cmd_header, txpacketlen);
   m_txbuf[txpacketlen - 1] = checksum;
@@ -341,8 +344,8 @@ bool FeiTeServoMotor::read_reg(uint8_t id, uint8_t add, uint8_t* data, uint8_t l
 
   uint8_t checksum = checksum_packet((uint8_t*)cmd_header, txpacketlen);
   ZASSERT(txpacketlen > 0);
-  ZASSERT(txpacketlen < sizeof(m_txbuf));
-  ZASSERT(rxpacketlen < sizeof(m_rxbuf));
+  ZASSERT((size_t)txpacketlen < sizeof(m_txbuf));
+  ZASSERT((size_t)rxpacketlen < sizeof(m_rxbuf));
 
   m_txbuf[txpacketlen - 1] = checksum;
   if (!tx_and_rx(m_txbuf, txpacketlen, m_rxbuf, rxpacketlen, OVERTIME)) {
@@ -359,13 +362,14 @@ 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);
 
+  // clear rx buffer
+
 #if 1
-  HAL_UART_Transmit(m_uart, tx, txdatalen, 1000);
   HAL_UART_Receive_DMA(m_uart, (uint8_t*)rx, expectrxsize);
+  HAL_UART_Transmit(m_uart, tx, txdatalen,1000);
 
   bool overtime_flag = false;
-  while (HAL_UART_GetState(m_uart) == HAL_UART_STATE_BUSY_RX ||  //
-         HAL_UART_GetState(m_uart) == HAL_UART_STATE_BUSY_TX_RX) {
+  while (HAL_UART_GetState(m_uart) != HAL_UART_STATE_READY ) {
     osDelay(1);
     int rxsize = expectrxsize - __HAL_DMA_GET_COUNTER(m_hdma_rx);
     if (rxsize == expectrxsize) {

components/step_motor_45/step_motor_45.cpp

@@ -35,6 +35,9 @@ stepmotor_control_matrix_t onestepsq2[] = {
 
 void StepMotor45::initialize(StepMotor45Scheduler* scheduler, cfg_t cfg) {
   m_cfg = cfg;
+  if (m_cfg.zerofilterCount == 0) {
+    m_cfg.zerofilterCount = 20;
+  }
 
   //   ³õʼ»¯GPIO
   ZASSERT(cfg.driverPin[0] != PinNull);
@@ -253,7 +256,16 @@ bool StepMotor45::getzeropinstate() {
     return false;
   }
   bool state = m_zeroPinZGPIO->getState();
-  return state;
+  if (nowiostate != state) {
+    m_io_state_filter_count++;
+    if (m_io_state_filter_count > m_cfg.zerofilterCount) {
+      nowiostate              = state;
+      m_io_state_filter_count = 0;
+    }
+  } else {
+    m_io_state_filter_count = 0;
+  }
+  return nowiostate;
 }
 void StepMotor45::setdriverpinstate(bool s1, bool s2, bool s3, bool s4) {
   // ZLOGI(TAG, "setdriverpinstate %d%d%d%d", s1, s2, s3, s4);

components/step_motor_45/step_motor_45.hpp

@@ -25,9 +25,10 @@ class StepMotor45 {
     bool enable_max_pos_limit = false;      // 是否启用最大位置限位
     bool mirror               = false;      // 是否镜像
 
-    Pin_t             zeroPin       = PinNull;
-    ZGPIO::GPIOMode_t ioPollType    = ZGPIO::kMode_nopull;
-    bool              zeroPinMirror = false;
+    Pin_t             zeroPin         = PinNull;
+    ZGPIO::GPIOMode_t ioPollType      = ZGPIO::kMode_nopull;
+    bool              zeroPinMirror   = false;
+    int               zerofilterCount = 0;
 
     Pin_t driverPin[4]    = {PinNull, PinNull, PinNull, PinNull};
     bool  driverPinMirror = false;
@@ -41,7 +42,10 @@ class StepMotor45 {
   bool    m_lastzeropinstate;  // 上一次零点限位状态
   int     m_acceleration;      // 加速度
 
-  bool posmode = false;
+  int m_io_state_filter_count = 0;
+
+  bool posmode    = false;
+  bool nowiostate = false;
   int  m_targetPos;
   /**
    * @brief 控制

components/step_motor_45/step_motor_45_scheduler.cpp

@@ -3,13 +3,13 @@
 #include "step_motor_45.hpp"
 using namespace iflytop;
 
-void StepMotor45Scheduler::initialize(zchip_tim_t* tim, int freq) {
+void StepMotor45Scheduler::initialize(zchip_tim_t* tim) {
   m_htim = tim;
 
   uint32_t prescaler  = 0;
   uint32_t autoreload = 0;
 
-  ZASSERT(chip_calculate_prescaler_and_autoreload_by_expect_freq(chip_get_timer_clock_sorce_freq(m_htim), freq, &prescaler, &autoreload));
+  ZASSERT(chip_calculate_prescaler_and_autoreload_by_expect_freq(chip_get_timer_clock_sorce_freq(m_htim), 1000, &prescaler, &autoreload));
 
   HAL_TIM_Base_DeInit(m_htim);
   m_htim->Init.Prescaler         = prescaler;

components/step_motor_45/step_motor_45_scheduler.hpp

@@ -12,7 +12,7 @@ class StepMotor45Scheduler {
   list<StepMotor45*> m_motor_list;
 
  public:
-  void initialize(zchip_tim_t* tim, int freq);
+  void initialize(zchip_tim_t* tim);
   void addMotor(StepMotor45* motor);
 
   void start();