Merge branch 'master' of 192.168.1.3:manufacturer_stm32/iflytop_stm32_os_sdk

chip/zgpio.cpp

@@ -119,9 +119,6 @@ bool ZGPIO::enableClock() {
   return false;
 }
 
-
-
-
 void regIRQGPIO(ZGPIO *gpio) {
   for (int i = 0; i < s_irqGPIO_num; i++) {
     if (s_irqGPIO[i] == gpio) {
@@ -144,27 +141,36 @@ void ZGPIO::initAsInput(Pin_t pin, GPIOMode_t mode, GPIOIrqType_t irqtype, bool
   m_gpio   = chip_get_gpio(pin);
   m_pinoff = chip_get_pinoff(pin);
 
+  uint32_t pulluptype = 0;
+  if (mode == kMode_nopull) {
+    pulluptype = GPIO_NOPULL;
+  } else if (mode == kMode_pullup) {
+    pulluptype = GPIO_PULLUP;
+  } else if (mode == kMode_pulldown) {
+    pulluptype = GPIO_PULLDOWN;
+  }
+
   enableClock();
   GPIO_InitTypeDef m_GPIO_InitStruct = {0};
   if (m_irqtype == kIRQ_noIrq) {
     m_GPIO_InitStruct.Pin   = m_pinoff;
     m_GPIO_InitStruct.Mode  = GPIO_MODE_INPUT;
-    m_GPIO_InitStruct.Pull  = 0;
+    m_GPIO_InitStruct.Pull  = pulluptype;
     m_GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
   } else if (m_irqtype == kIRQ_risingIrq) {
     m_GPIO_InitStruct.Pin   = m_pinoff;
     m_GPIO_InitStruct.Mode  = m_mirror ? GPIO_MODE_IT_FALLING : GPIO_MODE_IT_RISING;
-    m_GPIO_InitStruct.Pull  = 0;
+    m_GPIO_InitStruct.Pull  = pulluptype;
     m_GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
   } else if (m_irqtype == kIRQ_fallingIrq) {
     m_GPIO_InitStruct.Pin   = m_pinoff;
     m_GPIO_InitStruct.Mode  = !m_mirror ? GPIO_MODE_IT_FALLING : GPIO_MODE_IT_RISING;
-    m_GPIO_InitStruct.Pull  = 0;
+    m_GPIO_InitStruct.Pull  = pulluptype;
     m_GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
   } else if (m_irqtype == kIRQ_risingAndFallingIrq) {
     m_GPIO_InitStruct.Pin   = m_pinoff;
     m_GPIO_InitStruct.Mode  = GPIO_MODE_IT_RISING_FALLING;
-    m_GPIO_InitStruct.Pull  = 0;
+    m_GPIO_InitStruct.Pull  = pulluptype;
     m_GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
   }
 

components/cmdscheduler/cmd_scheduler.cpp

@@ -7,7 +7,7 @@
 #include "sdk\components\zprotocols\errorcode\errorcode.hpp"
 using namespace iflytop;
 
-#define TAG "CmdScheduler"
+#define TAG "CMD"
 
 void CmdScheduler::registerCmd(std::string cmd, const char* helpinfo, int npara, call_cmd_t call_cmd) {
   CMD cmdinfo;
@@ -19,13 +19,17 @@ void CmdScheduler::registerCmd(std::string cmd, const char* helpinfo, int npara,
 
 void CmdScheduler::regbasiccmd() {
   this->registerCmd("help", "", 0, [this](Context* context) {
-    ZLOGI(TAG, "help");
     ZLOGI(TAG, "cmdlist:");
     for (auto it = m_cmdMap.begin(); it != m_cmdMap.end(); it++) {
       ZLOGI(TAG, "  %s %s", it->first.c_str(), it->second.help_info.c_str());
     }
     return (int32_t)0;
   });
+  this->registerCmd("sleep_ms", "(ms)", 1, [this](Context* context) {
+    int ms = atoi(context->argv[1]);
+    osDelay(ms);
+    return 0;
+  });
 }
 
 void CmdScheduler::initialize(UART_HandleTypeDef* huart, uint32_t rxbufsize) {

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;
@@ -212,7 +214,7 @@ bool FeiTeServoMotor::reCalibration(int id, int16_t pos) {
   int16_t nowpos;
   DO(getNowPos(id, nowpos));
   ZLOGI(TAG, "reCalibration id:%d nowpos:%d:%d", id, nowpos, pos);
-  DO(setTargetPos(id, pos));
+  DO(setTargetPos(id, nowpos));
   return true;
 }
 
@@ -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);
 
@@ -345,9 +355,11 @@ bool FeiTeServoMotor::tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uin
   }
   HAL_UART_DMAStop(m_uart);
   if (overtime_flag) {
+    // ZLOGE(TAG, "txandrx overtime");
     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,12 @@ 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);
+  m_thread.init("MiniRobotCtrlModule", 1024, osPriorityNormal);
+}
 
 int32_t MiniRobotCtrlModule::enable(u8 enable) {
   bool suc = m_bus->write_u8(m_id, feite::reg_add_e::kRegServoTorqueSwitch, enable);
@@ -35,7 +40,7 @@ int32_t MiniRobotCtrlModule::stop(u8 stop_type) {
 int32_t MiniRobotCtrlModule::position_calibrate(s32 nowpos) {
   if (nowpos < 0 || nowpos > 4095) return err::kce_param_out_of_range;
   if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime;
-  if (m_bus->reCalibration(m_id, nowpos)) return err::kce_subdevice_overtime;
+  if (!m_bus->reCalibration(m_id, nowpos)) return err::kce_subdevice_overtime;
   return 0;
 }
 int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 torque, s32 run_time, action_cb_status_t status_cb) {

components/step_motor_45/script_cmder_step_motor_45.cpp

@@ -46,6 +46,7 @@ void ScriptCmderStepMotor45::regcmd() {
   REG_CMD___NO_ACK(PREFIX, zero_calibration, "(id)", 1);
   REG_CMD_WITH_ACK(PREFIX, get_pos, "(id)", 1, int32_t, ack);
   REG_CMD___NO_ACK(PREFIX, set_pos, "(id,pos)", 2, con->getInt(2));
+  REG_CMD_WITH_ACK(PREFIX, get_zero_pin_state, "(id,pos)", 1, int32_t, ack);
 
   // REG_CMD___NO_ACK(PREFIX, initialize, "(id,driverPin1,driverPin2,driverPin3,driverPin4,zeroPin)", 6, con->getInt(2), con->getInt(3), con->getInt(4), con->getInt(5), con->getInt(6), con->getInt(7));
 }

components/step_motor_45/step_motor_45.cpp

@@ -33,7 +33,7 @@ stepmotor_control_matrix_t onestepsq2[] = {
 
 #define MAX_STEP_TABLE_OFF (ZARRAY_SIZE(onestepsq1) - 1)
 
-void StepMotor45::initialize(cfg_t cfg) {
+void StepMotor45::initialize(StepMotor45Scheduler* scheduler, cfg_t cfg) {
   m_cfg = cfg;
 
   //   初始化GPIO
@@ -51,9 +51,10 @@ void StepMotor45::initialize(cfg_t cfg) {
   if (cfg.zeroPin != PinNull) {
     m_zeroPinZGPIO = new ZGPIO();
     ZASSERT(m_zeroPinZGPIO != NULL);
-    m_zeroPinZGPIO->initAsInput(cfg.zeroPin, ZGPIO::kMode_pullup, ZGPIO::kIRQ_noIrq, cfg.zeroPinMirror);
+    m_zeroPinZGPIO->initAsInput(cfg.zeroPin, cfg.ioPollType, ZGPIO::kIRQ_noIrq, cfg.zeroPinMirror);
   }
   setdriverpinstate(1, 1, 1, 1);
+  scheduler->addMotor(this);
   return;
 }
 
@@ -82,11 +83,14 @@ int32_t StepMotor45::stop() {
 }
 int32_t StepMotor45::zero_calibration() {
   CriticalContext cc;
+  // ZLOGI(TAG, "zero_calibration1 %d", getzeropinstate());
   if (getzeropinstate()) {
+    ZLOGI(TAG, "zero_calibration2");
     m_calibration = true;
     m_pos         = 0;
     return (int32_t)0;
   }
+  // ZLOGI(TAG, "zero_calibration");
   m_exec_zero_calibration_task = true;
   m_state                      = krollback;
   return (int32_t)0;
@@ -95,9 +99,19 @@ int32_t StepMotor45::zero_calibration() {
 void StepMotor45::onTimIRQ1ms() {
   CriticalContext cc;
   if (m_state == kstop) {
+    m_lastzeropinstate = getzeropinstate();
     return;
+  } else {
+    schedule();
   }
-  schedule();
+}
+
+int32_t StepMotor45::get_zero_pin_state(int32_t& state) {
+  CriticalContext cc;
+  state = getzeropinstate();
+  // ZLOGI(TAG, "zero_calibration1 %d", getzeropinstate());
+
+  return (int32_t)0;
 }
 
 void StepMotor45::schedule() {
@@ -116,57 +130,48 @@ void StepMotor45::schedule() {
     m_lastzeropinstate = iostate;
   }
 
-  if ((detect_rising_edge && m_state == krollback) || (detect_falling_edge && m_state == kforward_rotation)) {
-    /**
-     * @brief 触发零点
-     */
-    m_pos         = 0;
-    m_calibration = true;
-
-    if (m_exec_zero_calibration_task) {
+  if (m_exec_zero_calibration_task) {
+    if ((detect_rising_edge && m_state == krollback) || (detect_falling_edge && m_state == kforward_rotation)) {
+      m_pos                        = 0;
+      m_calibration                = true;
       m_exec_zero_calibration_task = 0;
       stop_motor();
       return;
     }
-  }
-
-  /**
-   * @brief 零点限位触发，电机停止运行
-   */
-  if (m_cfg.enable_zero_limit && m_state == krollback) {
-    if (iostate) {
-      stop_motor();
-      return;
+  } else {
+    // 触发零点
+    if ((detect_rising_edge && m_state == krollback) || (detect_falling_edge && m_state == kforward_rotation)) {
+      m_pos         = 0;
+      m_calibration = true;
     }
-  }
 
-  /**
-   * @brief 触发最大位置限位，电机停止运行
-   */
-  if (m_cfg.enable_max_pos_limit && m_state == kforward_rotation) {
-    if (m_pos >= m_cfg.max_pos) {
-      stop_motor();
-      return;
+    // 零点限位触发，电机停止运行
+    if (m_cfg.enable_zero_limit && m_state == krollback) {
+      if (iostate) {
+        stop_motor();
+        return;
+      }
     }
-  }
-
-  /**
-   * @brief 如果使能了最大位置限位，且电机没有校准过，则电机不允许正转
-   */
-  if (m_cfg.enable_max_pos_limit && !m_calibration) {
-    if (m_state == kforward_rotation) {
-      return;
+    // 触发最大位置限位，电机停止运行
+    if (m_cfg.enable_max_pos_limit && m_state == kforward_rotation) {
+      if (m_pos >= m_cfg.max_pos) {
+        stop_motor();
+        return;
+      }
     }
-  }
 
-  /**
-   * @brief 如果运行到目标位置则停止
-   */
-
-  if (posmode) {
-    if (m_pos == m_targetPos) {
-      stop_motor();
-      return;
+    // 如果使能了最大位置限位，且电机没有校准过，则电机不允许正转
+    if (m_cfg.enable_max_pos_limit && !m_calibration) {
+      if (m_state == kforward_rotation) {
+        return;
+      }
+    }
+    // 如果运行到目标位置则停止
+    if (posmode) {
+      if (m_pos == m_targetPos) {
+        stop_motor();
+        return;
+      }
     }
   }
 
@@ -221,7 +226,7 @@ int32_t StepMotor45::move_to(int to, int speed) {
   CriticalContext cc;
 
   if (to == m_pos) {
-	  return (int32_t)0;
+    return (int32_t)0;
   }
 
   if (to <= m_pos) {

components/step_motor_45/step_motor_45.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "sdk/os/zos.hpp"
+#include "sdk\components\step_motor_45\step_motor_45_scheduler.hpp"
 
 namespace iflytop {
 using namespace std;
@@ -24,8 +25,9 @@ class StepMotor45 {
     bool enable_max_pos_limit = false;      // 是否启用最大位置限位
     bool mirror               = false;      // 是否镜像
 
-    Pin_t zeroPin       = PinNull;
-    bool  zeroPinMirror = false;
+    Pin_t             zeroPin       = PinNull;
+    ZGPIO::GPIOMode_t ioPollType    = ZGPIO::kMode_nopull;
+    bool              zeroPinMirror = false;
 
     Pin_t driverPin[4]    = {PinNull, PinNull, PinNull, PinNull};
     bool  driverPinMirror = false;
@@ -59,7 +61,7 @@ class StepMotor45 {
   int defaultspeed = 1000;
 
  public:
-  void initialize(cfg_t cfg);
+  void initialize(StepMotor45Scheduler *scheduler, cfg_t cfg);
 
   int32_t rotate(int direction) { rotate(direction, defaultspeed); }
   int32_t rotate(int direction, int speed);
@@ -78,6 +80,8 @@ class StepMotor45 {
   int32_t zero_calibration();
   int32_t set_pos(int32_t pos) { m_pos = pos; }
 
+  int32_t get_zero_pin_state(int32_t &state);
+
   int32_t get_pos(int32_t &pos) {
     pos = m_pos;
     return (int32_t)0;

components/step_motor_45/step_motor_45_scheduler.cpp

@@ -1,5 +1,6 @@
 #include "step_motor_45_scheduler.hpp"
 
+#include "step_motor_45.hpp"
 using namespace iflytop;
 
 void StepMotor45Scheduler::initialize(zchip_tim_t* tim, int freq) {

components/step_motor_45/step_motor_45_scheduler.hpp

@@ -1,10 +1,10 @@
 #pragma once
 
 #include "sdk/os/zos.hpp"
-#include "step_motor_45.hpp"
+// #include "step_motor_45.hpp"
 namespace iflytop {
 using namespace std;
-
+class StepMotor45;
 class StepMotor45Scheduler {
  public:
   zchip_tim_t* m_htim;

components/tmc/ic/ztmc5130.cpp

@@ -209,8 +209,6 @@ int32_t TMC5130::readInt(uint8_t address) {
 
   return ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) | (data[3] << 8) | data[4];
 }
-#endif
-
 int32_t TMC5130::to_motor_acc(int32_t acc) {  //
   int32_t val = acc / 60.0 * 51200;
   return val;
@@ -231,4 +229,6 @@ int32_t TMC5130::to_user_pos(int32_t pos) {  //
 int32_t TMC5130::to_user_vel(int32_t vel) {  //
   int32_t val = vel * 60.0 / 51200.0;
   return val;
-}
+}
+#endif
+

components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp

@@ -84,12 +84,12 @@ void XYRobotScriptCmderModule::regcmd() {  //
   REG_CMD_WITH_ACK(PREIX, read_status, "(id)", 1, I_XYRobotCtrlModule::status_t, ack);
   REG_CMD_WITH_ACK(PREIX, read_detailed_status, "(id)", 1, I_XYRobotCtrlModule::detailed_status_t, ack);
   REG_CMD_WITH_ACK(PREIX, get_base_param, "(id)", 1, I_XYRobotCtrlModule::base_param_t, ack);
-  REG_CMD_WITH_ACK("xy_robot_ctrl_", read_detailed_status, "(id)", 1, I_XYRobotCtrlModule::detailed_status_t, ack);
-  REG_CMD_WITH_ACK("xy_robot_ctrl_", read_status, "(id)", 1, I_XYRobotCtrlModule::status_t, ack);
-  REG_CMD_WITH_ACK("xy_robot_ctrl_", read_version, "(id)", 1, I_XYRobotCtrlModule::version_t, ack);
-  REG_CMD___NO_ACK("xy_robot_ctrl_", flush, "(id)", 1);
-  REG_CMD___NO_ACK("xy_robot_ctrl_", factory_reset, "(id)", 1);
-  REG_CMD___NO_ACK("xy_robot_ctrl_", move_by_no_limit, "(id,dx,dy,v)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_by_no_limit status:%d", status); });
+  REG_CMD_WITH_ACK(PREIX, read_detailed_status, "(id)", 1, I_XYRobotCtrlModule::detailed_status_t, ack);
+  REG_CMD_WITH_ACK(PREIX, read_status, "(id)", 1, I_XYRobotCtrlModule::status_t, ack);
+  REG_CMD_WITH_ACK(PREIX, read_version, "(id)", 1, I_XYRobotCtrlModule::version_t, ack);
+  REG_CMD___NO_ACK(PREIX, flush, "(id)", 1);
+  REG_CMD___NO_ACK(PREIX, factory_reset, "(id)", 1);
+  REG_CMD___NO_ACK(PREIX, move_by_no_limit, "(id,dx,dy,v)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_by_no_limit status:%d", status); });
 
   m_cmdScheduler->registerCmd("xy_robot_ctrl_set_base_param", "(id,paramName,val)", 3, [this](CmdScheduler::Context* con) {
     const char* paramName = con->getString(2);