wangbeihong
65478d9f02
- 新增 HC-SR04 驱动与测距接口（bsp_sr04） - 增加循迹状态读取与输出，完善任务内日志 - 调整 CarCtrl 闭环更新周期，匹配霍尔测速周期，降低抖动 - 同步更新 CubeMX/CMake 生成配置与相关引脚定义
358 lines
11 KiB
C
358 lines
11 KiB
C
/**
|
||
* @file protocol.c
|
||
* @brief 协议处理函数实现
|
||
* @details 该文件包含了协议解析和处理的相关函数，主要用于处理从 ESP12F
|
||
* 模块接收到的数据。
|
||
* @author Beihong Wang
|
||
* @date 2026-04-01
|
||
*/
|
||
|
||
#include "protocol.h"
|
||
#include "bsp_motor.h"
|
||
#include "bsp_hall.h"
|
||
#include "bsp_pid.h"
|
||
#include "bsp_uart.h"
|
||
#include "checksum.h"
|
||
#include "cmsis_os.h"
|
||
#include "elog.h"
|
||
#include <stdio.h>
|
||
#include <string.h>
|
||
|
||
/* 定义日志 TAG */
|
||
#define Protocol_TAG "Protocol"
|
||
|
||
/* 小车控制状态相关定义 */
|
||
#define CAR_PWM_MAX 3599U // PWM最大值，对应100%
|
||
|
||
/* PID 闭环参数，针对低速高摩擦环境优化 */
|
||
#define MOTOR_KP 6.5f
|
||
#define MOTOR_KI 2.5f // 微增 KI，消除最后 2-3 RPM 的静差
|
||
#define MOTOR_KD 0.1f
|
||
#define TARGET_MAX_RPM 50.0f // 100% 对应 50 RPM
|
||
#define MOTOR_PWM_DEADZONE 1200 // 保持死区，克服静摩擦
|
||
#define MOTOR_PWM_MAX_ARR 3599 // 定时器重装载值
|
||
|
||
/* 电机位置定义 (基于用户规定) */
|
||
#define MOTOR_LR MOTOR_1 // 左后 (Left Rear)
|
||
#define MOTOR_LF MOTOR_2 // 左前 (Left Front)
|
||
#define MOTOR_RF MOTOR_3 // 右前 (Right Front)
|
||
#define MOTOR_RR MOTOR_4 // 右后 (Right Rear)
|
||
|
||
/* 运动解算临时变量 (RPM 单位) */
|
||
static float target_v_x = 0; // 前进后退分量 (-TARGET_MAX_RPM ~ TARGET_MAX_RPM)
|
||
static float target_v_y = 0; // 左右横移分量
|
||
static float target_v_w = 0; // 原地旋转分量
|
||
|
||
static uint8_t car_running = 0; // 小车运行状态（1=运行，0=停止）
|
||
static uint8_t car_speed_percent = 0; // 当前整体速度百分比（0~100）
|
||
static uint16_t car_target_station = 0; // 目标站点编号
|
||
|
||
/* 4个电机的 PID 控制器 */
|
||
static PID_TypeDef motor_pid[MOTOR_COUNT];
|
||
|
||
/**
|
||
* @brief 初始化闭环控制系统
|
||
*/
|
||
void CarCtrl_InitClosedLoop(void)
|
||
{
|
||
for(int i=0; i<MOTOR_COUNT; i++) {
|
||
// 恢复较高的积分限幅(2500)。要想达到稳态，单靠积分项必须能填补：3599(满载) - 1200(死区) = 2399 的差距。
|
||
pid_init(&motor_pid[i], MOTOR_KP, MOTOR_KI, MOTOR_KD, (float)MOTOR_PWM_MAX_ARR, 2500.0f);
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 百分比转PWM值
|
||
* @param percent 速度百分比（0~100）
|
||
* @return int16_t PWM占空比
|
||
*/
|
||
static int16_t CarCtrl_PercentToPwm(uint8_t percent)
|
||
{
|
||
if (percent > 100U) {
|
||
percent = 100U;
|
||
}
|
||
return (int16_t)((CAR_PWM_MAX * percent) / 100U);
|
||
}
|
||
|
||
/**
|
||
* @brief 停止所有电机
|
||
*/
|
||
static void CarCtrl_StopAll(void)
|
||
{
|
||
for(int i=0; i<MOTOR_COUNT; i++) {
|
||
motor_stop(i);
|
||
pid_reset(&motor_pid[i]);
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 按当前 car_speed_percent 设置所有电机速度
|
||
*/
|
||
static void CarCtrl_ApplySpeed(void)
|
||
{
|
||
int16_t pwm = CarCtrl_PercentToPwm(car_speed_percent);
|
||
|
||
motor_set_speed(MOTOR_1, pwm);
|
||
motor_set_speed(MOTOR_2, pwm);
|
||
motor_set_speed(MOTOR_3, pwm);
|
||
motor_set_speed(MOTOR_4, pwm);
|
||
}
|
||
|
||
/**
|
||
* @brief 执行一步 PID 闭环计算并更新电机 (建议 10ms-20ms 调用一次)
|
||
*/
|
||
void CarCtrl_UpdateClosedLoop(void)
|
||
{
|
||
if (!car_running) {
|
||
CarCtrl_StopAll();
|
||
return;
|
||
}
|
||
|
||
/* 麦克纳姆轮运动解算 (RPM单位)
|
||
* 根据你的电机位置规定：
|
||
* LR: M1, LF: M2, RF: M3, RR: M4
|
||
*/
|
||
float target_rpms[MOTOR_COUNT];
|
||
|
||
// 从前进速度分量计算基础速度
|
||
target_v_x = (float)car_speed_percent * TARGET_MAX_RPM / 100.0f;
|
||
|
||
// 麦轮全向解算公式：
|
||
// LF = Vx + Vy - Vw
|
||
// RF = Vx - Vy + Vw
|
||
// LR = Vx - Vy - Vw
|
||
// RR = Vx + Vy + Vw
|
||
target_rpms[MOTOR_LF] = target_v_x + target_v_y - target_v_w; // 左前
|
||
target_rpms[MOTOR_RF] = target_v_x - target_v_y + target_v_w; // 右前
|
||
target_rpms[MOTOR_LR] = target_v_x - target_v_y - target_v_w; // 左后
|
||
target_rpms[MOTOR_RR] = target_v_x + target_v_y + target_v_w; // 右后
|
||
|
||
for(int i=0; i<MOTOR_COUNT; i++) {
|
||
float actual_rpm = hall_get_speed(i);
|
||
|
||
// 限幅目标值，防止叠加后超出物理极限
|
||
if (target_rpms[i] > TARGET_MAX_RPM) target_rpms[i] = TARGET_MAX_RPM;
|
||
if (target_rpms[i] < -TARGET_MAX_RPM) target_rpms[i] = -TARGET_MAX_RPM;
|
||
|
||
float pid_out = pid_calculate(&motor_pid[i], target_rpms[i], actual_rpm);
|
||
|
||
// 优化死区逻辑：当目标速度不为0时，叠加死区偏移
|
||
float final_pwm = 0;
|
||
if (target_rpms[i] > 1.0f || target_rpms[i] < -1.0f) {
|
||
if (pid_out >= 0) {
|
||
final_pwm = pid_out + MOTOR_PWM_DEADZONE;
|
||
} else {
|
||
final_pwm = pid_out - MOTOR_PWM_DEADZONE;
|
||
}
|
||
// 最终限幅，防止总 PWM 超过 ARR (3599)
|
||
if (final_pwm > 3599) final_pwm = 3599;
|
||
if (final_pwm < -3599) final_pwm = -3599;
|
||
}
|
||
|
||
motor_set_speed(i, (int16_t)final_pwm);
|
||
|
||
// 修改打印逻辑：使用位置别名让日志更易读
|
||
const char* motor_names[] = {"LR", "LF", "RF", "RR"};
|
||
static uint32_t last_log_time = 0;
|
||
if (HAL_GetTick() - last_log_time > 500) {
|
||
elog_d(Protocol_TAG, "M[%s] T:%.1f A:%.1f Pw:%d",
|
||
motor_names[i], target_rpms[i], actual_rpm, (int16_t)final_pwm);
|
||
if (i == MOTOR_COUNT - 1) last_log_time = HAL_GetTick();
|
||
}
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 解析并执行协议命令
|
||
* @param cmd_payload 队列传入的命令字符串（如 "ST:RUN"、"SP:080"、"GS:005"）
|
||
*/
|
||
static void CarCtrl_HandleCommand(const char *cmd_payload)
|
||
{
|
||
const char *arg = NULL;
|
||
|
||
if (cmd_payload == NULL || cmd_payload[0] == '\0') {
|
||
return;
|
||
}
|
||
|
||
// 查找冒号分隔符，分割命令类型和参数
|
||
arg = strchr(cmd_payload, ':');
|
||
if (arg == NULL) {
|
||
elog_w(Protocol_TAG, "未知控制指令: %s", cmd_payload);
|
||
return;
|
||
}
|
||
|
||
// 启动/停止命令（仅作为自动循迹任务的启动/暂停信号，不直接控制电机）
|
||
if (strncmp(cmd_payload, "ST", 2) == 0) {
|
||
if (strcmp(arg + 1, "RUN") == 0) {
|
||
car_running = 1;
|
||
elog_i(Protocol_TAG, "小车自动循迹启动, speed=%u%%, station=%u", car_speed_percent,
|
||
car_target_station);
|
||
} else if (strcmp(arg + 1, "STOP") == 0) {
|
||
car_running = 0;
|
||
elog_i(Protocol_TAG, "小车自动循迹暂停");
|
||
} else {
|
||
elog_w(Protocol_TAG, "未知启动/停止命令: %s", cmd_payload);
|
||
}
|
||
return;
|
||
}
|
||
|
||
// 设置整体速度命令
|
||
if (strncmp(cmd_payload, "SP", 2) == 0) {
|
||
unsigned int speed = 0;
|
||
|
||
// 解析速度参数（百分比，0~100）
|
||
if (sscanf(arg + 1, "%u", &speed) == 1) {
|
||
if (speed > 100U) {
|
||
speed = 100U;
|
||
}
|
||
|
||
car_speed_percent = (uint8_t)speed;
|
||
elog_i(Protocol_TAG, "设置整体速度: %u%%", car_speed_percent);
|
||
|
||
// 如果当前处于运行状态，立即生效
|
||
if (car_running != 0U) {
|
||
CarCtrl_ApplySpeed();
|
||
}
|
||
} else {
|
||
elog_w(Protocol_TAG, "速度参数解析失败: %s", cmd_payload);
|
||
}
|
||
return;
|
||
}
|
||
|
||
// 设置目标站点命令
|
||
if (strncmp(cmd_payload, "GS", 2) == 0) {
|
||
unsigned int station = 0;
|
||
|
||
// 解析站点编号（0~999）
|
||
if (sscanf(arg + 1, "%u", &station) == 1) {
|
||
if (station > 999U) {
|
||
station = 999U;
|
||
}
|
||
|
||
car_target_station = (uint16_t)station;
|
||
elog_i(Protocol_TAG, "设置目标站点: %03u", car_target_station);
|
||
} else {
|
||
elog_w(Protocol_TAG, "站点参数解析失败: %s", cmd_payload);
|
||
}
|
||
return;
|
||
}
|
||
|
||
// 未知命令类型
|
||
elog_w(Protocol_TAG, "未支持的控制命令: %s", cmd_payload);
|
||
}
|
||
|
||
/* 引用在 freertos.c 中定义的消息队列句柄 */
|
||
extern osMessageQueueId_t CmdQueueHandle;
|
||
/**
|
||
* @brief 协议处理函数
|
||
* @details 严格按照协议文档：校验范围 = 帧头 + 命令 + 数据
|
||
* 即从下标 0 开始，一直加到最后一个冒号之前
|
||
*/
|
||
void Protocol_HandleMessage(uint8_t *data, uint16_t len) {
|
||
if (data == NULL)
|
||
return;
|
||
|
||
// 1. 基础检查：长度必须足够，且必须以 '#' 结尾
|
||
if (len < 10 || data[len - 1] != '#') {
|
||
elog_w(Protocol_TAG, "协议错误：长度不足或帧尾错误 (len: %d)", len);
|
||
return;
|
||
}
|
||
|
||
// 2. 寻找校验位前的分隔符
|
||
// 协议格式：LOGI:CMD:DATA:CS#
|
||
// 我们需要找到最后一个冒号 ':' 的位置，它前面是数据，后面是校验位
|
||
int last_colon_pos = -1;
|
||
for (int i = 0; i < len; i++) {
|
||
if (data[i] == ':') {
|
||
last_colon_pos = i;
|
||
}
|
||
}
|
||
|
||
// 如果找不到冒号，说明格式错误
|
||
if (last_colon_pos == -1 || last_colon_pos < 5) {
|
||
elog_w(Protocol_TAG, "协议错误：找不到分隔符 ':' 或位置非法");
|
||
return;
|
||
}
|
||
|
||
// 3. 提取接收到的校验位 (从 ASCII 转为 Hex 数值)
|
||
// 校验位紧跟在 last_colon_pos 之后，长度为 2 字节
|
||
char recv_cs_hex_str[3] = {0};
|
||
// 防止越界
|
||
if (last_colon_pos + 3 >= len) {
|
||
elog_w(Protocol_TAG, "协议错误：校验位数据越界");
|
||
return;
|
||
}
|
||
|
||
recv_cs_hex_str[0] = data[last_colon_pos + 1];
|
||
recv_cs_hex_str[1] = data[last_colon_pos + 2];
|
||
|
||
unsigned int received_checksum = 0;
|
||
sscanf(recv_cs_hex_str, "%02X", &received_checksum);
|
||
|
||
// 4. 计算本地校验和
|
||
// 【核心修改点】
|
||
// 严格按照协议文档：从下标 0 开始，长度为 last_colon_pos
|
||
// 也就是计算 "LOGI:SP:080" 的累加和
|
||
uint8_t calculated_checksum =
|
||
Calculate_CheckSum(data, 0, (uint16_t)last_colon_pos);
|
||
|
||
// 5. 对比校验和
|
||
if (calculated_checksum == (uint8_t)received_checksum) {
|
||
elog_i(Protocol_TAG, "✅ 校验通过！执行指令: %s", (char *)data);
|
||
|
||
/* 提取有效载荷发送到消息队列 */
|
||
char cmd_payload[16] = {0};
|
||
// 将 "LOGI:" 之后到最后一个冒号之前的内容作为指令
|
||
uint16_t payload_len = last_colon_pos - 5;
|
||
uint16_t copy_len = (payload_len > 15) ? 15 : payload_len;
|
||
if (copy_len > 0) {
|
||
memcpy(cmd_payload, &data[5], copy_len);
|
||
}
|
||
|
||
osStatus_t status = osMessageQueuePut(CmdQueueHandle, cmd_payload, 0, 0);
|
||
if (status != osOK) {
|
||
elog_e(Protocol_TAG, "Protocol: Queue put failed: %d", status);
|
||
}
|
||
} else {
|
||
elog_w(Protocol_TAG, "❌ 校验失败！计算值: 0x%02X, 接收值: 0x%02X",
|
||
calculated_checksum, (uint8_t)received_checksum);
|
||
|
||
// 辅助调试：打印实际参与计算的数据段
|
||
char debug_buf[32] = {0};
|
||
if (last_colon_pos < 32) {
|
||
memcpy(debug_buf, data, last_colon_pos);
|
||
elog_i(Protocol_TAG, " -> 单片机正在计算这段数据的校验和: [%s]",
|
||
debug_buf);
|
||
elog_i(Protocol_TAG, " -> 请检查上位机是否也是按照此范围计算累加和");
|
||
}
|
||
}
|
||
}
|
||
|
||
#define CarCtrlTask_TAG "CarCtrlTask"
|
||
|
||
void CarCtrl_Task(void *argument) {
|
||
/* USER CODE BEGIN CarCtrl_Task */
|
||
char cmd_payload[16] = {0};
|
||
|
||
/* 初始化闭环 PID 控制器 */
|
||
CarCtrl_InitClosedLoop();
|
||
|
||
/* Infinite loop */
|
||
for (;;) {
|
||
/* 1. 处理控制指令 (非阻塞获取，如果没有指令则继续执行闭环) */
|
||
if (osMessageQueueGet(CmdQueueHandle, cmd_payload, NULL, 0) == osOK) {
|
||
elog_d(CarCtrlTask_TAG, "CarCtrl: Command %s", cmd_payload);
|
||
CarCtrl_HandleCommand(cmd_payload);
|
||
}
|
||
|
||
/* 2. 执行 PID 闭环控制更新 */
|
||
CarCtrl_UpdateClosedLoop();
|
||
|
||
/*
|
||
* 与 hall_update_speed() 的 100ms 采样周期对齐，避免 PID 使用过期速度反复修正。
|
||
* 如果后续把测速周期改成 20ms，这里也要同步改回 20ms。
|
||
*/
|
||
osDelay(100);
|
||
}
|
||
/* USER CODE END CarCtrl_Task */
|
||
} |