update

src/iflytop/components/zcanreceiver/cmd.hpp

@@ -0,0 +1,48 @@
+#pragma once
+#include <fstream>
+#include <iostream>
+#include <list>
+#include <map>
+#include <memory>
+#include <sstream>
+#include <string>
+#include <vector>
+#include <set>
+#include <functional>
+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
+ */

src/iflytop/components/zcanreceiver/zcanreceiverhost.cpp

@@ -0,0 +1,163 @@
+#include "zcanreceiverhost.hpp"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+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<ZCanReceiverCMD> ZCanReceiverHost::sendcmdblock(shared_ptr<ZCanReceiverCMD> cmd, int overtime_ms) {  //
+  lock_guard<mutex> lock(m_txblocklock);
+
+  {
+    lock_guard<mutex> 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<SocketCanFrame> 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<ZCanReceiverCMD> rx = make_shared<ZCanReceiverCMD>();
+  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<mutex> lock(m_txContextLock);
+      m_txContext.rx      = rx;
+      m_txContext.isReady = true;
+    }
+    return;
+  }
+  /**
+   * @brief 处理其他类型的消息
+   */
+}
+
+void ZCanReceiverHost::processRx(shared_ptr<SocketCanFrame> 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>();
+  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<SocketCanFrame> canframe) {  //
+    // processRx(canframe);
+  });
+}

src/iflytop/components/zcanreceiver/zcanreceiverhost.hpp

@@ -0,0 +1,84 @@
+//
+// Created by zwsd
+//
+
+#pragma once
+#include <mutex>
+
+#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<SocketCanFrame> 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<ZCanReceiverCMD> txcmdcache;
+    shared_ptr<ZCanReceiverCMD> rx;
+    bool                        isReady;
+  };
+
+  typedef function<void(CanPacketRxBuffer *rxbuf, uint8_t *packet, size_t len)> onpacket_t;
+
+ public:
+  uint8_t           m_sendbuf[1000];
+  CanPacketRxBuffer m_canPacketRxBuffer[255];
+  onpacket_t        m_onpacket;
+  uint8_t           m_deviceId = 1;
+
+  shared_ptr<SocketCan> 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<ZCanReceiverCMD> sendcmdblock(shared_ptr<ZCanReceiverCMD> cmd, int overtime_ms);
+
+ private:
+  void processRx(shared_ptr<SocketCanFrame> 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