44 lines
1.2 KiB
C
44 lines
1.2 KiB
C
|
#include "robot_cmd_slot.h"
|
||
|
|
#include "FreeRTOS.h"
|
||
|
|
#include "task.h"
|
||
|
|
#include "main.h" // 提供 HAL_GetTick()
|
||
|
|
|
||
|
|
/* 全局唯一的指令槽实例 */
|
||
|
|
static RobotTargetCmd_t g_cmd_slot = {0};
|
||
|
|
|
||
|
|
void CmdSlot_Push(float vx, float wz, uint8_t flags)
|
||
|
|
{
|
||
|
|
// 1. 获取当前系统时间作为时间戳
|
||
|
|
uint32_t now_ms = HAL_GetTick();
|
||
|
|
|
||
|
|
// 2. 进入临界区，关中断，防止写入一半时被高优先级任务或中断打断（防数据撕裂）
|
||
|
|
taskENTER_CRITICAL();
|
||
|
|
g_cmd_slot.target_vx = vx;
|
||
|
|
g_cmd_slot.target_wz = wz;
|
||
|
|
g_cmd_slot.ctrl_flags = flags;
|
||
|
|
g_cmd_slot.gen_tick_ms = now_ms;
|
||
|
|
taskEXIT_CRITICAL();
|
||
|
|
}
|
||
|
|
|
||
|
|
bool CmdSlot_Pop(RobotTargetCmd_t *out_cmd, uint32_t timeout_ms)
|
||
|
|
{
|
||
|
|
if (out_cmd == NULL) {
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
// 1. 进入临界区，安全地拷贝走整个结构体
|
||
|
|
taskENTER_CRITICAL();
|
||
|
|
*out_cmd = g_cmd_slot;
|
||
|
|
taskEXIT_CRITICAL();
|
||
|
|
|
||
|
|
// 2. 计算指令老化时间 (利用无符号整型减法自然处理溢出回绕)
|
||
|
|
uint32_t now_ms = HAL_GetTick();
|
||
|
|
uint32_t age_ms = now_ms - out_cmd->gen_tick_ms;
|
||
|
|
|
||
|
|
// 3. 判定是否超时
|
||
|
|
if (age_ms > timeout_ms) {
|
||
|
|
return false; // 🚨 指令太老了，算法层可能死机了！
|
||
|
|
}
|
||
|
|
|
||
|
|
return true; // ✅ 指令新鲜，可以发送
|
||
|
|
}
|