v1023| ztmc5130 添加 等待IC上电代码,添加电机驱动相关的错误检查代码

a8000_protocol

@@ -1 +1 @@
-Subproject commit 5fb4985d0c6cae59ab4f00a1de65910938bca096
+Subproject commit 49fd379cc25952455fcf735c2e57f8937229aefb

sdk/components/pipette_module/pipette_ctrl_module_v2.cpp

@@ -200,14 +200,13 @@ int32_t PipetteModuleV2::module_xxx_reg(int32_t param_id, bool read, int32_t &va
   }
   m_config_update = false;
   return retval;
-
 }
- int32_t PipetteModuleV2::module_reset_reg() {
+int32_t PipetteModuleV2::module_reset_reg() {
   ZLOGI(TAG, "module_reset_reg");
   m_cfg = m_default_cfg;
   module_active_cfg();
   return 0;
- }
+}
 
 /***********************************************************************************************************************
  *                                                       EXT_API                                                       *
@@ -823,6 +822,14 @@ int32_t PipetteModuleV2::bfcall(int32_t cmdid, uint8_t *param, int32_t len) {
     MODULE_BFCALL_DUMP_FN_CALL(cmdid, param32, paramNum);
   }
 
+  if (!m_zm->isMotorIcInitOk()) {
+    return err::kstep_motor_subic_init_fail;
+  }
+
+  if (!m_zm->isOnline()) {
+    return err::kstep_motor_subic_offline;
+  }
+
   if (cmdid == kpipette_zmotor_move_zero) goto check_motor_is_enable;
   if (cmdid == kpipette_zmotor_move_by) goto check_motor_is_enable;
 

sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp

@@ -286,6 +286,11 @@ void StepMotorCtrlModule::setErrorFlag(int32_t ecode) { creg.module_errorcode =
 
 bool StepMotorCtrlModule::check_when_run() {
   //
+  if (!m_stepM1->isOnline()) {
+    creg.module_errorcode = err::kstep_motor_subic_offline;
+    return false;
+  }
+
   if (m_state.debugmode) return true;
 
   if (m_cfg.motor_enable_enc != 0) {
@@ -296,7 +301,7 @@ bool StepMotorCtrlModule::check_when_run() {
 
     vPortExitCritical();
 
-    int32_t dm1 = abs(m1enc - m1pos);
+    int32_t dm1       = abs(m1enc - m1pos);
     m_state.lost_step = dm1;
     if (m_cfg.pos_devi_tolerance != 0) {
       if (dm1 > m_cfg.pos_devi_tolerance) {
@@ -465,6 +470,14 @@ int32_t StepMotorCtrlModule::check_befor_run() {
     return err::kstep_motor_not_enable;
   }
 
+  if (!m_stepM1->isMotorIcInitOk()) {
+    return err::kstep_motor_subic_init_fail;
+  }
+
+  if (!m_stepM1->isOnline()) {
+    return err::kstep_motor_subic_offline;
+  }
+
   if (creg.module_errorcode == err::kstep_motor_subic_reset) {
     return creg.module_errorcode;
   } else if (creg.module_errorcode == err::kstep_motor_drv_err) {
@@ -719,9 +732,13 @@ void StepMotorCtrlModule::checkdpos(int32_t expect_dpos) {
 int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero_point_quick() {
   //
   ZLOGI(TAG, "m%d motor_move_to_zero_point_quick", m_id);
+  int32_t ecode = check_befor_run();
+  if (ecode != 0) return ecode;
+
   if (m_state.has_move_to_zero == 0) {
     return err::kstep_motor_not_move_to_zero;
   }
+
   m_thread.stop();
   creg.module_status = 1;
   m_thread.start(

sdk/components/tmc/ic/ztmc5130.cpp

@@ -30,6 +30,7 @@ void TMC51X0::initialize(cfg_t *cfg) {
   // m_port                      = config->m_port;
   m_registerAccessTable       = &tmc5130_defaultRegisterAccess[0];
   m_defaultRegisterResetState = &tmc5130_defaultRegisterResetState[0];
+
   if (cfg->csgpio != PinNull) {
     m_csnpin = new ZGPIO();
     m_csnpin->initAsOutput(cfg->csgpio, ZGPIO::kMode_nopull, false, true);
@@ -101,6 +102,22 @@ uint8_t TMC51X0::reset() {
   // zchip_clock_early_delayus(1000);
   SET_PIN(m_csnpin, true);
 
+  int icVersion = 0;
+  for (size_t i = 0; i < 50; i++) {
+    icVersion = readICVersion();
+    if (icVersion == kTMC2160AndTMC5160 || icVersion == kTMC5130) {
+      motorICInitOk = true;
+      break;
+    }
+    HAL_Delay(100);
+    ZLOGI("TMC5130", "WAITING FOR MOTOR POWER UP");
+  }
+
+  if (!motorICInitOk) {
+    ZLOGE("TMC5130", "MOTOR INIT FAIL, DRIVER_IC_IS_OFFLINE");
+    return;
+  }
+
   stop();
   enable(false);
   for (uint32_t add = 0; add < TMC5130_REGISTER_COUNT; add++) {
@@ -132,7 +149,6 @@ uint8_t TMC51X0::reset() {
   return 0;
 }
 
-
 int32_t TMC51X0::getXACTUAL() { return to_user_pos(readInt(TMC5130_XACTUAL)); }
 void    TMC51X0::setXACTUAL(int32_t value) {
   /**
@@ -148,6 +164,14 @@ void    TMC51X0::setDeceleration(float accelerationpps2) { writeInt(TMC5130_DMAX
 void    TMC51X0::setMotorShaft(bool reverse) { PRV_FIELD_WRITE(TMC5130_GCONF, TMC5130_SHAFT_MASK, TMC5130_SHAFT_SHIFT, reverse); }
 void    TMC51X0::setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) { writeInt(TMC5130_IHOLD_IRUN, (iholddelay << TMC5130_IHOLDDELAY_SHIFT) | (irun << TMC5130_IRUN_SHIFT) | (ihold << TMC5130_IHOLD_SHIFT)); }
 
+bool TMC51X0::isOnline() {
+  uint32_t chipId = readICVersion();
+  if (chipId == kTMC5130 || chipId == kTMC2160AndTMC5160) {
+    return true;
+  }
+  return false;
+}
+
 #define TMC5160_GLOBAL_SCALER       0x0B
 #define TMC5160_GLOBAL_SCALER_MASK  0xFF
 #define TMC5160_GLOBAL_SCALER_SHIFT 0

sdk/components/tmc/ic/ztmc5130.hpp

@@ -21,13 +21,13 @@ namespace iflytop {
  *
  *  1. 6点加速控制只在位置模式下有效，在速度模式下无效
  *  2. 速度模式下只有AMAX有效
- * 
- * 
- * 
+ *
+ *
+ *
  *  速度的时间基准 t:t=2^24/fclk
  *  加速度的时间基准:ta^2:ta^2=2^24/(fclk)^2
- * 
- * 
+ *
+ *
  *  VSTOP  1...(2^18)-1, 1...262144-1, 1...307
  *  VSTART 1...(2^18)-1, 1...262144-1, 1...307
  *  VEL    +-(2^23)-1 [µsteps / t]   +-8388608 +-9830.4
@@ -115,11 +115,15 @@ class TMC51X0 : public IStepperMotor {
 
   int32_t m_enc_resolution = 0;  // 编码器分辨率，默认只有在256细分的情况下有效
 
+  bool motorICInitOk = false;
+
  public:
   TMC51X0(/* args */);
 
   void initialize(cfg_t *cfg);
 
+  bool isMotorIcInitOk() { return motorICInitOk; }
+
   virtual void enable(bool enable);
 
   virtual void rotate(int32_t velocity);
@@ -132,6 +136,7 @@ class TMC51X0 : public IStepperMotor {
   virtual void    setXACTUAL(int32_t value);  // 设置当前位置
   virtual int32_t getXACTUAL();               // 当前位置
   virtual int32_t getVACTUAL();               // 当前速度
+  virtual bool    isOnline();
 
   virtual void setAcceleration(float accelerationpps2);  // 设置最大加速度
   virtual void setDeceleration(float accelerationpps2);  // 设置最大减速度
@@ -141,11 +146,10 @@ class TMC51X0 : public IStepperMotor {
   virtual void setScale(int32_t scale);
   virtual void setScaleDenominator(int32_t scale);
 
-  virtual void read_enc_val(int32_t &enc_val);
-  virtual void set_enc_val(int32_t enc_val);
+  virtual void    read_enc_val(int32_t &enc_val);
+  virtual void    set_enc_val(int32_t enc_val);
   virtual int32_t read_enc_val();
 
-
   virtual bool set_enc_resolution(int32_t enc_resolution);
   virtual void get_enc_resolution(int32_t *enc_resolution);
 

sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp

@@ -64,6 +64,22 @@ int32_t XYRobotCtrlModule::do_public_check() {
   if (!m_state.is_enable) {
     return err::kxymotor_not_enable;
   }
+
+  if (!m_stepM1->isMotorIcInitOk()) {
+    return err::kstep_motor_subic_init_fail;
+  }
+
+  if (!m_stepM1->isOnline()) {
+    return err::kstep_motor_subic_offline;
+  }
+  if (!m_stepM2->isMotorIcInitOk()) {
+    return err::kstep_motor_subic_init_fail;
+  }
+
+  if (!m_stepM2->isOnline()) {
+    return err::kstep_motor_subic_offline;
+  }
+
   return 0;
 }
 int32_t XYRobotCtrlModule::module_active_cfg() {