iflytoplinuxsdk_ptdv1/src/iflytop/components/zcanreceiver/zcanreceiverhost.cpp

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

  m_index++;
  if (m_index == 0) {
    m_index = 1;
  }
  if (m_index == UINT16_MAX) {
    m_index = 1;
  }
  cmd->packetindex = m_index;

  {
    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);
  // printf("sendcmd(%d:%d:%d) ", packetindex, cmdid, subcmdid);
  // for (size_t i = 0; i < len; i++) {
  //   printf("%02x ", data[i]);
  // }
  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) {
      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_deviceId << 16) | (npacket << 8) | packetIndex;
  // 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::createExtDataFrame(id, packet, 8); /*TODO，这里如果不写8,会报错，原因未知*/
  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->packetType                 = header->packetType;
  rx->len                        = len - sizeof(Cmdheader_t);
  memcpy(rx->data, header->data, rx->len);

  /**
   * @brief 匹配下发的命令
   */
  logger->debug("processOnePacket(index {}, packetType {} ,cmd {},subcmd {}),rx(index {})", header->packetindex, header->packetType, header->cmdid,
                header->subcmdid, rx->packetindex);
  if (header->packetType == kpt_ack || header->packetType == kpt_error_ack) {
    if (header->packetindex == m_txContext.txcmdcache->packetindex) {
      lock_guard<mutex> lock(m_txContextLock);
      logger->debug("rx ack");
      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) {  //
    logger->debug("onSocketCanFrame {}", canframe->toString());
    processRx(canframe);
  });
}