导航部分效果最好的版本，主分支

App/Contract/robot_cmd_slot.c

@@ -0,0 +1,44 @@
+#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; // ✅ 指令新鲜，可以发送
+}