update

2 years ago · 389df1c6f8
5 changed files with 464 additions and 0 deletions
--- a/components/single_axis_motor_control/zcr_single_axis_motor_controler.cpp
+++ b/components/single_axis_motor_control/zcr_single_axis_motor_controler.cpp
--- a/components/single_axis_motor_control/zcr_single_axis_motor_controler.hpp
+++ b/components/single_axis_motor_control/zcr_single_axis_motor_controler.hpp
@ -0,0 +1,130 @@
+#pragma once
+#include "sdk/hal/zhal.hpp"
+#ifdef HAL_CAN_MODULE_ENABLED
+#include <stdlib.h>
+
+#include "sdk/components/step_action_scheduler/step_scheduler.hpp"
+#include "sdk\components\iflytop_can_slave_v1\iflytop_can_slave.hpp"
+#include "sdk\components\tmc\basic\tmc_ic_interface.hpp"
+
+/**
+ * @brief
+ *
+ * service: SingleAxisMotorControler
+ *
+ */
+#define SINGLE_AXIS_MOTOR_CTRL_REG_NUM 99
+
+#define SAMC_REG_ACT_ROTATE          (0)  // 速度模式控制
+#define SAMC_REG_ACT_MOVE_TO         (1)  // 位置模式控制
+#define SAMC_REG_ACT_MOVE_BY         (2)  // 相对位置模式控制
+#define SAMC_REG_ACT_RUN_TO_HOME     (3)  // 归零
+#define SAMC_REG_ACT_DO_NOTHING      (4)  // __
+#define SAMC_REG_ACT_STOP            (8)  // 停止
+#define SAMC_REG_ACT_CLEAR_EXCEPTION (9)  // 清空异常
+
+namespace iflytop {
+using namespace std;
+
+class SingleAxisMotorControler : public ICPSListener {
+ public:
+  typedef enum {
+    kstate_idle      = 0,
+    kstate_rotate    = 1,
+    kstate_moveTo    = 2,
+    kstate_moveBy    = 3,
+    kstate_runToHome = 4,
+    kstate_exception = 65535,
+  } state_t;
+
+  typedef enum {
+    kexcep_motorBlocking = 1,
+    kexcep_loseZeroPoint = 2,
+  } exception_id_t;
+
+  class RunToHomeContext {
+   public:
+    bool    stopByIrq = false;
+    int32_t zeroPointPos;
+  };
+
+ private:
+  const char*                       m_name;
+  IflytopCanProtocolStackProcesser* m_processer;
+  int                               m_regaddoff;
+  // res
+  ZGPIO*         m_homegpio;
+  ZGPIO*         m_maxlimitgpio;
+  ZGPIO*         m_minlimitgpio;
+  IStepperMotor* m_motor;
+
+  ICPSSlaveModule* m_slave;
+
+  state_t m_state = kstate_idle;
+
+  icps::Reg_t* m_statusReg;
+  icps::Reg_t* m_currentVelocityReg;
+  icps::Reg_t* m_currentPosReg;
+  icps::Reg_t* m_exceptionReg;
+
+  RunToHomeContext m_runToHomeContext;
+
+  StepActionScheduler m_step_scheduler;
+
+  ZGPIO::onirq_t m_gpioirqprocesser;
+
+ public:
+  icps::Reg_t* cfg_acc;
+  icps::Reg_t* cfg_dec;
+  icps::Reg_t* cfg_velocity;
+  icps::Reg_t* cfg_zero_shift;
+  icps::Reg_t* cfg_runhome_velocity;
+  icps::Reg_t* cfg_runtohome_dec;
+  icps::Reg_t* cfg_runtohome_max_distance;
+  icps::Reg_t* cfg_runtohome_leave_zero_point_distance;
+  icps::Reg_t* cfg_min_pos;
+  icps::Reg_t* cfg_max_pos;
+  icps::Reg_t* cfg_runtohome_keep_move_distance;
+
+ public:
+  SingleAxisMotorControler(){};
+  virtual ~SingleAxisMotorControler(){};
+
+  void initialize(const char* name, IflytopCanProtocolStackProcesser* processer, int regaddoff,  //
+                  ZGPIO*         homegpio,                                                       //
+                  ZGPIO*         mingpio,                                                        //
+                  ZGPIO*         rightgpio,                                                      //
+                  IStepperMotor* motor);
+
+ public:
+  virtual icps::error_t onHostRegisterWriteEvent(icps::WriteEvent* event);
+
+  icps::error_t rotate(int32_t velocity);
+  icps::error_t moveTo(int32_t pos);
+  icps::error_t moveBy(int32_t pos);
+  icps::error_t runToHome();
+  icps::error_t stop();
+  icps::error_t clearException();
+
+ private:
+  void motor_moveTo(int32_t pos, int32_t velocity, int32_t acc, int32_t dec);
+  void motor_moveBy(int32_t pos, int32_t velocity, int32_t acc, int32_t dec);
+  void motor_rotate(int32_t velocity, int32_t acc, int32_t dec);
+
+  void periodJob(ZHALCORE::Context* context);
+  void changeToState(state_t state);
+
+ private:
+  bool motorIsStop(StepActionContext* context);
+  void regGpioIrqProcesser(ZGPIO::onirq_t irqprocesser) {
+    CriticalContext cc;
+    m_gpioirqprocesser = irqprocesser;
+  }
+  void clearGpioIrqProcesser() {
+    CriticalContext cc;
+    m_gpioirqprocesser = NULL;
+  }
+};
+
+}  // namespace iflytop
+#endif
--- a/components/zcanreceiver/cmd.hpp
+++ b/components/zcanreceiver/cmd.hpp
@ -0,0 +1,19 @@
+#pragma once
+#include "sdk/hal/zhal.hpp"
+
+namespace iflytop {
+namespace zcr {
+
+typedef struct {
+  uint16_t packetindex;
+  uint16_t cmdindex;
+  uint8_t  subcmdindex;
+  uint16_t var0;
+} cmdheader_t;
+
+}  // namespace zcr
+}  // namespace iflytop
+
+/**
+ * @brief 
+ */
--- a/components/zcanreceiver/zcanreceiver.cpp
+++ b/components/zcanreceiver/zcanreceiver.cpp
@ -0,0 +1,225 @@
+#include "zcanreceiver.hpp"
+
+#ifdef HAL_CAN_MODULE_ENABLED
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+using namespace iflytop;
+
+#define TAG "ZCanReceiver"
+
+#define OVER_TIME_MS 5
+
+ZCanReceiver::CFG *ZCanReceiver::createCFG(uint8_t deviceId) {
+  CFG *cfg = new CFG();
+  ZASSERT(cfg != NULL);
+  cfg->deviceId = deviceId;
+#ifdef STM32F103xB
+  cfg->canHandle = &hcan;
+#else
+  cfg->canHandle = &hcan1;
+#endif
+  cfg->canFilterIndex0 = 0;
+  cfg->maxFilterNum    = 7;
+  cfg->rxfifoNum       = CAN_RX_FIFO0;
+  return cfg;
+}
+void ZCanReceiver::init(CFG *cfg) {
+  HAL_StatusTypeDef hal_status;
+  m_config = cfg;
+
+  /**
+   * @brief 初始化CAN
+   */
+
+  /**
+   * @brief 初始化消息接收buf
+   */
+  m_canPacketRxBuffer[0].dataIsReady    = false;
+  m_canPacketRxBuffer[0].id             = 1;  // 只接收来自主机的消息
+  m_canPacketRxBuffer[0].m_canPacketNum = 0;
+
+  /**
+   * @brief 初始化过滤器
+   */
+  hal_status = initializeFilter();
+  if (hal_status != HAL_OK) {
+    ZLOGE(TAG, "start can initializeFilter fail\r\n");
+    return;
+  }
+  /**
+   * @brief 启动CAN
+   */
+  hal_status = HAL_CAN_Start(m_config->canHandle);  // 开启CAN
+  if (hal_status != HAL_OK) {
+    ZLOGE(TAG, "start can fail\r\n");
+    return;
+  }
+  /**
+   * @brief 监听回调
+   */
+  ZCanIRQDispatcher::instance().regListener(this);
+  HAL_StatusTypeDef status = activateRxIT();
+  if (status != HAL_OK) {
+    ZLOGE(TAG, "activateRxIT fail\r\n");
+    return;
+  }
+  ZHALCORE::getInstance()->regPeriodJob([this](ZHALCORE::Context &context) { loop(); }, 0);
+}
+HAL_StatusTypeDef ZCanReceiver::initializeFilter() {
+  /**
+   * @brief ID区帧格式
+   *  [                27:0                  ]
+   *  [    STDID        ] [      EXTID       ]
+   *  [11 :9] [8:6] [5:0] [17:16] [15:8] [7:0]
+   *   优先级  属性     帧类型      目标ID  源ID
+   */
+  HAL_StatusTypeDef HAL_Status;
+  CAN_FilterTypeDef sFilterConfig;
+
+  uint32_t filterId;
+  uint32_t mask;
+
+  memset(&sFilterConfig, 0, sizeof(sFilterConfig));
+  sFilterConfig.FilterMode           = CAN_FILTERMODE_IDMASK;   // 设为MASK模式
+  sFilterConfig.FilterScale          = CAN_FILTERSCALE_32BIT;   // CAN_FILTERSCALE_16BIT
+  sFilterConfig.FilterFIFOAssignment = m_config->rxfifoNum;     // 关联过滤器到rxfifoNum
+  sFilterConfig.FilterActivation     = ENABLE;                  // 激活过滤器
+  sFilterConfig.SlaveStartFilterBank = m_config->maxFilterNum;  // slave filter start index
+
+  /*******************************************************************************
+   *                        接收所有消息                                          *
+   *******************************************************************************/
+  filterId                       = (0);                            //
+  mask                           = (0);                            //
+  sFilterConfig.FilterBank       = m_config->canFilterIndex0;      //
+  sFilterConfig.FilterMaskIdLow  = mask & 0xffff;                  //
+  sFilterConfig.FilterMaskIdHigh = (mask & 0xffff0000) >> 16;      //
+  sFilterConfig.FilterIdLow      = filterId & 0xffff;              //
+  sFilterConfig.FilterIdHigh     = (filterId & 0xffff0000) >> 16;  //
+  HAL_Status                     = HAL_CAN_ConfigFilter(m_config->canHandle, &sFilterConfig);
+  if (HAL_Status != HAL_OK) {
+    ZLOGE(TAG, "HAL_CAN_ConfigFilter filter0 fail");
+    return HAL_Status;
+  }
+  ZLOGI(TAG, "HAL_CAN_ConfigFilter filterID1 %08x", filterId >> 3);
+  return HAL_Status;
+}
+
+void ZCanReceiver::registerListener(ZCanRceiverListener *listener) { m_listenerList.push_back(listener); }
+void ZCanReceiver::sendPacket(uint8_t *packet, size_t len) {}
+bool ZCanReceiver::getRxMessage(CAN_RxHeaderTypeDef *pHeader, uint8_t aData[] /*8byte table*/) {
+  /**
+   * @brief 读取当前FIFO中缓存了多少帧的数据
+   */
+  uint32_t level = HAL_CAN_GetRxFifoFillLevel(m_config->canHandle, m_config->rxfifoNum);
+  if (level == 0) {
+    return false;
+  }
+  HAL_StatusTypeDef HAL_RetVal;
+  HAL_RetVal = HAL_CAN_GetRxMessage(m_config->canHandle, m_config->rxfifoNum, pHeader, aData);
+  if (HAL_OK == HAL_RetVal) {
+    // 处理接收到的can总线数据
+    return true;
+  }
+  return false;
+}
+void ZCanReceiver::STM32_HAL_onCAN_RxFifo0MsgPending(CAN_HandleTypeDef *canHandle) {
+  /**
+   * @brief 中断上下文
+   */
+  // ZLOG_INFO("%s\n", __FUNCTION__);
+  // printf("------------------%s\n", __FUNCTION__);
+  if (canHandle != m_config->canHandle) {
+    return;
+  }
+  /**
+   * @brief 处理can接收到消息
+   */
+  CAN_RxHeaderTypeDef pHeader;
+  uint8_t             aData[8] /*8byte table*/;
+  while (getRxMessage(&pHeader, aData)) {
+    /**
+     * @brief 消息格式
+     *
+     * [2] [3bit]     [8bit]   [8bit]     [8bit]
+     *          ,      from   frameNum    frameId
+     */
+    uint8_t            from    = (pHeader.ExtId >> 16 & 0xFF);
+    uint8_t            nframe  = (pHeader.ExtId & 0xFF00) >> 8;
+    uint8_t            frameId = (pHeader.ExtId & 0x00FF);
+    CanPacketRxBuffer *rxbuf   = &m_canPacketRxBuffer[0];
+    if (from != rxbuf->id) {
+      // 目前只接收来自主机的消息
+      continue;
+    }
+
+    if (rxbuf->dataIsReady) {
+      // 上次接收到的消息还没有来的急处理
+      continue;
+    }
+    /**
+     * @TODO:判断是否丢包
+     */
+    if (frameId == 0) {
+      rxbuf->m_canPacketNum = 0;
+    }
+
+    if (rxbuf->m_canPacketNum < 255) {
+      rxbuf->m_canPacket[rxbuf->m_canPacketNum].pHeader = pHeader;
+      memcpy(rxbuf->m_canPacket[rxbuf->m_canPacketNum].aData, aData, 8);
+      rxbuf->m_canPacketNum++;
+    }
+    if (nframe == frameId) {
+      rxbuf->dataIsReady = true;
+    }
+  }
+
+  //   deactivateRxIT();
+}
+void ZCanReceiver::STM32_HAL_onCAN_Error(CAN_HandleTypeDef *canHandle) {
+  if (canHandle != m_config->canHandle) {
+    return;
+  }
+  ZLOGE(TAG, "onCAN_Error\r\n");
+}
+void ZCanReceiver::loop() {
+  CanPacketRxBuffer *rxbuf = &m_canPacketRxBuffer[0];
+  if (rxbuf->dataIsReady) {
+    int dataoff = 0;
+    for (size_t i = 0; i < rxbuf->m_canPacketNum; i++) {
+      memcpy(rxdata + dataoff, rxbuf->m_canPacket[i].aData, rxbuf->m_canPacket[i].pHeader.DLC);
+      dataoff += rxbuf->m_canPacket[i].pHeader.DLC;
+    }
+    for (auto &var : m_listenerList) {
+      var->onRceivePacket(rxbuf, rxdata, dataoff);
+    }
+    rxbuf->dataIsReady = false;
+  }
+}
+HAL_StatusTypeDef ZCanReceiver::activateRxIT() {
+  HAL_StatusTypeDef hal_status = HAL_ERROR;
+  if (m_config->rxfifoNum == CAN_RX_FIFO0) {
+    hal_status = HAL_CAN_ActivateNotification(m_config->canHandle, CAN_IT_RX_FIFO0_MSG_PENDING);
+  } else if (m_config->rxfifoNum == CAN_RX_FIFO1) {
+    hal_status = HAL_CAN_ActivateNotification(m_config->canHandle, CAN_IT_RX_FIFO1_MSG_PENDING);
+  } else {
+    ZLOGE(TAG, "start can HAL_CAN_ActivateNotification CAN_IT_RX_FIFO0_MSG_PENDING fail\r\n");
+    return hal_status;
+  }
+  return hal_status;
+}
+HAL_StatusTypeDef ZCanReceiver::deactivateRxIT() {
+  HAL_StatusTypeDef hal_status = HAL_ERROR;
+  if (m_config->rxfifoNum == CAN_RX_FIFO0) {
+    hal_status = HAL_CAN_DeactivateNotification(m_config->canHandle, CAN_IT_RX_FIFO0_MSG_PENDING);
+  } else if (m_config->rxfifoNum == CAN_RX_FIFO1) {
+    hal_status = HAL_CAN_DeactivateNotification(m_config->canHandle, CAN_IT_RX_FIFO1_MSG_PENDING);
+  } else {
+    ZLOGE(TAG, "start can HAL_CAN_ActivateNotification CAN_IT_RX_FIFO0_MSG_PENDING fail\r\n");
+    return hal_status;
+  }
+  return hal_status;
+}
+
+#endif
--- a/components/zcanreceiver/zcanreceiver.hpp
+++ b/components/zcanreceiver/zcanreceiver.hpp
@ -0,0 +1,90 @@
+//
+// Created by zwsd
+//
+
+#pragma once
+#include "sdk/hal/zhal.hpp"
+
+namespace iflytop {
+namespace zcr {
+typedef enum {
+  kpacketHeader = 0,
+  kpacketData   = 1,
+  kpacketTail   = 2,
+} packet_type_t;
+
+class CANPacket {
+ public:
+  CAN_RxHeaderTypeDef pHeader;
+  uint8_t             aData[8]; /*8byte table*/
+};
+
+class CanPacketRxBuffer {
+ public:
+  uint16_t  id;
+  CANPacket m_canPacket[256];  // 用于接收can消息
+  uint8_t   m_canPacketNum = 0;
+  bool      dataIsReady;
+};
+};  // namespace zcr
+
+using namespace zcr;
+
+class ZCanRceiverListener {
+ public:
+  virtual void onRceivePacket(CanPacketRxBuffer *rxbuf, uint8_t *packet, size_t len) = 0;
+};
+
+class ZCanReceiver : public ZCanIRQListener {
+ public:
+  class CFG {
+   public:
+    uint8_t deviceId;  //
+    /*******************************************************************************
+     *                                  CANConfig                                  *
+     *******************************************************************************/
+    CAN_HandleTypeDef *canHandle;            // 默认使用CAN1
+    int                canFilterIndex0;      // 过滤器0 接收，发给自身的消息
+    int                maxFilterNum;         // 使用的过滤器数量，最大值14，默认为7
+    int                rxfifoNum;            // 使用的FIFO,默认使用FIFO0
+    int                packetRxOvertime_ms;  //
+  };
+
+  uint8_t rxdata[255 * 8];
+
+ public:
+  class LoopJobContext {
+   public:
+    bool hasDoneSomething;
+  };
+
+ private:
+  CFG              *m_config           = NULL;   // 配置
+  bool              m_canOnRxDataFlag  = false;  // 是否有数据接收，用于从中断上下文转移到MainLoop上下文
+  uint32_t          m_lastPacketTicket = 0;      // 上一次接收到消息的时间，用于判断与主机是否断开连接
+  HAL_StatusTypeDef m_lastTransmitStatus;        // 上次调用can发送方法的返回值
+
+  list<ZCanRceiverListener *> m_listenerList;
+  CanPacketRxBuffer           m_canPacketRxBuffer[1];
+
+ public:
+  ZCanReceiver() {}
+  CFG *createCFG(uint8_t deviceId);
+  void init(CFG *cfg);
+
+  void registerListener(ZCanRceiverListener *listener);
+  void sendPacket(uint8_t *packet, size_t len);
+
+ public:
+  virtual void STM32_HAL_onCAN_RxFifo0MsgPending(CAN_HandleTypeDef *can);
+  virtual void STM32_HAL_onCAN_Error(CAN_HandleTypeDef *can);
+
+ private:
+  void              loop();
+  HAL_StatusTypeDef initializeFilter();
+  HAL_StatusTypeDef activateRxIT();
+  HAL_StatusTypeDef deactivateRxIT();
+  bool              getRxMessage(CAN_RxHeaderTypeDef *pHeader, uint8_t aData[] /*8byte table*/);
+};
+
+}  // namespace iflytop