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/Inc/app_tasks.h
+50
View File
@@ -0,0 +1,50 @@
/* =============================================================================
* app_tasks.h - Application 层任务公共声明
* 包含:任务心跳编号与 monitor_heartbeat()、freertos.c 弱函数的强定义、
* 新建任务函数声明、串口整帧回调声明(dr16/referee)
* ========================================================================== */
#ifndef APP_TASKS_H
#define APP_TASKS_H
/* Application 层任务公共声明(覆盖 freertos.c 中的弱定义 + 新建任务) */
#include <stdint.h>
/* 任务心跳编号(monitor_task 检查各任务存活) */
typedef enum
{
HB_IMU = 0,
HB_CMD,
HB_GIMBAL,
HB_SHOOT,
HB_CHASSIS,
HB_CCOMM,
HB_REFEREE,
HB_COUNT,
} heartbeat_id_e;
extern volatile uint8_t app_ready; // CAN_Task 完成硬件初始化后置 1
/* 各任务在循环内调用,向监控任务报活 */
extern void monitor_heartbeat(heartbeat_id_e id);
/* 串口整帧回调(usart 空闲中断递交,CAN_Task 初始化时注册):
* 云台板生效,底盘板为空实现 */
extern void dr16_frame_callback(const uint8_t * frame, uint16_t len);
extern void referee_frame_callback(const uint8_t * frame, uint16_t len);
/* ---- freertos.c 弱函数的强定义 ---- */
extern void CAN_Task(void const * argument);
extern void LED_Task(void const * argument);
extern void IMU_Task(void const * argument); // 仅云台板
/* ---- 新建任务 ---- */
extern void robot_cmd_task(void const * argument);
extern void gimbal_task(void const * argument); // 仅云台板
extern void shoot_task(void const * argument); // 仅云台板
extern void chassis_task(void const * argument); // 仅底盘板
extern void c_comm_task(void const * argument);
extern void referee_task(void const * argument);
extern void debug_task(void const * argument); // USB 串口诊断输出
#endif
云台/Application/Task/Inc/debug_task.h
+8
View File
@@ -0,0 +1,8 @@
/* debug_task.h - USB 虚拟串口诊断任务声明(仅诊断用) */
#ifndef DEBUG_TASK_H
#define DEBUG_TASK_H
/* debug_task:USB 虚拟串口(CDC)诊断输出(仅诊断用,稳定后可整组移除) */
extern void debug_task(void const * argument);
#endif
云台/Application/Task/Inc/referee_task.h
+9
View File
@@ -0,0 +1,9 @@
#ifndef REFEREE_TASK_H
#define REFEREE_TASK_H
#include <stdint.h>
/* usart6 整帧回调(CAN_Task 注册;can_task.c 引用) */
extern void referee_frame_callback(const uint8_t * frame, uint16_t len);
#endif
云台/Application/Task/Inc/robot_cmd_task.h
+27
View File
@@ -0,0 +1,27 @@
/* robot_cmd_task.h - 图传键鼠数据结构 keyboard_data_t(0x0304 解析结果) */
#ifndef ROBOT_CMD_TASK_H
#define ROBOT_CMD_TASK_H
#include <stdint.h>
#include "robot_typedef.h"
/* 图传键鼠数据(0x0304 解析) */
typedef struct
{
int16_t mouse_rl;
int16_t mouse_du;
int16_t mouse_wheel;
uint8_t mouse_key_l;
uint8_t mouse_key_r;
uint8_t key_w, key_s, key_a, key_d;
uint8_t key_shift, key_ctrl, key_q, key_e;
uint8_t key_alpha1, key_alpha2;
/* 边沿检测 */
uint8_t key_last_q, key_last_e, key_last_shift, key_last_ctrl;
uint8_t key_push_q, key_push_e, key_push_shift, key_push_ctrl;
uint32_t last_tick;
} keyboard_data_t;
extern keyboard_data_t keyboard_data;
#endif
云台/Application/Task/Src/c_comm_task.c
+74
View File
@@ -0,0 +1,74 @@
/* =============================================================================
* 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 姿态帧:跟随符号自动综合的数据源
* flags:bit0=att_ready bit1=tick增向CCW bit2=bit1有效 */
{
const int8_t enc = gimbal_encoder_ccw();
uint8_t flags = att_ready;
if (enc > 0) flags |= 0x02;
if (enc != 0) flags |= 0x04;
c_comm_master_send_attitude(att_deg_yaw, flags);
}
#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/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/gimbal_task.c
+121
View File
@@ -0,0 +1,121 @@
/* =============================================================================
* gimbal_task.c - 云台控制任务(2ms)
* 状态机:
* YAW_WAIT 等待"姿态就绪+云台电机在线"(遥控可后到)
* YAW_RUN 正常串级控制(符号用参数回退值,直至手势自检完成)
* YAW_DETECT 装配符号自检——**仅由操作手手势显式触发,绝不自动执行**:
* 遥控在线时"左摇杆推到最左 + 右摇杆推到最右"保持 0.5s。
* 检测期间云台会轻转几度(约 20% 电压脉冲 60ms),操作手应
* 确认云台周围无人后再做手势。失败重试 3 次后回退参数符号。
* 任意门控失效(遥控离线/电机掉线/姿态不可用/底盘故障)立即清零输出
* 拨弹电流由 shoot_task 写入共享变量,本任务与 Pitch 共帧发送(0x1FF CAN1)
* ========================================================================== */
#include "app_tasks.h"
#include "cmsis_os.h"
#include "FreeRTOS.h"
#include "task.h"
#include "robot_typedef.h"
#include "gimbal.h"
#include "bmi088.h"
#include "attitude.h"
#include "dr16.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
{
YAW_WAIT = 0,
YAW_RUN,
YAW_DETECT,
} yaw_state_e;
void gimbal_task(void const * argument)
{
TickType_t last_wake = xTaskGetTickCount();
yaw_state_e state = YAW_WAIT;
uint8_t detect_retry = 0;
uint16_t gesture_cnt = 0;
uint8_t force_reset = 0;
while (!app_ready)
osDelay(2);
for (;;)
{
const uint8_t gates_ok = (cmd_robot.online && !gimbal_motor_offline() &&
bmi088_online && att_ready && !ctoc_chassis_fault());
/* 校准手势检测(仅遥控在线时可能成立;500 个采样窗防抖) */
if (cmd_robot.online && !gimbal_motor_offline() &&
dr16.rc_l_RL < CALIB_GESTURE_L_RL && dr16.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 YAW_WAIT:
gimbal_output_zero();
/* 姿态与电机就绪即可进入运行(符号先用参数回退值) */
if (!gimbal_motor_offline() && bmi088_online && att_ready)
{
force_reset = 1; // 首次控制同步目标角
state = YAW_RUN;
}
break;
case YAW_DETECT:
if (gimbal_detect_yaw_sign())
{
force_reset = 1; // 检测期间云台被动转过,重锁目标
state = YAW_RUN;
}
else if (++detect_retry >= 3)
{
detect_retry = 0;
force_reset = 1;
state = YAW_RUN; // 回退参数符号
}
else
gimbal_output_zero();
break;
case YAW_RUN:
default:
if (gesture && !gimbal_motor_offline() && att_ready)
{
detect_retry = 0;
state = YAW_DETECT; // 操作手主动请求自检
}
else if (gates_ok)
{
gimbal_control(cmd_robot.gimbal_yaw_delta,
cmd_robot.gimbal_pitch_delta,
(uint8_t)(force_reset || cmd_robot.gimbal_reset));
force_reset = 0;
}
else
{
gimbal_output_zero();
force_reset = 1;
state = YAW_WAIT; // 门控失效回等待,恢复后重锁目标
}
break;
}
monitor_heartbeat(HB_GIMBAL);
vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(2));
}
}
云台/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
云台/Application/Task/Src/shoot_task.c
+42
View File
@@ -0,0 +1,42 @@
/* =============================================================================
* shoot_task.c - 发射机构任务(2ms)
* shoot_control() 计算摩擦轮/拨弹电流 -> shoot_send_cmd() 发 0x200;
* 拨弹电流由云台任务与 Pitch 共帧发送;发射机构上电状态来自底盘 0x019
* ========================================================================== */
#include "app_tasks.h"
#include "cmsis_os.h"
#include "FreeRTOS.h"
#include "task.h"
#include "robot_typedef.h"
#include "gimbal.h"
#include "shoot.h"
#include "c_comm.h"
#include "dr16.h"
/* shoot_task(云台板,2ms):摩擦轮/拨弹控制
* 发射机构上电状态来自底盘 0x019(ctoc_shooter_power) */
void shoot_task(void const * argument)
{
TickType_t last_wake = xTaskGetTickCount();
while (!app_ready)
osDelay(2);
for (;;)
{
if (cmd_robot.online && !gimbal_motor_offline() && !ctoc_chassis_fault())
{
shoot_control(cmd_robot.friction_on, cmd_robot.fire, cmd_robot.eject,
ctoc_shooter_power);
shoot_send_cmd();
}
else
{
shoot_output_zero();
}
monitor_heartbeat(HB_SHOOT);
vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(2));
}
}
云台/Application/robot_param.h
+143
View File
@@ -0,0 +1,143 @@
/* =============================================================================
* robot_param.h - 全部个性化参数(整车唯一调参入口)
* 分区:云台限位/转速 -> 云台 Yaw/Pitch PID(双陀螺环) -> 摩擦轮/拨弹 ->
* 底盘几何/轮速 PID/跟随(双 INS 世界系)/功率 -> CToC 帧 ID ->
* CAN ID 表 -> 掉线时间 -> IMU 恒温 -> 台架调试开关
* 控制周期 2ms(改周期需重调所有 PID!)
* ========================================================================== */
#ifndef ROBOT_PARAM_H
#define ROBOT_PARAM_H
/* ============================ 云台 ============================ */
#define PITCH_GM6020_POSITION_LOWER_LIMIT 3990 // Pitch 轴编码器值下限(云台抬起方向)
#define PITCH_GM6020_POSITION_UPPER_LIMIT 2220 // Pitch 轴编码器值上限(云台压低方向)
#define PITCH_GM6020_ENCODER_ZERO 3245 // Pitch 轴编码器零位
#define GIMBAL_FRICTION_WHEEL_SPEED 5500.0f // 摩擦轮转速(RPM)
#define GIMBAL_RAMMER_SPEED -5400.0f// 拨弹盘转速(RPM,负向发射)
#define GIMBAL_LEVER_SPEED_MAP_RATE 2.0f // 云台拨杆速度映射比例(Yaw 用)
#define GIMBAL_PITCH_LEVER_RATE 5.0f // Pitch 专用比例(满杆≈110°/s)
#define GIMBAL_YAW_PITCH_SPEED_RATE 3.0f // 云台偏航/俯仰速度比
#define PC_MOUSE_RL_SENSITIVITY 3.0f // 鼠标左右灵敏度
#define PC_MOUSE_DU_SENSITIVITY 8.0f // 鼠标上下灵敏度
/* ---- Yaw 双环(2ms):位置环与速度环都以陀螺仪为反馈 ----
* 设计要点(2026-09 重写):
* - 旧方案速度环用电机转子转速、位置环用陀螺角度,两类传感器的方向关系
* 由装配(皮带是否交叉)决定,一个符号参数无法同时保证两环负反馈——
* 这是反复"疯转"的根源,已废弃;
* - 现在两环共用陀螺,整条链只有一个装配符号,由上电自检自动测定
* (GIMBAL_YAW_DETECT 见下),无需人工猜;
* - 单位:角度环 误差° -> 目标角速度°/s;速度环 误差°/s -> 电压。
* 调参顺序:先速度环(点动不振荡),再位置环(刚性)——见移植 README §8.1 */
#define GIMBAL_YAW_ANGLE_KP 15.0f // ° -> °/s:10° 误差给 150°/s
#define GIMBAL_YAW_ANGLE_KI 0.0f
#define GIMBAL_YAW_ANGLE_KD 0.0f // 速度环本身即微分,不再叠加
#define GIMBAL_YAW_ANGLE_EK_DEADBAND 0.05f // 误差死区(°),QEKF 噪声小可用窄死区
#define GIMBAL_YAW_ANGLE_OUT_LIMIT 360.0f // 目标角速度上限(°/s)
#define GIMBAL_YAW_SPEED_KP 60.0f // °/s -> 电压:100°/s 误差给 6000(约 24% 满电压)
#define GIMBAL_YAW_SPEED_KI 0.0f // 稳定后可回填 0.5~2 消速度稳态差
#define GIMBAL_YAW_SPEED_KD 0.0f // 陀螺速率反馈噪声小,先不用 D
#define GIMBAL_YAW_SPEED_OUT_LIMIT 25000.0f// GM6020 电压限幅
/* Yaw 装配符号自动检测:上电自检时给 Yaw 电机一个正向小电压脉冲,
* 看陀螺 z 轴往哪边转——脉冲引起同向转动则符号 +1,反向则 -1。
* 检测时云台会轻转几度,勿扶云台;失败(被按住/电机离线)时用 FALLBACK */
#define GIMBAL_YAW_SIGN_AUTO 1 // 1=上电自动检测 0=直接用 FALLBACK
#define GIMBAL_YAW_SIGN_FALLBACK (1.0f)
#define GIMBAL_YAW_DETECT_VOLTAGE 5000 // 检测脉冲电压(约 20% 满电压)
#define GIMBAL_YAW_DETECT_RATE_MIN 20.0f // 判定转动的陀螺速率阈值(°/s)
/* ---- Pitch(位置环陀螺 Roll,速度环转子转速——Pitch 无符号史问题,维持原案) ---- */
#define GIMBAL_PITCH_ANGLE_EK_DEADBAND 0.3f // 位置环误差死区(°)
#define GIMBAL_PITCH_ANGLE_KP 15.0f
#define GIMBAL_PITCH_ANGLE_KI 0.0f
#define GIMBAL_PITCH_ANGLE_KD 100.0f
#define GIMBAL_PITCH_SPEED_KP 150.0f
#define GIMBAL_PITCH_SPEED_KI 0.001f
#define GIMBAL_PITCH_SPEED_KD 5.0f
#define GIMBAL_FRICTION_WHEEL_KP 16.0f
#define GIMBAL_FRICTION_WHEEL_KI 0.0f
#define GIMBAL_FRICTION_WHEEL_KD 30.0f
#define GIMBAL_RAMMER_KP 16.0f
#define GIMBAL_RAMMER_KI 0.0f
#define GIMBAL_RAMMER_KD 0.0f
/* ============================ 底盘 ============================ */
#define MECANUM_WHEEL_RADIUS 7.0f // 麦轮半径(cm)
#define MECANUM_RX 18.0f // 底盘中心到轮心的x轴分量(cm)
#define MECANUM_RY 18.75f // 底盘中心到轮心的y轴分量(cm)
/* 转速换算系数 = 19(减速比)*60*100/(2π*R_cm) */
#define MECANUM_SPEED_FACTOR 18143.663512f
#define MECANUM_CHASSIS_SPEED_SCALE 0.004f // 摇杆->线速度比例(legacy)
#define MECANUM_KEYBOARD_SPEED 2.4f // 键盘 WASD 平移速度(m/s 量纲映射值)
#define MECANUM_SPIN_SLOW_W 4.0f // 慢速小陀螺角速度(rad/s 量纲映射值)
#define MECANUM_SPIN_FAST_W 7.0f // 快速小陀螺角速度(legacy 7.0)
#define MECANUM_SHIFT_BOOST_RATE 1.3f // SHIFT 加速增益(超电接入前的临时方案)
/* 底盘轮速 PID(legacy 已调参数,2ms 控制周期) */
#define MECANUM_WHEEL_SPEED_KP 0.8f
#define MECANUM_WHEEL_SPEED_KI 0.0f
#define MECANUM_WHEEL_SPEED_KD 0.0f
#define MECANUM_WHEEL_SPEED_OUT_LIMIT 15000.0f
/* ---- 跟随(2ms):双 INS 世界系方案(2026-09 重写) ----
* 云台把世界系 yaw 经 0x1A9 帧发给底盘,底盘用自己的 INS yaw 相减:
* err = wrap180(云台yaw - 底盘yaw)
* 不再依赖底盘侧 GM6020(0x205) 反馈,皮带虚位不在环内,无基准锁定时序坑。
* 两板 INS 各自上电零点任意,但差值只随相对转动变化,天然正确 */
#define CHASSIS_FOLLOW_KP 0.4f // w 量纲/°:10° 偏差给 w=4
#define CHASSIS_FOLLOW_KI 0.0f // 调试期 0,同旧教训(积分残留)
#define CHASSIS_FOLLOW_KD 0.0f
#define CHASSIS_FOLLOW_OUT_LIMIT 4.0f // 输出限幅(介于慢/快陀螺之间)
#define CHASSIS_FOLLOW_DEADBAND 1.0f // 误差死区(°)
/* 跟随符号全自动综合:云台自检(0x1A9 flags)下发编码器 tick 增向,
* 底盘自检用小 w 脉冲+INS 测 w 正向对应的车身转向,两者相乘。
* CHASSIS_FOLLOW_SIGN 仅作自检失败时的回退值 */
#define CHASSIS_FOLLOW_SIGN (1.0f)
#define CHASSIS_DETECT_W 1.2f // 自检 w 脉冲幅值
#define CHASSIS_DETECT_TIME_MS 80 // 脉冲时长(落地整车轻转)
#define CHASSIS_DETECT_YAW_MIN 0.5f // 判定转动的 INS yaw 阈值(°)
/* 功率限制(简易方案:缓冲能量不足时按比例缩放轮电流) */
#define POWER_LIMIT_BUFFER_LOW 30.0f // 缓冲能量低于此值开始限流(J)
#define POWER_LIMIT_BUFFER_MIN 10.0f // 缓冲能量最低保护值(J)
/* ============================ CToC 板间通讯 ============================ */
#define CTOC_MASTER_ID1 0x019 // 底盘->云台:发射机构状态+底盘故障位
#define CTOC_SLAVE_ID1 0x149 // 云台->底盘:摇杆数据
#define CTOC_SLAVE_ID2 0x189 // 云台->底盘:控制状态+故障/陀螺使能位
#define CTOC_SLAVE_ID3 0x1A9 // 云台->底盘:云台世界系 yaw(float)+att_ready
/* ============================ CAN ID ============================ */
/* 云台板:CAN1 = Pitch GM6020(0x206)+摩擦轮 M3508(0x201/0x202)
* CAN2 = Yaw GM6020(0x205)+底盘回传(0x019)
* 底盘板:CAN1 = 四轮 M3508(0x201~0x204)
* CAN2 = 云台数据(0x149/0x189/0x1A9) */
/* ============================ 台架调试开关 ============================ */
#define DEBUG_SHOOT_WITHOUT_REFEREE 1 // 1=无裁判系统时也允许开摩擦轮(台架调试用),比赛请改 0
#define GIMBAL_RAMMER_INSTALLED 0 // 1=拨弹 M2006 已安装;0=暂缺,不参与掉线判定
/* ============================ 掉线检测 ============================ */
#define DEVICE_OFFLINE_TIME_MS 100 // CAN 设备/裁判系统掉线判定时间
#define CTOC_OFFLINE_TIME_MS 100 // 板间通讯掉线判定时间
/* ============================ IMU ============================ */
/* 姿态解算为 QuaternionEKF(见 modules/ins);z 轴零偏上电静止标定约 1s,
* x/y 由 EKF 在线估计;EKF 参数(Q1/Q2/R/lambda)在 ins.c 的 ins_init() 内调整 */
#define IMU_TEMPERATURE_TARGET 40.0f // BMI088 恒温目标(℃)
#define IMU_TEMPERATURE_LIMIT 50.0f // 温度报警阈值(℃)
#define IMU_TEMP_KP 20.0f
#define IMU_TEMP_KI 0.0003f
#define IMU_TEMP_KD 0.0f
#endif
云台/Application/robot_typedef.h
+61
View File
@@ -0,0 +1,61 @@
/* =============================================================================
* robot_typedef.h - 机器人全局类型定义
* 板型宏(编译选项定义,勿在文件中定义):ROBOT_TYPE_GIMBAL / ROBOT_TYPE_CHASSIS
* chassis_mode_e: OFF/MANUAL/FOLLOW/SPIN_SLOW/SPIN_FAST
* cmd_robot_t: robot_cmd_task 发布的全局控制指令(各控制任务只读),
* 字段按底盘/云台/发射/控制源分组,注释见结构体内部
* ========================================================================== */
#ifndef ROBOT_TYPEDEF_H
#define ROBOT_TYPEDEF_H
#include <stdint.h>
/* 板型由编译选项定义:云台板 -DROBOT_TYPE_GIMBAL,底盘板 -DROBOT_TYPE_CHASSIS */
#if !defined(ROBOT_TYPE_GIMBAL) && !defined(ROBOT_TYPE_CHASSIS)
#error "必须定义 ROBOT_TYPE_GIMBAL 或 ROBOT_TYPE_CHASSIS"
#endif
/* 底盘模式 */
typedef enum
{
CHASSIS_MODE_OFF = 0, // 停车
CHASSIS_MODE_MANUAL, // 手动平移+手动旋转
CHASSIS_MODE_FOLLOW, // 跟随云台(叠加手动平移)
CHASSIS_MODE_SPIN_SLOW, // 慢速小陀螺(叠加手动平移)
CHASSIS_MODE_SPIN_FAST, // 快速小陀螺(叠加手动平移)
} chassis_mode_e;
/* 机器人控制指令(robot_cmd_task 发布,各控制任务读取) */
typedef struct
{
uint8_t online; // 控制源在线(遥控器或键鼠链路)
/* ---- 底盘 ---- */
float vx; // 前进方向速度指令(m/s 量纲的映射值)
float vy; // 左右方向速度指令
float wz_manual; // [已废弃,恒为 0]原"左摇杆=底盘旋转"与云台 Yaw
// 摇杆冲突(legacy 一杆两用遗留),底盘旋转由
// 跟随/小陀螺完全接管
chassis_mode_e chassis_mode;
uint8_t spin_flag; // 小陀螺使能(Ctrl 沿翻转,云台侧检测,随 0x189
// bit2 下发;底盘只读状态位,不做沿检测)
/* ---- 云台 ---- */
float gimbal_yaw_delta; // 本周期 yaw 角度增量(°)
float gimbal_pitch_delta; // 本周期 pitch 角度增量(°)
uint8_t gimbal_reset; // 云台角度复位请求(遥控器重新连接瞬间)
/* ---- 发射 ---- */
uint8_t friction_on; // 摩擦轮开关(保持型)
uint8_t fire; // 发射(瞬间/持续)
uint8_t eject; // 退弹
int16_t thumb_wheel; // 遥控器拨轮
/* ---- 控制 ---- */
uint8_t source; // 0-遥控器 1-键鼠(图传链路)
} cmd_robot_t;
extern volatile cmd_robot_t cmd_robot;
#endif