update

components/cmdscheduler/cmd_scheduler.cpp

@@ -74,10 +74,10 @@ void CmdScheduler::callcmd(const char* cmd, CmdProcessContext& context) {
   strcpy(cmdcache, cmd);
   prase_cmd(cmdcache, strlen(cmdcache), argc, argv);
 
-  printf("argc:%d\n", argc);
-  for (size_t i = 0; i < argc; i++) {
-    printf("argv[%d]:%s\n", i, argv[i]);
-  }
+  // printf("argc:%d\n", argc);
+  // for (size_t i = 0; i < argc; i++) {
+  //   printf("argv[%d]:%s\n", i, argv[i]);
+  // }
 #if 1
   /**
    * @brief 在这里处理指令

components/eq_20_asb_motor/README.md

@@ -0,0 +1 @@
+国脉电机

components/eq_20_asb_motor/eq20_servomotor.cpp

@@ -0,0 +1,28 @@
+#include "eq20_servomotor.hpp"
+
+#include <stdint.h>
+using namespace std;
+using namespace iflytop;
+
+static void dumpbuf(const char *mark, const char *buf, int len) {
+  printf("%s:", mark);
+  for (size_t i = 0; i < len; i++) {
+    printf("0x%02x ", (uint8_t)buf[i]);
+  }
+  printf("\n");
+}
+void ServoMotor::init(ModbusBlockHost *modbusBlockHost) {
+  //  this->com = com;
+  m_modbusBlockHost = modbusBlockHost;
+}
+bool ServoMotor::writereg32(int deviceid, uint32_t regadd, int value) {  //
+  m_modbusBlockHost->writeReg10(deviceid, regadd, value, 50);
+}
+bool ServoMotor::readreg(int deviceid, uint32_t regadd, int &value) {
+  uint16_t value16[2];
+  m_modbusBlockHost->readReg03Muti(deviceid, regadd, value16, 2, 50);
+  value = value16[0] + (value16[1] << 16);
+  return true;
+}
+
+//

components/eq_20_asb_motor/eq20_servomotor.hpp

@@ -0,0 +1,24 @@
+#pragma once
+#include <stdint.h>
+// #include "com.hpp"
+#include "sdk/os/zos.hpp"
+#include "sdk\components\modbus\modbus_block_host.hpp"
+namespace iflytop {
+using namespace std;
+
+class ServoMotor {
+ private:
+  ModbusBlockHost    *m_modbusBlockHost;
+
+ public:
+  ServoMotor(/* args */){};
+  ~ServoMotor(){};
+
+  void init(ModbusBlockHost *modbusBlockHost);
+  bool writereg32(int deviceid, uint32_t regadd, int value);
+  bool readreg(int deviceid, uint32_t regadd, int &value);
+
+ private:
+};
+
+}  // namespace iflytop

components/iflytop_can_slave_module_master_end/stepmotor.cpp

@@ -0,0 +1,139 @@
+#include "stepmotor.hpp"
+using namespace iflytop;
+
+#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)  // 清空异常
+
+#define SAMC_REG_STAT_STATUS           (10)  // 设备状态
+#define SAMC_REG_STAT_CURRENT_VELOCITY (12)  // 当前转速
+#define SAMC_REG_STAT_CURRENT_POS      (13)  // 当前位置
+#define SAMC_REG_STAT_EXCEPTION        (14)  // 异常状态
+
+#define SAMC_REG_CFG_ACC                                 (20)  // 加速度pps^2
+#define SAMC_REG_CFG_DEC                                 (21)  // 减速度pps^2
+#define SAMC_REG_CFG_VELOCITY                            (22)  // 速度
+#define SAMC_REG_CFG_ZERO_SHIFT                          (23)  // 零点偏移
+#define SAMC_REG_CFG_RUNHOME_VELOCITY                    (24)  // 归零速度
+#define SAMC_REG_CFG_RUNTOHOME_DEC                       (25)  // 归零减速度
+#define SAMC_REG_CFG_RUNTOHOME_MAX_DISTANCE              (26)  // 归零最大移动距离
+#define SAMC_REG_CFG_RUNTOHOME_LEAVE_ZERO_POINT_DISTANCE (27)  // 归零阶段1移动距离
+#define SAMC_REG_CFG_MIN_POS                             (28)  // 最小位置
+#define SAMC_REG_CFG_MAX_POS                             (29)  // 最大位置
+#define SAMC_REG_CFG_RUNTOHOME_KEEP_MOVE_DISTANCE        (30)  // 最大位置
+#define TAG                                              "StepMotor"
+void StepMotor::initialize(int id, int baseCtrlPoint, IflytopCanMaster* canMaster) {
+  m_id            = id;
+  m_baseCtrlPoint = baseCtrlPoint;
+  m_canMaster     = canMaster;
+  ZASSERT(m_canMaster != NULL);
+}
+bool StepMotor::setVelocity(int32_t velocity) {  //
+  if (m_fakeMotorl) {
+    ZLOGE(TAG, "call stepmotor fail,motor is null");
+    return false;
+  }
+  return m_canMaster->writeReg(m_id, m_baseCtrlPoint + SAMC_REG_CFG_VELOCITY, velocity, 20);
+}
+int32_t StepMotor::getVelocity() {
+  if (m_fakeMotorl) {
+    ZLOGE(TAG, "call stepmotor fail,motor is null");
+    return 0;
+  }
+  int32_t status = 0;
+  if (!m_canMaster->readReg(m_id, m_baseCtrlPoint + SAMC_REG_CFG_VELOCITY, status, 20)) {
+    return 0;
+  }
+  return status;
+}
+
+bool StepMotor::setAcc(int32_t acc) {  //
+  if (m_fakeMotorl) {
+    ZLOGE(TAG, "call stepmotor fail,motor is null");
+    return false;
+  }
+  return m_canMaster->writeReg(m_id, m_baseCtrlPoint + SAMC_REG_CFG_ACC, acc, 20);
+}
+bool StepMotor::setDec(int32_t dec) {  //
+  if (m_fakeMotorl) {
+    ZLOGE(TAG, "call stepmotor fail,motor is null");
+    return false;
+  }
+  return m_canMaster->writeReg(m_id, m_baseCtrlPoint + SAMC_REG_CFG_DEC, dec, 20);
+}
+bool StepMotor::moveTo(int32_t pos) {  //
+  if (m_fakeMotorl) {
+    ZLOGE(TAG, "call stepmotor fail,motor is null");
+    return false;
+  }
+  return m_canMaster->writeReg(m_id, m_baseCtrlPoint + SAMC_REG_ACT_MOVE_TO, pos, 20);
+}
+bool StepMotor::moveBy(int32_t pos) {  //
+  if (m_fakeMotorl) {
+    ZLOGE(TAG, "call stepmotor fail,motor is null");
+    return false;
+  }
+  return m_canMaster->writeReg(m_id, m_baseCtrlPoint + SAMC_REG_ACT_MOVE_BY, pos, 20);
+}
+bool StepMotor::rotate(int32_t velocity) {  //
+  if (m_fakeMotorl) {
+    ZLOGE(TAG, "call stepmotor fail,motor is null");
+    return false;
+  }
+  return m_canMaster->writeReg(m_id, m_baseCtrlPoint + SAMC_REG_ACT_ROTATE, velocity, 20);
+}
+int32_t StepMotor::getPos() {
+  if (m_fakeMotorl) {
+    ZLOGE(TAG, "call stepmotor fail,motor is null");
+    return 0;
+  }
+  int32_t status = 0;
+  if (!m_canMaster->readReg(m_id, m_baseCtrlPoint + SAMC_REG_STAT_CURRENT_POS, status, 20)) {
+    return false;
+  }
+  return status;
+}
+
+bool StepMotor::moveToZero() {  //
+  if (m_fakeMotorl) {
+    ZLOGE(TAG, "call stepmotor fail,motor is null");
+    return false;
+  }
+  return m_canMaster->writeReg(m_id, m_baseCtrlPoint + SAMC_REG_ACT_RUN_TO_HOME, 0, 20);
+}
+bool StepMotor::stop() {
+  if (m_fakeMotorl) {
+    ZLOGE(TAG, "call stepmotor fail,motor is null");
+    return false;
+  }
+  return m_canMaster->writeReg(m_id, m_baseCtrlPoint + SAMC_REG_ACT_STOP, 0, 20);
+}
+
+bool StepMotor::isIdleState() {
+  if (m_fakeMotorl) {
+    ZLOGE(TAG, "call stepmotor fail,motor is null");
+    return false;
+  }
+  int32_t status = 0;
+  if (!m_canMaster->readReg(m_id, m_baseCtrlPoint + SAMC_REG_STAT_STATUS, status, 20)) {
+    return false;
+  }
+  if (status == 0) return true;
+  return false;
+}
+
+void StepMotor::clearException() {
+  if (m_fakeMotorl) {
+    ZLOGE(TAG, "call stepmotor fail,motor is null");
+    return;
+  }
+  m_canMaster->writeReg(m_id, m_baseCtrlPoint + SAMC_REG_ACT_CLEAR_EXCEPTION, 0, 20);
+}
+
+namespace iflytop {
+StepMotor g_nullStepMotor;
+}

components/iflytop_can_slave_module_master_end/stepmotor.hpp

@@ -0,0 +1,47 @@
+#pragma once
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "sdk/os/zos.hpp"
+#include "sdk\components\iflytop_can_slave_v1\iflytop_can_master.hpp"
+#ifdef HAL_CAN_MODULE_ENABLED
+
+namespace iflytop {
+class StepMotor {
+ private:
+  int               m_id            = 0;
+  int               m_baseCtrlPoint = 0;
+  IflytopCanMaster* m_canMaster     = NULL;
+
+  bool m_fakeMotorl;
+
+ public:
+  StepMotor(){};
+  ~StepMotor(){};
+
+  int getId() { return m_id; }
+  int getBaseCtrlPoint() { return m_baseCtrlPoint; }
+
+  void    initialize(int id, int baseCtrlPoint, IflytopCanMaster* canMaster);
+  void    setFakeMotor(bool fakeMotor) { m_fakeMotorl = fakeMotor; };
+  bool    setVelocity(int32_t velocity);
+  int32_t getVelocity();
+
+  bool setAcc(int32_t acc);
+  bool setDec(int32_t dec);
+  bool moveTo(int32_t pos);
+  bool moveBy(int32_t pos);
+  bool rotate(int32_t velocity);
+
+  void clearException();
+
+  int32_t getPos();
+  bool    stop();
+  bool    isIdleState();
+  bool    moveToZero();
+};
+
+extern StepMotor g_nullStepMotor;
+}  // namespace iflytop
+#endif

components/iflytop_can_slave_v1/iflytop_can_master.cpp

@@ -0,0 +1,398 @@
+#include "sdk/os/zos.hpp"
+
+#ifdef HAL_CAN_MODULE_ENABLED
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "iflytop_can_master.hpp"
+using namespace iflytop;
+
+#define TAG          "CanMaster"
+#define OVER_TIME_MS 10
+
+IflytopCanMaster::IflytopCanMaster() {
+  memset(&m_config, 0, sizeof(m_config));
+  m_canOnRxDataFlag    = false;
+  m_dumpPacketFlag     = false;
+  m_lastPacketTicket   = 0;
+  m_lastTransmitStatus = HAL_OK;
+  m_reportSeq          = 0;
+}
+
+IflytopCanMaster::Config_t *IflytopCanMaster::createDefaultConfig(uint16_t deviceId) {
+  Config_t *config = (Config_t *)malloc(sizeof(Config_t));
+  ZASSERT(config != NULL);
+  config->deviceId        = deviceId;
+  config->canHandle       = &hcan1;
+  config->canFilterIndex0 = 0;
+  config->canFilterIndex1 = 1;
+  config->maxFilterNum    = 7;
+  config->rxfifoNum       = CAN_RX_FIFO0;
+  return config;
+}
+void IflytopCanMaster::initialize(Config_t *config) {
+  m_reportSeq = 0;
+  HAL_StatusTypeDef hal_status = HAL_OK;
+  m_config          = config;
+  m_canOnRxDataFlag = false;
+  m_cmdseq          = 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;
+  }
+}
+void IflytopCanMaster::periodicJob() {
+  LoopJobContext loopContext = {0};
+  if (loopContext.hasDoneSomething) return;
+}
+
+bool IflytopCanMaster::readReg(uint8_t targetDeviceId, int16_t regadd, int32_t &val, int overtime) {
+  icps::packet_t tx;
+  icps::packet_t rx;
+
+  tx.attribute = icps::knormal_packet;
+  tx.priority  = icps::klevel4;
+  tx.type      = icps::kread;
+  tx.targetId  = targetDeviceId;
+  tx.sourceId  = m_config->deviceId;
+  tx.seq       = m_cmdseq;
+  tx.regAdd    = regadd;
+  tx.regValue  = 0;
+
+  bool suc = txcmd(&tx, &rx, overtime);
+  if (suc) {
+    val = rx.regValue;
+    return true;
+  }
+  return false;
+}
+bool IflytopCanMaster::readRegSlient(uint8_t targetDeviceId, int16_t regadd, int32_t &val, int overtime) {
+  icps::packet_t tx;
+  icps::packet_t rx;
+
+  tx.attribute = icps::knormal_packet;
+  tx.priority  = icps::klevel4;
+  tx.type      = icps::kread;
+  tx.targetId  = targetDeviceId;
+  tx.sourceId  = m_config->deviceId;
+  tx.seq       = m_cmdseq;
+  tx.regAdd    = regadd;
+  tx.regValue  = 0;
+
+  bool suc = _txcmd(&tx, &rx, overtime, false);
+  if (suc) {
+    val = rx.regValue;
+    return true;
+  }
+  return false;
+}
+bool IflytopCanMaster::writeReg(uint8_t targetDeviceId, int16_t regadd, int32_t val, int overtime) {
+  icps::packet_t tx;
+  icps::packet_t rx;
+
+  tx.attribute = icps::knormal_packet;
+  tx.priority  = icps::klevel4;
+  tx.type      = icps::kwrite;
+  tx.targetId  = targetDeviceId;
+  tx.sourceId  = m_config->deviceId;
+  tx.seq       = m_cmdseq;
+  tx.regAdd    = regadd;
+  tx.regValue  = val;
+
+  bool suc = txcmd(&tx, &rx, overtime);
+  if (suc) {
+    return true;
+  }
+  if (rx.type == icps::kerror_receipt) {
+    ZLOGE(TAG, "writeReg fail, error code:%ld", rx.regValue);
+  }
+  return false;
+}
+bool IflytopCanMaster::txcmd(icps::packet_t *tx, icps::packet_t *rx, int overtime) { return _txcmd(tx, rx, overtime, true); }
+
+bool IflytopCanMaster::_txcmd(icps::packet_t *tx, icps::packet_t *rx, int overtime, bool dumplog) {
+  tx->seq = m_cmdseq;
+  m_cmdseq++;
+
+  int32_t           endTicket = zos_get_tick() + overtime;
+  HAL_StatusTypeDef ret       = translate(tx, OVER_TIME_MS);
+  if (ret != HAL_OK) {
+    if (dumplog) {
+      ZLOGE(TAG, "txcmd device:%d fail,translate overtime", tx->targetId);
+    }
+    return false;
+  }
+  while (true) {
+    bool suc = tryReceivePacket(*rx);
+    if (suc) {
+      if ((rx->type == icps::kreceipt || rx->type == icps::kerror_receipt) && (tx->seq == rx->seq)) {
+        return true;
+      }
+    }
+
+    if ((int)zos_get_tick() > endTicket) {
+      if (dumplog) {
+        ZLOGE(TAG, "txcmd fail,tryReceivePacket overtime")
+      };
+      return false;
+    }
+  }
+  return false;
+}
+
+/**************************************************************************************************************************************************************
+ *                                                              CAN初始化相关方法                                                                              *
+ **************************************************************************************************************************************************************/
+
+HAL_StatusTypeDef IflytopCanMaster::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 = HAL_OK;
+  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
+
+  /*******************************************************************************
+   *                        配置过滤器0，只接受自己的指令                        *
+   *******************************************************************************/
+  filterId                 = ((icps::knormal_packet) << (3 + 24)) | ((0xFF & m_config->deviceId) << (3 + 8));  // 3:低三位不属于ID区 8:目标ID偏移
+  mask                     = ((0x03) << (3 + 24)) | (0xFF << (3 + 8));                                         //
+  sFilterConfig.FilterBank = m_config->canFilterIndex0;                                                        //
+  sFilterConfig.FilterMaskIdLow  = mask & 0xffff;                  // MASK: 0000 0111 1111 1111(接收自己的消息)
+  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 %08lx", filterId >> 3);
+
+  /*******************************************************************************
+   *                         配置过滤器1, 接收通用指令,广播包                       *
+   *******************************************************************************/
+  filterId                       = ((0xFF) << (3 + 8));            //
+  mask                           = ((0xFF << (3 + 8)));            //
+  sFilterConfig.FilterBank       = m_config->canFilterIndex1;      // 过滤器1
+  sFilterConfig.FilterMaskIdLow  = mask & 0xffff;                  // MASK: 0000 0111 1111 1111(接收自己的消息)
+  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 filter1 fail");
+    return HAL_Status;
+  }
+  ZLOGI(TAG, "HAL_CAN_ConfigFilter filterID2 %08lx", filterId >> 3);
+  return HAL_Status;
+}
+/**************************************************************************************************************************************************************
+ *                                                              CAN发送数据处理流程                                                                            *
+ **************************************************************************************************************************************************************/
+/**
+ * @brief 发送数据
+ *
+ * @param desId      目标设备ID
+ * @param data       数据
+ * @param len        数据长度，最大为8
+ * @param overtimems 超时时间，单位ms
+ */
+HAL_StatusTypeDef IflytopCanMaster::translate(uint32_t Id, uint8_t *data, uint8_t len, int overtimems) {
+  CAN_TxHeaderTypeDef TxMeg;
+  HAL_StatusTypeDef   HAL_RetVal;
+
+  uint32_t            txMailBox = 0;
+  uint32_t            now       = 0;
+
+  TxMeg.StdId = 0;             //
+  TxMeg.ExtId = Id;            //
+  TxMeg.DLC   = len;           //
+  TxMeg.IDE   = CAN_ID_EXT;    // 标准帧，还是扩展帧
+  TxMeg.RTR   = CAN_RTR_DATA;  // 数据帧，非远程帧
+
+  HAL_CAN_ActivateNotification(m_config->canHandle, CAN_IT_TX_MAILBOX_EMPTY);
+  HAL_RetVal = HAL_CAN_AddTxMessage(m_config->canHandle, &TxMeg, data, &txMailBox);
+  if (HAL_RetVal != HAL_OK) {
+    m_lastTransmitStatus = HAL_RetVal;
+    goto endtag;
+  }
+  now = zos_get_tick();
+  while (HAL_CAN_IsTxMessagePending(m_config->canHandle, txMailBox)) {
+    if ((int)zos_haspassedms(now) > overtimems) {
+      m_lastTransmitStatus = HAL_TIMEOUT;
+      HAL_CAN_AbortTxRequest(m_config->canHandle, txMailBox);
+      goto endtag;
+    }
+    // m_os->sleepMS(1);
+  }
+  m_lastTransmitStatus = HAL_OK;
+endtag:
+
+  if (m_dumpPacketFlag) {
+    ZLOGI(TAG, "send packet: id=0x%032lx, data=0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x %s",  //
+          Id, data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7],                       //
+          m_lastTransmitStatus == HAL_OK ? "success" : "fail");
+  }
+  return m_lastTransmitStatus;
+}
+HAL_StatusTypeDef IflytopCanMaster::translate(icps::packet_t *packet, int overtimems) {
+  uint8_t  data[8];
+  uint32_t id = (((uint32_t)packet->attribute /**/ & 0x03 /*11       */) << 24) |  //
+                (((uint32_t)packet->priority /* */ & 0x07 /*111      */) << 26) |  //
+                (((uint32_t)packet->type /*     */ & 0xff /*1111,1111*/) << 16) |  //
+                (((uint32_t)packet->targetId /* */ & 0xff /*1111,1111*/) << 8) |   //
+                (((uint32_t)packet->sourceId /* */ & 0xff /*1111,1111*/) << 0);
+  data[0] = (packet->seq >> 8) & 0xFF;
+  data[1] = packet->seq & 0xFF;
+  data[2] = (packet->regAdd >> 8) & 0xFF;
+  data[3] = packet->regAdd & 0xFF;
+  data[4] = (packet->regValue >> 24) & 0xFF;
+  data[5] = (packet->regValue >> 16) & 0xFF;
+  data[6] = (packet->regValue >> 8) & 0xFF;
+  data[7] = packet->regValue & 0xFF;
+  return translate(id, data, 8, overtimems);
+}
+
+/**************************************************************************************************************************************************************
+ *                                                              CAN接收数据处理流程                                                                            *
+ **************************************************************************************************************************************************************/
+void IflytopCanMaster::STM32_HAL_onCAN_RxFifo0MsgPending(CAN_HandleTypeDef *canHandle) {
+  /**
+   * @brief 中断上下文
+   */
+  if (canHandle != m_config->canHandle) {
+    return;
+  }
+  m_canOnRxDataFlag = true;
+  deactivateRxIT();
+}
+void IflytopCanMaster::STM32_HAL_onCAN_Error(CAN_HandleTypeDef *canHandle) {
+  if (canHandle != m_config->canHandle) {
+    return;
+  }
+  // ZLOGE(TAG, "onCAN_Error");
+}
+bool IflytopCanMaster::tryReceivePacket(icps::packet_t &packet) {
+  if (m_canOnRxDataFlag) {
+    m_canOnRxDataFlag = false;
+    while (getRxPacket(&packet)) {
+      if (m_dumpPacketFlag) {
+        ZLOGI(TAG, "rx packet:");
+        ZLOGI(TAG, "\ttype:%d, targetId:%d, sourceId:%d, seq:%d, regAdd:%d, regValue:%ld",  //
+              packet.type, packet.targetId, packet.sourceId, packet.seq, packet.regAdd, packet.regValue);
+      }
+      if (packet.targetId == m_config->deviceId || packet.targetId == 0xff) {
+        m_lastPacketTicket = zos_get_tick();
+        activateRxIT();
+        return true;
+      }
+    }
+    activateRxIT();
+  }
+  return false;
+}
+bool IflytopCanMaster::getRxPacket(icps::packet_t *packet) {
+  CAN_RxHeaderTypeDef pHeader;
+  uint8_t             aData[8]; /*8byte table*/
+
+  /**
+   * @brief ID区帧格式
+   *  [                27:0                  ]
+   *  [    STDID        ] [      EXTID       ]
+   *  [11 :9] [8:6] [5:0] [17:16] [15:8] [7:0]
+   *   优先级  属性     帧类型      目标ID  源ID
+   */
+  bool rxsuccess    = getRxMessage(&pHeader, aData);
+  packet->priority  = (icps::packet_priority_t)(pHeader.ExtId >> 26 & 0x07);
+  packet->attribute = (icps::packet_attribute_t)(pHeader.ExtId >> 24 & 0x03);
+  packet->type      = (icps::packet_type_t)(pHeader.ExtId >> 16 & 0xFF);
+  packet->targetId  = (pHeader.ExtId & 0xFF00) >> 8;
+  packet->sourceId  = (pHeader.ExtId & 0x00FF);
+  packet->seq       = (aData[0] << 8) | aData[1];
+  packet->regAdd    = (aData[2] << 8) | aData[3];
+  packet->regValue  = (aData[4] << 24) | (aData[5] << 16) | (aData[6] << 8) | aData[7];
+  return rxsuccess;
+}
+bool IflytopCanMaster::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;
+}
+/**
+ * @brief 读取当前FIFO中缓存了多少帧的数据
+ */
+int               IflytopCanMaster::getRxPacketRemain() { return HAL_CAN_GetRxFifoFillLevel(m_config->canHandle, m_config->rxfifoNum); }
+HAL_StatusTypeDef IflytopCanMaster::activateRxIT() {
+  HAL_StatusTypeDef hal_status = HAL_OK;
+  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 IflytopCanMaster::deactivateRxIT() {
+  HAL_StatusTypeDef hal_status= HAL_OK;
+  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

components/iflytop_can_slave_v1/iflytop_can_master.hpp

@@ -0,0 +1,116 @@
+//
+// Created by zwsd
+//
+
+#pragma once
+#include "sdk/os/zos.hpp"
+
+#ifdef HAL_CAN_MODULE_ENABLED
+#include "iflytop_can_slave_protocol.hpp"
+/**
+ * @brief
+ *
+ * service: IflytopCanMaster
+ * 该类主要实现以下协议
+ *    https://iflytop1.feishu.cn/docx/ZBsddjrL1oHAiYx8DdmcccEBnPf
+ *
+ */
+
+/**
+ * @brief ID区帧格式
+ *  [                27:0                  ]
+ *  [    STDID        ] [      EXTID       ]
+ *  [11 :9] [8:6] [5:0] [17:16] [15:8] [7:0]
+ *   优先级  属性     帧类型      目标ID  源ID
+ */
+namespace iflytop {
+using namespace std;
+
+class IflytopCanMaster : public ZCanIRQListener {
+ public:
+  typedef struct {
+    uint16_t deviceId;  //
+    /*******************************************************************************
+     *                                  CANConfig                                  *
+     *******************************************************************************/
+    CAN_HandleTypeDef *canHandle;        // 默认使用CAN1
+    int                canFilterIndex0;  // 过滤器0 接收，发给自身的消息
+    int                canFilterIndex1;  // 过滤器1 接收，全局广播包
+    int                maxFilterNum;     // 使用的过滤器数量，最大值14，默认为7
+    int                rxfifoNum;        // 使用的FIFO,默认使用FIFO0
+  } Config_t;
+
+  class LoopJobContext {
+   public:
+    bool hasDoneSomething;
+  };
+
+ private:
+  Config_t       *m_config;  // 配置
+  // IflytopMicroOS *m_os;      // 操作系统相关方法
+
+  bool              m_canOnRxDataFlag;     // 是否有数据接收，用于从中断上下文转移到MainLoop上下文
+  bool              m_dumpPacketFlag;      // 标志当前是否打印数据包
+  uint32_t          m_lastPacketTicket;    // 上一次接收到消息的时间，用于判断与主机是否断开连接
+  HAL_StatusTypeDef m_lastTransmitStatus;  // 上次调用can发送方法的返回值
+  uint8_t           m_reportSeq;           // 上报序列号
+
+  uint16_t m_cmdseq;
+
+ public:
+  IflytopCanMaster();
+  Config_t *createDefaultConfig(uint16_t deviceId);
+  /**
+   * @brief 初始化对象
+   *
+   * @param deviceId 7位设备ID
+   */
+  void initialize(Config_t *config);
+  /**
+   * @brief 在Main的Loop中尽可能频繁的的调用该方法
+   */
+  void periodicJob();
+
+  bool readReg(uint8_t targetDeviceId, int16_t regadd, int32_t &val, int overtime);
+  bool writeReg(uint8_t targetDeviceId, int16_t regadd, int32_t val, int overtime);
+
+  bool txcmd(icps::packet_t *tx, icps::packet_t *rx, int overtime);
+  bool readRegSlient(uint8_t targetDeviceId, int16_t regadd, int32_t &val, int overtime);
+
+ private:
+  void processRxPacket(icps::packet_t *packet);
+  bool _txcmd(icps::packet_t *tx, icps::packet_t *rx, int overtime, bool dumplog);
+
+ public:
+  /*******************************************************************************
+   *                         OVERRIDE STM32_HAL_LISTENER                         *
+   *******************************************************************************/
+  virtual void STM32_HAL_onCAN_RxFifo0MsgPending(CAN_HandleTypeDef *can);
+  virtual void STM32_HAL_onCAN_Error(CAN_HandleTypeDef *can);
+
+ private:
+  HAL_StatusTypeDef initializeFilter();
+
+  /*******************************************************************************
+   *                            CAN接口基本操作方法                               *
+   *******************************************************************************/
+  bool getRxMessage(CAN_RxHeaderTypeDef *pHeader, uint8_t aData[] /*8byte table*/);
+  bool getRxPacket(icps::packet_t *packet);
+  int  getRxPacketRemain();
+
+  HAL_StatusTypeDef activateRxIT();
+  HAL_StatusTypeDef deactivateRxIT();
+
+  HAL_StatusTypeDef translate(uint32_t Id, uint8_t *data, uint8_t len, int overtimems);
+  HAL_StatusTypeDef translate(icps::packet_t *packet, int overtimems);
+
+  /*******************************************************************************
+   *                                 RXPacketJob                                 *
+   *******************************************************************************/
+  bool tryReceivePacket(icps::packet_t &packet);
+
+ private:
+};
+
+}  // namespace iflytop
+#endif

components/iflytop_can_slave_v1/iflytop_can_slave_protocol.hpp

@@ -0,0 +1,188 @@
+//
+// Created by zwsd
+//
+#pragma once
+#include "sdk/os/zos.hpp"
+
+#ifdef HAL_CAN_MODULE_ENABLED
+/**
+ * @brief
+ *
+ * service: IflytopCanProtocolStackProcesser
+ * 该类主要实现以下协议
+ *    https://iflytop1.feishu.cn/docx/ZBsddjrL1oHAiYx8DdmcccEBnPf
+ *
+ */
+/**
+ * @brief ID区帧格式
+ *  [                27:0                  ]
+ *  [    STDID        ] [      EXTID       ]
+ *  [11 :9] [8:6] [5:0] [17:16] [15:8] [7:0]
+ *   优先级  属性     帧类型      目标ID  源ID
+ */
+namespace iflytop {
+using namespace std;
+namespace icps {
+/**
+ * @brief 协议通用错误码
+ */
+typedef enum {
+  kSuccess                       = 0x00,
+  kRegNotFound                   = 2,
+  kRegNotWritable                = 3,
+  kRegNotReadable                = 4,
+  kIllegalValue                  = 5,
+  kDeviceBusy                    = 6,
+  kDeviceException               = 7,
+  kChannelDeviceException        = 8,
+  kParameterError                = 9,
+  kOvertime                      = 10,
+  kOperationInvalid              = 11,
+  kunsupportOperation            = 12,
+  kIllegalOperation              = 13,
+  kPermissionDenied              = 14,
+  koutOfRange                    = 15,
+  kmotorNotRunToHome             = 16,
+  kParameterOutOfLimit           = 17,
+  kDeviceLockByInternalControler = 18,
+  kUserErrorBegin                = 0xFF,
+} error_t;
+/**
+ * @brief 消息优先级，数值越大优先级越低
+ *
+ * 通用消息:klevel4
+ */
+typedef enum {
+  klevel0 = 0,  //
+  klevel1 = 1,  //
+  klevel2 = 2,  //
+  klevel3 = 3,  //
+  klevel4 = 4,  //
+  klevel5 = 5,  //
+  klevel6 = 6,  //
+  klevel7 = 7,  //
+} packet_priority_t;
+/**
+ * @brief 消息类型
+ */
+typedef enum {
+  kwrite                       = 1,    // 写
+  kread                        = 2,    // 读
+  kreceipt                     = 3,    // 回执
+  kreport                      = 4,    // 上报
+  kerror_receipt               = 5,    // 错误回执
+  kset_auto_report_period      = 16,   // 设置自动上报周期
+  kset_auto_report_enable_flag = 17,   // 设置自动上报
+  kext_report_packet_header    = 109,  // 上报
+  kext_report_packet           = 110,  // 上报
+  kext_report_packet_tail      = 111,  // 上报
+  kheart_packet_receipt        = 254,  // 心跳包回执
+  kheart_packet                = 255,  // 心跳包
+} packet_type_t;
+/**
+ * @brief 消息属性
+ */
+typedef enum {
+  knormal_packet  = 0,  // 通用指令
+  knormal_message = 2,  // 通用消息
+} packet_attribute_t;
+/**
+ * @brief 消息
+ */
+typedef struct {
+  packet_attribute_t attribute;
+  packet_priority_t  priority;
+  packet_type_t      type;
+  uint8_t            targetId;
+  uint8_t            sourceId;
+  uint8_t            seq;
+  uint16_t           regAdd;
+  int32_t            regValue;
+} packet_t;
+
+/*******************************************************************************
+ *                                寄存器标志位                                 *
+ *******************************************************************************/
+
+typedef enum {
+  kactived = 0x01,       //
+  kr       = 0x01 << 1,  //
+  kw       = 0x01 << 2,  //
+  /**
+   * 1. 如果上报周期为0，  那么就是只有值发生变化才上报
+   * 2. 如果上报周期不为0，当数值发生变化时会自动上报，但一个周期内至多上报一次
+   */
+  kautoReport  = 0x01 << 3,  //
+  khasUpdated  = 0x01 << 4,  //
+  kextreg      = 0x01 << 6,  //
+  kreadyReport = 0x01 << 7,  // 待上报，只用于扩展寄存器
+  //
+  klimit = 0x01 << 8,  //
+  // kengineerModeLimit = (0x01 << 9) | klimit,  // 待上报，只用于扩展寄存器
+
+  kwr = kr | kw,
+
+} reg_flag_t;
+
+class Reg_t {
+ private:
+  int32_t min;    // 最小值
+  int32_t max;    // 最大值
+  int32_t value;  // 寄存器数值
+
+ public:
+  uint16_t mask;              // 标志位
+  uint16_t add;               // 寄存器地址
+  uint16_t reportPeriod;      // 自动上报周期
+  uint32_t lastReportTicket;  // 上次上报的时间
+
+ public:
+  void setFlag(reg_flag_t flag) { mask |= flag; }
+  void clearFlag(reg_flag_t flag) { mask &= ~flag; }
+  bool is(reg_flag_t flag) { return mask & flag; }
+
+  int32_t       getValue() { return value; }
+  icps::error_t setValue(int32_t v) {
+    if (is(klimit)) {
+      if (v < min)
+        value = min;
+      else if (v > max)
+        value = max;
+      else
+        value = v;
+    } else {
+      value = v;
+    }
+    return icps::kSuccess;
+  }
+  void setLimit(int32_t min, int32_t max) {
+    this->min = min;
+    this->max = max;
+  }
+};
+
+class WriteEvent {
+ public:
+  icps::Reg_t *reg;
+  int32_t      oldvalue;
+  int32_t      newvalue;
+  int32_t      extRegSubOff;
+};
+
+class ReportEvent {
+ public:
+  icps::Reg_t *reg;
+  int32_t      value;
+  int32_t      extRegSubOff;
+};
+
+class ReadEvent {
+ public:
+  icps::Reg_t *reg;
+  int32_t      value;
+  int32_t      extRegSubOff;
+};
+
+};  // namespace icps
+}  // namespace iflytop
+#endif

components/modbus/modbus_basic.cpp

@@ -0,0 +1,67 @@
+/**
+ *  Modbus
+ * | add(1byte) | functionCode(1byte) | ...data... | crc1(1byte) | crc2(2byte)|
+ *
+ *
+ *
+ *
+ */
+#include "modbus_basic.hpp"
+
+#include <stdint.h>
+// CRC_16高8位数据区
+#define ASSERT(con) \
+  while (!(con)) {  \
+  }
+static const uint8_t auchCRCHi[] = {
+    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1,
+    0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
+    0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
+    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
+    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
+    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
+    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
+    0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
+    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1,
+    0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40};
+// CRC低位字节值表
+static const uint8_t auchCRCLo[] = {  // CRC_16低8位数据区
+    0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA,
+    0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15,
+    0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35,
+    0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA,
+    0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60,
+    0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB,
+    0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, 0x77, 0xB7,
+    0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54,
+    0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E,
+    0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, 0x40};
+
+/*CRC16查表计算函数*/
+
+uint16_t modbus_gen_crc16(uint8_t *puckMsg, uint8_t usDataLen) {
+  volatile uint8_t  uchCRCHi = 0xFF;  // 高CRC字节初始化
+  volatile uint8_t  uchCRCLo = 0xFF;  // 低CRC 字节初始化
+  volatile uint32_t uIndex;           // CRC循环中的索引
+  // 传输消息缓冲区
+  while (usDataLen--) {
+    // 计算CRC
+    uIndex   = uchCRCLo ^ *puckMsg++;
+    uchCRCLo = uchCRCHi ^ auchCRCHi[uIndex];
+    uchCRCHi = auchCRCLo[uIndex];
+  }
+  // 返回结果，高位在前
+  return (uchCRCLo << 8 | uchCRCHi);
+}
+
+void modbus_pack_crc_to_packet(uint8_t *puckMsg, uint8_t packetlen) {
+  uint16_t crc           = modbus_gen_crc16(puckMsg, packetlen - 2);
+  puckMsg[packetlen - 2] = crc >> 8;
+  puckMsg[packetlen - 1] = crc;
+}
+
+/*函数功能：用于校验接收到的信息是否有误
+输入参数：message是接收到的待校验消息，length是消息字节的长度
+函数输出：输出校验结果，没有错返回True，有错返回False
+通过CRC校验校验接收的信息是否正确*/
+bool modbus_checkcrc16(uint8_t *message, uint8_t length) { return (modbus_gen_crc16(message, length) == 0x00) ? true : false; }

components/modbus/modbus_basic.hpp

@@ -0,0 +1,28 @@
+#pragma once
+#include <stdbool.h>
+#include <stdint.h>
+
+/**
+ * @brief 生成CRC16校验码
+ *
+ * @param puckMsg 数据指针
+ * @param usDataLen 数据长度(不包含CRC16)
+ * @return uint16_t
+ */
+uint16_t modbus_gen_crc16(uint8_t *puckMsg, uint8_t usDataLen);
+/**
+ * @brief 将CRC16校验码添加到数据包中
+ *
+ * @param puckMsg 数据指针，数据中预留两个位置给CRC16
+ * @param packetlen 数据长度
+ */
+void modbus_pack_crc_to_packet(uint8_t *puckMsg, uint8_t packetlen);
+/**
+ * @brief 检查CRC16校验码
+ *
+ * @param message 数据指针
+ * @param length 数据长度(包含CRC16)
+ * @return true 校验成功
+ * @return false 校验失败
+ */
+bool modbus_checkcrc16(uint8_t *message, uint8_t length);

components/modbus/modbus_block_host.cpp

@@ -0,0 +1,138 @@
+#include "modbus_block_host.hpp"
+
+#include "modbus_basic.hpp"
+using namespace iflytop;
+#define DEBUG 1
+ModbusBlockHost::ModbusBlockHost() {}
+ModbusBlockHost::~ModbusBlockHost() {}
+
+void ModbusBlockHost::initialize(UART_HandleTypeDef *huart) { this->huart = huart; }
+void ModbusBlockHost::cleanRxBuff() {  //
+  HAL_StatusTypeDef status;
+  do {
+    status = HAL_UART_Receive(huart, rxbuff, 1, 1);
+  } while (status == HAL_OK);
+}
+
+void ModbusBlockHost::uarttx(uint8_t *buff, size_t len) {
+  ZASSERT(len < sizeof(txbuff));
+#if DEBUG
+  printf("uarttx:\n");
+  for (size_t i = 0; i < len; i++) {
+    printf("%02x ", buff[i]);
+  }
+  printf("\n");
+#endif
+  HAL_UART_Transmit(huart, buff, len, 1000);
+}
+bool ModbusBlockHost::uartrx(uint8_t *buff, size_t len, int overtimems) {
+  HAL_StatusTypeDef status;
+  ZASSERT(len < sizeof(rxbuff));
+
+  status = HAL_UART_Receive(huart, buff, len, overtimems);
+#if DEBUG
+  if (status == HAL_OK) {
+    printf("uartrx:");
+    for (size_t i = 0; i < len; i++) {
+      printf("%02x ", buff[i]);
+    }
+    printf("\n");
+  }
+#endif
+  return status == HAL_OK;
+}
+
+bool ModbusBlockHost::readReg03(uint8_t slaveAddr, uint16_t regAddr, uint16_t *regVal, int overtimems) {
+  txbuff[0] = slaveAddr;
+  txbuff[1] = 0x03;
+  txbuff[2] = regAddr >> 8;
+  txbuff[3] = regAddr & 0xff;
+  txbuff[4] = 0x00;
+  txbuff[5] = 0x01;
+  modbus_pack_crc_to_packet(txbuff, 6 + 2);
+
+  cleanRxBuff();
+
+  uarttx(txbuff, 6 + 2);
+
+  bool status;
+  status = uartrx(rxbuff, 5 + 2, overtimems);
+
+  if (status && modbus_checkcrc16(rxbuff, 7)) {
+    *regVal = rxbuff[3] << 8 | rxbuff[4];
+    return true;
+  }
+  return false;
+}
+
+bool ModbusBlockHost::readReg03Muti(uint8_t slaveAddr, uint16_t regAddr, uint16_t *regVal, int regNum, int overtimems) {
+  txbuff[0] = slaveAddr;
+  txbuff[1] = 0x03;
+  txbuff[2] = regAddr >> 8;
+  txbuff[3] = regAddr & 0xff;
+  txbuff[4] = 0x00;
+  txbuff[5] = regNum;
+  modbus_pack_crc_to_packet(txbuff, 6 + 2);
+
+  cleanRxBuff();
+
+  uarttx(txbuff, 6 + 2);
+
+  bool status;
+  // 14*2 = 28
+  status = uartrx(rxbuff, 5 + regNum * 2, overtimems);
+
+  if (status && modbus_checkcrc16(rxbuff, 7 + regNum * 2)) {
+    for (int i = 0; i < regNum; i++) {
+      regVal[i] = rxbuff[3 + i * 2] << 8 | rxbuff[4 + i * 2];
+    }
+    return true;
+  }
+  return false;
+}
+
+bool ModbusBlockHost::writeReg06(uint8_t slaveAddr, uint16_t regAddr, uint16_t regVal, int overtimems) {
+  txbuff[0] = slaveAddr;
+  txbuff[1] = 0x06;
+  txbuff[2] = regAddr >> 8;
+  txbuff[3] = regAddr & 0xff;
+  txbuff[4] = regVal >> 8;
+  txbuff[5] = regVal & 0xff;
+  modbus_pack_crc_to_packet(txbuff, 6 + 2);
+
+  cleanRxBuff();
+
+  uarttx(txbuff, 6 + 2);
+
+  bool status;
+  status = uartrx(rxbuff, 8, overtimems);
+
+  if (status && modbus_checkcrc16(rxbuff, 8)) {
+    return true;
+  }
+  return false;
+}
+bool ModbusBlockHost::writeReg10(uint8_t slaveAddr, uint16_t regAddr, uint16_t regVal, int overtimems) {
+  txbuff[0] = slaveAddr;
+  txbuff[1] = 0x10;
+  txbuff[2] = regAddr >> 8;
+  txbuff[3] = regAddr & 0xff;
+  txbuff[4] = 0x00;
+  txbuff[5] = 0x01;
+  txbuff[6] = 0x02;  // ×Ö½ÚÊý
+  txbuff[7] = regVal >> 8;
+  txbuff[8] = regVal & 0xff;
+  modbus_pack_crc_to_packet(txbuff, 9 + 2);
+
+  cleanRxBuff();
+
+  uarttx(txbuff, 9 + 2);
+
+  bool status;
+  status = uartrx(rxbuff, 8, overtimems);
+
+  if (status && modbus_checkcrc16(rxbuff, 8)) {
+    return true;
+  }
+  return false;
+}

components/modbus/modbus_block_host.hpp

@@ -0,0 +1,28 @@
+#pragma once
+#include "sdk/os/zos.hpp"
+
+
+namespace iflytop {
+class ModbusBlockHost {
+  UART_HandleTypeDef *huart;
+
+  uint8_t txbuff[100];
+  uint8_t rxbuff[100];
+
+ public:
+  ModbusBlockHost();
+  ~ModbusBlockHost();
+
+  void initialize(UART_HandleTypeDef *huart);
+
+  bool readReg03(uint8_t slaveAddr, uint16_t regAddr, uint16_t *regVal, int overtimems);
+  bool readReg03Muti(uint8_t slaveAddr, uint16_t regAddr, uint16_t *regVal, int regNum, int overtimems);
+  bool writeReg06(uint8_t slaveAddr, uint16_t regAddr, uint16_t regVal, int overtimems);
+  bool writeReg10(uint8_t slaveAddr, uint16_t regAddr, uint16_t regVal, int overtimems);
+
+ private:
+  void cleanRxBuff();
+  void uarttx(uint8_t *buff, size_t len);
+  bool uartrx(uint8_t *buff, size_t len, int overtimems);
+};
+}  // namespace iflytop

components/step_motor_45/step_motor_45.cpp

@@ -64,7 +64,9 @@ void StepMotor45::rotate(bool direction, int speed) {
       m_pos         = 0;
     }
   }
+  posmode = false;
   m_state = direction ? kforward_rotation : krollback;
+  ZLOGI(TAG, "rotate %d", direction);
 }
 void StepMotor45::stop() {
   CriticalContext cc;
@@ -149,6 +151,17 @@ void StepMotor45::schedule() {
   }
 
   /**
+   * @brief 如果运行到目标位置则停止
+   */
+
+  if (posmode) {
+    if (m_pos == m_targetPos) {
+      stop_motor();
+      return;
+    }
+  }
+
+  /**
    * @brief
    *    按照相位表执行电机驱动
    */
@@ -162,7 +175,7 @@ void StepMotor45::schedule() {
   m_off += direction;
   if (m_off < 0) {
     m_off = MAX_STEP_TABLE_OFF;
-  } else if (m_off > MAX_STEP_TABLE_OFF) {
+  } else if (m_off >(int) MAX_STEP_TABLE_OFF) {
     m_off = 0;
   }
 
@@ -178,6 +191,48 @@ void StepMotor45::schedule() {
   m_pos += direction;
 }
 
+void StepMotor45::moveBy(int bypos, int speed) {
+  CriticalContext cc;
+  if (bypos == 0) {
+    m_targetPos = m_pos;
+    return;
+  }
+  if (bypos > 0) {
+    m_state = kforward_rotation;
+  } else {
+    m_state = krollback;
+  }
+
+  m_targetPos = bypos + m_pos;
+  posmode     = true;
+}
+
+void StepMotor45::moveTo(int to, int speed) {
+  CriticalContext cc;
+
+  if (to == m_pos) {
+    return;
+  }
+
+  if (to <= m_pos) {
+    m_state = krollback;
+  } else {
+    m_state = kforward_rotation;
+  }
+
+  m_targetPos = to;
+  posmode     = true;
+}
+void StepMotor45::moveTo(int to) { moveTo(to, defaultspeed); }
+
+bool StepMotor45::isReachTargetPos() {
+  CriticalContext cc;
+  if (posmode) {
+    return m_pos == m_targetPos;
+  }
+  return false;
+}
+
 bool StepMotor45::getzeropinstate() {
   if (!m_zeroPinZGPIO) {
     return false;
@@ -186,7 +241,7 @@ bool StepMotor45::getzeropinstate() {
   return state;
 }
 void StepMotor45::setdriverpinstate(bool s1, bool s2, bool s3, bool s4) {
-  ZLOGI(TAG, "setdriverpinstate %d%d%d%d", s1, s2, s3, s4);
+  // ZLOGI(TAG, "setdriverpinstate %d%d%d%d", s1, s2, s3, s4);
 
   m_driverPinZGPIO[0]->setState(s1);
   m_driverPinZGPIO[1]->setState(s2);

components/step_motor_45/step_motor_45.hpp

@@ -38,6 +38,9 @@ class StepMotor45 {
   state_t m_state;             // 当前状态
   bool    m_lastzeropinstate;  // 上一次零点限位状态
   int     m_acceleration;      // 加速度
+
+  bool posmode = false;
+  int  m_targetPos;
   /**
    * @brief 控制
    */
@@ -53,13 +56,25 @@ class StepMotor45 {
 
   cfg_t m_cfg;
 
+  int defaultspeed = 1000;
+
  public:
   void initialize(cfg_t cfg);
 
+  void rotate(bool direction) { rotate(direction, defaultspeed); }
   void rotate(bool direction, int speed);
+  void moveBy(int pos, int speed);
+  void moveBy(int pos) { moveBy(pos, defaultspeed); }
+
+  void moveTo(int to, int speed);
+  void moveTo(int to);
+  void setDefaultSpeed(int speed) { defaultspeed = speed; }
+  bool isReachTargetPos();
   void stop();
   void zeroCalibration();
 
+  int getPos() { return m_pos; }
+
  private:
   bool getzeropinstate();
   void setdriverpinstate(bool s1, bool s2, bool s3, bool s4);