512 lines
17 KiB
C
512 lines
17 KiB
C
// #include "uart_handler.h"
|
||
// #include "pca9685.h"
|
||
// #include "driver/uart.h"
|
||
// #include "esp_log.h"
|
||
// #include "freertos/FreeRTOS.h"
|
||
// #include "freertos/task.h"
|
||
// #include "freertos/queue.h"
|
||
// #include <string.h>
|
||
// #include <math.h>
|
||
|
||
// #define UART_BUF_SIZE 1024
|
||
// #define TAG "UART_HANDLER"
|
||
// #define MAX_SERVOS 32
|
||
// #define MAX_DEVICES 2
|
||
|
||
// // 设备配置 (在main.c中初始化)
|
||
// extern PCA9685_Device devices[MAX_DEVICES];
|
||
// // 当前角度状态
|
||
// static ServoAngle current_angles[MAX_SERVOS];
|
||
// static int angle_count = 0;
|
||
// // 舵机控制队列
|
||
// static QueueHandle_t servo_queue = NULL;
|
||
|
||
// // 获取当前角度
|
||
// static float get_angle(uint8_t addr, uint8_t channel) {
|
||
// for(int i = 0; i < angle_count; i++) {
|
||
// if(current_angles[i].addr == addr &&
|
||
// current_angles[i].channel == channel) {
|
||
// return current_angles[i].angle;
|
||
// }
|
||
// }
|
||
// // 不存在则添加默认值
|
||
// if(angle_count < MAX_SERVOS) {
|
||
// current_angles[angle_count] = (ServoAngle){addr, channel, 90.0f};
|
||
// return current_angles[angle_count++].angle;
|
||
// }
|
||
// return 90.0f; // 安全默认值
|
||
// }
|
||
|
||
// // 更新角度
|
||
// static void update_angle(uint8_t addr, uint8_t channel, float angle) {
|
||
// for(int i = 0; i < angle_count; i++) {
|
||
// if(current_angles[i].addr == addr &&
|
||
// current_angles[i].channel == channel) {
|
||
// current_angles[i].angle = angle;
|
||
// return;
|
||
// }
|
||
// }
|
||
// // 不存在则添加
|
||
// if(angle_count < MAX_SERVOS) {
|
||
// current_angles[angle_count++] = (ServoAngle){addr, channel, angle};
|
||
// }
|
||
// }
|
||
|
||
// // 线性缓动执行
|
||
// static void move_servo_linear(uint8_t addr, uint8_t channel, float from, float to, uint16_t duration_ms) {
|
||
// int steps = duration_ms / 20;
|
||
// steps = steps < 1 ? 1 : steps;
|
||
|
||
// for (int i = 0; i <= steps; ++i) {
|
||
// float t = (float)i / steps;
|
||
// float angle = from + (to - from) * t;
|
||
|
||
// // 查找设备
|
||
// for (int d = 0; d < MAX_DEVICES; d++) {
|
||
// if (devices[d].addr == addr) {
|
||
// pca9685_set_angle(devices[d].port, addr, channel, angle);
|
||
// break;
|
||
// }
|
||
// }
|
||
// vTaskDelay(20 / portTICK_PERIOD_MS);
|
||
// }
|
||
// update_angle(addr, channel, to);
|
||
// }
|
||
|
||
// // 舵机控制任务
|
||
// static void servo_control_task(void *param) {
|
||
// ServoCommand cmd;
|
||
|
||
// while (1) {
|
||
// if (xQueueReceive(servo_queue, &cmd, portMAX_DELAY) == pdTRUE) {
|
||
// float current = get_angle(cmd.addr, cmd.channel);
|
||
|
||
// if (cmd.duration <= 1) {
|
||
// // 立即设置
|
||
// for (int d = 0; d < MAX_DEVICES; d++) {
|
||
// if (devices[d].addr == cmd.addr) {
|
||
// pca9685_set_angle(devices[d].port, cmd.addr, cmd.channel, cmd.angle);
|
||
// break;
|
||
// }
|
||
// }
|
||
// update_angle(cmd.addr, cmd.channel, cmd.angle);
|
||
// } else {
|
||
// // 缓动控制
|
||
// move_servo_linear(cmd.addr, cmd.channel, current, cmd.angle, cmd.duration);
|
||
// }
|
||
// }
|
||
// }
|
||
// }
|
||
|
||
// void uart_handler_init(uart_port_t uart_num) {
|
||
|
||
// uart_config_t config = {
|
||
// .baud_rate = 115200,
|
||
// .data_bits = UART_DATA_8_BITS,
|
||
// .parity = UART_PARITY_DISABLE,
|
||
// .stop_bits = UART_STOP_BITS_1,
|
||
// .flow_ctrl = UART_HW_FLOWCTRL_DISABLE
|
||
// };
|
||
// uart_param_config(uart_num, &config);
|
||
// esp_err_t ret = uart_driver_install(uart_num, UART_BUF_SIZE * 4, 0, 0, NULL, 0);
|
||
// if (ret != ESP_OK) {
|
||
// ESP_LOGE(TAG, "UART driver install failed: 0x%X", ret);
|
||
// } else {
|
||
// ESP_LOGI(TAG, "UART%d driver installed", uart_num);
|
||
// }
|
||
|
||
// // 创建舵机控制队列
|
||
// servo_queue = xQueueCreate(20, sizeof(ServoCommand));
|
||
// if (servo_queue == NULL) {
|
||
// ESP_LOGE(TAG, "Failed to create servo queue");
|
||
// } else {
|
||
// ESP_LOGI(TAG, "Servo queue created");
|
||
// }
|
||
|
||
// // 创建舵机控制任务
|
||
// BaseType_t task_ret = xTaskCreate(servo_control_task, "servo_ctrl", 4096, (void*)UART_NUM_0, 5, NULL);
|
||
// if (task_ret != pdPASS) {
|
||
// ESP_LOGE(TAG, "Failed to create servo_control_task");
|
||
// } else {
|
||
// ESP_LOGI(TAG, "servo_control_task created");
|
||
// }
|
||
// }
|
||
|
||
// void uart_handler_task(void *param) {
|
||
// uart_port_t uart_num = (uart_port_t)param;
|
||
// uint8_t buf[UART_BUF_SIZE];
|
||
|
||
// while (1) {
|
||
// ESP_LOGI(TAG, "++++++++++++++++++++++++++++++++++++++++++++++++++++++++");
|
||
// int len = uart_read_bytes(uart_num, buf, UART_BUF_SIZE, 0 / portTICK_PERIOD_MS);
|
||
// if (len <= 0) continue;
|
||
// ESP_LOGI("UART", "收到 %d 字节数据", len);
|
||
// ESP_LOG_BUFFER_HEXDUMP("UART DATA", buf, len, ESP_LOG_INFO);
|
||
|
||
// ESP_LOGI(TAG, "Received %d bytes", len);
|
||
// esp_log_buffer_hex(TAG, buf, len);
|
||
|
||
// // 寻找帧头 0xAA
|
||
// int i = 0;
|
||
// while (i < len && buf[i] != 0xAA) ++i;
|
||
// if (i >= len - 1) continue;
|
||
|
||
// int remaining = len - i;
|
||
// uint8_t count = buf[i + 1];
|
||
// if (remaining < 2 + count * 5) {
|
||
// ESP_LOGW(TAG, "Incomplete frame, need %d bytes, have %d", 2+count*5, remaining);
|
||
// continue;
|
||
// }
|
||
|
||
// // 计算校验和
|
||
// uint8_t calc_checksum = 0;
|
||
// for (int j = 0; j < 2 + count * 5; j++) {
|
||
// calc_checksum ^= buf[i + j];
|
||
// }
|
||
// uint8_t packet_checksum = buf[i + 2 + count * 5]; // 最后一个字节
|
||
|
||
// if (calc_checksum != packet_checksum) {
|
||
// ESP_LOGE(TAG, "Checksum error: calc=0x%02X, packet=0x%02X",
|
||
// calc_checksum, packet_checksum);
|
||
// continue;
|
||
// }
|
||
// // 解析指令
|
||
// for (int j = 0; j < count; ++j) {
|
||
// int idx = i + 2 + j * 5;
|
||
// ServoCommand cmd = {
|
||
// .addr = buf[idx],
|
||
// .channel = buf[idx + 1],
|
||
// .angle = (float)buf[idx + 2],
|
||
// .duration = (uint16_t)(buf[idx + 3] | (buf[idx + 4] << 8))
|
||
// };
|
||
|
||
// // 发送到舵机控制队列
|
||
// if (xQueueSend(servo_queue, &cmd, 10 / portTICK_PERIOD_MS) != pdTRUE) {
|
||
// ESP_LOGE(TAG, "Failed to send command to queue");
|
||
// }
|
||
// }
|
||
// }
|
||
// }
|
||
|
||
|
||
|
||
// #include "uart_handler.h"
|
||
// #include "pca9685.h"
|
||
// #include "driver/uart.h"
|
||
// #include "esp_log.h"
|
||
// #include "freertos/FreeRTOS.h"
|
||
// #include "freertos/task.h"
|
||
// #include "freertos/queue.h"
|
||
// #include <string.h>
|
||
// #include <math.h>
|
||
|
||
// #define UART_BUF_SIZE 1024
|
||
// #define TAG "UART_HANDLER"
|
||
// #define MAX_SERVOS 32
|
||
// #define MAX_DEVICES 2
|
||
|
||
// extern PCA9685_Device devices[MAX_DEVICES];
|
||
// static ServoAngle current_angles[MAX_SERVOS];
|
||
// static int angle_count = 0;
|
||
// static QueueHandle_t servo_queue = NULL;
|
||
|
||
// static float get_angle(uint8_t addr, uint8_t channel) {
|
||
// for(int i = 0; i < angle_count; i++) {
|
||
// if(current_angles[i].addr == addr && current_angles[i].channel == channel) {
|
||
// return current_angles[i].angle;
|
||
// }
|
||
// }
|
||
// if(angle_count < MAX_SERVOS) {
|
||
// current_angles[angle_count] = (ServoAngle){addr, channel, 90.0f};
|
||
// return current_angles[angle_count++].angle;
|
||
// }
|
||
// return 90.0f;
|
||
// }
|
||
|
||
// static void update_angle(uint8_t addr, uint8_t channel, float angle) {
|
||
// for(int i = 0; i < angle_count; i++) {
|
||
// if(current_angles[i].addr == addr && current_angles[i].channel == channel) {
|
||
// current_angles[i].angle = angle;
|
||
// return;
|
||
// }
|
||
// }
|
||
// if(angle_count < MAX_SERVOS) {
|
||
// current_angles[angle_count++] = (ServoAngle){addr, channel, angle};
|
||
// }
|
||
// }
|
||
|
||
// static void move_servo_linear(uint8_t addr, uint8_t channel, float from, float to, uint16_t duration_ms) {
|
||
// int steps = duration_ms / 20;
|
||
// steps = steps < 1 ? 1 : steps;
|
||
|
||
// for (int i = 0; i <= steps; ++i) {
|
||
// float t = (float)i / steps;
|
||
// float angle = from + (to - from) * t;
|
||
|
||
// for (int d = 0; d < MAX_DEVICES; d++) {
|
||
// if (devices[d].addr == addr) {
|
||
// pca9685_set_angle(devices[d].port, addr, channel, angle);
|
||
// break;
|
||
// }
|
||
// }
|
||
// vTaskDelay(20 / portTICK_PERIOD_MS);
|
||
// }
|
||
// update_angle(addr, channel, to);
|
||
// }
|
||
|
||
// static void servo_control_task(void *param) {
|
||
// ServoCommand cmd;
|
||
|
||
// while (1) {
|
||
// if (xQueueReceive(servo_queue, &cmd, portMAX_DELAY) == pdTRUE) {
|
||
// float current[MAX_SERVOS];
|
||
// int count = 0;
|
||
|
||
// for (int i = 0; i < MAX_SERVOS; ++i) {
|
||
// if (current_angles[i].addr == cmd.addr) {
|
||
// current[count++] = current_angles[i].angle;
|
||
// }
|
||
// }
|
||
|
||
// if (count > 1) {
|
||
// pca9685_set_angles(devices[cmd.addr].port, cmd.addr, 0, count, current);
|
||
// } else {
|
||
// pca9685_set_angle(devices[cmd.addr].port, cmd.addr, cmd.channel, cmd.angle);
|
||
// }
|
||
|
||
// update_angle(cmd.addr, cmd.channel, cmd.angle);
|
||
// }
|
||
// }
|
||
// }
|
||
|
||
// void uart_handler_init(uart_port_t uart_num) {
|
||
// uart_config_t config = {
|
||
// .baud_rate = 115200,
|
||
// .data_bits = UART_DATA_8_BITS,
|
||
// .parity = UART_PARITY_DISABLE,
|
||
// .stop_bits = UART_STOP_BITS_1,
|
||
// .flow_ctrl = UART_HW_FLOWCTRL_DISABLE
|
||
// };
|
||
// uart_param_config(uart_num, &config);
|
||
// esp_err_t ret = uart_driver_install(uart_num, UART_BUF_SIZE * 4, 0, 0, NULL, 0);
|
||
// if (ret != ESP_OK) {
|
||
// ESP_LOGE(TAG, "UART driver install failed: 0x%X", ret);
|
||
// } else {
|
||
// ESP_LOGI(TAG, "UART%d driver installed", uart_num);
|
||
// }
|
||
|
||
// servo_queue = xQueueCreate(20, sizeof(ServoCommand));
|
||
// if (servo_queue == NULL) {
|
||
// ESP_LOGE(TAG, "Failed to create servo queue");
|
||
// } else {
|
||
// ESP_LOGI(TAG, "Servo queue created");
|
||
// }
|
||
|
||
// BaseType_t task_ret = xTaskCreate(servo_control_task, "servo_ctrl", 4096, (void*)UART_NUM_0, 5, NULL);
|
||
// if (task_ret != pdPASS) {
|
||
// ESP_LOGE(TAG, "Failed to create servo_control_task");
|
||
// } else {
|
||
// ESP_LOGI(TAG, "servo_control_task created");
|
||
// }
|
||
// }
|
||
|
||
// void uart_handler_task(void *param) {
|
||
// uart_port_t uart_num = (uart_port_t)param;
|
||
// uint8_t buf[UART_BUF_SIZE];
|
||
|
||
// while (1) {
|
||
// int len = uart_read_bytes(uart_num, buf, UART_BUF_SIZE, 0 / portTICK_PERIOD_MS);
|
||
// if (len <= 0) continue;
|
||
|
||
// int i = 0;
|
||
// while (i < len && buf[i] != 0xAA) ++i;
|
||
// if (i >= len - 1) continue;
|
||
|
||
// int remaining = len - i;
|
||
// uint8_t count = buf[i + 1];
|
||
// if (remaining < 2 + count * 5) {
|
||
// ESP_LOGW(TAG, "Incomplete frame, need %d bytes, have %d", 2 + count * 5, remaining);
|
||
// continue;
|
||
// }
|
||
|
||
// uint8_t calc_checksum = 0;
|
||
// for (int j = 0; j < 2 + count * 5; j++) {
|
||
// calc_checksum ^= buf[i + j];
|
||
// }
|
||
// uint8_t packet_checksum = buf[i + 2 + count * 5];
|
||
// if (calc_checksum != packet_checksum) {
|
||
// ESP_LOGE(TAG, "Checksum error: calc=0x%02X, packet=0x%02X",
|
||
// calc_checksum, packet_checksum);
|
||
// continue;
|
||
// }
|
||
|
||
// for (int j = 0; j < count; ++j) {
|
||
// int idx = i + 2 + j * 5;
|
||
// ServoCommand cmd = {
|
||
// .addr = buf[idx],
|
||
// .channel = buf[idx + 1],
|
||
// .angle = (float)buf[idx + 2],
|
||
// .duration = (uint16_t)(buf[idx + 3] | (buf[idx + 4] << 8))
|
||
// };
|
||
|
||
// if (xQueueSend(servo_queue, &cmd, 10 / portTICK_PERIOD_MS) != pdTRUE) {
|
||
// ESP_LOGE(TAG, "Failed to send command to queue");
|
||
// }
|
||
// }
|
||
// }
|
||
// }
|
||
#include "uart_handler.h"
|
||
#include "pca9685.h"
|
||
#include "driver/uart.h"
|
||
#include "esp_log.h"
|
||
#include "freertos/FreeRTOS.h"
|
||
#include "freertos/task.h"
|
||
#include "freertos/queue.h"
|
||
#include <string.h>
|
||
#include <math.h>
|
||
|
||
#define UART_BUF_SIZE 1024
|
||
#define TAG "UART_HANDLER"
|
||
#define MAX_SERVOS 32
|
||
#define MAX_DEVICES 2
|
||
#define MAX_CHANNELS 16 // PCA9685 每个设备最多 16 通道
|
||
|
||
extern PCA9685_Device devices[MAX_DEVICES];
|
||
static QueueHandle_t servo_queue = NULL;
|
||
|
||
// 设备通道角度缓存
|
||
static float device_angles[MAX_DEVICES][MAX_CHANNELS];
|
||
|
||
// 根据 I2C 地址查设备索引
|
||
static int find_device_index(uint8_t addr) {
|
||
for (int d = 0; d < MAX_DEVICES; ++d) {
|
||
if (devices[d].addr == addr) return d;
|
||
}
|
||
return -1;
|
||
}
|
||
|
||
// 线性缓动批量更新
|
||
static void move_servo_linear(uint8_t addr, uint8_t channel, float from, float to, uint16_t duration_ms) {
|
||
int steps = duration_ms / 20;
|
||
steps = steps < 1 ? 1 : steps;
|
||
int idx = find_device_index(addr);
|
||
if (idx < 0) return;
|
||
|
||
for (int i = 0; i <= steps; ++i) {
|
||
float t = (float)i / steps;
|
||
float angle = from + (to - from) * t;
|
||
device_angles[idx][channel] = angle;
|
||
// 一次性批量更新所有通道，保证同步
|
||
pca9685_set_angles(devices[idx].port, addr, 0, MAX_CHANNELS, device_angles[idx]);
|
||
vTaskDelay(20 / portTICK_PERIOD_MS);
|
||
}
|
||
}
|
||
|
||
// 舵机控制任务
|
||
static void servo_control_task(void *param) {
|
||
ServoCommand cmd;
|
||
while (1) {
|
||
if (xQueueReceive(servo_queue, &cmd, portMAX_DELAY) == pdTRUE) {
|
||
int idx = find_device_index(cmd.addr);
|
||
if (idx < 0) continue;
|
||
float from = device_angles[idx][cmd.channel];
|
||
float to = cmd.angle;
|
||
|
||
if (cmd.duration <= 1) {
|
||
// 立即批量更新
|
||
device_angles[idx][cmd.channel] = to;
|
||
pca9685_set_angles(devices[idx].port, cmd.addr, 0, MAX_CHANNELS, device_angles[idx]);
|
||
} else {
|
||
// 缓动控制
|
||
move_servo_linear(cmd.addr, cmd.channel, from, to, cmd.duration);
|
||
// 最终角度已在 move_servo_linear 内更新
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
void uart_handler_init(uart_port_t uart_num) {
|
||
uart_config_t config = {
|
||
.baud_rate = 115200,
|
||
.data_bits = UART_DATA_8_BITS,
|
||
.parity = UART_PARITY_DISABLE,
|
||
.stop_bits = UART_STOP_BITS_1,
|
||
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE
|
||
};
|
||
uart_param_config(uart_num, &config);
|
||
esp_err_t ret = uart_driver_install(uart_num, UART_BUF_SIZE * 4, 0, 0, NULL, 0);
|
||
if (ret != ESP_OK) {
|
||
ESP_LOGE(TAG, "UART driver install failed: 0x%X", ret);
|
||
} else {
|
||
ESP_LOGI(TAG, "UART%d driver installed", uart_num);
|
||
}
|
||
|
||
// 初始化角度缓存为 90°
|
||
for (int d = 0; d < MAX_DEVICES; ++d) {
|
||
for (int c = 0; c < MAX_CHANNELS; ++c) {
|
||
device_angles[d][c] = 90.0f;
|
||
}
|
||
}
|
||
|
||
// 创建舵机控制队列
|
||
servo_queue = xQueueCreate(20, sizeof(ServoCommand));
|
||
if (!servo_queue) {
|
||
ESP_LOGE(TAG, "Failed to create servo queue");
|
||
return;
|
||
}
|
||
ESP_LOGI(TAG, "Servo queue created");
|
||
|
||
// 启动舵机控制任务
|
||
BaseType_t task_ret = xTaskCreate(servo_control_task, "servo_ctrl", 4096,
|
||
NULL, configMAX_PRIORITIES - 1, NULL);
|
||
if (task_ret != pdPASS) {
|
||
ESP_LOGE(TAG, "Failed to create servo_control_task");
|
||
} else {
|
||
ESP_LOGI(TAG, "servo_control_task created");
|
||
}
|
||
}
|
||
|
||
void uart_handler_task(void *param) {
|
||
uart_port_t uart_num = (uart_port_t)param;
|
||
uint8_t buf[UART_BUF_SIZE];
|
||
|
||
while (1) {
|
||
int len = uart_read_bytes(uart_num, buf, UART_BUF_SIZE, 10 / portTICK_PERIOD_MS);
|
||
if (len <= 0) continue;
|
||
|
||
// 找到帧头 0xAA
|
||
int i = 0;
|
||
while (i < len && buf[i] != 0xAA) ++i;
|
||
if (i >= len - 1) continue;
|
||
|
||
uint8_t count = buf[i + 1];
|
||
int frame_len = 2 + count * 5 + 1;
|
||
if (i + frame_len > len) {
|
||
ESP_LOGW(TAG, "不完整帧，需要 %d 字节，实际 %d", frame_len, len - i);
|
||
continue;
|
||
}
|
||
|
||
// 校验
|
||
uint8_t chk = 0;
|
||
for (int j = 0; j < frame_len - 1; ++j) chk ^= buf[i + j];
|
||
if (chk != buf[i + frame_len - 1]) {
|
||
ESP_LOGE(TAG, "校验失败，calc=0x%02X, packet=0x%02X", chk, buf[i + frame_len - 1]);
|
||
continue;
|
||
}
|
||
|
||
// 解析并发送命令
|
||
for (int j = 0; j < count; ++j) {
|
||
int idx = i + 2 + j * 5;
|
||
ServoCommand cmd = {
|
||
.addr = buf[idx],
|
||
.channel = buf[idx + 1],
|
||
.angle = (float)buf[idx + 2],
|
||
.duration = (uint16_t)(buf[idx + 3] | (buf[idx + 4] << 8))
|
||
};
|
||
if (xQueueSend(servo_queue, &cmd, 10 / portTICK_PERIOD_MS) != pdTRUE) {
|
||
ESP_LOGE(TAG, "发送命令到队列失败");
|
||
}
|
||
}
|
||
}
|
||
}
|