From bbfc6b8d6d7910b4100cd0f57ea91fec2e39ccfc Mon Sep 17 00:00:00 2001 From: zhaohe Date: Wed, 6 Sep 2023 02:29:20 +0800 Subject: [PATCH] update --- components/cmdscheduler/cmd_scheduler.cpp | 8 +- components/eq_20_asb_motor/README.md | 1 + components/eq_20_asb_motor/eq20_servomotor.cpp | 28 ++ components/eq_20_asb_motor/eq20_servomotor.hpp | 24 ++ .../stepmotor.cpp | 139 +++++++ .../stepmotor.hpp | 47 +++ .../iflytop_can_slave_v1/iflytop_can_master.cpp | 398 +++++++++++++++++++++ .../iflytop_can_slave_v1/iflytop_can_master.hpp | 116 ++++++ .../iflytop_can_slave_protocol.hpp | 188 ++++++++++ components/modbus/modbus_basic.cpp | 67 ++++ components/modbus/modbus_basic.hpp | 28 ++ components/modbus/modbus_block_host.cpp | 138 +++++++ components/modbus/modbus_block_host.hpp | 28 ++ components/step_motor_45/step_motor_45.cpp | 59 ++- components/step_motor_45/step_motor_45.hpp | 15 + 15 files changed, 1278 insertions(+), 6 deletions(-) create mode 100644 components/eq_20_asb_motor/README.md create mode 100644 components/eq_20_asb_motor/eq20_servomotor.cpp create mode 100644 components/eq_20_asb_motor/eq20_servomotor.hpp create mode 100644 components/iflytop_can_slave_module_master_end/stepmotor.cpp create mode 100644 components/iflytop_can_slave_module_master_end/stepmotor.hpp create mode 100644 components/iflytop_can_slave_v1/iflytop_can_master.cpp create mode 100644 components/iflytop_can_slave_v1/iflytop_can_master.hpp create mode 100644 components/iflytop_can_slave_v1/iflytop_can_slave_protocol.hpp create mode 100644 components/modbus/modbus_basic.cpp create mode 100644 components/modbus/modbus_basic.hpp create mode 100644 components/modbus/modbus_block_host.cpp create mode 100644 components/modbus/modbus_block_host.hpp diff --git a/components/cmdscheduler/cmd_scheduler.cpp b/components/cmdscheduler/cmd_scheduler.cpp index f91397f..b699398 100644 --- a/components/cmdscheduler/cmd_scheduler.cpp +++ b/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 在这里处理指令 diff --git a/components/eq_20_asb_motor/README.md b/components/eq_20_asb_motor/README.md new file mode 100644 index 0000000..1f00b36 --- /dev/null +++ b/components/eq_20_asb_motor/README.md @@ -0,0 +1 @@ +国脉电机 \ No newline at end of file diff --git a/components/eq_20_asb_motor/eq20_servomotor.cpp b/components/eq_20_asb_motor/eq20_servomotor.cpp new file mode 100644 index 0000000..e6bf880 --- /dev/null +++ b/components/eq_20_asb_motor/eq20_servomotor.cpp @@ -0,0 +1,28 @@ +#include "eq20_servomotor.hpp" + +#include +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; +} + +// diff --git a/components/eq_20_asb_motor/eq20_servomotor.hpp b/components/eq_20_asb_motor/eq20_servomotor.hpp new file mode 100644 index 0000000..b5fe3dc --- /dev/null +++ b/components/eq_20_asb_motor/eq20_servomotor.hpp @@ -0,0 +1,24 @@ +#pragma once +#include +// #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 \ No newline at end of file diff --git a/components/iflytop_can_slave_module_master_end/stepmotor.cpp b/components/iflytop_can_slave_module_master_end/stepmotor.cpp new file mode 100644 index 0000000..2832fa7 --- /dev/null +++ b/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; +} diff --git a/components/iflytop_can_slave_module_master_end/stepmotor.hpp b/components/iflytop_can_slave_module_master_end/stepmotor.hpp new file mode 100644 index 0000000..b1a6adc --- /dev/null +++ b/components/iflytop_can_slave_module_master_end/stepmotor.hpp @@ -0,0 +1,47 @@ +#pragma once +#include +#include +#include + +#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 \ No newline at end of file diff --git a/components/iflytop_can_slave_v1/iflytop_can_master.cpp b/components/iflytop_can_slave_v1/iflytop_can_master.cpp new file mode 100644 index 0000000..422aac7 --- /dev/null +++ b/components/iflytop_can_slave_v1/iflytop_can_master.cpp @@ -0,0 +1,398 @@ +#include "sdk/os/zos.hpp" + +#ifdef HAL_CAN_MODULE_ENABLED +#include +#include +#include + +#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 diff --git a/components/iflytop_can_slave_v1/iflytop_can_master.hpp b/components/iflytop_can_slave_v1/iflytop_can_master.hpp new file mode 100644 index 0000000..329f56e --- /dev/null +++ b/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 \ No newline at end of file diff --git a/components/iflytop_can_slave_v1/iflytop_can_slave_protocol.hpp b/components/iflytop_can_slave_v1/iflytop_can_slave_protocol.hpp new file mode 100644 index 0000000..d66f04f --- /dev/null +++ b/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 \ No newline at end of file diff --git a/components/modbus/modbus_basic.cpp b/components/modbus/modbus_basic.cpp new file mode 100644 index 0000000..0f4b1e8 --- /dev/null +++ b/components/modbus/modbus_basic.cpp @@ -0,0 +1,67 @@ +/** + * Modbus + * | add(1byte) | functionCode(1byte) | ...data... | crc1(1byte) | crc2(2byte)| + * + * + * + * + */ +#include "modbus_basic.hpp" + +#include +// 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; } \ No newline at end of file diff --git a/components/modbus/modbus_basic.hpp b/components/modbus/modbus_basic.hpp new file mode 100644 index 0000000..06498f7 --- /dev/null +++ b/components/modbus/modbus_basic.hpp @@ -0,0 +1,28 @@ +#pragma once +#include +#include + +/** + * @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); \ No newline at end of file diff --git a/components/modbus/modbus_block_host.cpp b/components/modbus/modbus_block_host.cpp new file mode 100644 index 0000000..18de163 --- /dev/null +++ b/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; +} diff --git a/components/modbus/modbus_block_host.hpp b/components/modbus/modbus_block_host.hpp new file mode 100644 index 0000000..cef4318 --- /dev/null +++ b/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 diff --git a/components/step_motor_45/step_motor_45.cpp b/components/step_motor_45/step_motor_45.cpp index 544e2b6..4abd164 100644 --- a/components/step_motor_45/step_motor_45.cpp +++ b/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); diff --git a/components/step_motor_45/step_motor_45.hpp b/components/step_motor_45/step_motor_45.hpp index 48ad869..85fb920 100644 --- a/components/step_motor_45/step_motor_45.hpp +++ b/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);