diff --git a/src/iflytop/components/zcanreceiver/cmd.hpp b/src/iflytop/components/zcanreceiver/cmd.hpp new file mode 100644 index 0000000..eeb96b0 --- /dev/null +++ b/src/iflytop/components/zcanreceiver/cmd.hpp @@ -0,0 +1,48 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +namespace iflytop { +namespace zcr { + +typedef struct { + uint16_t packetindex; + uint16_t cmdid; + uint8_t subcmdid; + uint8_t packetType; + uint8_t data[]; +} Cmdheader_t; + +typedef enum { + kpt_cmd = 0, + kpt_ack = 1, + kpt_error_ack = 2, + kpt_status_report = 3, +} PacketType_t; + +typedef enum { + kcmd_ping = 0, + kcmd_read_io = 1, + kcmd_set_io = 2, + + kcmd_m211887_operation = 1000, // 维萨拉压力传感器 + kcmd_read_presure_sensor = 1001, + kcmd_triple_warning_light_ctl = 1002, + kcmd_high_power_electrical_ctl = 1003, + kcmd_peristaltic_pump_ctl = 1004, + kcmd_read_huacheng_pressure_sensor = 1005, +} CmdID_t; + +} // namespace zcr +} // namespace iflytop + +/** + * @brief + */ diff --git a/src/iflytop/components/zcanreceiver/zcanreceiverhost.cpp b/src/iflytop/components/zcanreceiver/zcanreceiverhost.cpp new file mode 100644 index 0000000..7f6b152 --- /dev/null +++ b/src/iflytop/components/zcanreceiver/zcanreceiverhost.cpp @@ -0,0 +1,163 @@ +#include "zcanreceiverhost.hpp" + +#include +#include +#include +using namespace iflytop; + +#define TAG "ZCanReceiver" + +#define OVER_TIME_MS 5 + +using namespace iflytop; +ZCanReceiverHost::ZCanReceiverHost() {} + +void ZCanReceiverHost::initialize(string can_if_name, int baudrate, bool enablLoopback) { + // m_iflytopCanProtocolStack.reset(new IflytopCanProtocolStack()); + m_can_if_name = can_if_name; + m_baudrate = baudrate; + m_enablLoopback = enablLoopback; + resetSocketCan(); +} +void ZCanReceiverHost::registerListener(onpacket_t onpacket) { m_onpacket = onpacket; } + +shared_ptr ZCanReceiverHost::sendcmdblock(shared_ptr cmd, int overtime_ms) { // + lock_guard lock(m_txblocklock); + + { + lock_guard lock(m_txContextLock); + m_txContext.txcmdcache = cmd; + m_txContext.isReady = false; + } + + auto enter = zsteady_clock().now(); + sendcmd(cmd->packetindex, cmd->cmdid, cmd->subcmdid, cmd->data, cmd->len); + + while (true) { + if (m_txContext.isReady) { + break; + } + if (zsteady_clock().elapsedTimeMs(enter) > overtime_ms) { + break; + } + this_thread::sleep_for(chrono::milliseconds(1)); + } + + if (!m_txContext.isReady) { + logger->warn("sendcmdblock(index{},cmd{},subcmd{}) fail,timeout", cmd->packetindex, cmd->cmdid, cmd->subcmdid); + return nullptr; + } + return m_txContext.rx; +} + +void ZCanReceiverHost::sendcmd(uint16_t packetindex, uint16_t cmdid, uint8_t subcmdid, uint8_t *data, size_t len) { + Cmdheader_t *header = (Cmdheader_t *)m_sendbuf; + header->packetindex = packetindex; + header->cmdid = cmdid; + header->subcmdid = subcmdid; + header->packetType = 0x00; + memcpy(header->data, data, len); + sendPacket(m_sendbuf, sizeof(Cmdheader_t) + len); +} +void ZCanReceiverHost::sendPacket(uint8_t *packet, size_t len) { + /** + * @brief + */ + int npacket = len / 8 + (len % 8 == 0 ? 0 : 1); + if (npacket > 255) { + return; + } + int finalpacketlen = len % 8 == 0 ? 8 : len % 8; + for (uint8_t i = 0; i < npacket; i++) { + bool suc = false; + if (i == npacket - 1) { + suc = sendPacketSub(npacket, i, packet + i * 8, finalpacketlen, OVER_TIME_MS); + } else { + suc = sendPacketSub(npacket, i, packet + i * 8, 8, OVER_TIME_MS); + } + if (!suc) { + printf("sendPacket fail, packet(%d:%d)\n", npacket, i); + return; + } + } +} + +bool ZCanReceiverHost::sendPacketSub(int npacket, int packetIndex, uint8_t *packet, size_t len, int overtimems) { + uint32_t id = (m_deviceId << 16) | (npacket << 8) | packetIndex; + // m_comcan.sendframe(CANUSB_FRAME_EXTENDED, id, packet, len); + shared_ptr frame = SocketCanFrame::createExtDataFrame(id, packet, len); + m_socketCan->sendFrame(frame, overtimems); + return true; +} + +void ZCanReceiverHost::processOnePacket(CanPacketRxBuffer *rxbuf, uint8_t *packet, size_t len) { // + + Cmdheader_t *header = (Cmdheader_t *)packet; + // uint8_t from = rxbuf->id; + + shared_ptr rx = make_shared(); + rx->packetindex = header->packetindex; + rx->cmdid = header->cmdid; + rx->subcmdid = header->subcmdid; + rx->len = len - sizeof(Cmdheader_t); + memcpy(rx->data, header->data, rx->len); + + /** + * @brief 匹配下发的命令 + */ + if (header->packetType == kpt_ack || header->packetType == kpt_error_ack) { + if (header->packetindex == m_txContext.txcmdcache->packetindex) { + lock_guard lock(m_txContextLock); + m_txContext.rx = rx; + m_txContext.isReady = true; + } + return; + } + /** + * @brief 处理其他类型的消息 + */ +} + +void ZCanReceiverHost::processRx(shared_ptr frame) { + uint8_t from = (frame->getId() >> 16 & 0xFF); + uint8_t nframe = (frame->getId() & 0xFF00) >> 8; + uint8_t frameId = (frame->getId() & 0x00FF); + + CanPacketRxBuffer *rxbuf = &m_canPacketRxBuffer[from]; + rxbuf->id = from; + + if (frameId == 0) { + rxbuf->m_canPacketNum = 0; + rxbuf->m_rxdataLen = 0; + } + + if (rxbuf->m_canPacketNum < 255) { + rxbuf->m_canPacket[rxbuf->m_canPacketNum] = frame; + rxbuf->m_canPacketNum++; + memcpy(rxbuf->m_rxdata + rxbuf->m_rxdataLen, frame->getData(), frame->getDlc()); + rxbuf->m_rxdataLen += frame->getDlc(); + } + if (nframe == frameId + 1) { + /** + * @brief 接收到最后一包数据 + */ + processOnePacket(rxbuf, rxbuf->m_rxdata, rxbuf->m_rxdataLen); + } +} + +void ZCanReceiverHost::resetSocketCan() { + auto socketCanConfig = make_shared(); + socketCanConfig->enablLoopback = m_enablLoopback; // 根据 SocketCan::dumpCanDriverInfo() 的输出，确定该标志位是false还是true + socketCanConfig->m_canName = m_can_if_name; + socketCanConfig->m_canBaudrate = m_baudrate; + socketCanConfig->m_canfilters = {}; + + logger->info("initialize() m_canName:{} {}", socketCanConfig->m_canName, socketCanConfig->m_canBaudrate); + + m_socketCan.reset(new SocketCan()); + m_socketCan->initialize(socketCanConfig); + m_socketCan->startListen(); + m_socketCan->onSocketCanFrame.connect([this](shared_ptr canframe) { // + // processRx(canframe); + }); +} \ No newline at end of file diff --git a/src/iflytop/components/zcanreceiver/zcanreceiverhost.hpp b/src/iflytop/components/zcanreceiver/zcanreceiverhost.hpp new file mode 100644 index 0000000..886d2c4 --- /dev/null +++ b/src/iflytop/components/zcanreceiver/zcanreceiverhost.hpp @@ -0,0 +1,84 @@ +// +// Created by zwsd +// + +#pragma once +#include + +#include "cmd.hpp" +#include "iflytop/core/basic/nlohmann/json.hpp" +#include "iflytop/core/core.hpp" +#include "iflytop/core/driver/socketcan/socket_can.hpp" +#include "iflytop/core/spdlogfactory/logger.hpp" +#include "zservice_container/zservice_container.hpp" + +namespace iflytop { + +using namespace std; +using namespace zcr; + +class ZCanReceiverCMD { + public: + uint16_t packetindex; + uint16_t cmdid; + uint8_t subcmdid; + uint8_t data[255 * 8]; + size_t len; +}; + +class ZCanReceiverHost { + ENABLE_LOGGER(ZCanReceiverHost); + + public: + class CanPacketRxBuffer { + public: + uint16_t id; + shared_ptr m_canPacket[256]; // 用于接收can消息 + uint8_t m_rxdata[255 * 8]; + uint8_t m_canPacketNum = 0; + uint8_t m_rxdataLen = 0; + }; + + class TxContext { + public: + shared_ptr txcmdcache; + shared_ptr rx; + bool isReady; + }; + + typedef function onpacket_t; + + public: + uint8_t m_sendbuf[1000]; + CanPacketRxBuffer m_canPacketRxBuffer[255]; + onpacket_t m_onpacket; + uint8_t m_deviceId = 1; + + shared_ptr m_socketCan; + + mutex m_txblocklock; + mutex m_txContextLock; + TxContext m_txContext; + + string m_can_if_name; + int m_baudrate; + bool m_enablLoopback; + + public: + ZCanReceiverHost(); + + void initialize(string can_if_name, int baudrate, bool enablLoopback); + void registerListener(onpacket_t onpacket); + + shared_ptr sendcmdblock(shared_ptr cmd, int overtime_ms); + + private: + void processRx(shared_ptr frame); + void processOnePacket(CanPacketRxBuffer *rxbuf, uint8_t *packet, size_t len); + void resetSocketCan(); + void sendcmd(uint16_t packetindex, uint16_t cmdid, uint8_t subcmdid, uint8_t *data, size_t len); + void sendPacket(uint8_t *packet, size_t len); + bool sendPacketSub(int npacket, int packetIndex, uint8_t *packet, size_t len, int overtimems); +}; + +} // namespace iflytop \ No newline at end of file