EDC/edc-mcu
3
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

releasev1.2

Browse Source
This commit is contained in:
kirl committed 2026-08-03 04:09:26 +08:00
1 parent 74010ef611
commit c296caa328
10 files changed
+693 -267
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

components/emm_v5/CMakeLists.txt
+1 -1
View File
@@ -1,3 +1,3 @@
idf_component_register(SRCS "emm_v5.c"
INCLUDE_DIRS "."
REQUIRES esp_driver_uart esp_driver_gpio driver esp_timer)
REQUIRES esp_driver_uart esp_driver_gpio driver esp_timer freertos)
components/emm_v5/emm_v5.c
+276 -90
View File
@@ -8,6 +8,9 @@
#include <math.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
#include "freertos/event_groups.h"
#include "esp_timer.h"
static const char *TAG = "EMM_V5";
@@ -15,6 +18,40 @@ static const char *TAG = "EMM_V5";
/* 运行时配置 (由 Emm_V5_Init 设置) */
static uart_port_t s_uart_port;
/* ============================================================
* RX 中断/事件驱动状态
*
* 驱动 Response 菜单设为 Reached/Both/Other 后, 位置模式 (0xFD) 命令
* 运动到位会主动回传到位帧: 地址 + FD + 9F + 6B。
* UART RX 由内部任务独占读取, 解析出到位帧后置位事件组位
* (bit = 电机地址 - 1); 调用方用 Emm_V5_Wait_Arrival 事件等待,
* 全部到位即返回 —— 不再轮询 S_TPOS/S_CPOS, 避免"没到位就下一步"。
* ============================================================ */
#define EMM_V5_ARRIVAL_MAX_MOTORS 8
static EventGroupHandle_t s_arrival_evt = NULL;
static QueueHandle_t s_uart_evt_queue = NULL;
static TaskHandle_t s_rx_task = NULL;
/* 请求-响应 (网页手动读参数等) */
static SemaphoreHandle_t s_resp_mutex = NULL;
static struct {
bool active;
uint8_t addr;
uint8_t func;
uint8_t *out;
uint8_t max;
uint8_t len;
SemaphoreHandle_t done;
} s_resp;
/* RX 帧累积缓冲 (0x6B 为帧尾) */
static uint8_t s_rx_frame[64];
static uint8_t s_rx_frame_len;
static void emm_v5_rx_task(void *arg); /* 前置声明 (Emm_V5_Init 中创建) */
/* ============================================================
* 初始化
* ============================================================ */
@@ -32,10 +69,13 @@ void Emm_V5_Init(uart_port_t port, int tx_pin, int rx_pin)
.source_clk = UART_SCLK_DEFAULT,
};
/* 事件队列: 中断驱动 RX (到位帧解析); 64 深度防突发丢事件 */
s_uart_evt_queue = xQueueCreate(64, sizeof(uart_event_t));
ESP_ERROR_CHECK(uart_driver_install(s_uart_port,
EMM_V5_UART_BUF_SIZE * 2,
EMM_V5_UART_BUF_SIZE * 2,
0, NULL, 0));
16, &s_uart_evt_queue, 0));
ESP_ERROR_CHECK(uart_param_config(s_uart_port, &uart_cfg));
ESP_ERROR_CHECK(uart_set_pin(s_uart_port,
tx_pin,
@@ -43,7 +83,15 @@ void Emm_V5_Init(uart_port_t port, int tx_pin, int rx_pin)
UART_PIN_NO_CHANGE,
UART_PIN_NO_CHANGE));
ESP_LOGI(TAG, "初始化完成 (UART%d, TX=%d, RX=%d)",
/* 到位事件组 / 请求-响应信号量 / RX 任务 */
if (s_arrival_evt == NULL) s_arrival_evt = xEventGroupCreate();
if (s_resp_mutex == NULL) s_resp_mutex = xSemaphoreCreateMutex();
if (s_resp.done == NULL) s_resp.done = xSemaphoreCreateBinary();
if (s_rx_task == NULL) {
xTaskCreate(emm_v5_rx_task, "emm_rx", 3072, NULL, 5, &s_rx_task);
}
ESP_LOGI(TAG, "初始化完成 (UART%d, TX=%d, RX=%d, 到位帧中断已启用)",
s_uart_port, tx_pin, rx_pin);
}
@@ -57,31 +105,178 @@ static inline void Emm_V5_Send(const uint8_t *cmd, uint8_t len)
}
/* ============================================================
* 接收数据
* RX 中断/事件驱动
*
* 帧格式: 地址 + 功能码 + 数据... + 0x6B (校验字节)
* 到位帧: 地址 + FD + 9F + 6B (位置模式命令运动到位后主动回传)
* ============================================================ */
/* 处理一帧完整数据 (以 0x6B 结尾) */
static void emm_v5_handle_frame(const uint8_t *f, uint8_t len)
{
if (len < 4) return;
/* 到位帧: 地址 + FD + 9F + 6B → 置位事件组 (bit = 地址 - 1) */
if (f[1] == 0xFD && f[2] == 0x9F) {
uint8_t addr = f[0];
if (addr >= 1 && addr <= EMM_V5_ARRIVAL_MAX_MOTORS && s_arrival_evt) {
xEventGroupSetBits(s_arrival_evt, (EventBits_t)(1u << (addr - 1)));
ESP_LOGI(TAG, "到位帧: 电机%d", addr);
}
}
/* 请求-响应匹配 (网页手动读参数等) */
if (s_resp_mutex) {
xSemaphoreTake(s_resp_mutex, portMAX_DELAY);
if (s_resp.active && f[0] == s_resp.addr && f[1] == s_resp.func) {
uint8_t n = (len < s_resp.max) ? len : s_resp.max;
if (s_resp.out && n) memcpy(s_resp.out, f, n);
s_resp.len = n;
s_resp.active = false;
xSemaphoreGive(s_resp.done);
}
xSemaphoreGive(s_resp_mutex);
}
}
/* UART RX 独占任务: 事件队列驱动, 解析帧并分派 */
static void emm_v5_rx_task(void *arg)
{
uart_event_t evt;
uint8_t buf[64];
while (1) {
if (xQueueReceive(s_uart_evt_queue, &evt, portMAX_DELAY) != pdTRUE)
continue;
if (evt.type == UART_DATA || evt.type == UART_FIFO_OVF ||
evt.type == UART_BUFFER_FULL) {
int len;
while ((len = uart_read_bytes(s_uart_port, buf, sizeof(buf), 0)) > 0) {
for (int i = 0; i < len; i++) {
s_rx_frame[s_rx_frame_len++] = buf[i];
if (s_rx_frame_len >= sizeof(s_rx_frame)) {
s_rx_frame_len = 0; /* 缓冲溢出: 重新同步 */
}
if (buf[i] == 0x6B) { /* 帧尾 */
emm_v5_handle_frame(s_rx_frame, s_rx_frame_len);
s_rx_frame_len = 0;
}
}
}
if (evt.type == UART_FIFO_OVF || evt.type == UART_BUFFER_FULL) {
/* 溢出只说明有部分字节被丢弃: 重置帧累积重新同步即可。
不能 uart_flush_input —— 那会把缓冲里仍有效的到位帧一并冲掉,
是偶发丢电机2/3/4 到位帧的根因之一 */
s_rx_frame_len = 0;
}
}
}
}
/* 内部: 发送命令并等待匹配的响应帧 (地址+功能码), 供读函数复用 */
static int emm_v5_request_response(const uint8_t *cmd, uint8_t cmd_len,
uint8_t addr, uint8_t func,
uint8_t *out, uint8_t max,
uint32_t timeout_ms)
{
if (!s_resp_mutex || !s_resp.done) return -1;
xSemaphoreTake(s_resp_mutex, portMAX_DELAY);
if (s_resp.active) {
xSemaphoreGive(s_resp_mutex);
ESP_LOGW(TAG, "请求-响应忙, 放弃 (addr=%u func=0x%02X)", addr, func);
return -1;
}
s_resp.active = true;
s_resp.addr = addr;
s_resp.func = func;
s_resp.out = out;
s_resp.max = max;
s_resp.len = 0;
xSemaphoreGive(s_resp_mutex);
xSemaphoreTake(s_resp.done, 0); /* 清残留计数 */
Emm_V5_Send(cmd, cmd_len);
if (xSemaphoreTake(s_resp.done, pdMS_TO_TICKS(timeout_ms)) != pdTRUE) {
xSemaphoreTake(s_resp_mutex, portMAX_DELAY);
s_resp.active = false; /* 放弃等待 */
xSemaphoreGive(s_resp_mutex);
ESP_LOGW(TAG, "响应超时 (addr=%u func=0x%02X)", addr, func);
return -1;
}
xSemaphoreTake(s_resp_mutex, portMAX_DELAY);
int n = s_resp.len;
xSemaphoreGive(s_resp_mutex);
return n;
}
/* ============================================================
* 到位等待 (事件驱动)
*
* 注意: Emm_V5_Wait_Arrival 不消费到位位 (便于调用方分段等待时闪烁 LED);
* 一段 motion 的等待结束后必须调用 Emm_V5_Arrival_Consume 清除到位位,
* 防止残留位污染下一段 motion。
* ============================================================ */
bool Emm_V5_Wait_Arrival(const uint8_t *addrs, uint8_t count, uint32_t timeout_ms)
{
if (!s_arrival_evt) return false;
EventBits_t expected = 0;
for (uint8_t i = 0; i < count; i++) {
uint8_t a = addrs[i];
if (a >= 1 && a <= EMM_V5_ARRIVAL_MAX_MOTORS)
expected |= (EventBits_t)(1u << (a - 1));
}
if (expected == 0) return true;
EventBits_t got = xEventGroupWaitBits(s_arrival_evt, expected,
pdFALSE, pdTRUE,
pdMS_TO_TICKS(timeout_ms));
return ((got & expected) == expected);
}
void Emm_V5_Arrival_Consume(const uint8_t *addrs, uint8_t count)
{
if (!s_arrival_evt) return;
EventBits_t expected = 0;
for (uint8_t i = 0; i < count; i++) {
uint8_t a = addrs[i];
if (a >= 1 && a <= EMM_V5_ARRIVAL_MAX_MOTORS)
expected |= (EventBits_t)(1u << (a - 1));
}
if (expected)
xEventGroupClearBits(s_arrival_evt, expected);
}
void Emm_V5_Arrival_Reset(void)
{
if (!s_arrival_evt) return;
xEventGroupClearBits(s_arrival_evt, 0xFF); /* 清 addr 1~8 对应位 (bit=地址-1) */
}
/* 查询当前已置位的到位位 (调试用): 返回 addr1~8 对应位掩码 */
uint32_t Emm_V5_Arrival_Get_Bits(void)
{
if (!s_arrival_evt) return 0;
return xEventGroupGetBits(s_arrival_evt) & 0xFF;
}
/* ============================================================
* 接收数据 (已废弃)
* ============================================================ */
uint8_t Emm_V5_Receive_Data(uint8_t *rxCmd, uint8_t maxLen)
{
uint8_t count = 0;
int64_t last_time, now;
last_time = esp_timer_get_time();
while (1) {
int len = uart_read_bytes(s_uart_port, &rxCmd[count], 1,
pdMS_TO_TICKS(10));
if (len > 0) {
count++;
if (count >= maxLen) break;
last_time = esp_timer_get_time();
} else {
now = esp_timer_get_time();
if ((now - last_time) > (EMM_V5_RX_TIMEOUT_MS * 1000)) {
break;
}
}
}
return count;
/* 已废弃: UART RX 由内部 RX 任务独占 (中断驱动), 此函数不再直接读 UART;
网页手动读参数请改用 Emm_V5_Read_Sys_Params_Resp */
(void)rxCmd;
(void)maxLen;
return 0;
}
/* ============================================================
@@ -100,33 +295,62 @@ void Emm_V5_Reset_Clog_Pro(uint8_t addr)
Emm_V5_Send(cmd, 4);
}
/* 系统参数 → 功能码 (读取响应帧按 地址+功能码 匹配) */
static uint8_t emm_v5_sysparam_func(EmmSysParams_t s)
{
switch (s) {
case S_VER: return 0x1F;
case S_RL: return 0x20;
case S_PID: return 0x21;
case S_VBUS: return 0x24;
case S_CPHA: return 0x27;
case S_ENCL: return 0x31;
case S_TPOS: return 0x33;
case S_VEL: return 0x35;
case S_CPOS: return 0x36;
case S_PERR: return 0x37;
case S_FLAG: return 0x3A;
case S_ORG: return 0x3B;
case S_Conf: return 0x42;
case S_State: return 0x43;
default: return 0x00;
}
}
void Emm_V5_Read_Sys_Params(uint8_t addr, EmmSysParams_t s)
{
uint8_t cmd[8] = {0};
uint8_t i = 0;
uint8_t func = emm_v5_sysparam_func(s);
cmd[i++] = addr;
switch (s) {
case S_VER: cmd[i++] = 0x1F; break;
case S_RL: cmd[i++] = 0x20; break;
case S_PID: cmd[i++] = 0x21; break;
case S_VBUS: cmd[i++] = 0x24; break;
case S_CPHA: cmd[i++] = 0x27; break;
case S_ENCL: cmd[i++] = 0x31; break;
case S_TPOS: cmd[i++] = 0x33; break;
case S_VEL: cmd[i++] = 0x35; break;
case S_CPOS: cmd[i++] = 0x36; break;
case S_PERR: cmd[i++] = 0x37; break;
case S_FLAG: cmd[i++] = 0x3A; break;
case S_ORG: cmd[i++] = 0x3B; break;
case S_Conf: cmd[i++] = 0x42; cmd[i++] = 0x6C; break;
case S_State: cmd[i++] = 0x43; cmd[i++] = 0x7A; break;
default: break;
}
if (func) cmd[i++] = func;
if (s == S_Conf) cmd[i++] = 0x6C;
if (s == S_State) cmd[i++] = 0x7A;
cmd[i++] = 0x6B;
Emm_V5_Send(cmd, i);
}
uint8_t Emm_V5_Read_Sys_Params_Resp(uint8_t addr, EmmSysParams_t s,
uint8_t *rxOut, uint8_t maxLen,
uint32_t timeout_ms)
{
uint8_t cmd[8] = {0};
uint8_t i = 0;
uint8_t func = emm_v5_sysparam_func(s);
cmd[i++] = addr;
if (func) cmd[i++] = func;
if (s == S_Conf) cmd[i++] = 0x6C;
if (s == S_State) cmd[i++] = 0x7A;
cmd[i++] = 0x6B;
int n = emm_v5_request_response(cmd, i, addr, func, rxOut, maxLen,
(timeout_ms > 0) ? timeout_ms
: EMM_V5_RX_TIMEOUT_MS);
return (n > 0) ? (uint8_t)n : 0;
}
/* ============================================================
* 读取电机状态标志 (S_FLAG 0x3A) 并解析返回
* ============================================================ */
@@ -137,29 +361,10 @@ uint8_t Emm_V5_Read_Status_Flag(uint8_t addr, uint32_t timeout_ms)
uint8_t cmd[3] = {addr, 0x3A, 0x6B};
uint8_t read_to = (timeout_ms > 0) ? timeout_ms : EMM_V5_RX_TIMEOUT_MS;
/* 清掉历史残留 (运动命令的 ACK/到位帧等), 防止误判 */
uart_flush_input(s_uart_port);
Emm_V5_Send(cmd, 3);
uint8_t n = 0;
int64_t last = esp_timer_get_time();
int64_t deadline = last + (int64_t)read_to * 1000;
while (esp_timer_get_time() < deadline) {
int len = uart_read_bytes(s_uart_port, &rx[n], 1, pdMS_TO_TICKS(5));
if (len > 0) {
if (n < sizeof(rx) - 1) n++;
last = esp_timer_get_time();
/* 帧收完整 (地址 + 3A + 状态 + 6B) 立即返回, 不再等空闲超时,
把单次轮询从 ~17ms 压到 ~3ms, 减小到位检测抖动 */
for (int i = 0; i + 3 < (int)n; i++) {
if (rx[i] == addr && rx[i + 1] == 0x3A && rx[i + 3] == 0x6B)
return rx[i + 2];
}
} else if ((esp_timer_get_time() - last) > 5000) {
break; /* 5ms 无新字节: 认为帧已收完/无响应 */
}
}
int n = emm_v5_request_response(cmd, sizeof(cmd), addr, 0x3A,
rx, sizeof(rx), read_to);
if (n >= 4 && rx[0] == addr && rx[1] == 0x3A && rx[3] == 0x6B)
return rx[2];
return 0xFF;
}
@@ -174,34 +379,15 @@ static int32_t Emm_V5_Read_Pos(uint8_t addr, uint8_t func)
{
uint8_t rx[16] = {0};
uint8_t cmd[3] = {addr, func, 0x6B};
uint8_t read_to = EMM_V5_RX_TIMEOUT_MS;
/* 清掉历史残留 (ACK/到位帧等), 防止误判 */
uart_flush_input(s_uart_port);
Emm_V5_Send(cmd, 3);
uint8_t n = 0;
int64_t last = esp_timer_get_time();
int64_t deadline = last + (int64_t)read_to * 1000;
while (esp_timer_get_time() < deadline) {
int len = uart_read_bytes(s_uart_port, &rx[n], 1, pdMS_TO_TICKS(5));
if (len > 0) {
if (n < sizeof(rx) - 1) n++;
last = esp_timer_get_time();
/* 帧收完整 (地址+功能码+符号+位置4B+6B) 立即返回 */
for (int i = 0; i + 7 < (int)n; i++) {
if (rx[i] == addr && rx[i + 1] == func &&
(rx[i + 2] == 0 || rx[i + 2] == 1) && rx[i + 7] == 0x6B)
return (int32_t)(((uint32_t)rx[i + 3] << 24) |
((uint32_t)rx[i + 4] << 16) |
((uint32_t)rx[i + 5] << 8) |
(uint32_t)rx[i + 6]);
}
} else if ((esp_timer_get_time() - last) > 5000) {
break; /* 5ms 无新字节: 认为帧已收完/无响应 */
}
}
int n = emm_v5_request_response(cmd, sizeof(cmd), addr, func,
rx, sizeof(rx), EMM_V5_RX_TIMEOUT_MS);
if (n >= 8 && rx[0] == addr && rx[1] == func &&
(rx[2] == 0 || rx[2] == 1) && rx[7] == 0x6B)
return (int32_t)(((uint32_t)rx[3] << 24) |
((uint32_t)rx[4] << 16) |
((uint32_t)rx[5] << 8) |
(uint32_t)rx[6]);
return -1;
}
components/emm_v5/emm_v5.h
+58 -5
View File
@@ -25,7 +25,7 @@ extern "C" {
* 可配置常量
* ============================================================ */
#define EMM_V5_UART_BAUDRATE 115200
#define EMM_V5_UART_BUF_SIZE 256
#define EMM_V5_UART_BUF_SIZE 512
/* 接收超时时间 (毫秒) */
#define EMM_V5_RX_TIMEOUT_MS 100
@@ -220,10 +220,63 @@ void Emm_V5_Origin_Trigger_Return(uint8_t addr, uint8_t o_mode, bool snF);
void Emm_V5_Origin_Interrupt(uint8_t addr);
/**
* @brief 接收电机返回数据 (100ms 超时判定帧结束)
* @param rxCmd 接收缓冲区
* @param maxLen 缓冲区最大长度
* @return 实际接收到的字节数
* @brief 等待指定电机到位 (事件驱动, 不消费到位位)
*
* 电机驱动 Response 菜单设为 Reached/Both/Other 后, 位置模式 (0xFD) 命令
* 运动到位会主动回传到位帧 (地址 + FD + 9F + 6B)。UART RX 由本组件内部
* 任务中断读取并解析, 此处事件等待全部到位, 不再轮询 S_TPOS/S_CPOS。
*
* @note 本函数不清除到位位, 便于调用方分段等待 (如等待期间闪烁 LED);
* 一段 motion 等待结束后必须调用 Emm_V5_Arrival_Consume 清除,
* 防止残留位污染下一段 motion。
* @param addrs 电机地址数组, 如 {ACTUATOR_ADDR_X}
* @param count 数组长度
* @param timeout_ms 本次等待超时 (ms); 0 = 立即返回当前状态
* @return true=全部到位, false=超时/未到齐
*/
bool Emm_V5_Wait_Arrival(const uint8_t *addrs, uint8_t count, uint32_t timeout_ms);
/**
* @brief 消费 (清除) 指定电机的到位位
* @param addrs 电机地址数组
* @param count 数组长度
* @note 一段 motion 的到位等待结束后调用 (成功/超时都要调)
*/
void Emm_V5_Arrival_Consume(const uint8_t *addrs, uint8_t count);
/**
* @brief 清除全部电机到位位 (开始新一段 motion 前调用)
* @note 防止上一段 motion 超时后迟到的到位帧 (或 0 脉冲命令仍回传的帧)
* 污染新一段 motion 的到位等待
*/
void Emm_V5_Arrival_Reset(void);
/**
* @brief 查询当前已置位的到位位 (调试用)
* @return 位掩码: bit = (电机地址 - 1), 仅低 8 位有效
*/
uint32_t Emm_V5_Arrival_Get_Bits(void);
/**
* @brief 发送读取系统参数命令并等待响应帧返回 (请求-响应式)
* @param addr 电机地址
* @param s 系统参数类型 (@see EmmSysParams_t)
* @param rxOut 接收缓冲区
* @param maxLen 缓冲区最大长度
* @param timeout_ms 等待响应超时 (ms)
* @return 实际收到的响应字节数 (含 地址/功能码/.../0x6B), 失败返回 0
*/
uint8_t Emm_V5_Read_Sys_Params_Resp(uint8_t addr, EmmSysParams_t s,
uint8_t *rxOut, uint8_t maxLen,
uint32_t timeout_ms);
/**
* @brief 接收电机返回数据
* @deprecated UART RX 已由内部 RX 任务独占 (中断驱动), 此函数不再读取 UART;
* 网页手动读参数请改用 Emm_V5_Read_Sys_Params_Resp
* @param rxCmd 接收缓冲区 (保留, 未使用)
* @param maxLen 缓冲区最大长度 (保留, 未使用)
* @return 恒为 0
*/
uint8_t Emm_V5_Receive_Data(uint8_t *rxCmd, uint8_t maxLen);