491 lines
15 KiB
C
491 lines
15 KiB
C
/**
|
||
* @file protocol.c
|
||
* @brief EDC 通讯协议实现 — CRC16 / 组帧 / 解析 / I2C / UART
|
||
*/
|
||
|
||
#include "protocol.h"
|
||
#include <string.h>
|
||
#include "esp_log.h"
|
||
#include "freertos/task.h"
|
||
|
||
static const char *TAG = "EDC_PROTO";
|
||
|
||
/* ================================================================
|
||
* CRC-16/MODBUS
|
||
*
|
||
* 多项式: 0x8005 (逆序 0xA001)
|
||
* 初始值: 0xFFFF
|
||
* 输入/输出: 反射
|
||
* 异或输出: 0x0000
|
||
* 校验范围: CMD + LEN + DATA
|
||
* ================================================================ */
|
||
|
||
uint16_t edc_crc16(const uint8_t *data, size_t len)
|
||
{
|
||
uint16_t crc = 0xFFFF;
|
||
for (size_t i = 0; i < len; i++) {
|
||
crc ^= data[i];
|
||
for (int j = 0; j < 8; j++) {
|
||
if (crc & 0x0001) {
|
||
crc = (crc >> 1) ^ 0xA001;
|
||
} else {
|
||
crc >>= 1;
|
||
}
|
||
}
|
||
}
|
||
return crc;
|
||
}
|
||
|
||
/* ================================================================
|
||
* 大端序 uint16_t 写入 / 读取
|
||
* ================================================================ */
|
||
|
||
static inline void write_u16_be(uint8_t *buf, uint16_t val)
|
||
{
|
||
buf[0] = (uint8_t)(val >> 8);
|
||
buf[1] = (uint8_t)(val);
|
||
}
|
||
|
||
static inline uint16_t read_u16_be(const uint8_t *buf)
|
||
{
|
||
return ((uint16_t)buf[0] << 8) | buf[1];
|
||
}
|
||
|
||
/* ================================================================
|
||
* Cycle 组帧
|
||
*
|
||
* DATA 区: cycle_id(2B,BE) + frag_count(1B) + fragments(N×12B)
|
||
* Fragment: src.x(2B) src.y(2B) src.angle(2B) dst.x(2B) dst.y(2B) dst.angle(2B)
|
||
* ================================================================ */
|
||
|
||
int edc_cycle_pack(const edc_cycle_t *cycle, uint8_t *out)
|
||
{
|
||
uint8_t count = cycle->frag_count;
|
||
if (count < EDC_FRAG_MIN) count = EDC_FRAG_MIN;
|
||
if (count > EDC_FRAG_MAX) count = EDC_FRAG_MAX;
|
||
|
||
uint8_t data_len = 3 + count * EDC_FRAG_BYTES; /* id(2) + cnt(1) + frags */
|
||
|
||
out[0] = EDC_PROTO_SOF1;
|
||
out[1] = EDC_PROTO_SOF2;
|
||
out[2] = EDC_CMD_CYCLE_DATA;
|
||
out[3] = data_len;
|
||
|
||
/* cycle_id (大端) */
|
||
uint8_t *p = &out[4];
|
||
write_u16_be(p, cycle->cycle_id);
|
||
p += 2;
|
||
|
||
/* frag_count */
|
||
*p++ = count;
|
||
|
||
/* fragments */
|
||
for (int i = 0; i < count; i++) {
|
||
const edc_fragment_t *f = &cycle->fragments[i];
|
||
write_u16_be(p, f->source.x); p += 2;
|
||
write_u16_be(p, f->source.y); p += 2;
|
||
write_u16_be(p, f->source.angle_x10); p += 2;
|
||
write_u16_be(p, f->target.x); p += 2;
|
||
write_u16_be(p, f->target.y); p += 2;
|
||
write_u16_be(p, f->target.angle_x10); p += 2;
|
||
}
|
||
|
||
/* CRC16 over [CMD, LEN, DATA] */
|
||
uint16_t crc = edc_crc16(&out[2], 2 + data_len);
|
||
*p++ = (uint8_t)(crc);
|
||
*p++ = (uint8_t)(crc >> 8);
|
||
|
||
return (int)(p - out); /* 2 + 1 + 1 + data_len + 2 */
|
||
}
|
||
|
||
/* ================================================================
|
||
* Cycle 解析 (逐字节)
|
||
* ================================================================ */
|
||
|
||
edc_parse_result_t edc_cycle_parse_byte(edc_frame_ctx_t *ctx, uint8_t byte,
|
||
edc_cycle_t *cycle)
|
||
{
|
||
switch (ctx->state) {
|
||
case EDC_PARSE_OK:
|
||
ctx->idx = 0;
|
||
ctx->state = EDC_PARSE_WAITING;
|
||
/* fall through */
|
||
case EDC_PARSE_WAITING:
|
||
default:
|
||
if (byte == EDC_PROTO_SOF1) {
|
||
ctx->buf[0] = byte;
|
||
ctx->idx = 1;
|
||
}
|
||
return EDC_PARSE_WAITING;
|
||
}
|
||
|
||
/* ---- 收帧中 (idx >= 1) ---- */
|
||
|
||
if (ctx->idx == 1) {
|
||
if (byte != EDC_PROTO_SOF2) {
|
||
ctx->idx = 0;
|
||
if (byte == EDC_PROTO_SOF1) { ctx->buf[0] = byte; ctx->idx = 1; }
|
||
return EDC_PARSE_WAITING;
|
||
}
|
||
ctx->buf[1] = byte;
|
||
ctx->idx = 2;
|
||
return EDC_PARSE_WAITING;
|
||
}
|
||
|
||
if (ctx->idx < EDC_PROTO_FRAME_MAX) {
|
||
ctx->buf[ctx->idx] = byte;
|
||
}
|
||
ctx->idx++;
|
||
|
||
/* 确定帧总长 */
|
||
if (ctx->idx == 4) {
|
||
uint8_t data_len = ctx->buf[3];
|
||
if (data_len < 3 || data_len > EDC_PROTO_FRAME_MAX - 6) {
|
||
ctx->idx = 0; ctx->state = EDC_PARSE_WAITING;
|
||
return EDC_PARSE_LEN_ERR;
|
||
}
|
||
ctx->len = 2 + 1 + 1 + data_len + 2; /* SOF+CMD+LEN+DATA+CRC */
|
||
}
|
||
|
||
/* 收满一帧 */
|
||
if (ctx->idx >= ctx->len) {
|
||
ctx->idx = ctx->len;
|
||
|
||
/* CRC 校验 */
|
||
uint16_t crc_calc = edc_crc16(&ctx->buf[2], 2 + ctx->buf[3]);
|
||
uint16_t crc_recv = ctx->buf[ctx->len - 2] | ((uint16_t)ctx->buf[ctx->len - 1] << 8);
|
||
if (crc_calc != crc_recv) {
|
||
ctx->idx = 0; ctx->state = EDC_PARSE_WAITING;
|
||
return EDC_PARSE_CRC_ERR;
|
||
}
|
||
|
||
/* 仅处理 CYCLE_DATA 命令 */
|
||
uint8_t cmd = ctx->buf[2];
|
||
if (cmd != EDC_CMD_CYCLE_DATA) {
|
||
ctx->idx = 0; ctx->state = EDC_PARSE_WAITING;
|
||
return EDC_PARSE_CMD_ERR;
|
||
}
|
||
|
||
/* 解析 DATA 区 */
|
||
const uint8_t *d = &ctx->buf[4];
|
||
cycle->cycle_id = read_u16_be(d); d += 2;
|
||
cycle->frag_count = *d; d += 1;
|
||
|
||
if (cycle->frag_count < EDC_FRAG_MIN || cycle->frag_count > EDC_FRAG_MAX) {
|
||
ctx->idx = 0; ctx->state = EDC_PARSE_WAITING;
|
||
return EDC_PARSE_LEN_ERR;
|
||
}
|
||
|
||
for (int i = 0; i < cycle->frag_count; i++) {
|
||
edc_fragment_t *f = &cycle->fragments[i];
|
||
f->source.x = read_u16_be(d); d += 2;
|
||
f->source.y = read_u16_be(d); d += 2;
|
||
f->source.angle_x10 = read_u16_be(d); d += 2;
|
||
f->target.x = read_u16_be(d); d += 2;
|
||
f->target.y = read_u16_be(d); d += 2;
|
||
f->target.angle_x10 = read_u16_be(d); d += 2;
|
||
}
|
||
cycle->timestamp_ms = 0;
|
||
|
||
ctx->state = EDC_PARSE_OK;
|
||
return EDC_PARSE_OK;
|
||
}
|
||
|
||
return EDC_PARSE_WAITING;
|
||
}
|
||
|
||
/* ================================================================
|
||
* Cycle 解析 (一次性)
|
||
* ================================================================ */
|
||
|
||
edc_parse_result_t edc_cycle_parse_buf(const uint8_t *data, size_t len,
|
||
edc_cycle_t *cycle)
|
||
{
|
||
edc_frame_ctx_t ctx = { .state = EDC_PARSE_WAITING };
|
||
edc_parse_result_t res;
|
||
for (size_t i = 0; i < len; i++) {
|
||
res = edc_cycle_parse_byte(&ctx, data[i], cycle);
|
||
if (res != EDC_PARSE_WAITING) return res;
|
||
}
|
||
return EDC_PARSE_WAITING;
|
||
}
|
||
|
||
/* ================================================================
|
||
* 统一收发器
|
||
* ================================================================ */
|
||
|
||
/* ---- 共享最新 Cycle ---- */
|
||
static edc_cycle_t s_latest_cycle;
|
||
static volatile bool s_cycle_updated;
|
||
static portMUX_TYPE s_lock = portMUX_INITIALIZER_UNLOCKED;
|
||
|
||
/* ---- 统计 ---- */
|
||
static edc_proto_stat_t s_stat;
|
||
|
||
/* ---- 通道使能 ---- */
|
||
static bool s_i2c_enabled;
|
||
static bool s_uart_enabled;
|
||
|
||
/* ---- I2C ---- */
|
||
static i2c_slave_dev_handle_t s_i2c_handle;
|
||
static edc_cycle_t s_i2c_rx_cycle;
|
||
static volatile bool s_i2c_rx_pending;
|
||
|
||
/* ---- UART ---- */
|
||
static uart_port_t s_uart_port;
|
||
static edc_frame_ctx_t s_uart_ctx;
|
||
|
||
/* ================================================================
|
||
* 内部: 存入共享 Cycle
|
||
* ================================================================ */
|
||
|
||
static void store_cycle(const edc_cycle_t *cycle, edc_channel_t ch)
|
||
{
|
||
portENTER_CRITICAL(&s_lock);
|
||
memcpy(&s_latest_cycle, cycle, sizeof(edc_cycle_t));
|
||
s_latest_cycle.timestamp_ms = xTaskGetTickCount() * portTICK_PERIOD_MS;
|
||
s_cycle_updated = true;
|
||
portEXIT_CRITICAL(&s_lock);
|
||
|
||
if (ch == EDC_CH_I2C) s_stat.i2c.cycles_ok++;
|
||
else if (ch == EDC_CH_UART) s_stat.uart.cycles_ok++;
|
||
s_stat.last_channel = ch;
|
||
s_stat.last_tick = xTaskGetTickCount();
|
||
}
|
||
|
||
/* ================================================================
|
||
* I2C 从机 接收
|
||
* ================================================================ */
|
||
|
||
static bool on_i2c_rx(i2c_slave_dev_handle_t handle,
|
||
const i2c_slave_rx_done_event_data_t *evt,
|
||
void *arg)
|
||
{
|
||
s_stat.i2c.bytes_total += evt->length;
|
||
|
||
edc_cycle_t cycle;
|
||
if (edc_cycle_parse_buf(evt->buffer, evt->length, &cycle) == EDC_PARSE_OK) {
|
||
memcpy(&s_i2c_rx_cycle, &cycle, sizeof(edc_cycle_t));
|
||
s_i2c_rx_cycle.timestamp_ms = xTaskGetTickCountFromISR() * portTICK_PERIOD_MS;
|
||
s_i2c_rx_pending = true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
static bool i2c_poll_once(void)
|
||
{
|
||
if (!s_i2c_rx_pending) return false;
|
||
|
||
edc_cycle_t cycle;
|
||
portENTER_CRITICAL(&s_lock);
|
||
memcpy(&cycle, &s_i2c_rx_cycle, sizeof(edc_cycle_t));
|
||
s_i2c_rx_pending = false;
|
||
portEXIT_CRITICAL(&s_lock);
|
||
|
||
store_cycle(&cycle, EDC_CH_I2C);
|
||
ESP_LOGD(TAG, "I2C cycle OK (id=%u frags=%u)", cycle.cycle_id, cycle.frag_count);
|
||
return true;
|
||
}
|
||
|
||
/* ================================================================
|
||
* UART 接收
|
||
* ================================================================ */
|
||
|
||
static bool uart_poll_once(void)
|
||
{
|
||
uint8_t byte;
|
||
bool got = false;
|
||
|
||
while (uart_read_bytes(s_uart_port, &byte, 1, pdMS_TO_TICKS(0)) > 0) {
|
||
s_stat.uart.bytes_total++;
|
||
|
||
edc_cycle_t cycle;
|
||
edc_parse_result_t res = edc_cycle_parse_byte(&s_uart_ctx, byte, &cycle);
|
||
if (res == EDC_PARSE_OK) {
|
||
store_cycle(&cycle, EDC_CH_UART);
|
||
got = true;
|
||
ESP_LOGD(TAG, "UART cycle OK (id=%u frags=%u)", cycle.cycle_id, cycle.frag_count);
|
||
} else if (res < 0) {
|
||
s_stat.uart.cycles_err++;
|
||
ESP_LOGW(TAG, "UART parse err: %d", res);
|
||
}
|
||
}
|
||
return got;
|
||
}
|
||
|
||
/* ================================================================
|
||
* 内部: 通过 UART 发送短帧
|
||
* ================================================================ */
|
||
|
||
static void uart_send_frame(uint8_t cmd, const uint8_t *data, uint8_t data_len)
|
||
{
|
||
uint8_t buf[EDC_PROTO_FRAME_MAX];
|
||
buf[0] = EDC_PROTO_SOF1;
|
||
buf[1] = EDC_PROTO_SOF2;
|
||
buf[2] = cmd;
|
||
buf[3] = data_len;
|
||
if (data && data_len) memcpy(&buf[4], data, data_len);
|
||
|
||
uint16_t crc = edc_crc16(&buf[2], 2 + data_len);
|
||
buf[4 + data_len] = (uint8_t)(crc);
|
||
buf[4 + data_len + 1] = (uint8_t)(crc >> 8);
|
||
|
||
int total = 4 + data_len + 2;
|
||
uart_write_bytes(s_uart_port, (const char *)buf, total);
|
||
}
|
||
|
||
/* ================================================================
|
||
* 初始化
|
||
* ================================================================ */
|
||
|
||
void edc_proto_init(const edc_proto_config_t *cfg)
|
||
{
|
||
edc_proto_config_t dft = {
|
||
.i2c_enable = true,
|
||
.i2c_port = EDC_I2C_PORT_DEFAULT,
|
||
.i2c_addr = EDC_I2C_SLAVE_ADDR,
|
||
.i2c_sda = EDC_I2C_SDA_DEFAULT,
|
||
.i2c_scl = EDC_I2C_SCL_DEFAULT,
|
||
.uart_enable = true,
|
||
.uart_port = EDC_UART_PORT_DEFAULT,
|
||
.uart_baudrate = EDC_UART_BAUDRATE,
|
||
.uart_rx = EDC_UART_RX_DEFAULT,
|
||
.uart_tx = EDC_UART_TX_DEFAULT,
|
||
};
|
||
if (cfg) dft = *cfg;
|
||
|
||
s_i2c_enabled = dft.i2c_enable;
|
||
s_uart_enabled = dft.uart_enable;
|
||
memset(&s_stat, 0, sizeof(s_stat));
|
||
|
||
/* I2C */
|
||
if (s_i2c_enabled) {
|
||
i2c_slave_config_t i2c_cfg = {
|
||
.i2c_port = dft.i2c_port,
|
||
.sda_io_num = dft.i2c_sda,
|
||
.scl_io_num = dft.i2c_scl,
|
||
.clk_source = I2C_CLK_SRC_DEFAULT,
|
||
.send_buf_depth = 0,
|
||
.receive_buf_depth = EDC_I2C_BUF_SIZE,
|
||
.slave_addr = dft.i2c_addr,
|
||
.addr_bit_len = I2C_ADDR_BIT_LEN_7,
|
||
.intr_priority = 0,
|
||
.flags.enable_internal_pullup = true,
|
||
};
|
||
ESP_ERROR_CHECK(i2c_new_slave_device(&i2c_cfg, &s_i2c_handle));
|
||
|
||
i2c_slave_event_callbacks_t cbs = {
|
||
.on_receive = on_i2c_rx,
|
||
.on_request = NULL,
|
||
};
|
||
ESP_ERROR_CHECK(i2c_slave_register_event_callbacks(s_i2c_handle, &cbs, NULL));
|
||
ESP_LOGI(TAG, "I2C slave OK (0x%02X SDA=%d SCL=%d)",
|
||
dft.i2c_addr, dft.i2c_sda, dft.i2c_scl);
|
||
}
|
||
|
||
/* UART */
|
||
if (s_uart_enabled) {
|
||
s_uart_port = dft.uart_port;
|
||
memset(&s_uart_ctx, 0, sizeof(s_uart_ctx));
|
||
s_uart_ctx.state = EDC_PARSE_WAITING;
|
||
|
||
uart_config_t uart_cfg = {
|
||
.baud_rate = dft.uart_baudrate,
|
||
.data_bits = UART_DATA_8_BITS,
|
||
.parity = UART_PARITY_DISABLE,
|
||
.stop_bits = UART_STOP_BITS_1,
|
||
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
|
||
.source_clk = UART_SCLK_DEFAULT,
|
||
};
|
||
ESP_ERROR_CHECK(uart_driver_install(dft.uart_port,
|
||
EDC_UART_BUF_SIZE, EDC_UART_BUF_SIZE,
|
||
0, NULL, 0));
|
||
ESP_ERROR_CHECK(uart_param_config(dft.uart_port, &uart_cfg));
|
||
ESP_ERROR_CHECK(uart_set_pin(dft.uart_port,
|
||
dft.uart_tx, dft.uart_rx,
|
||
UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
|
||
ESP_LOGI(TAG, "UART OK (port=%d baud=%d RX=%d TX=%d)",
|
||
dft.uart_port, dft.uart_baudrate, dft.uart_rx, dft.uart_tx);
|
||
}
|
||
}
|
||
|
||
/* ================================================================
|
||
* 轮询
|
||
* ================================================================ */
|
||
|
||
int edc_proto_poll(edc_cycle_t *cycle)
|
||
{
|
||
bool got = false;
|
||
if (s_i2c_enabled) got |= i2c_poll_once();
|
||
if (s_uart_enabled) got |= uart_poll_once();
|
||
|
||
if (got) {
|
||
portENTER_CRITICAL(&s_lock);
|
||
memcpy(cycle, &s_latest_cycle, sizeof(edc_cycle_t));
|
||
s_cycle_updated = false;
|
||
portEXIT_CRITICAL(&s_lock);
|
||
return 1;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
/* ================================================================
|
||
* 发送 DONE / REQUEST
|
||
* ================================================================ */
|
||
|
||
void edc_proto_send_done(uint16_t cycle_id)
|
||
{
|
||
if (!s_uart_enabled) return;
|
||
uint8_t data[2];
|
||
write_u16_be(data, cycle_id);
|
||
uart_send_frame(EDC_CMD_CYCLE_DONE, data, 2);
|
||
s_stat.done_sent++;
|
||
ESP_LOGI(TAG, "DONE sent (cycle_id=%u)", cycle_id);
|
||
}
|
||
|
||
void edc_proto_send_request(void)
|
||
{
|
||
if (!s_uart_enabled) return;
|
||
uart_send_frame(EDC_CMD_CYCLE_REQUEST, NULL, 0);
|
||
s_stat.request_sent++;
|
||
ESP_LOGI(TAG, "REQUEST sent");
|
||
}
|
||
|
||
/* ================================================================
|
||
* 统计
|
||
* ================================================================ */
|
||
|
||
void edc_proto_get_stats(edc_proto_stat_t *stat)
|
||
{
|
||
memcpy(stat, &s_stat, sizeof(edc_proto_stat_t));
|
||
}
|
||
|
||
/* ================================================================
|
||
* 弱回调
|
||
* ================================================================ */
|
||
|
||
__attribute__((weak))
|
||
void edc_proto_on_cycle(const edc_cycle_t *cycle, edc_channel_t ch)
|
||
{
|
||
(void)cycle; (void)ch;
|
||
}
|
||
|
||
/* ================================================================
|
||
* FreeRTOS 任务
|
||
* ================================================================ */
|
||
|
||
void edc_proto_task(void *pvParam)
|
||
{
|
||
edc_cycle_t cycle;
|
||
while (1) {
|
||
if (edc_proto_poll(&cycle) > 0) {
|
||
edc_channel_t ch;
|
||
portENTER_CRITICAL(&s_lock);
|
||
ch = s_stat.last_channel;
|
||
portEXIT_CRITICAL(&s_lock);
|
||
edc_proto_on_cycle(&cycle, ch);
|
||
}
|
||
vTaskDelay(pdMS_TO_TICKS(2));
|
||
}
|
||
}
|