ELCT/Simba-OmniInfantry
9
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

chore: 导入 Simba-OmniInfantry 基线(云台/底盘双工程)

Browse Source 
- 云台(ROBOT_TYPE_GIMBAL)与底盘(ROBOT_TYPE_CHASSIS)完整固件源码
- 新增根级 .gitignore,排除 Keil MDK-ARM 构建产物(.o/.d/.crf/.axf/.lnp/.map/.htm/.sct/.dep/.iex/.lst 等)
  与 *.uvguix.* / DebugConfig/(与机器绑定的 UI 布局)
- core.autocrlf=false,避免固件源码在检出时被改动换行符
This commit is contained in:
LSMushui
2026-09-12 10:28:21 +08:00
commit a4da5ad17c
517 changed files with 503795 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
底盘/Application/Task/Src/c_comm_task.c
+67
View File
@@ -0,0 +1,67 @@
/* =============================================================================
* c_comm_task.c - 板间通讯周期发送(2ms)
* 云台板: c_comm_master_send() 转发摇杆/控制状态(键鼠在线时用合成量)
* 底盘板: c_comm_slave_send() 回传发射机构上电状态
* 接收方向在中断内由 c_comm 模块的回调完成,本任务只负责发送节拍
* ========================================================================== */
#include "app_tasks.h"
#include "cmsis_os.h"
#include "FreeRTOS.h"
#include "task.h"
#include "c_comm.h"
#include "robot_typedef.h"
#include "robot_param.h"
#if defined(ROBOT_TYPE_GIMBAL)
#include "robot_cmd_task.h"
#include "gimbal.h"
#include "attitude.h"
#elif defined(ROBOT_TYPE_CHASSIS)
#include "chassis.h"
#endif
/* c_comm_task(2ms):板间通讯周期发送
* 云台板:向底盘转发摇杆与控制状态(键鼠在线时用合成量替代摇杆)
* 底盘板:向云台回传发射机构上电状态 */
void c_comm_task(void const * argument)
{
TickType_t last_wake = xTaskGetTickCount();
while (!app_ready)
osDelay(2);
for (;;)
{
#if defined(ROBOT_TYPE_GIMBAL)
uint8_t kb_online = (keyboard_data.last_tick != 0 &&
HAL_GetTick() - keyboard_data.last_tick < DEVICE_OFFLINE_TIME_MS * 10);
uint8_t kb[12];
kb[0] = (uint8_t)(keyboard_data.mouse_rl & 0xFF);
kb[1] = (uint8_t)((uint16_t)keyboard_data.mouse_rl >> 8);
kb[2] = (uint8_t)(keyboard_data.mouse_du & 0xFF);
kb[3] = (uint8_t)((uint16_t)keyboard_data.mouse_du >> 8);
kb[4] = (uint8_t)(keyboard_data.mouse_wheel & 0xFF);
kb[5] = (uint8_t)((uint16_t)keyboard_data.mouse_wheel >> 8);
kb[6] = keyboard_data.mouse_key_l;
kb[7] = keyboard_data.mouse_key_r;
kb[8] = (uint8_t)(keyboard_data.key_w | (keyboard_data.key_s << 1) |
(keyboard_data.key_a << 2) | (keyboard_data.key_d << 3) |
(keyboard_data.key_shift << 4) | (keyboard_data.key_ctrl << 5) |
(keyboard_data.key_q << 6) | (keyboard_data.key_e << 7));
kb[9] = (uint8_t)(keyboard_data.key_alpha1 | (keyboard_data.key_alpha2 << 1));
kb[10] = 0;
kb[11] = 0;
c_comm_master_send(kb_online, kb, gimbal_motor_offline(),
cmd_robot.spin_flag); // 故障位+小陀螺使能位
/* 0x1A9 姿态帧:双 INS 跟随方案的数据源 */
c_comm_master_send_attitude(att_deg_yaw, att_ready);
#elif defined(ROBOT_TYPE_CHASSIS)
c_comm_slave_send(chassis_motor_offline()); // 携带底盘电机故障位
#endif
monitor_heartbeat(HB_CCOMM);
vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(2));
}
}
底盘/Application/Task/Src/can_task.c
+63
View File
@@ -0,0 +1,63 @@
/* =============================================================================
* can_task.c - CAN_Task 强定义:全部硬件层初始化 + 蓝灯自检
* 顺序: led_init(蓝灯常亮=调度器/任务栈正常的证据) -> buzzer_init ->
* MX_USB_DEVICE_Init -> can_start -> 电机/发射/板间通讯初始化 ->
* 串口初始化 -> app_ready=1(其余任务在此之前的 while 等待)
* 注意: 本文件双板共用,板差异用 ROBOT_TYPE 宏条件编译
* ========================================================================== */
#include "app_tasks.h"
#include "cmsis_os.h"
#include "bsp_can.h"
#include "bsp_usart.h"
#include "usb_device.h"
#include "c_comm.h"
#include "led.h"
#include "buzzer.h"
#if defined(ROBOT_TYPE_GIMBAL)
#include "gimbal.h"
#include "shoot.h"
#elif defined(ROBOT_TYPE_CHASSIS)
#include "chassis.h"
#endif
volatile uint8_t app_ready = 0;
volatile uint32_t dbg_can_loop = 0; // 诊断:CAN_Task 循环计数
static uint8_t dr16_rx_buf[36]; // DR16 单帧 18 字节,缓冲放大一倍防止帧粘连
static uint8_t referee_rx_buf[128]; // 裁判系统变长帧缓冲
/* CAN_Task:完成全部硬件层初始化后置 app_ready,其余任务等待该标志
* 蓝灯常亮 = 本任务正在初始化(调度器与任务栈正常的最直观证据) */
void CAN_Task(void const * argument)
{
led_init();
buzzer_init();
led_set_blue(1);
MX_USB_DEVICE_Init();
can_start(); // CAN 过滤器/启动/中断
#if defined(ROBOT_TYPE_GIMBAL)
gimbal_motor_init(); // 电机实例与 CAN 回调注册
shoot_init();
c_comm_init(); // 板间通讯回调注册
usart3_init(dr16_rx_buf, 36, dr16_frame_callback); // DR16 遥控器
usart6_init(referee_rx_buf, 128, referee_frame_callback); // 图传链路(0x0304)
usart1_tx_dma_init(); // 视觉串口发送 DMA(TODO:视觉自瞄)
#elif defined(ROBOT_TYPE_CHASSIS)
chassis_motor_init(); // 电机实例与 CAN 回调注册
c_comm_init();
usart6_init(referee_rx_buf, 128, referee_frame_callback); // 裁判系统(0x0201/0x0202)
#endif
app_ready = 1;
for (;;)
{
dbg_can_loop++;
osDelay(1000);
}
}
底盘/Application/Task/Src/chassis_task.c
+146
View File
@@ -0,0 +1,146 @@
/* =============================================================================
* chassis_task.c - 底盘运动控制(2ms)
* 状态机:
* F_WAIT 等待"底盘姿态就绪+四轮在线+云台自检结果下发(0x1A9)"
* F_RUN 正常模式控制(跟随/小陀螺/停车;符号用参数回退值直至自检完成)
* F_DETECT 跟随符号自检——**仅由操作手手势显式触发,绝不自动执行**:
* 遥控在线时"左摇杆推到最左 + 右摇杆推到最右"保持 0.5s。
* 检测给四轮约 5% 电流脉冲 80ms,落地时整车轻转一下,操作手
* 应确认车周围无人后再做手势。失败重试 3 次后回退参数符号。
* 功率限制:缓冲能量不足时按比例缩放输出(裁判系统 0x0202)
* SHIFT:临时速度增益(超电驱动 TODO,接入后替换)
* ========================================================================== */
#include "app_tasks.h"
#include "cmsis_os.h"
#include "FreeRTOS.h"
#include "task.h"
#include "robot_typedef.h"
#include "chassis.h"
#include "attitude.h"
#include "referee.h"
#include "c_comm.h"
#include "robot_param.h"
/* 校准手势:左摇杆推最左 + 右摇杆推最右(与云台侧手势一致,两板同时自检) */
#define CALIB_GESTURE_L_RL 400 // < 400 即最左
#define CALIB_GESTURE_R_RL 1600 // > 1600 即最右
#define CALIB_GESTURE_MS 500 // 保持时长
#define CALIB_GESTURE_TICKS (CALIB_GESTURE_MS / 2) // 2ms 任务
typedef enum
{
F_WAIT = 0,
F_RUN,
F_DETECT,
} follow_state_e;
void chassis_task(void const * argument)
{
TickType_t last_wake = xTaskGetTickCount();
follow_state_e state = F_WAIT;
uint8_t detect_retry = 0;
uint16_t gesture_cnt = 0;
while (!app_ready)
osDelay(2);
for (;;)
{
/* 校准手势检测(遥控在线=ctoc_alive 时才可能成立;摇杆值来自 0x149 转发) */
if (cmd_robot.online &&
ctoc_remote.rc_l_rl < CALIB_GESTURE_L_RL && ctoc_remote.rc_r_rl > CALIB_GESTURE_R_RL)
{
if (gesture_cnt < CALIB_GESTURE_TICKS)
gesture_cnt++;
}
else
gesture_cnt = 0;
const uint8_t gesture = (gesture_cnt >= CALIB_GESTURE_TICKS);
if (gesture)
gesture_cnt = 0;
switch (state)
{
case F_WAIT:
chassis_output_zero();
/* 自身就绪即可进入运行(符号先用参数回退值) */
if (!chassis_motor_offline() && att_ready)
state = F_RUN;
break;
case F_DETECT:
if (chassis_detect_follow_sign())
{
chassis_follow_reset(); // 检测期间车被动转过,重锁基准
state = F_RUN;
}
else if (++detect_retry >= 3)
{
detect_retry = 0;
chassis_follow_reset();
state = F_RUN; // 回退参数符号
}
else
chassis_output_zero();
break;
case F_RUN:
default:
if (gesture && !chassis_motor_offline() && att_ready && ctoc_encoder_ccw != 0)
{
detect_retry = 0;
chassis_output_zero();
chassis_follow_reset();
state = F_DETECT; // 操作手主动请求自检
}
else if (cmd_robot.online && !chassis_motor_offline())
{
float w = 0;
switch (cmd_robot.chassis_mode)
{
case CHASSIS_MODE_SPIN_SLOW:
w = MECANUM_SPIN_SLOW_W;
break;
case CHASSIS_MODE_SPIN_FAST:
w = MECANUM_SPIN_FAST_W;
break;
case CHASSIS_MODE_FOLLOW:
default:
w = chassis_follow_w(); // 编码器相对角,零漂移
break;
}
/* SHIFT 加速(超电接入前的临时方案,受功率限制约束) */
float boost = 1.0f;
if (ctoc_remote.shift_push)
boost = MECANUM_SHIFT_BOOST_RATE;
float vx = cmd_robot.vx * boost;
float vy = cmd_robot.vy * boost;
/* 简易功率限制:缓冲能量线性映射为缩放系数 */
float scale = 1.0f;
if (referee.buffer_energy < POWER_LIMIT_BUFFER_LOW)
scale = (referee.buffer_energy - POWER_LIMIT_BUFFER_MIN) /
(POWER_LIMIT_BUFFER_LOW - POWER_LIMIT_BUFFER_MIN);
if (scale < 0.0f)
scale = 0.0f;
else if (scale > 1.0f)
scale = 1.0f;
chassis_control(vx, vy, w, scale);
}
else
{
chassis_follow_reset();
chassis_output_zero();
}
break;
}
monitor_heartbeat(HB_CHASSIS);
vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(2));
}
}
底盘/Application/Task/Src/debug_task.c
+82
View File
@@ -0,0 +1,82 @@
/* =============================================================================
* debug_task.c - USB 虚拟串口(CDC)诊断输出(仅诊断用,稳定后可整组移除)
* 每 500ms 一行状态:各芯片实测 ID/DR16 帧计数/USART 空闲中断计数/任务循环
* 计数等;板子插电脑出现虚拟 COM 口,串口助手打开即可(波特率随意)。
* 字段含义见重构说明与编译指南.md 的诊断章节
* ========================================================================== */
#include "debug_task.h"
#include "app_tasks.h"
#include "cmsis_os.h"
#include "FreeRTOS.h"
#include "task.h"
#include "usb_device.h"
#include "usbd_cdc_if.h"
#include "usart.h"
#include "bsp_can.h"
#include "bsp_usart.h"
#include <stdio.h>
#include "dr16.h"
#include "referee.h"
#include "c_comm.h"
#if defined(ROBOT_TYPE_GIMBAL)
#include "bmi088.h"
#include "ist8310.h"
#include "ins.h"
#include "attitude.h"
#include "gimbal.h"
#elif defined(ROBOT_TYPE_CHASSIS)
#include "chassis.h"
#include "bmi088.h"
#include "ins.h"
#include "robot_typedef.h"
#endif
/* debug_task:USB 虚拟串口(CDC)诊断输出,每 500ms 一行
* 板子插上电脑会出现虚拟 COM 口,用串口助手打开即可看到(波特率随意)
* 输出为纯整数,用于远程定位传感器/通讯初始化问题 */
/* 各模块导出的诊断变量(定义在 imu_task.c / can_task.c) */
extern volatile uint32_t dbg_imu_loop;
extern volatile uint32_t dbg_can_loop;
void debug_task(void const * argument)
{
char line[128];
for (;;)
{
int len = 0;
#if defined(ROBOT_TYPE_GIMBAL)
len = snprintf(line, sizeof(line),
"app=%u can=%u imu=%u acc=%02X gyro=%02X bmi=%u ist=%u who=%02X "
"cvg=%u ys=%d yaw=%.1f u3g=%02X i3=%u l3=%u d16=%u ref=%u/%u mot=%u\r\n",
app_ready, dbg_can_loop, dbg_imu_loop,
bmi088_last_acc_id, bmi088_last_gyro_id, bmi088_online,
ist8310_online, ist8310_last_who,
INS.ConvergeFlag,
(int)gimbal_yaw_sign(), att_deg_yaw,
(unsigned)(huart3.gState), dbg_usart3_idle, dbg_usart3_last_len,
dr16_frame_count,
referee.link_status, (unsigned)(HAL_GetTick() - referee.last_update_tick),
gimbal_motor_offline());
#elif defined(ROBOT_TYPE_CHASSIS)
len = snprintf(line, sizeof(line),
"app=%u can=%u imu=%u acc=%02X gyro=%02X cvg=%u "
"ctoc=%u/%u att=%u/%d mot=%u md=%u err=%.1f fw=%d spin=%u lim=%u\r\n",
app_ready, dbg_can_loop, dbg_imu_loop,
bmi088_last_acc_id, bmi088_last_gyro_id,
INS.ConvergeFlag,
ctoc_remote.status,
(unsigned)(ctoc_remote.last_tick ? HAL_GetTick() - ctoc_remote.last_tick : 0),
ctoc_att_fresh(), (int)ctoc_gimbal_yaw,
chassis_motor_offline(),
cmd_robot.chassis_mode,
dbg_follow_err_deg, (int)dbg_follow_out,
ctoc_remote.spin_flag, referee.chassis_power_limit);
#endif
if (len > 0)
CDC_Transmit_FS((uint8_t *)line, (uint16_t)len);
osDelay(500);
}
}
底盘/Application/Task/Src/imu_task.c
+71
View File
@@ -0,0 +1,71 @@
/* =============================================================================
* imu_task.c - IMU_Task 强定义(两板通用,1kHz,优先级最高)
* 启动: 恒温 PID/加热 PWM(TIM10_CH1=PF6) -> attitude_init(传感器检测
* 有界重试;QuaternionEKF 无需静止校准,任务入口已先行上报心跳)
* 循环: attitude_update()(1kHz QuaternionEKF 姿态解算)+ ist8310_poll()
* (磁力计 200Hz 轮询,硬件 I2C3)+ 恒温控制(目标 40 度;
* BMI088 失败或过温时强制关加热,legacy 的 while(1) 死等已移除)
* 底盘板:本任务随 INS 移植同样运行(姿态供跟随/功能扩展,诊断可见)
* ========================================================================== */
#include "app_tasks.h"
#include "cmsis_os.h"
#include "FreeRTOS.h"
#include "task.h"
#include "attitude.h"
#include "bmi088.h"
#include "ist8310.h"
#include "pid.h"
#include "tim.h"
#include "robot_param.h"
extern TIM_HandleTypeDef htim10;
volatile uint32_t dbg_imu_loop = 0; // 诊断:IMU 任务循环计数
/* IMU_Task 强定义(云台板,1kHz):
* 1. BMI088/IST8310 初始化(阻塞,含 ID 检测与报警)
* 2. 陀螺零偏校准(机器人须静止)
* 3. 循环:1kHz 姿态解算 + 恒温控制(TIM10_CH1/PF6 PWM,目标 40℃)
* 过温处理与 legacy 不同:不再死循环,而是关加热并由监控任务绿灯报警 */
static PID_PositionTypeDef imu_temp_pid;
void IMU_Task(void const * argument)
{
monitor_heartbeat(HB_IMU); // 初始化期间先行报活,防止看门狗误判
/* 恒温 PID(legacy IMUTemperatureControl_PIDInit) */
PID_PositionStructureInit(&imu_temp_pid, IMU_TEMPERATURE_TARGET);
PID_PositionSetParameter(&imu_temp_pid, IMU_TEMP_KP, IMU_TEMP_KI, IMU_TEMP_KD);
PID_PositionSetEkRange(&imu_temp_pid, -1, 1);
PID_PositionSetOUTRange(&imu_temp_pid, 0, 100);
/* 加热 PWM:与 legacy 相同的 1kHz/千分位配置 */
__HAL_TIM_SET_PRESCALER(&htim10, 168 - 1);
__HAL_TIM_SET_AUTORELOAD(&htim10, 1000 - 1);
__HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, 0);
HAL_TIM_PWM_Start(&htim10, TIM_CHANNEL_1);
attitude_init(); // 含阻塞式校准
TickType_t last_wake = xTaskGetTickCount();
for (;;)
{
attitude_update();
ist8310_poll(); // 磁力计 200Hz 轮询(硬件 I2C3,阻塞约 0.7ms,内部 5ms 分频)
/* 恒温控制(legacy 比例为 PID.OUT*10,对应 ARR=999 的千分位)
* BMI088 检测失败时温度恒为 0,禁止加热(否则会全功率加热失控) */
if (bmi088_online && bmi088_temperature <= IMU_TEMPERATURE_LIMIT)
{
PID_PositionCalc(&imu_temp_pid, bmi088_temperature);
__HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, (uint32_t)(imu_temp_pid.OUT * 10.0f));
}
else
imu_heater_off();
monitor_heartbeat(HB_IMU);
dbg_imu_loop++;
vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(1));
}
}
底盘/Application/Task/Src/monitor_task.c
+301
View File
@@ -0,0 +1,301 @@
/* =============================================================================
* monitor_task.c - LED_Task 强定义:任务心跳 + LED/蜂鸣报警 + IWDG 看门狗
* 对应 legacy LinkCheck + Warming 模块,报警灯语:
* 蓝灯每 2s 短闪 = 系统健康心跳(区分"空闲"与"卡死")
* 红灯 1s 双闪 = 遥控器掉线 红灯常亮+蜂鸣 = CAN 设备掉线
* 绿灯连闪两下 = BMI088 失败 绿灯慢闪 = IST8310 失败
* 绿灯常亮 = IMU 过温(已关加热)/双传感器失败
* 连接音效 = 遥控器上线瞬间两声上扬(M3->H3)
* CAN 掉线蜂鸣(legacy 语义): 按设备序号 n 响 n 下,2s 一轮,每次只报
* 最先掉线的设备,设备序号表见 linkcheck_first_offline()
* 看门狗: IWDG 约 2.5s;上电 5s 宽限期无条件喂,之后仅当全部任务心跳正常
* 才喂——任何任务卡死都会复位,不会整车失控
* ========================================================================== */
#include "app_tasks.h"
#include "cmsis_os.h"
#include "stm32f4xx_hal.h"
#include "led.h"
#include "buzzer.h"
#include "dr16.h"
#include "referee.h"
#include "c_comm.h"
#include "robot_param.h"
#if defined(ROBOT_TYPE_GIMBAL)
#include "gimbal.h"
#include "bmi088.h"
#include "ist8310.h"
#include "attitude.h"
#elif defined(ROBOT_TYPE_CHASSIS)
#include "chassis.h"
#endif
/* 监控任务(LED_Task 强定义):任务心跳检查 + LED/蜂鸣器报警 + 喂 IWDG
* 报警语义对应 legacy Warming 模块:
* 遥控器掉线 -> 1s 内红灯连闪两下
* CAN 设备掉线 -> 红灯常亮 + 2s 周期蜂鸣
* 图传链路掉线 -> 绿灯闪烁
* IMU 过温 -> 绿灯常亮
* 各任务心跳正常 -> 喂独立看门狗(约 1.6s 溢出) */
static volatile uint32_t heartbeat_tick[HB_COUNT];
void monitor_heartbeat(heartbeat_id_e id)
{
heartbeat_tick[id] = HAL_GetTick();
}
static uint8_t all_heartbeats_alive(void)
{
#if defined(ROBOT_TYPE_GIMBAL)
static const heartbeat_id_e expected[] = {HB_IMU, HB_CMD, HB_GIMBAL, HB_SHOOT, HB_CCOMM, HB_REFEREE};
#elif defined(ROBOT_TYPE_CHASSIS)
static const heartbeat_id_e expected[] = {HB_IMU, HB_CMD, HB_CHASSIS, HB_CCOMM, HB_REFEREE};
#endif
uint32_t now = HAL_GetTick();
for (uint8_t i = 0; i < sizeof(expected) / sizeof(expected[0]); i++)
{
if (heartbeat_tick[expected[i]] == 0 || now - heartbeat_tick[expected[i]] > 500)
return 0;
}
return 1;
}
/* 启动后 5s 内无条件喂狗(宽限期),之后要求全部任务心跳正常 */
static uint8_t iwdg_should_feed(void)
{
if (HAL_GetTick() < 5000)
return 1;
return all_heartbeats_alive();
}
/* LinkCheck:返回第一个掉线 CAN 设备的序号(无掉线返回 -1)
* 序号即蜂鸣次数-1(legacy 语义:报警蜂鸣器以高音 6 响 n 下) */
#if defined(ROBOT_TYPE_GIMBAL)
#define LINKCHECK_DEVICES
#endif
#if defined(ROBOT_TYPE_CHASSIS)
#define LINKCHECK_DEVICES
#endif
#ifdef LINKCHECK_DEVICES
#define LINK_OFFLINE(tick) ((tick) == 0 || now - (tick) > DEVICE_OFFLINE_TIME_MS)
static int8_t linkcheck_first_offline(void)
{
uint32_t now = HAL_GetTick();
#if defined(ROBOT_TYPE_GIMBAL)
/* 设备序号:0 摩擦轮L(0x201) 1 摩擦轮R(0x202) 2 Pitch(0x206)
* 3 Yaw(0x205) 4 拨弹(0x207,安装时) 5 底盘回传(0x019) */
if (LINK_OFFLINE(motor_friction_l.fdb.last_online_tick))
return 0;
if (LINK_OFFLINE(motor_friction_r.fdb.last_online_tick))
return 1;
if (LINK_OFFLINE(motor_pitch_gimbal.fdb.last_online_tick))
return 2;
if (LINK_OFFLINE(motor_yaw_gimbal.fdb.last_online_tick))
return 3;
#if GIMBAL_RAMMER_INSTALLED
if (LINK_OFFLINE(motor_rammer.fdb.last_online_tick))
return 4;
#endif
if (LINK_OFFLINE(ctoc_reply_last_tick))
return 5;
#elif defined(ROBOT_TYPE_CHASSIS)
/* 设备序号:0~3 四轮(0x201~0x204) 4 云台数据(0x149/0x189/0x1A9) */
if (LINK_OFFLINE(motor_wheel_rf.fdb.last_online_tick))
return 0;
if (LINK_OFFLINE(motor_wheel_lf.fdb.last_online_tick))
return 1;
if (LINK_OFFLINE(motor_wheel_lr.fdb.last_online_tick))
return 2;
if (LINK_OFFLINE(motor_wheel_rr.fdb.last_online_tick))
return 3;
if (ctoc_remote.last_tick == 0 || now - ctoc_remote.last_tick > CTOC_OFFLINE_TIME_MS)
return 4;
#endif
return -1;
}
#undef LINK_OFFLINE
#endif /* LINKCHECK_DEVICES */
static void iwdg_init(void)
{
/* 直接写寄存器(HAL IWDG 模块未在 CubeMX 中启用)
* PR=4 -> LSI 32kHz / 64 分频 = 500Hz,重装载 1250 -> 约 2.5s 溢出
* (留足 IMU 零偏校准约 1s 的启动余量) */
IWDG->KR = 0x5555; // 解除写保护
IWDG->PR = 4; // 000:/4 001:/8 010:/16 011:/32 100:/64
IWDG->RLR = 1250;
IWDG->KR = 0xAAAA; // 重装载
IWDG->KR = 0xCCCC; // 启动
}
void LED_Task(void const * argument)
{
uint8_t counter = 0;
uint8_t imu_overheat = 0;
uint8_t can_alarm;
uint8_t first_ready = 1;
/* 看门狗在本任务入口立即启动(约 2.5s):
* 启动后前 5s 为宽限期无条件喂狗(容忍传感器初始化/校准耗时),
* 宽限期后仅在所有任务心跳正常时喂狗,任何任务卡死都会复位 */
iwdg_init();
/* 等待硬件初始化完成(CAN_Task 的蓝灯常亮即初始化中) */
while (!app_ready)
{
IWDG->KR = 0xAAAA;
osDelay(10);
}
for (;;)
{
counter++;
if (counter > 100)
counter = 1;
/* 初始化完成的第一拍:熄灭蓝灯,交由报警逻辑接管 */
if (first_ready)
{
first_ready = 0;
led_set_all_off();
}
/* ---- CAN 设备掉线:红灯常亮 + 2s 周期内响 n 下(n=掉线设备序号+1,legacy 语义) ---- */
#if defined(ROBOT_TYPE_GIMBAL)
can_alarm = gimbal_motor_offline();
#elif defined(ROBOT_TYPE_CHASSIS)
can_alarm = chassis_motor_offline();
#endif
if (can_alarm)
{
led_set_red(1);
int8_t id = linkcheck_first_offline();
if (id >= 0)
{
uint8_t n = (uint8_t)(id + 1);
uint8_t w = counter % 20; // 2s 窗口
/* 第 j 声:占 w=[2j,2j+1],响 100ms 停 100ms */
if ((w / 2) < n && (w % 2) == 0)
buzzer_on(BUZZER_L1);
else
buzzer_off();
}
else
buzzer_off();
}
else
{
#if defined(ROBOT_TYPE_GIMBAL)
/* ---- 遥控器连接成功音效:离线->在线瞬间,两声上扬(M3 -> H3) ---- */
static uint8_t prev_remote_online = 0;
static uint8_t chime_step = 0;
uint8_t remote_online = !dr16_offline();
if (remote_online && !prev_remote_online)
chime_step = 4; // 触发音效序列(100ms/步)
prev_remote_online = remote_online;
if (chime_step > 0)
{
if (chime_step == 4)
buzzer_on(BUZZER_M3);
else if (chime_step == 3)
buzzer_off();
else if (chime_step == 2)
buzzer_on(BUZZER_H3);
else
buzzer_off();
chime_step--;
}
else
buzzer_off();
#else
buzzer_off();
#endif
#if !defined(ROBOT_TYPE_GIMBAL)
led_set_red(0);
#endif
}
#if defined(ROBOT_TYPE_GIMBAL)
/* ---- 遥控器掉线:1s 内红灯连闪两下 ---- */
if (dr16_offline())
{
if (counter % 20 == 1)
led_set_red(1);
else if (counter % 20 == 5)
led_set_red(0);
else if (counter % 20 == 13)
led_set_red(1);
else if (counter % 20 == 17)
led_set_red(0);
}
else if (!can_alarm)
led_set_red(0);
/* ---- 图传链路超时:绿灯 1s 周期闪烁 ---- */
if (referee.link_status &&
HAL_GetTick() - referee.last_update_tick > DEVICE_OFFLINE_TIME_MS * 10)
{
if (counter % 20 < 10)
led_set_green(1);
else
led_set_green(0);
}
/* ---- IMU 过温:绿灯常亮并关闭加热 ---- */
if (bmi088_temperature > IMU_TEMPERATURE_LIMIT)
{
imu_overheat = 1;
imu_heater_off();
}
if (imu_overheat)
led_set_green(1);
/* ---- 传感器检测失败(不同灯语便于区分) ----
* BMI088 失败:绿灯 1s 内连闪两下;IST8310 失败:绿灯 1s 周期慢闪;
* 两者皆失败:绿灯常亮 */
if (!bmi088_online && !ist8310_online)
led_set_green(1);
else if (!bmi088_online)
{
if (counter % 20 == 1)
led_set_green(1);
else if (counter % 20 == 5)
led_set_green(0);
else if (counter % 20 == 13)
led_set_green(1);
else if (counter % 20 == 17)
led_set_green(0);
}
else if (!ist8310_online)
{
if (counter % 20 < 10)
led_set_green(1);
else
led_set_green(0);
}
#elif defined(ROBOT_TYPE_CHASSIS)
led_set_green(0);
#endif
/* ---- 健康心跳:无报警时蓝灯每 2s 闪 100ms,用于区分"空闲"与"卡死" ---- */
uint8_t healthy = !can_alarm && !imu_overheat;
#if defined(ROBOT_TYPE_GIMBAL)
healthy = healthy && bmi088_online && ist8310_online;
#endif
if (healthy)
{
if (counter % 20 == 10)
led_set_blue(1);
else if (counter % 20 == 11)
led_set_blue(0);
}
/* ---- 心跳全部正常才喂狗(宽限期除外):任何任务卡死都会触发复位 ---- */
if (iwdg_should_feed())
IWDG->KR = 0xAAAA;
osDelay(100);
}
}
底盘/Application/Task/Src/referee_task.c
+35
View File
@@ -0,0 +1,35 @@
/* =============================================================================
* referee_task.c - 裁判系统链路监测
* 解析在 USART6 空闲中断完成(referee_parse_bytes 流式喂入),
* 本任务仅做 100ms 周期的链路超时监测与心跳上报
* ========================================================================== */
#include "app_tasks.h"
#include "cmsis_os.h"
#include "bsp_usart.h"
#include "referee.h"
#include "robot_param.h"
/* referee_task:裁判系统链路监测
* 解析在接收中断完成(referee_parse_bytes),本任务仅做超时监测与心跳 */
void referee_frame_callback(const uint8_t * frame, uint16_t len)
{
referee_parse_bytes(frame, len);
}
void referee_task(void const * argument)
{
while (!app_ready)
osDelay(2);
for (;;)
{
/* 链路超时置断线 */
if (referee.link_status &&
HAL_GetTick() - referee.last_update_tick > DEVICE_OFFLINE_TIME_MS * 10)
referee.link_status = 0;
monitor_heartbeat(HB_REFEREE);
osDelay(100);
}
}
底盘/Application/Task/Src/robot_cmd_task.c
+244
View File
@@ -0,0 +1,244 @@
/* =============================================================================
* robot_cmd_task.c - 控制源汇总(2ms),发布 cmd_robot_t(robot_typedef.h)
* 云台板: DR16 遥控器 + 图传键鼠(0x0304,referee 键盘钩子填充 keyboard_data)
* 键鼠/遥控互斥(与 legacy 一致);摇杆->云台增量与限位门控;
* Q 摩擦轮/左键发射/E 退弹/拨轮;首帧触发 gimbal_reset
* 底盘板: 从 ctoc_remote(0x149/0x189 解析结果)生成:
* S2 上=慢陀螺 下=快陀螺 中=跟随;Ctrl 切换保持型小陀螺;
* vx/vy 来自右摇杆(键鼠由云台合成);左摇杆水平=手动旋转(死区外
* 覆盖跟随输出);Shift=加速
* ========================================================================== */
#include "app_tasks.h"
#include "cmsis_os.h"
#include "dr16.h"
#include "c_comm.h"
#include "robot_typedef.h"
#include "robot_param.h"
#if defined(ROBOT_TYPE_GIMBAL)
#include "robot_cmd_task.h"
#include "referee.h"
#include "gimbal.h"
#elif defined(ROBOT_TYPE_CHASSIS)
#include "referee.h"
#endif
/* robot_cmd_task(2ms):汇总控制源,发布 cmd_robot_t
* 云台板:DR16 遥控器 + 图传键鼠(0x0304)
* 底盘板:CToC 转发的"虚拟遥控器"(0x149/0x189) */
volatile cmd_robot_t cmd_robot;
#if defined(ROBOT_TYPE_GIMBAL)
keyboard_data_t keyboard_data;
static uint8_t keyboard_online = 0;
void dr16_frame_callback(const uint8_t * frame, uint16_t len)
{
if (len == 18)
dr16_parse(frame);
}
/* 0x0304 自定义控制器数据 -> keyboard_data(中断上下文) */
static void referee_keyboard_hook(const uint8_t data[12])
{
keyboard_data.mouse_rl = (int16_t)(((uint16_t)data[1] << 8) | data[0]);
keyboard_data.mouse_du = (int16_t)(((uint16_t)data[3] << 8) | data[2]);
keyboard_data.mouse_wheel = (int16_t)(((uint16_t)data[5] << 8) | data[4]);
keyboard_data.mouse_key_l = data[6];
keyboard_data.mouse_key_r = data[7];
keyboard_data.key_w = data[8] & 0x01;
keyboard_data.key_s = (data[8] >> 1) & 0x01;
keyboard_data.key_a = (data[8] >> 2) & 0x01;
keyboard_data.key_d = (data[8] >> 3) & 0x01;
keyboard_data.key_shift = (data[8] >> 4) & 0x01;
keyboard_data.key_ctrl = (data[8] >> 5) & 0x01;
keyboard_data.key_q = (data[8] >> 6) & 0x01;
keyboard_data.key_e = (data[8] >> 7) & 0x01;
/* Q/E/Shift/Ctrl 按下翻转(legacy 键盘边沿语义) */
if (keyboard_data.key_last_q == 0 && keyboard_data.key_q == 1)
keyboard_data.key_push_q = !keyboard_data.key_push_q;
if (keyboard_data.key_last_e == 0 && keyboard_data.key_e == 1)
keyboard_data.key_push_e = !keyboard_data.key_push_e;
if (keyboard_data.key_last_shift == 0 && keyboard_data.key_shift == 1)
keyboard_data.key_push_shift = !keyboard_data.key_push_shift;
if (keyboard_data.key_last_ctrl == 0 && keyboard_data.key_ctrl == 1)
keyboard_data.key_push_ctrl = !keyboard_data.key_push_ctrl;
keyboard_data.key_last_q = keyboard_data.key_q;
keyboard_data.key_last_e = keyboard_data.key_e;
keyboard_data.key_last_shift = keyboard_data.key_shift;
keyboard_data.key_last_ctrl = keyboard_data.key_ctrl;
keyboard_data.last_tick = HAL_GetTick();
}
void robot_cmd_task(void const * argument)
{
referee_set_keyboard_hook(referee_keyboard_hook);
while (!app_ready)
osDelay(2);
for (;;)
{
keyboard_online = (keyboard_data.last_tick != 0 &&
HAL_GetTick() - keyboard_data.last_tick < DEVICE_OFFLINE_TIME_MS * 10);
cmd_robot.online = (dr16_status == 1 && !dr16_offline());
cmd_robot.source = keyboard_online ? 1 : 0;
if (cmd_robot.online)
{
/* ---- 云台轴增量 ---- */
float yaw_delta = 0, pitch_delta = 0;
uint16_t pos = motor_pitch_gimbal.fdb.raw_angle;
if (!keyboard_online)
{
/* 遥控器:左摇杆,输出与摇杆偏量成比例(legacy 为过死区即全速,
* 体感过灵敏),限位门控与方向保持 legacy */
if (dr16.rc_l_UD > 1050 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT)
pitch_delta -= GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
((float)dr16.rc_l_UD - 1024.0f) / 660.0f;
else if (dr16.rc_l_UD < 1000 && pos < PITCH_GM6020_POSITION_LOWER_LIMIT)
pitch_delta += GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
(1024.0f - (float)dr16.rc_l_UD) / 660.0f;
if (dr16.rc_l_RL > 1050 || dr16.rc_l_RL < 1000)
yaw_delta -= GIMBAL_LEVER_SPEED_MAP_RATE * GIMBAL_YAW_PITCH_SPEED_RATE *
GIMBAL_YAW_PITCH_SPEED_RATE * 0.0439453125f *
((float)dr16.rc_l_RL - 1024.0f) / 660.0f;
}
else
{
/* 键鼠:同改为比例输出 */
if (keyboard_data.mouse_du < -8 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT)
pitch_delta -= GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
(-(float)keyboard_data.mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f;
else if (keyboard_data.mouse_du > 8 && pos < PITCH_GM6020_POSITION_LOWER_LIMIT)
pitch_delta += GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
((float)keyboard_data.mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f;
if (keyboard_data.mouse_rl != 0)
yaw_delta -= GIMBAL_LEVER_SPEED_MAP_RATE * GIMBAL_YAW_PITCH_SPEED_RATE *
GIMBAL_YAW_PITCH_SPEED_RATE * 0.0439453125f *
((float)keyboard_data.mouse_rl * PC_MOUSE_RL_SENSITIVITY / 660.0f * 2.0f);
}
cmd_robot.gimbal_yaw_delta = yaw_delta;
cmd_robot.gimbal_pitch_delta = pitch_delta;
/* ---- 发射机构 ---- */
if (!keyboard_online)
{
cmd_robot.friction_on = (dr16.s1 == 1);
cmd_robot.fire = (dr16.thumb_wheel < 1000);
cmd_robot.eject = (dr16.thumb_wheel > 1050);
}
else
{
cmd_robot.friction_on = keyboard_data.key_push_q;
cmd_robot.fire = keyboard_data.mouse_key_l;
cmd_robot.eject = keyboard_data.key_push_e;
}
cmd_robot.thumb_wheel = dr16.thumb_wheel;
}
else
{
cmd_robot.gimbal_yaw_delta = 0;
cmd_robot.gimbal_pitch_delta = 0;
cmd_robot.friction_on = 0;
cmd_robot.fire = 0;
cmd_robot.eject = 0;
}
/* 小陀螺使能:Ctrl 沿翻转(仅在遥控源检测,键鼠 TODO);
* 云台侧是数据源头,底盘只收状态位——重连瞬间保持态跳变不会在
* 底盘误触发沿(修复"重连误开小陀螺疯转") */
{
static uint8_t spin_flag = 0, last_ctrl = 0;
uint8_t ctrl_now = (!keyboard_online) ? dr16.key_push_ctrl : 0;
if (last_ctrl == 0 && ctrl_now == 1)
spin_flag = !spin_flag;
last_ctrl = ctrl_now;
cmd_robot.spin_flag = spin_flag;
}
/* 遥控器重新连接瞬间(start_flag==2)请求云台复位,与 legacy 一致;
* 同时清小陀螺使能(重连不应保持按 Ctrl 的历史状态) */
cmd_robot.gimbal_reset = 0;
if (dr16_start_flag == 2)
{
dr16_start_flag = 0;
cmd_robot.gimbal_reset = 1;
cmd_robot.spin_flag = 0;
}
monitor_heartbeat(HB_CMD);
osDelay(2);
}
}
#elif defined(ROBOT_TYPE_CHASSIS)
void robot_cmd_task(void const * argument)
{
while (!app_ready)
osDelay(2);
for (;;)
{
/* 板间通讯在线 = 云台侧遥控器在线 且 收帧新鲜 */
/* 云台电机故障位(0x189 Data[6] bit7)置位时同样视为离线,底盘停车 */
uint8_t ctoc_alive = (ctoc_remote.status == 1 &&
ctoc_remote.last_tick != 0 &&
HAL_GetTick() - ctoc_remote.last_tick < CTOC_OFFLINE_TIME_MS &&
ctoc_remote.gimbal_fault == 0);
cmd_robot.online = ctoc_alive;
cmd_robot.source = 0;
if (cmd_robot.online)
{
/* 底盘模式:小陀螺使能位(0x189 bit2,云台侧做 Ctrl 沿检测)优先
* SPIN_FAST;否则右拨杆 上=慢陀螺 下=快陀螺 中=跟随。
* 底盘不做沿检测——遥控重连瞬间转发保持态跳变会被误判为按键,
* 导致误开小陀螺疯转(已踩坑) */
if (ctoc_remote.spin_flag)
cmd_robot.chassis_mode = CHASSIS_MODE_SPIN_FAST;
else if (ctoc_remote.s2 == 1)
cmd_robot.chassis_mode = CHASSIS_MODE_SPIN_SLOW;
else if (ctoc_remote.s2 == 2)
cmd_robot.chassis_mode = CHASSIS_MODE_SPIN_FAST;
else
cmd_robot.chassis_mode = CHASSIS_MODE_FOLLOW;
/* 平移:右摇杆(键盘模式由云台合成 WASD 进摇杆量) */
cmd_robot.vx = (1024.0f - (float)ctoc_remote.rc_r_ud) * MECANUM_CHASSIS_SPEED_SCALE;
cmd_robot.vy = (1024.0f - (float)ctoc_remote.rc_r_rl) * MECANUM_CHASSIS_SPEED_SCALE;
/* 无手动旋转:左摇杆水平是云台 Yaw 控制量,底盘再取用即一杆
* 两用冲突(拨 yaw 摇杆底盘同转);旋转由跟随/小陀螺接管 */
cmd_robot.wz_manual = 0;
cmd_robot.gimbal_reset = 0;
if (ctoc_remote.start_flag == 2)
{
ctoc_remote.start_flag = 0;
cmd_robot.gimbal_reset = 1; // 复用字段:跟随朝向重新锁定
}
}
else
{
cmd_robot.chassis_mode = CHASSIS_MODE_OFF;
cmd_robot.vx = 0;
cmd_robot.vy = 0;
cmd_robot.wz_manual = 0;
}
monitor_heartbeat(HB_CMD);
osDelay(2);
}
}
#endif