project_bycmd/by_frame.c

#include "by_frame.h"

#include "by_serial.h"
#include "crc16/crc16.h"
#include "logc/log.h"

by_serial_t serial_port;
by_frame_queue_t queue_recv;

uint8_t frame_buffer_recv[100];
uint8_t frame_buffer_send[4 + BY_FRAME_DATA_NUM * sizeof(uint32_t)];

int frame_parse_busy;

int by_frame_queue_pop(by_frame_queue_t *queue, uint8_t *element)
{
  if (!queue->len) {
    return -1;
  }

  *element = queue->buff[0];
  queue->len -= 1;
  memmove(queue->buff, queue->buff + 1, queue->len);
  return 0;
}

int by_frame_queue_add(by_frame_queue_t *queue, uint8_t element)
{
  if (queue->size == queue->len) {
    return -1;
  }

  queue->buff[queue->len] = element;
  queue->len += 1;

  return 0;
}

int by_frame_queue_copy(by_frame_queue_t *queue, uint8_t *buff_out, int num)
{
  int copy_num = (queue->len < num) ? queue->len : num;
  memcpy(buff_out, queue->buff, copy_num);

  return copy_num;
}

int by_frame_queue_drop(by_frame_queue_t *queue, int num)
{
  int drop_num = 0;
  uint8_t element_temp;

  for (int i = 0; i < num; i++) {
    drop_num += (!by_frame_queue_pop(queue, &element_temp));
  }

  return drop_num;
}

int by_frame_queue_find(by_frame_queue_t *queue, const uint8_t element)
{
  int found_num = -1;

  for (int i = 0; i < queue->len; i++) {
    if (queue->buff[i] == element) {
      found_num = i;
      break;
    }
  }

  return found_num;
}

int by_frame_queue_get_used(by_frame_queue_t *queue)
{
  return queue->len;
}

void by_frame_queue_init(by_frame_queue_t *queue, uint8_t *buff, int buff_len)
{
  memset(queue, 0, sizeof(by_frame_queue_t));
  memset(buff, 0, buff_len);

  queue->buff = buff;
  queue->size = buff_len;
}

void by_frame_send(uint8_t cmd, uint8_t *data_array, uint8_t len)
{
  uint16_t crc_cal            = 0;
  const uint8_t data_byte_num = BY_FRAME_DATA_NUM * sizeof(uint32_t);

  memset(frame_buffer_send, 0, sizeof(frame_buffer_send));
  frame_buffer_send[0] = BY_FRAME_HEAD;
  frame_buffer_send[1] = cmd;

  if (len > BY_FRANE_DATA_LEN) {
    len = BY_FRANE_DATA_LEN;
  }

  memcpy(frame_buffer_send + 2, data_array, len);
  crc_cal = crc16_check(frame_buffer_send, 2 + BY_FRANE_DATA_LEN);

  frame_buffer_send[2 + BY_FRANE_DATA_LEN] = (uint8_t)(crc_cal >> 8);
  frame_buffer_send[3 + BY_FRANE_DATA_LEN] = (uint8_t)(crc_cal);

  by_serial_write(&serial_port, frame_buffer_send, BY_FRANE_LEN);
}

/**
 * @brief
 *
 * @param data_num
 * @param data_array
 * @todo 将其中写死的数据长度按照宏定义给出
 */
int by_frame_parse(uint8_t *cmd, uint32_t *data_array)
{
  uint8_t frame_buff[BY_FRANE_LEN] = {0};

  // 先从缓冲区内复制数据到临时 buffer 里
  for (int i = 0; i < by_serial_get_used_buffer_len(&serial_port); i++) {
    uint8_t data = 0;
    by_serial_read(&serial_port, &data, 1);
    by_frame_queue_add(&queue_recv, data);
  }

  // 丢弃缓冲区头部无法解析的数据，如果剩下的数据过少则不解析
  //   log_debug("drop %d", by_frame_queue_find(&queue_recv, BY_FRAME_HEAD));
  int used_num = by_frame_queue_find(&queue_recv, BY_FRAME_HEAD);
  by_frame_queue_drop(&queue_recv, used_num);
  // 不存在帧头时退出
  if (-1 == used_num) {
    return -1;
  }
  // 剩余数据过少时退出
  if (BY_FRANE_LEN > by_frame_queue_get_used(&queue_recv)) {
    return -1;
  }

  log_debug("start parsing");
  by_frame_queue_copy(&queue_recv, frame_buff, BY_FRANE_LEN);

  uint16_t crc_val = (uint16_t)frame_buff[BY_FRANE_LEN - 1];
  crc_val |= (uint16_t)(frame_buff[BY_FRANE_LEN - 2] << 8);

  uint16_t crc_cal = crc16_check(frame_buff, BY_FRANE_LEN - 2);

  if (crc_val == crc_cal) {
    log_info("received successful");
    // 丢掉当前帧头，下次解析时就不从该帧头开始
    by_frame_queue_drop(&queue_recv, 1);
    // TODO 复制数据
    *cmd = frame_buff[1];
    memcpy(data_array, &frame_buff[2], BY_FRANE_DATA_LEN);
    return 0;
  } else {
    log_warn("receive failed");
    log_warn("cal crc 0x%04X, but got 0x%04X", crc_cal, crc_val);
    // 丢掉当前帧头，下次解析时就不从该帧头开始
    by_frame_queue_drop(&queue_recv, 1);
    return -1;
  }
}

int by_frame_init(void)
{
  by_frame_queue_init(&queue_recv, frame_buffer_recv, sizeof(frame_buffer_recv));
  return (by_serial_init(&serial_port, "/dev/ttyTHS0"));
}