523 lines
20 KiB
C
523 lines
20 KiB
C
|
#include "app_tasks.h"
|
|||
|
|
#include "FreeRTOS.h"
|
|||
|
|
#include "cmsis_os.h"
|
|||
|
|
#include "main.h"
|
|||
|
|
#include "snc_can_app.h"
|
|||
|
|
#include "robot_params.h" /* 统一参数配置 */
|
|||
|
|
#include <stdio.h>
|
|||
|
|
#include <string.h>
|
|||
|
|
|
|||
|
|
/* 引入各个传感器和黑板的头文件 */
|
|||
|
|
#include "Contract/robot_blackboard.h"
|
|||
|
|
#include "Contract/robot_odom.h"
|
|||
|
|
#include "laser/laser_manager.h"
|
|||
|
|
#include "IMU/hwt101.h"
|
|||
|
|
#include "vl53_board.h"
|
|||
|
|
#include "Contract/robot_cmd_slot.h"
|
|||
|
|
#include "preproc/corridor_preproc.h"
|
|||
|
|
#include "est/corridor_filter.h"
|
|||
|
|
#include "nav/corridor_ctrl.h"
|
|||
|
|
#include "nav/segment_fsm.h"
|
|||
|
|
#include "nav/nav_script.h"
|
|||
|
|
#include "nav/global_nav.h"
|
|||
|
|
#include "nav/track_map.h"
|
|||
|
|
extern osMutexId_t logMutexHandle;
|
|||
|
|
|
|||
|
|
/* 如果你的项目中没有引入 i2c.h，可以通过 extern 声明 I2C 句柄 */
|
|||
|
|
extern I2C_HandleTypeDef hi2c1;
|
|||
|
|
extern I2C_HandleTypeDef hi2c2;
|
|||
|
|
|
|||
|
|
static float g_test_vx = 0.0f;
|
|||
|
|
static float g_test_wz = 0.0f;
|
|||
|
|
|
|||
|
|
static uint32_t AppTasks_MsToTicks(uint32_t ms)
|
|||
|
|
{
|
|||
|
|
uint32_t ticks = pdMS_TO_TICKS(ms);
|
|||
|
|
return (ticks == 0U) ? 1U : ticks;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static void App_UpdateTestCommand(void)
|
|||
|
|
{
|
|||
|
|
g_test_vx = 0.0f;
|
|||
|
|
g_test_wz = 0.0f;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static void App_PrintStatus(void)
|
|||
|
|
{
|
|||
|
|
// 【新增】静态计数器，由于是 static 修饰，它的值在函数退出后不会丢失
|
|||
|
|
static uint8_t print_divider = 0;
|
|||
|
|
|
|||
|
|
// 假设外部任务是 100ms 调用一次该函数：
|
|||
|
|
// 如果设定 >= 5，就是 5 * 100ms = 500ms 打印一次
|
|||
|
|
// 如果设定 >= 10，就是 10 * 100ms = 1000ms (1秒) 打印一次
|
|||
|
|
if (++print_divider < 5) {
|
|||
|
|
return; // 还没到设定的次数，直接 return，不执行后续获取快照和打印的动作
|
|||
|
|
}
|
|||
|
|
print_divider = 0; // 计数器清零，重新开始下一轮计数
|
|||
|
|
|
|||
|
|
// 1. 获取全局黑板的快照
|
|||
|
|
RobotBlackboard_t snap;
|
|||
|
|
Blackboard_GetSnapshot(&snap);
|
|||
|
|
|
|||
|
|
// 2. 申请串口互斥锁，防止多线程打印打架导致乱码
|
|||
|
|
if (logMutexHandle != NULL) {
|
|||
|
|
(void)osMutexAcquire(logMutexHandle, osWaitForever);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 3. 打印表头和当前系统时间
|
|||
|
|
printf("\r\n========== Robot Status [%lu ms] ==========\r\n", HAL_GetTick());
|
|||
|
|
|
|||
|
|
// 4. 打印 IMU 姿态
|
|||
|
|
if (snap.imu_yaw.is_valid) {
|
|||
|
|
printf("[IMU] Yaw: %7.2f deg | Cont: %7.2f deg | Wz: %7.2f\r\n",
|
|||
|
|
snap.imu_yaw.value, snap.imu_yaw_continuous.value, snap.imu_wz.value);
|
|||
|
|
} else {
|
|||
|
|
printf("[IMU] --- OFFLINE or INVALID ---\r\n");
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 5. 打印侧向阵列雷达 (强制转为 int，去掉多余的小数点)
|
|||
|
|
printf("[%s] Left-F : %4d mm | Left-R : %4d mm\r\n",
|
|||
|
|
VL53_BOARD_DRIVER_NAME,
|
|||
|
|
(int)snap.dist_left_f.value, (int)snap.dist_left_r.value);
|
|||
|
|
printf(" Right-F: %4d mm | Right-R: %4d mm\r\n",
|
|||
|
|
(int)snap.dist_right_f.value, (int)snap.dist_right_r.value);
|
|||
|
|
|
|||
|
|
// 6. 打印前后激光测距雷达 (四个探头独立显示)
|
|||
|
|
printf("[LASER] F-STP: %4d mm | F-ATK: %4d mm\r\n",
|
|||
|
|
(int)snap.dist_front_stp.value, (int)snap.dist_front_atk.value);
|
|||
|
|
printf(" R-STP: %4d mm | R-ATK: %4d mm\r\n",
|
|||
|
|
(int)snap.dist_rear_stp.value, (int)snap.dist_rear_atk.value);
|
|||
|
|
printf("==================================================\r\n");
|
|||
|
|
|
|||
|
|
// 7. 释放锁
|
|||
|
|
if (logMutexHandle != NULL) {
|
|||
|
|
(void)osMutexRelease(logMutexHandle);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void AppTasks_RunCanTxTask(void *argument)
|
|||
|
|
{
|
|||
|
|
(void)argument;
|
|||
|
|
uint32_t wake_tick = osKernelGetTickCount();
|
|||
|
|
const uint32_t period_ticks = AppTasks_MsToTicks(20U);
|
|||
|
|
// 用于存放从指令槽里取出来的控制指令
|
|||
|
|
RobotTargetCmd_t current_cmd;
|
|||
|
|
|
|||
|
|
for (;;) {
|
|||
|
|
// 维持心跳发送
|
|||
|
|
SNC_CAN_20msTask();
|
|||
|
|
|
|||
|
|
// 从指令槽中获取最新指令，设置 100ms 超时容忍度
|
|||
|
|
if (CmdSlot_Pop(¤t_cmd, 100)) {
|
|||
|
|
// 如果算法层在 100ms 内有更新指令，正常下发给底盘
|
|||
|
|
(void)SNC_CAN_SendCmdVel(current_cmd.target_vx, current_cmd.target_wz, current_cmd.ctrl_flags);
|
|||
|
|
} else {
|
|||
|
|
// 🚨 触发安全保护：如果超过 100ms 没收到上层新指令（可能算法死机或网络断开）
|
|||
|
|
// 强制发送 0 速度，让底盘立刻停车，防止飞车撞墙
|
|||
|
|
(void)SNC_CAN_SendCmdVel(0.0f, 0.0f, 0U);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
wake_tick += period_ticks;
|
|||
|
|
(void)osDelayUntil(wake_tick);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void AppTasks_RunMonitorTask(void *argument)
|
|||
|
|
{
|
|||
|
|
(void)argument;
|
|||
|
|
uint32_t wake_tick = osKernelGetTickCount();
|
|||
|
|
const uint32_t period_ticks = AppTasks_MsToTicks(100U);
|
|||
|
|
|
|||
|
|
for (;;) {
|
|||
|
|
uint32_t now_ms = HAL_GetTick();
|
|||
|
|
|
|||
|
|
// 1. 执行 CAN 侧的周期性监控 (心跳/超时判断)
|
|||
|
|
SNC_CAN_100msTask();
|
|||
|
|
SNC_CAN_PollOnlineState(now_ms);
|
|||
|
|
|
|||
|
|
// 2. 获取 CAN 数据上下文
|
|||
|
|
const SNC_CAN_AppContext_t *can_ctx = SNC_CAN_GetContext();
|
|||
|
|
|
|||
|
|
// 3. 将底盘的核心状态写入全局黑板
|
|||
|
|
Blackboard_UpdateChassisState(can_ctx->status.state,
|
|||
|
|
can_ctx->status.diag_bits,
|
|||
|
|
can_ctx->status.online);
|
|||
|
|
|
|||
|
|
// 4. 里程计更新已迁移到 navTask(20ms)，此处不再消费
|
|||
|
|
// 原因：odom 实时性对 EKF 预测至关重要，需要与导航闭环同频
|
|||
|
|
|
|||
|
|
/* 指令下发已由 navTask 接管，此处不再发送测试指令 */
|
|||
|
|
// 调用纯净版播报员 (内部自带分频逻辑)
|
|||
|
|
// App_PrintStatus();
|
|||
|
|
|
|||
|
|
wake_tick += period_ticks;
|
|||
|
|
(void)osDelayUntil(wake_tick);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
/* =========================================================
|
|||
|
|
* 1. 前后激光雷达任务 (Laser Manager)
|
|||
|
|
* ========================================================= */
|
|||
|
|
void AppTasks_RunLaserTestTask_Impl(void *argument)
|
|||
|
|
{
|
|||
|
|
(void)argument;
|
|||
|
|
|
|||
|
|
// 初始化前后防撞雷达
|
|||
|
|
LASER_SIMPLE_Init();
|
|||
|
|
|
|||
|
|
for (;;)
|
|||
|
|
{
|
|||
|
|
// 底层高频巡检
|
|||
|
|
LASER_SIMPLE_Poll(HAL_GetTick());
|
|||
|
|
|
|||
|
|
// 获取无撕裂快照
|
|||
|
|
const laser_simple_snapshot_t *snap = LASER_SIMPLE_GetSnapshot();
|
|||
|
|
|
|||
|
|
// 写入全局数据黑板
|
|||
|
|
Blackboard_UpdateLaser(snap);
|
|||
|
|
|
|||
|
|
// 维持 50ms 巡检频率 (避障数据建议稍快一些)
|
|||
|
|
osDelay(50);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* =========================================================
|
|||
|
|
* 2. IMU 姿态传感器任务 (HWT101)
|
|||
|
|
* ========================================================= */
|
|||
|
|
void AppTasks_RunImuTask_Impl(void *argument)
|
|||
|
|
{
|
|||
|
|
(void)argument;
|
|||
|
|
|
|||
|
|
// 初始化 IMU (开启 DMA 接收)，使用 UART7
|
|||
|
|
HWT101_Init(&huart7);
|
|||
|
|
osDelay(100);
|
|||
|
|
HWT101_ZeroYaw();
|
|||
|
|
for (;;) {
|
|||
|
|
// 高频解析 DMA 数据
|
|||
|
|
HWT101_Process();
|
|||
|
|
|
|||
|
|
// 安全拷贝 IMU 数据并写入黑板
|
|||
|
|
HWT101_Data_t imu_data;
|
|||
|
|
HWT101_GetData(&imu_data);
|
|||
|
|
Blackboard_UpdateIMU(&imu_data);
|
|||
|
|
|
|||
|
|
osDelay(10); // 10ms 解析与更新周期
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* =========================================================
|
|||
|
|
* 3. 侧向阵列雷达任务 (VL53 双总线)
|
|||
|
|
* ========================================================= */
|
|||
|
|
void AppTasks_RunVl53Task_Impl(void *argument)
|
|||
|
|
{
|
|||
|
|
(void)argument;
|
|||
|
|
static Vl53Board_t left_board;
|
|||
|
|
static Vl53Board_t right_board;
|
|||
|
|
/* --- 配置左侧雷达 (I2C2) --- */
|
|||
|
|
Vl53BoardHwCfg_t left_cfgs[2] = {
|
|||
|
|
{&hi2c2, GPIOB, GPIO_PIN_1, 0x62, "LF", 0}, // 左前 (PB1)
|
|||
|
|
{&hi2c2, GPIOC, GPIO_PIN_5, 0x64, "LR", 1} // 左后 (PC5)
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
// 【修改 1】：测距预算从 robot_params.h 读取
|
|||
|
|
Vl53Board_Init(&left_board, left_cfgs, 2, PARAM_VL53_TIMING_BUDGET);
|
|||
|
|
Vl53Board_StartContinuous(&left_board);
|
|||
|
|
|
|||
|
|
/* --- 配置右侧雷达 (I2C1) --- */
|
|||
|
|
Vl53BoardHwCfg_t right_cfgs[2] = {
|
|||
|
|
{&hi2c1, GPIOD, GPIO_PIN_13, 0x56, "RF", 2}, // 右前 (PD13)
|
|||
|
|
{&hi2c1, GPIOD, GPIO_PIN_14, 0x58, "RR", 3} // 右后 (PD14)
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
// 【修改 2】：同样从 robot_params.h 读取
|
|||
|
|
Vl53Board_Init(&right_board, right_cfgs, 2, PARAM_VL53_TIMING_BUDGET);
|
|||
|
|
Vl53Board_StartContinuous(&right_board);
|
|||
|
|
|
|||
|
|
Vl53BoardSnapshot_t left_snap, right_snap, combined_snap;
|
|||
|
|
|
|||
|
|
for(;;) {
|
|||
|
|
// 读取左右两侧的雷达快照
|
|||
|
|
Vl53Board_ReadAll(&left_board, &left_snap);
|
|||
|
|
Vl53Board_ReadAll(&right_board, &right_snap);
|
|||
|
|
|
|||
|
|
// 合并到一个统一的 Snapshot 中送给黑板
|
|||
|
|
memset(&combined_snap, 0, sizeof(combined_snap));
|
|||
|
|
combined_snap.tick_ms = HAL_GetTick();
|
|||
|
|
|
|||
|
|
// 映射约定：[0]左前，[1]左后，[2]右前，[3]右后
|
|||
|
|
for(int i = 0; i < 2; i++) {
|
|||
|
|
// 左侧复制到 [0], [1]
|
|||
|
|
combined_snap.range_mm[i] = left_snap.range_mm[i];
|
|||
|
|
combined_snap.range_mm_filtered[i] = left_snap.range_mm_filtered[i];
|
|||
|
|
combined_snap.range_status[i] = left_snap.range_status[i];
|
|||
|
|
|
|||
|
|
// 右侧偏移 2 个身位，复制到 [2], [3]
|
|||
|
|
combined_snap.range_mm[i+2] = right_snap.range_mm[i];
|
|||
|
|
combined_snap.range_mm_filtered[i+2] = right_snap.range_mm_filtered[i];
|
|||
|
|
combined_snap.range_status[i+2] = right_snap.range_status[i];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 合并掩码 (右侧掩码左移 2 位)
|
|||
|
|
combined_snap.valid_mask = (left_snap.valid_mask & 0x03) | ((right_snap.valid_mask & 0x03) << 2);
|
|||
|
|
|
|||
|
|
// 统一推送到黑板
|
|||
|
|
Blackboard_UpdateVl53(&combined_snap);
|
|||
|
|
|
|||
|
|
// 与 VL53 约 33ms 的连续测量节奏匹配，避免高频空轮询旧数据。
|
|||
|
|
osDelay(33);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* =========================================================
|
|||
|
|
* 4. 走廊导航控制任务 (Nav Pipeline)
|
|||
|
|
* 周期：20ms (50Hz)，与 CAN 发送同频
|
|||
|
|
* 流水线：Obs → Filter → Ctrl → Script → FSM → CmdSlot_Push
|
|||
|
|
* ========================================================= */
|
|||
|
|
void AppTasks_RunNavTask_Impl(void *argument)
|
|||
|
|
{
|
|||
|
|
(void)argument;
|
|||
|
|
uint32_t wake_tick = osKernelGetTickCount();
|
|||
|
|
const uint32_t period_ticks = AppTasks_MsToTicks(20U);
|
|||
|
|
uint32_t last_ms = HAL_GetTick();
|
|||
|
|
|
|||
|
|
/* 等传感器全部就绪再启动 (避免刚上电全是脏数据)。
|
|||
|
|
* 实车上 IMU 清零和多路测距任务稳定需要数秒，500ms 不够。 */
|
|||
|
|
osDelay(PARAM_NAV_STARTUP_DELAY_MS);
|
|||
|
|
|
|||
|
|
#if USE_GLOBAL_NAV
|
|||
|
|
GlobalNav_Start(); /* 开始赛道级导航 */
|
|||
|
|
#else
|
|||
|
|
NavScript_Start(); /* 单沟测试模式 */
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
for (;;) {
|
|||
|
|
uint32_t now_ms = HAL_GetTick();
|
|||
|
|
float dt_s = (float)(now_ms - last_ms) / 1000.0f;
|
|||
|
|
if (dt_s <= 0.0f || dt_s > 0.5f) {
|
|||
|
|
dt_s = 0.02f; /* 容错：防止首拍或溢出 */
|
|||
|
|
}
|
|||
|
|
last_ms = now_ms;
|
|||
|
|
|
|||
|
|
/* --- Step 0: 里程计更新（从 monitorTask 迁移至此，20ms 实时消费）--- */
|
|||
|
|
{
|
|||
|
|
int16_t odom_fl, odom_rl, odom_fr, odom_rr;
|
|||
|
|
uint32_t odom_span_ms;
|
|||
|
|
uint8_t odom_frames = SNC_CAN_ConsumeOdomDelta(
|
|||
|
|
&odom_fl, &odom_rl,
|
|||
|
|
&odom_fr, &odom_rr,
|
|||
|
|
&odom_span_ms);
|
|||
|
|
|
|||
|
|
if (odom_frames > 0U) {
|
|||
|
|
/* 有新的增量帧需要积分 */
|
|||
|
|
Odom_Update(now_ms,
|
|||
|
|
odom_fl, odom_rl,
|
|||
|
|
odom_fr, odom_rr,
|
|||
|
|
odom_span_ms);
|
|||
|
|
} else {
|
|||
|
|
/* odom 断流保护：超过协议允许时间后主动清零黑板里的速度 */
|
|||
|
|
Odom_HandleTimeout(now_ms, SNC_ODOM_TIMEOUT_MS);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* --- Step 1: 拍摄黑板快照 --- */
|
|||
|
|
RobotBlackboard_t board;
|
|||
|
|
Blackboard_GetSnapshot(&board);
|
|||
|
|
|
|||
|
|
/* --- Step 2: 预处理 → 清洗观测 --- */
|
|||
|
|
CorridorObs_t obs;
|
|||
|
|
CorridorPreproc_ExtractObs(&board, now_ms, &obs);
|
|||
|
|
|
|||
|
|
/* --- Step 3: EKF → 走廊状态估计 --- */
|
|||
|
|
/* 注意: HWT101 输出 wz 单位是 °/s，EKF 需要 rad/s，必须转换！ */
|
|||
|
|
float imu_wz_raw = board.imu_wz.is_valid ? board.imu_wz.value : 0.0f;
|
|||
|
|
float imu_wz = PARAM_DEG2RAD(imu_wz_raw);
|
|||
|
|
float odom_vx = board.odom_vx;
|
|||
|
|
|
|||
|
|
/* IMU 连续 yaw → rad，作为 EKF 额外航向观测 */
|
|||
|
|
float imu_yaw_cont_rad = board.imu_yaw_continuous.is_valid
|
|||
|
|
? PARAM_DEG2RAD(board.imu_yaw_continuous.value) : 0.0f;
|
|||
|
|
bool imu_yaw_ok = board.imu_yaw_continuous.is_valid;
|
|||
|
|
|
|||
|
|
CorridorState_t corridor_state;
|
|||
|
|
CorridorFilter_Update(&obs, imu_wz, odom_vx, dt_s,
|
|||
|
|
imu_yaw_cont_rad, imu_yaw_ok, &corridor_state);
|
|||
|
|
|
|||
|
|
#if USE_GLOBAL_NAV
|
|||
|
|
/* ========== 赛道级导航模式 (6沟 S 型遍历) ========== */
|
|||
|
|
|
|||
|
|
/* --- Step 4: 赛道级导航 --- */
|
|||
|
|
GlobalNavOutput_t nav_out;
|
|||
|
|
GlobalNav_Update(&obs, &corridor_state, &board, now_ms, &nav_out);
|
|||
|
|
|
|||
|
|
/* --- Step 5: 控制律 --- */
|
|||
|
|
RawCmd_t raw_cmd;
|
|||
|
|
memset(&raw_cmd, 0, sizeof(raw_cmd));
|
|||
|
|
raw_cmd.t_ms = now_ms;
|
|||
|
|
|
|||
|
|
if (nav_out.request_corridor) {
|
|||
|
|
/* 沟内闭环：使用走廊控制器 */
|
|||
|
|
CorridorCtrl_Compute(&corridor_state, &obs, imu_wz, &raw_cmd);
|
|||
|
|
} else if (nav_out.use_override) {
|
|||
|
|
/* 赛道级覆盖：直接用导航输出 */
|
|||
|
|
raw_cmd.v = nav_out.override_v;
|
|||
|
|
raw_cmd.w = nav_out.override_w;
|
|||
|
|
}
|
|||
|
|
/* else: raw_cmd 已是零速 */
|
|||
|
|
|
|||
|
|
/* --- Step 6: 安全仲裁 (带动作模式感知) --- */
|
|||
|
|
SegFsmOutput_t fsm_out;
|
|||
|
|
SegFsm_Update(&raw_cmd, &obs, &corridor_state, nav_out.safety_mode, &fsm_out);
|
|||
|
|
|
|||
|
|
#else
|
|||
|
|
/* ========== 单沟测试模式 (原 nav_script) ========== */
|
|||
|
|
|
|||
|
|
/* --- Step 4: 段脚本执行器 --- */
|
|||
|
|
NavScriptOutput_t script_out;
|
|||
|
|
float imu_yaw_cont_deg = board.imu_yaw_continuous.is_valid
|
|||
|
|
? board.imu_yaw_continuous.value : 0.0f;
|
|||
|
|
NavScript_Update(&obs, &corridor_state, imu_yaw_cont_deg, &script_out);
|
|||
|
|
|
|||
|
|
/* --- Step 5: 控制律 --- */
|
|||
|
|
RawCmd_t raw_cmd;
|
|||
|
|
if (script_out.use_override) {
|
|||
|
|
raw_cmd.t_ms = now_ms;
|
|||
|
|
raw_cmd.v = script_out.override_v;
|
|||
|
|
raw_cmd.w = script_out.override_w;
|
|||
|
|
raw_cmd.flags = 0U;
|
|||
|
|
} else if (script_out.request_corridor) {
|
|||
|
|
CorridorCtrl_Compute(&corridor_state, &obs, imu_wz, &raw_cmd);
|
|||
|
|
} else {
|
|||
|
|
raw_cmd.t_ms = now_ms;
|
|||
|
|
raw_cmd.v = 0.0f;
|
|||
|
|
raw_cmd.w = 0.0f;
|
|||
|
|
raw_cmd.flags = 0U;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* --- Step 6: 安全仲裁 (按脚本阶段切换模式) --- */
|
|||
|
|
SegFsmOutput_t fsm_out;
|
|||
|
|
SafetyMode_t safety_mode = SAFETY_MODE_CORRIDOR;
|
|||
|
|
if (!script_out.active) {
|
|||
|
|
safety_mode = SAFETY_MODE_IDLE;
|
|||
|
|
} else if (script_out.stage == SCRIPT_STAGE_TURN_AT_END) {
|
|||
|
|
safety_mode = SAFETY_MODE_TURN;
|
|||
|
|
}
|
|||
|
|
SegFsm_Update(&raw_cmd, &obs, &corridor_state, safety_mode, &fsm_out);
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
/* --- Step 7: 将安全后的指令喂给 CAN 发送层 --- */
|
|||
|
|
CmdSlot_Push(fsm_out.safe_v, fsm_out.safe_w, 0U);
|
|||
|
|
|
|||
|
|
wake_tick += period_ticks;
|
|||
|
|
(void)osDelayUntil(wake_tick);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void AppTasks_Init(void)
|
|||
|
|
{
|
|||
|
|
SNC_CAN_AppInit();
|
|||
|
|
Odom_Init();
|
|||
|
|
|
|||
|
|
/* --- 初始化走廊滤波器 (EKF) --- */
|
|||
|
|
CorridorFilterConfig_t filter_cfg = {
|
|||
|
|
.sensor_base_length = PARAM_SENSOR_BASE_LENGTH, /* 实测：同侧前后雷达间距 */
|
|||
|
|
.corridor_width = PARAM_CORRIDOR_WIDTH, /* 实测：走廊宽度 */
|
|||
|
|
.y_offset = 0.0f, /* 0 = 绝对居中 */
|
|||
|
|
.side_sensor_inset = PARAM_VL53_SIDE_INSET, /* [兼容] 统一内缩距离 */
|
|||
|
|
.left_sensor_inset = PARAM_VL53_LEFT_INSET, /* [改进A] 左侧独立内缩 (实测后填入!) */
|
|||
|
|
.right_sensor_inset = PARAM_VL53_RIGHT_INSET, /* [改进A] 右侧独立内缩 (实测后填入!) */
|
|||
|
|
.robot_width = PARAM_ROBOT_WIDTH, /* 实测：车体宽度 */
|
|||
|
|
.alpha_theta = 0.98f, /* 保留兼容，EKF 内部使用 Q/R */
|
|||
|
|
.alpha_y = 0.7f,
|
|||
|
|
};
|
|||
|
|
CorridorFilter_Init(&filter_cfg);
|
|||
|
|
|
|||
|
|
/* --- 初始化走廊控制器 --- */
|
|||
|
|
CorridorCtrlConfig_t ctrl_cfg = {
|
|||
|
|
.kp_theta = PARAM_CTRL_KP_THETA, /* 调优：航向比例增益 */
|
|||
|
|
.kd_theta = PARAM_CTRL_KD_THETA, /* 调优：航向微分增益 */
|
|||
|
|
.kp_y = PARAM_CTRL_KP_Y, /* 调优：横向比例增益 */
|
|||
|
|
.v_cruise = PARAM_CTRL_V_CRUISE, /* 调优：巡航速度 */
|
|||
|
|
.w_max = PARAM_CTRL_W_MAX, /* 角速度限幅 */
|
|||
|
|
.v_max = PARAM_CTRL_V_MAX, /* 线速度限幅 */
|
|||
|
|
.speed_reduction_k = PARAM_CTRL_SPEED_REDUCTION, /* 调优：弯道减速系数 */
|
|||
|
|
.exit_front_dist = PARAM_CTRL_EXIT_FRONT_DIST, /* 调优：出沟检测距离 */
|
|||
|
|
.wall_escape_dist = PARAM_CTRL_WALL_ESCAPE_DIST,
|
|||
|
|
.wall_escape_kp = PARAM_CTRL_WALL_ESCAPE_KP,
|
|||
|
|
.wall_escape_w_max = PARAM_CTRL_WALL_ESCAPE_WMAX,
|
|||
|
|
};
|
|||
|
|
CorridorCtrl_Init(&ctrl_cfg);
|
|||
|
|
|
|||
|
|
/* --- 初始化段状态机 --- */
|
|||
|
|
SegFsmConfig_t fsm_cfg = {
|
|||
|
|
.d_front_stop = PARAM_SAFE_D_FRONT_STOP, /* 安全：前向停车距离 */
|
|||
|
|
.d_front_approach = PARAM_SAFE_D_FRONT_APPROACH, /* 安全：前向减速距离 */
|
|||
|
|
.approach_min_v = PARAM_SAFE_APPROACH_MIN_V, /* 安全：减速区最低速度 */
|
|||
|
|
.conf_estop_thresh = PARAM_SAFE_CONF_ESTOP, /* 安全：E-Stop 置信度阈值 */
|
|||
|
|
};
|
|||
|
|
SegFsm_Init(&fsm_cfg);
|
|||
|
|
SegFsm_Start(); /* P0 修复: 必须显式启动安全状态机，否则 IDLE 状态直接输出零速 */
|
|||
|
|
|
|||
|
|
#if USE_GLOBAL_NAV
|
|||
|
|
/* --- 初始化赛道地图 --- */
|
|||
|
|
TrackMap_Init();
|
|||
|
|
|
|||
|
|
/* --- 初始化赛道级导航状态机 --- */
|
|||
|
|
GlobalNavConfig_t gnav_cfg = {
|
|||
|
|
.entry_v = PARAM_GNAV_ENTRY_V,
|
|||
|
|
.entry_distance = PARAM_GNAV_ENTRY_DISTANCE,
|
|||
|
|
.entry_timeout_ms = PARAM_GNAV_ENTRY_TIMEOUT,
|
|||
|
|
.turn_omega = PARAM_GNAV_TURN_OMEGA,
|
|||
|
|
.turn_tolerance_rad = PARAM_GNAV_TURN_TOLERANCE,
|
|||
|
|
.turn_decel_zone_rad = PARAM_GNAV_TURN_DECEL_ZONE,
|
|||
|
|
.turn_min_omega = PARAM_GNAV_TURN_MIN_OMEGA,
|
|||
|
|
.turn_timeout_ms = PARAM_GNAV_TURN_TIMEOUT,
|
|||
|
|
.reacquire_v = PARAM_GNAV_REACQUIRE_V,
|
|||
|
|
.reacquire_conf_thresh = PARAM_GNAV_REACQUIRE_CONF,
|
|||
|
|
.reacquire_width_tol = PARAM_GNAV_REACQUIRE_WIDTH_TOL,
|
|||
|
|
.reacquire_min_odom = PARAM_GNAV_REACQUIRE_MIN_ODOM,
|
|||
|
|
.reacquire_confirm_ticks = PARAM_GNAV_REACQUIRE_TICKS,
|
|||
|
|
.reacquire_timeout_ms = PARAM_GNAV_REACQUIRE_TIMEOUT,
|
|||
|
|
.align_kp_th = PARAM_GNAV_ALIGN_KP_TH,
|
|||
|
|
.align_kp_y = PARAM_GNAV_ALIGN_KP_Y,
|
|||
|
|
.align_th_tol_rad = PARAM_GNAV_ALIGN_TH_TOL,
|
|||
|
|
.align_y_tol_m = PARAM_GNAV_ALIGN_Y_TOL,
|
|||
|
|
.align_confirm_ticks = PARAM_GNAV_ALIGN_TICKS,
|
|||
|
|
.align_timeout_ms = PARAM_GNAV_ALIGN_TIMEOUT,
|
|||
|
|
.reacquire_min_back_dist = PARAM_GNAV_REACQUIRE_MIN_BACK,
|
|||
|
|
.corridor_end_detect_dist = PARAM_GNAV_CORRIDOR_END_DIST,
|
|||
|
|
.corridor_length_max = PARAM_GNAV_CORRIDOR_MAX_LEN,
|
|||
|
|
.link_v = PARAM_GNAV_LINK_V,
|
|||
|
|
.link_distance = PARAM_GNAV_LINK_DISTANCE,
|
|||
|
|
.link_timeout_ms = PARAM_GNAV_LINK_TIMEOUT,
|
|||
|
|
.link_gap_runout = PARAM_GNAV_LINK_GAP_RUNOUT,
|
|||
|
|
.link_wall_target = PARAM_GNAV_LINK_WALL_TARGET,
|
|||
|
|
.link_wall_kp = PARAM_GNAV_LINK_WALL_KP,
|
|||
|
|
.link_wall_heading_kp = PARAM_GNAV_LINK_WALL_HEADING_KP,
|
|||
|
|
.link_wall_blend = PARAM_GNAV_LINK_WALL_BLEND,
|
|||
|
|
.exit_v = PARAM_GNAV_EXIT_V,
|
|||
|
|
.exit_runout = PARAM_GNAV_EXIT_RUNOUT,
|
|||
|
|
.exit_max_dist = PARAM_GNAV_EXIT_MAX_DIST,
|
|||
|
|
.exit_timeout_ms = PARAM_GNAV_EXIT_TIMEOUT,
|
|||
|
|
.dock_v = PARAM_GNAV_DOCK_V,
|
|||
|
|
.dock_distance = PARAM_GNAV_DOCK_DISTANCE,
|
|||
|
|
.heading_kp = PARAM_GNAV_HEADING_KP,
|
|||
|
|
.corridor_width = PARAM_CORRIDOR_WIDTH,
|
|||
|
|
};
|
|||
|
|
GlobalNav_Init(&gnav_cfg);
|
|||
|
|
|
|||
|
|
#else
|
|||
|
|
/* --- 初始化段脚本执行器 (单沟测试模式) --- */
|
|||
|
|
NavScriptConfig_t script_cfg = {
|
|||
|
|
.turn_target_angle = 3.14159265f, /* 固定：180度转向 */
|
|||
|
|
.turn_omega = PARAM_SCRIPT_TURN_OMEGA, /* 调优：转向角速度 */
|
|||
|
|
.corridor_length = 3.0f, /* 备用：垄沟长度估计 */
|
|||
|
|
.entry_align_timeout = PARAM_SCRIPT_ENTRY_TIMEOUT, /* 调优：入口对准超时 */
|
|||
|
|
.d_entry_exit_front = 0.12f, /* 调优：出入口距离阈值 */
|
|||
|
|
.entry_align_v = PARAM_SCRIPT_ENTRY_V, /* 调优：入口对准速度 */
|
|||
|
|
.exit_runout_m = PARAM_SCRIPT_EXIT_RUNOUT, /* 调优：退出后冲出距离 */
|
|||
|
|
.exit_v = PARAM_SCRIPT_EXIT_V, /* P1 修复：退出直线速度独立参数 */
|
|||
|
|
};
|
|||
|
|
NavScript_Init(&script_cfg);
|
|||
|
|
#endif
|
|||
|
|
}
|