tdcan/src/components/zcanreceiver/zcanreceiverhost.cpp

#include "zcanreceiverhost.hpp"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "utils/stringutils.hpp"
using namespace iflytop;

#define TAG "ZCanReceiver"

#define OVER_TIME_MS 20

using namespace iflytop;

ZCanReceiverHost::ZCanReceiverHost() {}

void ZCanReceiverHost::initialize(string can_if_name, int baudrate) {
  // m_iflytopCanProtocolStack.reset(new IflytopCanProtocolStack());
  m_can_if_name   = can_if_name;
  m_baudrate      = baudrate;
  m_enablLoopback = false;

  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->debug("initialize() m_canName:{} {}", socketCanConfig->m_canName, socketCanConfig->m_canBaudrate);

  m_socketCan.reset(new SocketCan());
  m_socketCan->initialize(socketCanConfig);
  m_socketCan->onSocketCanFrame.connect([this](shared_ptr<SocketCanFrame> canframe) {  //
    if (canframe->getId() == m_hostId) {
      logger->debug("TX RAW: {} SUC", canframe->toString());
      return;
    }
    logger->debug("RX RAW: {}", canframe->toString());
    processRx(canframe);
  });
}
void ZCanReceiverHost::registerListener(onpacket_t onpacket) { m_onpacket = onpacket; }

void ZCanReceiverHost::sendPacket(uint8_t *packet, size_t len) {
  logger->debug("TX: {}", StringUtils().bytesToString(packet, len));
  int npacket = len / 7 + (len % 7 == 0 ? 0 : 1);
  if (npacket > 255) {
    return;
  }
  int finalpacketlen = len % 7 == 0 ? 7 : len % 7;
  for (uint8_t i = 0; i < npacket; i++) {
    bool suc = false;
    if (i == npacket - 1) {
      suc = sendPacketSub(npacket, i, packet + i * 7, finalpacketlen, OVER_TIME_MS);
    } else {
      suc = sendPacketSub(npacket, i, packet + i * 7, 7, OVER_TIME_MS);
    }
    if (!suc) {
      logger->warn("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_hostId;
  uint8_t  txdata[8] = {0};
  txdata[0]          = npacket << 4 | packetIndex;
  memcpy(txdata + 1, packet, len);
  // m_comcan.sendframe(CANUSB_FRAME_EXTENDED, id, packet, len);
  // printf("sendPacketSub(%d:%d) ", npacket, packetIndex);
  // for (size_t i = 0; i < len; i++) {
  //   printf("%02x ", packet[i]);
  // }
  // printf("\n");

  shared_ptr<SocketCanFrame> frame = SocketCanFrame::createStdDataFrame(id, txdata, len + 1);
  logger->debug("TX RAW :{}", frame->toString());
  m_socketCan->sendFrame(frame, overtimems);
  return true;
}

void ZCanReceiverHost::processOnePacket(CanPacketRxBuffer *rxbuf, uint8_t *packet, size_t len) {  //
  if (m_onpacket) m_onpacket(rxbuf->id, packet, len);
}

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);

  uint8_t from    = (frame->getId() & 0xFF);
  uint8_t nframe  = ((frame->getData()[0] >> 4) & 0x0F);
  uint8_t frameId = (frame->getData()[0] & 0x0F);

  CanPacketRxBuffer *rxbuf = &m_canPacketRxBuffer[from];
  rxbuf->id                = from;

  if (frameId == 0) {
    rxbuf->m_canPacketNum = 0;
    rxbuf->m_rxdataLen    = 0;
  }

  if (frameId != rxbuf->m_canPacketNum) {
    rxbuf->m_canPacketNum = 0;
    rxbuf->m_rxdataLen    = 0;
    return;
  }

  if (rxbuf->m_canPacketNum < 255) {
    rxbuf->m_canPacket[rxbuf->m_canPacketNum] = frame;
    rxbuf->m_canPacketNum++;
    memcpy(rxbuf->m_rxdata + rxbuf->m_rxdataLen, frame->getData() + 1, frame->getDlc() - 1);
    rxbuf->m_rxdataLen += frame->getDlc() - 1;
  }

  if (nframe == frameId + 1) {
    /**
     * @brief 接收到最后一包数据
     */
    logger->debug("RX: {}", StringUtils().bytesToString(rxbuf->m_rxdata, rxbuf->m_rxdataLen));
    processOnePacket(rxbuf, rxbuf->m_rxdata, rxbuf->m_rxdataLen);
  }
}

void ZCanReceiverHost::restartCanInterface() { m_socketCan->reStartCanInterface(); }