bc
This commit is contained in:
parent
25eabae70c
commit
7c883d09d8
@ -96,6 +96,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
User/module/shoot.c
|
||||
User/module/track.c
|
||||
User/module/vision_bridge.c
|
||||
User/module/aimbot.c
|
||||
# User/task sources
|
||||
User/task/atti_esti.c
|
||||
User/task/blink.c
|
||||
@ -108,7 +109,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
User/task/supercap.c
|
||||
User/task/user_task.c
|
||||
User/task/ai.c
|
||||
User/task/referee.c
|
||||
User/task/ref_main.c
|
||||
)
|
||||
|
||||
# Add include paths
|
||||
|
||||
@ -62,7 +62,10 @@ void TIM2_IRQHandler(void);
|
||||
void SPI2_IRQHandler(void);
|
||||
void EXTI15_10_IRQHandler(void);
|
||||
void UART5_IRQHandler(void);
|
||||
void DMA2_Stream1_IRQHandler(void);
|
||||
void DMA2_Stream2_IRQHandler(void);
|
||||
void DMA2_Stream5_IRQHandler(void);
|
||||
void USART10_IRQHandler(void);
|
||||
void FDCAN3_IT0_IRQHandler(void);
|
||||
void FDCAN3_IT1_IRQHandler(void);
|
||||
/* USER CODE BEGIN EFP */
|
||||
|
||||
@ -50,6 +50,12 @@ void MX_DMA_Init(void)
|
||||
/* DMA1_Stream1_IRQn interrupt configuration */
|
||||
HAL_NVIC_SetPriority(DMA1_Stream1_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(DMA1_Stream1_IRQn);
|
||||
/* DMA2_Stream1_IRQn interrupt configuration */
|
||||
HAL_NVIC_SetPriority(DMA2_Stream1_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(DMA2_Stream1_IRQn);
|
||||
/* DMA2_Stream2_IRQn interrupt configuration */
|
||||
HAL_NVIC_SetPriority(DMA2_Stream2_IRQn, 0, 0);
|
||||
HAL_NVIC_EnableIRQ(DMA2_Stream2_IRQn);
|
||||
/* DMA2_Stream5_IRQn interrupt configuration */
|
||||
HAL_NVIC_SetPriority(DMA2_Stream5_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(DMA2_Stream5_IRQn);
|
||||
|
||||
@ -62,7 +62,10 @@ extern DMA_HandleTypeDef hdma_spi2_rx;
|
||||
extern DMA_HandleTypeDef hdma_spi2_tx;
|
||||
extern SPI_HandleTypeDef hspi2;
|
||||
extern DMA_HandleTypeDef hdma_uart5_rx;
|
||||
extern DMA_HandleTypeDef hdma_usart10_rx;
|
||||
extern DMA_HandleTypeDef hdma_usart10_tx;
|
||||
extern UART_HandleTypeDef huart5;
|
||||
extern UART_HandleTypeDef huart10;
|
||||
extern TIM_HandleTypeDef htim2;
|
||||
|
||||
/* USER CODE BEGIN EV */
|
||||
@ -309,6 +312,34 @@ void UART5_IRQHandler(void)
|
||||
/* USER CODE END UART5_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles DMA2 stream1 global interrupt.
|
||||
*/
|
||||
void DMA2_Stream1_IRQHandler(void)
|
||||
{
|
||||
/* USER CODE BEGIN DMA2_Stream1_IRQn 0 */
|
||||
|
||||
/* USER CODE END DMA2_Stream1_IRQn 0 */
|
||||
HAL_DMA_IRQHandler(&hdma_usart10_rx);
|
||||
/* USER CODE BEGIN DMA2_Stream1_IRQn 1 */
|
||||
|
||||
/* USER CODE END DMA2_Stream1_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles DMA2 stream2 global interrupt.
|
||||
*/
|
||||
void DMA2_Stream2_IRQHandler(void)
|
||||
{
|
||||
/* USER CODE BEGIN DMA2_Stream2_IRQn 0 */
|
||||
|
||||
/* USER CODE END DMA2_Stream2_IRQn 0 */
|
||||
HAL_DMA_IRQHandler(&hdma_usart10_tx);
|
||||
/* USER CODE BEGIN DMA2_Stream2_IRQn 1 */
|
||||
|
||||
/* USER CODE END DMA2_Stream2_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles DMA2 stream5 global interrupt.
|
||||
*/
|
||||
@ -323,6 +354,20 @@ void DMA2_Stream5_IRQHandler(void)
|
||||
/* USER CODE END DMA2_Stream5_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles USART10 global interrupt.
|
||||
*/
|
||||
void USART10_IRQHandler(void)
|
||||
{
|
||||
/* USER CODE BEGIN USART10_IRQn 0 */
|
||||
|
||||
/* USER CODE END USART10_IRQn 0 */
|
||||
HAL_UART_IRQHandler(&huart10);
|
||||
/* USER CODE BEGIN USART10_IRQn 1 */
|
||||
|
||||
/* USER CODE END USART10_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles FDCAN3 interrupt 0.
|
||||
*/
|
||||
|
||||
@ -27,6 +27,8 @@
|
||||
UART_HandleTypeDef huart5;
|
||||
UART_HandleTypeDef huart10;
|
||||
DMA_HandleTypeDef hdma_uart5_rx;
|
||||
DMA_HandleTypeDef hdma_usart10_rx;
|
||||
DMA_HandleTypeDef hdma_usart10_tx;
|
||||
|
||||
/* UART5 init function */
|
||||
void MX_UART5_Init(void)
|
||||
@ -222,6 +224,46 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
|
||||
GPIO_InitStruct.Alternate = GPIO_AF11_USART10;
|
||||
HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
|
||||
|
||||
/* USART10 DMA Init */
|
||||
/* USART10_RX Init */
|
||||
hdma_usart10_rx.Instance = DMA2_Stream1;
|
||||
hdma_usart10_rx.Init.Request = DMA_REQUEST_USART10_RX;
|
||||
hdma_usart10_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
|
||||
hdma_usart10_rx.Init.PeriphInc = DMA_PINC_DISABLE;
|
||||
hdma_usart10_rx.Init.MemInc = DMA_MINC_ENABLE;
|
||||
hdma_usart10_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
|
||||
hdma_usart10_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
|
||||
hdma_usart10_rx.Init.Mode = DMA_NORMAL;
|
||||
hdma_usart10_rx.Init.Priority = DMA_PRIORITY_LOW;
|
||||
hdma_usart10_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
|
||||
if (HAL_DMA_Init(&hdma_usart10_rx) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
|
||||
__HAL_LINKDMA(uartHandle,hdmarx,hdma_usart10_rx);
|
||||
|
||||
/* USART10_TX Init */
|
||||
hdma_usart10_tx.Instance = DMA2_Stream2;
|
||||
hdma_usart10_tx.Init.Request = DMA_REQUEST_USART10_TX;
|
||||
hdma_usart10_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
|
||||
hdma_usart10_tx.Init.PeriphInc = DMA_PINC_DISABLE;
|
||||
hdma_usart10_tx.Init.MemInc = DMA_MINC_ENABLE;
|
||||
hdma_usart10_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
|
||||
hdma_usart10_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
|
||||
hdma_usart10_tx.Init.Mode = DMA_NORMAL;
|
||||
hdma_usart10_tx.Init.Priority = DMA_PRIORITY_LOW;
|
||||
hdma_usart10_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
|
||||
if (HAL_DMA_Init(&hdma_usart10_tx) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
|
||||
__HAL_LINKDMA(uartHandle,hdmatx,hdma_usart10_tx);
|
||||
|
||||
/* USART10 interrupt Init */
|
||||
HAL_NVIC_SetPriority(USART10_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(USART10_IRQn);
|
||||
/* USER CODE BEGIN USART10_MspInit 1 */
|
||||
|
||||
/* USER CODE END USART10_MspInit 1 */
|
||||
@ -270,6 +312,12 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
|
||||
*/
|
||||
HAL_GPIO_DeInit(GPIOE, GPIO_PIN_2|GPIO_PIN_3);
|
||||
|
||||
/* USART10 DMA DeInit */
|
||||
HAL_DMA_DeInit(uartHandle->hdmarx);
|
||||
HAL_DMA_DeInit(uartHandle->hdmatx);
|
||||
|
||||
/* USART10 interrupt Deinit */
|
||||
HAL_NVIC_DisableIRQ(USART10_IRQn);
|
||||
/* USER CODE BEGIN USART10_MspDeInit 1 */
|
||||
|
||||
/* USER CODE END USART10_MspDeInit 1 */
|
||||
|
||||
@ -431,11 +431,6 @@ int8_t BSP_FDCAN_Init(void) {
|
||||
#undef FDCANX_MSG_PENDING_CB_INNER
|
||||
#undef FDCANX_MSG_PENDING_CB
|
||||
|
||||
//Power-en
|
||||
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_13, GPIO_PIN_SET);
|
||||
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_14, GPIO_PIN_SET);
|
||||
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_15, GPIO_PIN_SET);
|
||||
|
||||
return BSP_OK;
|
||||
}
|
||||
|
||||
|
||||
@ -28,8 +28,8 @@ extern "C" {
|
||||
/* UART实体枚举，与设备对应 */
|
||||
typedef enum {
|
||||
BSP_UART_RC,
|
||||
BSP_UART_AI,
|
||||
BSP_UART_REF,
|
||||
BSP_UART_AI,
|
||||
BSP_UART_NUM,
|
||||
BSP_UART_ERR,
|
||||
} BSP_UART_t;
|
||||
|
||||
@ -27,11 +27,11 @@ extern "C" {
|
||||
#define SIGNAL_BMI088_GYRO_RAW_REDY (1u << 2)
|
||||
#define SIGNAL_BMI088_ACCL_NEW_DATA (1u << 3)
|
||||
#define SIGNAL_BMI088_GYRO_NEW_DATA (1u << 4)
|
||||
#define SIGNAL_AT9S_RAW_REDY (1u << 7)
|
||||
#define SIGNAL_VT13_RAW_REDY (1u << 8)
|
||||
#define SIGNAL_REFEREE_RAW_REDY (1u << 10)
|
||||
#define SIGNAL_REFEREE_FAST_REFRESH_UI (1u << 11)
|
||||
#define SIGNAL_REFEREE_SLOW_REFRESH_UI (1u << 12)
|
||||
#define SIGNAL_AT9S_RAW_REDY (1u << 5)
|
||||
#define SIGNAL_VT13_RAW_REDY (1u << 6)
|
||||
#define SIGNAL_REFEREE_RAW_REDY (1u << 7)
|
||||
#define SIGNAL_REFEREE_FAST_REFRESH_UI (1u << 8)
|
||||
#define SIGNAL_REFEREE_SLOW_REFRESH_UI (1u << 9)
|
||||
/* AUTO GENERATED SIGNALS END */
|
||||
|
||||
/* USER SIGNALS BEGIN */
|
||||
|
||||
@ -366,7 +366,15 @@ int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output) {
|
||||
if (motor == NULL) {
|
||||
return DEVICE_ERR_NO_DEV;
|
||||
}
|
||||
|
||||
|
||||
if (motor->param.reverse) {
|
||||
MOTOR_MIT_Output_t reversed = *output;
|
||||
reversed.angle = -reversed.angle;
|
||||
reversed.velocity = -reversed.velocity;
|
||||
reversed.torque = -reversed.torque;
|
||||
return MOTOR_DM_SendMITCmd(motor, &reversed);
|
||||
}
|
||||
|
||||
return MOTOR_DM_SendMITCmd(motor, output);
|
||||
}
|
||||
|
||||
|
||||
@ -31,7 +31,8 @@
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
static volatile uint32_t drop_message = 0;
|
||||
|
||||
static uint8_t rxbuf[REF_LEN_RX_BUFF];
|
||||
// static uint8_t rxbuf[REF_LEN_RX_BUFF];
|
||||
uint8_t rxbuf[REF_LEN_RX_BUFF];
|
||||
|
||||
static osThreadId_t thread_alert;
|
||||
static bool inited = false;
|
||||
@ -88,7 +89,7 @@ int8_t Referee_Init(Referee_t *ref, Referee_UI_t *ui,
|
||||
osTimerStart(ref->ui_fast_timer_id, fast_period_ms);
|
||||
osTimerStart(ref->ui_slow_timer_id, slow_period_ms);
|
||||
|
||||
__HAL_UART_ENABLE_IT(BSP_UART_GetHandle(BSP_UART_REF), UART_IT_IDLE);
|
||||
// __HAL_UART_ENABLE_IT(BSP_UART_GetHandle(BSP_UART_REF), UART_IT_IDLE);
|
||||
|
||||
inited = true;
|
||||
return 0;
|
||||
@ -103,7 +104,7 @@ int8_t Referee_Restart(void) {
|
||||
int8_t Referee_StartReceiving(Referee_t *ref) {
|
||||
(void)ref;
|
||||
|
||||
if ( BSP_UART_Receive(BSP_UART_REF, rxbuf, REF_LEN_RX_BUFF,true)
|
||||
if ( BSP_UART_Receive(BSP_UART_REF, rxbuf, REF_LEN_RX_BUFF,0)
|
||||
== BSP_OK)
|
||||
return DEVICE_OK;
|
||||
return DEVICE_ERR;
|
||||
@ -646,8 +647,8 @@ uint8_t Referee_UIRefresh(Referee_UI_t *ui) {
|
||||
UI_DrawLine(Referee_GetGrapicAdd(ui), "6", UI_GRAPIC_OPERATION_ADD,
|
||||
UI_GRAPIC_LAYER_CHASSIS, GREEN, UI_DEFAULT_WIDTH * 12,
|
||||
ui->screen->width * 0.4, ui->screen->height * 0.2,
|
||||
ui->screen->width * 0.4 + sin(ui->chassis_ui.angle) * 46,
|
||||
ui->screen->height * 0.2 + cos(ui->chassis_ui.angle) * 46);
|
||||
ui->screen->width * 0.4f + sinf(ui->chassis_ui.angle) * 46.0f,
|
||||
ui->screen->height * 0.2f + cosf(ui->chassis_ui.angle) * 46.0f);
|
||||
float start_pos_h = 0.0f;
|
||||
switch (ui->chassis_ui.mode) {
|
||||
case CHASSIS_MODE_RELAX:
|
||||
|
||||
@ -574,11 +574,6 @@ typedef struct {
|
||||
osTimerId_t ui_slow_timer_id;
|
||||
} Referee_t;
|
||||
|
||||
/* Referee_ChassisUI_t 已移至 module/chassis.h */
|
||||
/* Referee_CapUI_t 已移至 device/supercap.h */
|
||||
/* Referee_GimbalUI_t 已移至 module/gimbal.h */
|
||||
/* Referee_ShootUI_t 已移至 module/shoot.h */
|
||||
|
||||
typedef struct __packed {
|
||||
/* UI缓冲数据 */
|
||||
UI_Ele_t grapic[REF_UI_MAX_GRAPIC_NUM];
|
||||
@ -589,10 +584,10 @@ typedef struct __packed {
|
||||
uint8_t character_counter;
|
||||
uint8_t del_counter;
|
||||
/* UI所需信息 */
|
||||
Referee_CapUI_t cap_ui;
|
||||
Referee_ChassisUI_t chassis_ui;
|
||||
Referee_ShootUI_t shoot_ui;
|
||||
Referee_GimbalUI_t gimbal_ui;
|
||||
Cap_RefereeUI_t cap_ui;
|
||||
Chassis_RefereeUI_t chassis_ui;
|
||||
Shoot_RefereeUI_t shoot_ui;
|
||||
Gimbal_RefereeUI_t gimbal_ui;
|
||||
bool cmd_pc;
|
||||
/* 屏幕分辨率 */
|
||||
const Referee_Screen_t *screen;
|
||||
|
||||
@ -101,7 +101,7 @@ int8_t SuperCap_Update(void);
|
||||
typedef struct {
|
||||
float percentage;
|
||||
SuperCap_Status_t status;
|
||||
} Referee_CapUI_t;
|
||||
} Cap_RefereeUI_t;
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
||||
338
User/module/aimbot.c
Normal file
338
User/module/aimbot.c
Normal file
@ -0,0 +1,338 @@
|
||||
#include "module/aimbot.h"
|
||||
#include "device/device.h"
|
||||
#include "bsp/uart.h"
|
||||
#include "bsp/fdcan.h"
|
||||
#include "component/crc16.h"
|
||||
#include <string.h>
|
||||
|
||||
/* =====================================================================
|
||||
* CAN 帧索引（反馈方向：下层板 → 上层板）
|
||||
* ===================================================================== */
|
||||
#define FB_FRAME_Q01 0u /* IMU 四元数 q[0](w), q[1](x) */
|
||||
#define FB_FRAME_Q23 1u /* IMU 四元数 q[2](y), q[3](z) */
|
||||
#define FB_FRAME_EULR 2u /* IMU 欧拉角 yaw, pitch (float x2) */
|
||||
#define FB_FRAME_VEL 3u /* IMU 角速度 yaw_vel, pitch_vel (float x2) */
|
||||
#define FB_FRAME_SHOOT 4u /* 弹速(4B)+弹数(2B)+模式(1B)+保留(1B) */
|
||||
#define FB_FRAME_MOTOR 5u /* 云台电机绝对角度 yaw, pit (float x2) */
|
||||
|
||||
/* =====================================================================
|
||||
* UART 通信接口（上层板 ↔ 上位机 PC）
|
||||
* ===================================================================== */
|
||||
|
||||
int8_t Aimbot_AIStartRecv(Aimbot_AI_t *ai) {
|
||||
if (BSP_UART_Receive(BSP_UART_AI, (uint8_t *)ai, sizeof(*ai), true) == DEVICE_OK) {
|
||||
return DEVICE_OK;
|
||||
}
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
int8_t Aimbot_AIGetResult(Aimbot_AI_t *ai, Aimbot_AIResult_t *result) {
|
||||
if (ai->id != ID_AI) {
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
if (!CRC16_Verify((const uint8_t *)ai, sizeof(*ai))) {
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
result->mode = ai->data.mode;
|
||||
result->gimbal_t.setpoint.yaw = ai->data.yaw;
|
||||
result->gimbal_t.vel.yaw = ai->data.yaw_vel;
|
||||
result->gimbal_t.accl.yaw = ai->data.yaw_acc;
|
||||
result->gimbal_t.setpoint.pit = ai->data.pitch;
|
||||
result->gimbal_t.vel.pit = ai->data.pitch_vel;
|
||||
result->gimbal_t.accl.pit = ai->data.pitch_acc;
|
||||
result->chassis_t.Vx = ai->data.vx;
|
||||
result->chassis_t.Vy = ai->data.vy;
|
||||
result->chassis_t.Vw = ai->data.wz;
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 打包 MCU 数据（UART 发给上位机格式），修正了原始实现中的字段错误。
|
||||
* 四元数来自 quat 参数；欧拉角、角速度来自 gimbal_fb->imu。
|
||||
*/
|
||||
int8_t Aimbot_MCUPack(Aimbot_MCU_t *mcu, const Gimbal_Feedback_t *gimbal_fb,
|
||||
const AHRS_Quaternion_t *quat,
|
||||
float bullet_speed, uint16_t bullet_count) {
|
||||
if (mcu == NULL || gimbal_fb == NULL || quat == NULL) {
|
||||
return DEVICE_ERR_NULL;
|
||||
}
|
||||
mcu->id = ID_MCU;
|
||||
mcu->data.mode = 0;
|
||||
mcu->data.q[0] = quat->q0;
|
||||
mcu->data.q[1] = quat->q1;
|
||||
mcu->data.q[2] = quat->q2;
|
||||
mcu->data.q[3] = quat->q3;
|
||||
mcu->data.yaw = gimbal_fb->imu.eulr.yaw;
|
||||
mcu->data.yaw_vel = gimbal_fb->imu.gyro.z;
|
||||
mcu->data.pitch = gimbal_fb->imu.eulr.pit;
|
||||
mcu->data.pitch_vel = gimbal_fb->imu.gyro.x;
|
||||
mcu->data.bullet_speed = bullet_speed;
|
||||
mcu->data.bullet_count = bullet_count;
|
||||
mcu->crc16 = CRC16_Calc((const uint8_t *)mcu,
|
||||
sizeof(*mcu) - sizeof(uint16_t), CRC16_INIT);
|
||||
if (!CRC16_Verify((const uint8_t *)mcu, sizeof(*mcu))) {
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
int8_t Aimbot_MCUStartSend(Aimbot_MCU_t *mcu) {
|
||||
if (BSP_UART_Transmit(BSP_UART_AI, (uint8_t *)mcu, sizeof(*mcu), true) == DEVICE_OK) {
|
||||
return DEVICE_OK;
|
||||
}
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
/* =====================================================================
|
||||
* CAN 通信接口（下层板 ↔ 上层板）
|
||||
* ===================================================================== */
|
||||
|
||||
/**
|
||||
* @brief 初始化 Aimbot CAN 通信：注册指令接收队列和反馈收发队列。
|
||||
* 下层板只需注册 cmd_id；上层板只需注册 fb_base_id 的 6 个 ID。
|
||||
* 本函数同时注册两侧所需 ID，上/下层板共用同一初始化流程即可。
|
||||
*/
|
||||
int8_t Aimbot_Init(Aimbot_Param_t *param) {
|
||||
if (param == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
BSP_FDCAN_Init();
|
||||
|
||||
/* 注册 AI 指令帧队列（下层板接收／上层板发送） */
|
||||
BSP_FDCAN_RegisterId(param->can, param->cmd_id,
|
||||
BSP_FDCAN_DEFAULT_QUEUE_SIZE);
|
||||
|
||||
/* 注册反馈数据帧队列（上层板接收／下层板发送） */
|
||||
for (uint8_t i = 0; i < AIMBOT_FB_FRAME_NUM; i++) {
|
||||
BSP_FDCAN_RegisterId(param->can, param->fb_base_id + i,
|
||||
BSP_FDCAN_DEFAULT_QUEUE_SIZE);
|
||||
}
|
||||
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 从 Gimbal/IMU/Shoot 数据打包 CAN 反馈结构体。
|
||||
*/
|
||||
int8_t Aimbot_PackFeedback(Aimbot_FeedbackData_t *fb,
|
||||
const Gimbal_Feedback_t *gimbal_fb,
|
||||
const AHRS_Quaternion_t *quat,
|
||||
float bullet_speed, uint16_t bullet_count,
|
||||
uint8_t mode) {
|
||||
if (fb == NULL || gimbal_fb == NULL || quat == NULL) {
|
||||
return DEVICE_ERR_NULL;
|
||||
}
|
||||
fb->mode = mode;
|
||||
fb->q[0] = quat->q0;
|
||||
fb->q[1] = quat->q1;
|
||||
fb->q[2] = quat->q2;
|
||||
fb->q[3] = quat->q3;
|
||||
fb->yaw = gimbal_fb->imu.eulr.yaw;
|
||||
fb->pitch = gimbal_fb->imu.eulr.pit;
|
||||
fb->yaw_vel = gimbal_fb->imu.gyro.z;
|
||||
fb->pitch_vel = gimbal_fb->imu.gyro.x;
|
||||
fb->bullet_speed = bullet_speed;
|
||||
fb->bullet_count = bullet_count;
|
||||
fb->gimbal_yaw = gimbal_fb->motor.yaw.rotor_abs_angle;
|
||||
fb->gimbal_pit = gimbal_fb->motor.pit.rotor_abs_angle;
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 【下层板】将反馈数据打成 6 个 CAN 标准帧发给上层板。
|
||||
*
|
||||
* 帧格式（每帧 8 字节）：
|
||||
* 帧0: q[0](float,4B) q[1](float,4B)
|
||||
* 帧1: q[2](float,4B) q[3](float,4B)
|
||||
* 帧2: yaw(float,4B) pitch(float,4B)
|
||||
* 帧3: yaw_vel(float,4B) pitch_vel(float,4B)
|
||||
* 帧4: bullet_speed(float,4B) bullet_count(uint16,2B) mode(1B) rsv(1B)
|
||||
* 帧5: gimbal_yaw(float,4B) gimbal_pit(float,4B)
|
||||
*/
|
||||
void Aimbot_SendFeedbackOnCAN(const Aimbot_Param_t *param,
|
||||
const Aimbot_FeedbackData_t *fb) {
|
||||
if (param == NULL || fb == NULL) return;
|
||||
|
||||
BSP_FDCAN_StdDataFrame_t frame;
|
||||
frame.dlc = AIMBOT_CAN_DLC;
|
||||
|
||||
/* 帧0: IMU 四元数 q[0], q[1] */
|
||||
frame.id = param->fb_base_id + FB_FRAME_Q01;
|
||||
memcpy(&frame.data[0], &fb->q[0], 4);
|
||||
memcpy(&frame.data[4], &fb->q[1], 4);
|
||||
BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);
|
||||
|
||||
/* 帧1: IMU 四元数 q[2], q[3] */
|
||||
frame.id = param->fb_base_id + FB_FRAME_Q23;
|
||||
memcpy(&frame.data[0], &fb->q[2], 4);
|
||||
memcpy(&frame.data[4], &fb->q[3], 4);
|
||||
BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);
|
||||
|
||||
/* 帧2: IMU 欧拉角 yaw, pitch */
|
||||
frame.id = param->fb_base_id + FB_FRAME_EULR;
|
||||
memcpy(&frame.data[0], &fb->yaw, 4);
|
||||
memcpy(&frame.data[4], &fb->pitch, 4);
|
||||
BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);
|
||||
|
||||
/* 帧3: IMU 角速度 yaw_vel, pitch_vel */
|
||||
frame.id = param->fb_base_id + FB_FRAME_VEL;
|
||||
memcpy(&frame.data[0], &fb->yaw_vel, 4);
|
||||
memcpy(&frame.data[4], &fb->pitch_vel, 4);
|
||||
BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);
|
||||
|
||||
/* 帧4: 弹速 + 弹数 + 模式 */
|
||||
frame.id = param->fb_base_id + FB_FRAME_SHOOT;
|
||||
memcpy(&frame.data[0], &fb->bullet_speed, 4);
|
||||
frame.data[4] = (uint8_t)(fb->bullet_count & 0xFFu);
|
||||
frame.data[5] = (uint8_t)((fb->bullet_count >> 8u) & 0xFFu);
|
||||
frame.data[6] = fb->mode;
|
||||
frame.data[7] = 0u;
|
||||
BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);
|
||||
|
||||
/* 帧5: 云台电机绝对角度 yaw, pit */
|
||||
frame.id = param->fb_base_id + FB_FRAME_MOTOR;
|
||||
memcpy(&frame.data[0], &fb->gimbal_yaw, 4);
|
||||
memcpy(&frame.data[4], &fb->gimbal_pit, 4);
|
||||
BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 【下层板】从 CAN 队列中非阻塞地取出上层板发来的 AI 指令并解析。
|
||||
*
|
||||
* 指令帧格式（8 字节，与 vision_bridge 一致）：
|
||||
* data[0] : mode (1B)
|
||||
* data[1..3.5] : yaw (28bit 有符号定点数，0.1µrad/LSB)
|
||||
* data[4.5..7] : pit (28bit 有符号定点数，0.1µrad/LSB)
|
||||
*
|
||||
* @return DEVICE_OK 成功解析到新指令
|
||||
* DEVICE_ERR 队列空，无新数据
|
||||
*/
|
||||
int8_t Aimbot_ParseCmdFromCAN(const Aimbot_Param_t *param,
|
||||
Aimbot_AIResult_t *result) {
|
||||
if (param == NULL || result == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
BSP_FDCAN_Message_t msg;
|
||||
if (BSP_FDCAN_GetMessage(param->can, param->cmd_id, &msg,
|
||||
BSP_FDCAN_TIMEOUT_IMMEDIATE) != 0) {
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
result->mode = msg.data[0];
|
||||
|
||||
/* 解析 yaw（高 28 位），符号扩展为 int32 */
|
||||
int32_t yaw_raw = (int32_t)(((uint32_t)msg.data[1] << 20u) |
|
||||
((uint32_t)msg.data[2] << 12u) |
|
||||
((uint32_t)msg.data[3] << 4u) |
|
||||
((uint32_t)(msg.data[4] >> 4u) & 0xFu));
|
||||
if (yaw_raw & 0x08000000) yaw_raw |= (int32_t)0xF0000000;
|
||||
result->gimbal_t.setpoint.yaw = (float)yaw_raw / AIMBOT_ANGLE_SCALE;
|
||||
|
||||
/* 解析 pit（低 28 位），符号扩展为 int32 */
|
||||
int32_t pit_raw = (int32_t)(((uint32_t)(msg.data[4] & 0xFu) << 24u) |
|
||||
((uint32_t)msg.data[5] << 16u) |
|
||||
((uint32_t)msg.data[6] << 8u) |
|
||||
(uint32_t)msg.data[7]);
|
||||
if (pit_raw & 0x08000000) pit_raw |= (int32_t)0xF0000000;
|
||||
result->gimbal_t.setpoint.pit = (float)pit_raw / AIMBOT_ANGLE_SCALE;
|
||||
|
||||
/* 速度/加速度前馈由上层板另行扩展，此处置零 */
|
||||
result->gimbal_t.vel.yaw = 0.0f;
|
||||
result->gimbal_t.vel.pit = 0.0f;
|
||||
result->gimbal_t.accl.yaw = 0.0f;
|
||||
result->gimbal_t.accl.pit = 0.0f;
|
||||
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 【上层板】将 AI 指令通过 CAN 发送给下层板（单帧 8 字节）。
|
||||
*
|
||||
* 与 vision_bridge.c 的 AI_SendCmdOnFDCAN 编码格式完全一致，
|
||||
* 上层板可直接调用本函数替代 vision_bridge 中的同名函数。
|
||||
*/
|
||||
void Aimbot_SendCmdOnCAN(const Aimbot_Param_t *param,
|
||||
const Aimbot_AIResult_t *cmd) {
|
||||
if (param == NULL || cmd == NULL) return;
|
||||
|
||||
const int32_t yaw = (int32_t)(cmd->gimbal_t.setpoint.yaw * AIMBOT_ANGLE_SCALE);
|
||||
const int32_t pit = (int32_t)(cmd->gimbal_t.setpoint.pit * AIMBOT_ANGLE_SCALE);
|
||||
|
||||
BSP_FDCAN_StdDataFrame_t frame = {0};
|
||||
frame.id = param->cmd_id;
|
||||
frame.dlc = AIMBOT_CAN_DLC;
|
||||
|
||||
frame.data[0] = cmd->mode;
|
||||
frame.data[1] = (uint8_t)((yaw >> 20) & 0xFF);
|
||||
frame.data[2] = (uint8_t)((yaw >> 12) & 0xFF);
|
||||
frame.data[3] = (uint8_t)((yaw >> 4) & 0xFF);
|
||||
frame.data[4] = (uint8_t)(((yaw & 0xF) << 4) | ((pit >> 24) & 0xF));
|
||||
frame.data[5] = (uint8_t)((pit >> 16) & 0xFF);
|
||||
frame.data[6] = (uint8_t)((pit >> 8) & 0xFF);
|
||||
frame.data[7] = (uint8_t)(pit & 0xFF);
|
||||
|
||||
BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 【上层板】从 CAN 队列中逐帧非阻塞地解析下层板发来的反馈数据。
|
||||
* 各帧独立读取，缺失的帧保留上一次的旧值，不影响其他帧。
|
||||
*
|
||||
* @return DEVICE_OK（始终返回，允许部分帧缺失）
|
||||
*/
|
||||
int8_t Aimbot_ParseFeedbackFromCAN(const Aimbot_Param_t *param,
|
||||
Aimbot_FeedbackData_t *fb) {
|
||||
if (param == NULL || fb == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
BSP_FDCAN_Message_t msg;
|
||||
|
||||
/* 帧0: IMU 四元数 q[0], q[1] */
|
||||
if (BSP_FDCAN_GetMessage(param->can,
|
||||
param->fb_base_id + FB_FRAME_Q01,
|
||||
&msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
|
||||
memcpy(&fb->q[0], &msg.data[0], 4);
|
||||
memcpy(&fb->q[1], &msg.data[4], 4);
|
||||
}
|
||||
|
||||
/* 帧1: IMU 四元数 q[2], q[3] */
|
||||
if (BSP_FDCAN_GetMessage(param->can,
|
||||
param->fb_base_id + FB_FRAME_Q23,
|
||||
&msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
|
||||
memcpy(&fb->q[2], &msg.data[0], 4);
|
||||
memcpy(&fb->q[3], &msg.data[4], 4);
|
||||
}
|
||||
|
||||
/* 帧2: IMU 欧拉角 yaw, pitch */
|
||||
if (BSP_FDCAN_GetMessage(param->can,
|
||||
param->fb_base_id + FB_FRAME_EULR,
|
||||
&msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
|
||||
memcpy(&fb->yaw, &msg.data[0], 4);
|
||||
memcpy(&fb->pitch, &msg.data[4], 4);
|
||||
}
|
||||
|
||||
/* 帧3: IMU 角速度 yaw_vel, pitch_vel */
|
||||
if (BSP_FDCAN_GetMessage(param->can,
|
||||
param->fb_base_id + FB_FRAME_VEL,
|
||||
&msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
|
||||
memcpy(&fb->yaw_vel, &msg.data[0], 4);
|
||||
memcpy(&fb->pitch_vel, &msg.data[4], 4);
|
||||
}
|
||||
|
||||
/* 帧4: 弹速 + 弹数 + 模式 */
|
||||
if (BSP_FDCAN_GetMessage(param->can,
|
||||
param->fb_base_id + FB_FRAME_SHOOT,
|
||||
&msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
|
||||
memcpy(&fb->bullet_speed, &msg.data[0], 4);
|
||||
fb->bullet_count = (uint16_t)(msg.data[4] | ((uint16_t)msg.data[5] << 8u));
|
||||
fb->mode = msg.data[6];
|
||||
}
|
||||
|
||||
/* 帧5: 云台电机绝对角度 yaw, pit */
|
||||
if (BSP_FDCAN_GetMessage(param->can,
|
||||
param->fb_base_id + FB_FRAME_MOTOR,
|
||||
&msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
|
||||
memcpy(&fb->gimbal_yaw, &msg.data[0], 4);
|
||||
memcpy(&fb->gimbal_pit, &msg.data[4], 4);
|
||||
}
|
||||
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
205
User/module/aimbot.h
Normal file
205
User/module/aimbot.h
Normal file
@ -0,0 +1,205 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "component\user_math.h"
|
||||
// #include "remote_control.h"
|
||||
#include "module/gimbal.h"
|
||||
#include "bsp/fdcan.h"
|
||||
#include <stdint.h>
|
||||
|
||||
// 数据包 ID 定义
|
||||
#define ID_MCU (0xC4) // MCU 数据包
|
||||
#define ID_REF (0xA8) // 裁判系统数据包
|
||||
#define ID_AI (0xB5) // AI 控制数据包
|
||||
|
||||
/* ============================ CAN 通信定义 ============================
|
||||
* 通路：上位机 <--串口--> 上层板 <--CAN--> 下层板（本代码）
|
||||
*
|
||||
* 下层板 → 上层板：反馈数据，共 AIMBOT_FB_FRAME_NUM 帧，CAN ID 连续
|
||||
* 帧0 (fb_base_id+0): IMU 四元数 q[0], q[1]
|
||||
* 帧1 (fb_base_id+1): IMU 四元数 q[2], q[3]
|
||||
* 帧2 (fb_base_id+2): IMU 欧拉角 yaw, pitch (rad)
|
||||
* 帧3 (fb_base_id+3): IMU 角速度 yaw_vel, pitch_vel (rad/s)
|
||||
* 帧4 (fb_base_id+4): 弹速(4B) + 弹数(2B) + 模式(1B) + 保留(1B)
|
||||
* 帧5 (fb_base_id+5): 云台电机绝对角度 yaw, pit (rad)
|
||||
*
|
||||
* 上层板 → 下层板：AI 指令，单帧 8 字节
|
||||
* 帧 (cmd_id): mode(1B) + yaw(28bit) + pit(28bit)，定点数精度 0.1µrad
|
||||
* ====================================================================== */
|
||||
#define AIMBOT_FB_FRAME_NUM 6u /* 反馈数据帧总数 */
|
||||
#define AIMBOT_CAN_DLC 8u /* CAN 帧数据长度 */
|
||||
#define AIMBOT_ANGLE_SCALE 10000000.0f /* 指令角度定点数比例（0.1µrad/LSB） */
|
||||
|
||||
/* CAN 通信参数结构体 */
|
||||
typedef struct {
|
||||
BSP_FDCAN_t can; /* 使用的 CAN 总线实例 */
|
||||
uint32_t cmd_id; /* 上层板→下层板 AI 指令帧 CAN ID */
|
||||
uint32_t fb_base_id; /* 下层板→上层板 反馈数据起始 CAN ID（共6帧，ID连续递增） */
|
||||
} Aimbot_Param_t;
|
||||
|
||||
/* CAN 反馈数据结构体（下层板打包后经 CAN 发给上层板的内容） */
|
||||
typedef struct {
|
||||
uint8_t mode; /* 下层板当前工作模式 */
|
||||
float q[4]; /* IMU 四元数 q0(w) q1(x) q2(y) q3(z) */
|
||||
float yaw; /* IMU yaw 角（rad） */
|
||||
float yaw_vel; /* IMU yaw 角速度（rad/s） */
|
||||
float pitch; /* IMU pitch 角（rad） */
|
||||
float pitch_vel; /* IMU pitch 角速度（rad/s） */
|
||||
float bullet_speed; /* 弹速（m/s） */
|
||||
uint16_t bullet_count; /* 子弹累计发射次数 */
|
||||
float gimbal_yaw; /* 云台 yaw 电机绝对角度（rad） */
|
||||
float gimbal_pit; /* 云台 pitch 电机绝对角度（rad） */
|
||||
} Aimbot_FeedbackData_t;
|
||||
|
||||
/*-----------------------------to上位机---------------------------------*/
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint8_t mode; // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
|
||||
float q[4]; // 四元数 wxyz 顺序
|
||||
float yaw; // 偏航角
|
||||
float yaw_vel; // 偏航角速度
|
||||
float pitch; // 俯仰角
|
||||
float pitch_vel; // 俯仰角速度
|
||||
float bullet_speed; // 弹速
|
||||
uint16_t bullet_count; // 子弹累计发送次数
|
||||
} Aimbot_MCUData_t;
|
||||
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint8_t id; // 数据包 ID: 0xC4
|
||||
Aimbot_MCUData_t data;
|
||||
uint16_t crc16;
|
||||
} Aimbot_MCU_t;
|
||||
|
||||
// 裁判系统数据结构
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint16_t remain_hp; // 剩余血量
|
||||
uint8_t game_progress : 4; // 比赛进度
|
||||
uint16_t stage_remain_time; // 比赛剩余时间
|
||||
} Aimbot_RefereeData_t;
|
||||
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint8_t id; // 数据包 ID: 0xA8
|
||||
Aimbot_RefereeData_t data;
|
||||
uint16_t crc16;
|
||||
} Aimbot_Referee_t;
|
||||
|
||||
/*------------------------------------上位机back-------------------------------------*/
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint8_t mode; // 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火
|
||||
float yaw; // 目标偏航角
|
||||
float yaw_vel; // 偏航角速度
|
||||
float yaw_acc; // 偏航角加速度
|
||||
float pitch; // 目标俯仰角
|
||||
float pitch_vel; // 俯仰角速度
|
||||
float pitch_acc; // 俯仰角加速度
|
||||
float vx; // X 方向速度
|
||||
float vy; // Y 方向速度
|
||||
float wz; // Z 方向角速度
|
||||
uint8_t reserved; // 预留字段
|
||||
} Aimbot_AIData_t;
|
||||
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint8_t id; // 数据包 ID: 0xB5
|
||||
Aimbot_AIData_t data;
|
||||
uint16_t crc16;
|
||||
} Aimbot_AI_t;
|
||||
|
||||
typedef struct __attribute__((packed)) {
|
||||
uint8_t mode; // 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火
|
||||
struct{
|
||||
// Gimbal_CMD_t g_cmd;
|
||||
struct{
|
||||
float yaw; // 目标偏航角
|
||||
float pit; // 目标俯仰角
|
||||
}setpoint;
|
||||
|
||||
struct{
|
||||
float pit; // 俯仰角加速度
|
||||
float yaw; // 偏航角加速度
|
||||
}accl;
|
||||
struct{
|
||||
float pit; // 俯仰角速度
|
||||
float yaw; // 偏航角速度
|
||||
}vel;
|
||||
}gimbal_t;
|
||||
|
||||
struct{
|
||||
float Vx; // X 方向速度
|
||||
float Vy; // Y 方向速度
|
||||
float Vw; // Z 方向角速度
|
||||
}chassis_t;
|
||||
|
||||
uint8_t reserved; // 预留字段
|
||||
} Aimbot_AIResult_t; // 解析后的AI控制指令
|
||||
|
||||
/* ---------- UART 通信接口（上层板与上位机 PC 间） ---------- */
|
||||
int8_t Aimbot_MCUPack(Aimbot_MCU_t *mcu, const Gimbal_Feedback_t *gimbal_fb,
|
||||
const AHRS_Quaternion_t *quat,
|
||||
float bullet_speed, uint16_t bullet_count);
|
||||
int8_t Aimbot_MCUStartSend(Aimbot_MCU_t *mcu);
|
||||
int8_t Aimbot_AIGetResult(Aimbot_AI_t *ai, Aimbot_AIResult_t *result);
|
||||
int8_t Aimbot_AIStartRecv(Aimbot_AI_t *ai);
|
||||
|
||||
/* ---------- CAN 通信接口（下层板与上层板间） ---------- */
|
||||
|
||||
/**
|
||||
* @brief 初始化 Aimbot CAN 通信，注册所有收发 CAN ID 队列
|
||||
* @param param CAN 通信参数
|
||||
* @return DEVICE_OK / DEVICE_ERR_NULL
|
||||
*/
|
||||
int8_t Aimbot_Init(Aimbot_Param_t *param);
|
||||
|
||||
/**
|
||||
* @brief 从 Gimbal/IMU/Shoot 数据中打包反馈结构体
|
||||
* @param fb 输出：反馈数据
|
||||
* @param gimbal_fb 云台反馈（含 IMU 欧拉角/角速度、电机角度）
|
||||
* @param quat IMU 四元数
|
||||
* @param bullet_speed 弹速（m/s）
|
||||
* @param bullet_count 已发射子弹数
|
||||
* @param mode 当前工作模式
|
||||
*/
|
||||
int8_t Aimbot_PackFeedback(Aimbot_FeedbackData_t *fb,
|
||||
const Gimbal_Feedback_t *gimbal_fb,
|
||||
const AHRS_Quaternion_t *quat,
|
||||
float bullet_speed, uint16_t bullet_count,
|
||||
uint8_t mode);
|
||||
|
||||
/**
|
||||
* @brief 【下层板】将反馈数据经 CAN 发送给上层板（6 帧）
|
||||
*/
|
||||
void Aimbot_SendFeedbackOnCAN(const Aimbot_Param_t *param,
|
||||
const Aimbot_FeedbackData_t *fb);
|
||||
|
||||
/**
|
||||
* @brief 【下层板】从 CAN 解析上层板发来的 AI 指令
|
||||
* @return DEVICE_OK 表示成功取到新数据；DEVICE_ERR 表示队列暂无数据
|
||||
*/
|
||||
int8_t Aimbot_ParseCmdFromCAN(const Aimbot_Param_t *param,
|
||||
Aimbot_AIResult_t *result);
|
||||
|
||||
/**
|
||||
* @brief 【上层板】通过 CAN 将 AI 指令发送给下层板（单帧 8 字节）
|
||||
*/
|
||||
void Aimbot_SendCmdOnCAN(const Aimbot_Param_t *param,
|
||||
const Aimbot_AIResult_t *cmd);
|
||||
|
||||
/**
|
||||
* @brief 【上层板】从 CAN 解析下层板发来的反馈数据（逐帧读取，非阻塞）
|
||||
* @return DEVICE_OK（部分或全部帧已更新均返回 OK）
|
||||
*/
|
||||
int8_t Aimbot_ParseFeedbackFromCAN(const Aimbot_Param_t *param,
|
||||
Aimbot_FeedbackData_t *fb);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
@ -317,7 +317,7 @@ void Chassis_ResetOutput(Chassis_t *c) {
|
||||
* @param chassis 底盘数据结构体
|
||||
* @param ui UI数据结构体
|
||||
*/
|
||||
void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui) {
|
||||
void Chassis_DumpUI(const Chassis_t *c, Chassis_RefereeUI_t *ui) {
|
||||
ui->mode = c->mode;
|
||||
ui->angle = c->feedback.encoder_gimbalYawMotor - c->mech_zero;
|
||||
}
|
||||
|
||||
@ -46,7 +46,7 @@ typedef enum {
|
||||
typedef struct {
|
||||
Chassis_Mode_t mode;
|
||||
float angle;
|
||||
} Referee_ChassisUI_t;
|
||||
} Chassis_RefereeUI_t;
|
||||
|
||||
/* µ×ÅÌ¿ØÖÆÃüÁî */
|
||||
typedef struct {
|
||||
@ -233,9 +233,9 @@ void Chassis_Power_Control(Chassis_t *c,float max_power);
|
||||
* @param chassis µ×ÅÌÊý¾Ý½á¹¹Ìå
|
||||
* @param ui UIÊý¾Ý½á¹¹Ìå
|
||||
*/
|
||||
//void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui);
|
||||
//void Chassis_DumpUI(const Chassis_t *c, Chassis_RefereeUI_t *ui);
|
||||
|
||||
void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui);
|
||||
void Chassis_DumpUI(const Chassis_t *c, Chassis_RefereeUI_t *ui);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
||||
@ -119,11 +119,12 @@ static void CMD_RC_BuildTrackCmd(CMD_t *ctx) {
|
||||
break;
|
||||
}
|
||||
ctx->output.track.cmd.enable = ctx->input.online[CMD_SRC_RC];
|
||||
ctx->output.track.cmd.vel = ctx->input.rc.joy_right.y * 2.0f;
|
||||
if(fabsf(ctx->input.rc.joy_right.y * 2.0f) > 1.0f)
|
||||
if (ctx->input.rc.joy_right.y>=0) {
|
||||
ctx->output.track.cmd.vel = ctx->input.rc.joy_right.y * 2.0f;
|
||||
if(fabsf(ctx->input.rc.joy_right.y * 2.0f) > 1.0f)
|
||||
ctx->output.track.cmd.vel = ctx->output.track.cmd.vel > 0 ? 1.0f
|
||||
: -1.0f;
|
||||
|
||||
: -1.0f;
|
||||
}
|
||||
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_TRACK);
|
||||
}
|
||||
#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_TRACK */
|
||||
@ -211,10 +212,13 @@ static void CMD_NUC_BuildGimbalCmd(CMD_t *ctx) {
|
||||
/* 使用AI提供的云台控制数据 */
|
||||
|
||||
if (ctx->input.nuc.mode!=0) {
|
||||
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELATIVE;
|
||||
ctx->output.gimbal.cmd.is_ai = true;
|
||||
ctx->output.gimbal.cmd.setpoint_yaw = ctx->input.nuc.gimbal.setpoint.yaw;
|
||||
ctx->output.gimbal.cmd.setpoint_pit = ctx->input.nuc.gimbal.setpoint.pit;
|
||||
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_AI_CONTROL;
|
||||
ctx->output.gimbal.cmd.setpoint_yaw = ctx->input.nuc.gimbal.setpoint.yaw;
|
||||
ctx->output.gimbal.cmd.setpoint_pit = ctx->input.nuc.gimbal.setpoint.pit;
|
||||
ctx->output.gimbal.cmd.ff_vel_yaw = ctx->input.nuc.gimbal.vel.yaw;
|
||||
ctx->output.gimbal.cmd.ff_vel_pit = ctx->input.nuc.gimbal.vel.pit;
|
||||
ctx->output.gimbal.cmd.ff_accl_yaw = ctx->input.nuc.gimbal.accl.yaw;
|
||||
ctx->output.gimbal.cmd.ff_accl_pit = ctx->input.nuc.gimbal.accl.pit;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -124,32 +124,6 @@ CMD_DEFINE_ADAPTER(AT9S, at9s, CMD_SRC_RC, CMD_AT9S_Init, CMD_AT9S_GetInput, CMD
|
||||
/* ========================================================================== */
|
||||
/* NUC/AI 适配器实现 */
|
||||
/* ========================================================================== */
|
||||
/* ========================================================================== */
|
||||
/* REF/裁判系统 适配器实现 */
|
||||
/* ========================================================================== */
|
||||
#if CMD_ENABLE_SRC_REF
|
||||
|
||||
int8_t CMD_REF_AdapterInit(void *data) {
|
||||
(void)data;
|
||||
return CMD_OK;
|
||||
}
|
||||
|
||||
int8_t CMD_REF_GetInput(void *data, CMD_RawInput_t *output) {
|
||||
CMD_RawInput_REF_t *ref = (CMD_RawInput_REF_t *)data;
|
||||
output->online[CMD_SRC_REF] = true;
|
||||
output->ref = *ref;
|
||||
return CMD_OK;
|
||||
}
|
||||
|
||||
bool CMD_REF_IsOnline(void *data) {
|
||||
(void)data;
|
||||
return true;
|
||||
}
|
||||
|
||||
CMD_DEFINE_ADAPTER(REF, cmd_ref, CMD_SRC_REF, CMD_REF_AdapterInit, CMD_REF_GetInput, CMD_REF_IsOnline)
|
||||
|
||||
#endif /* CMD_ENABLE_SRC_REF */
|
||||
|
||||
#if CMD_ENABLE_SRC_NUC
|
||||
|
||||
int8_t CMD_NUC_AdapterInit(void *data) {
|
||||
@ -183,7 +157,34 @@ extern AI_cmd_t ai_cmd;
|
||||
CMD_DEFINE_ADAPTER(NUC, cmd_ai, CMD_SRC_NUC, CMD_NUC_AdapterInit, CMD_NUC_GetInput, CMD_NUC_IsOnline)
|
||||
|
||||
#endif /* CMD_ENABLE_SRC_NUC */
|
||||
/* ========================================================================== */
|
||||
/* REF/裁判系统 适配器实现 */
|
||||
/* ========================================================================== */
|
||||
#if CMD_ENABLE_SRC_REF
|
||||
|
||||
int8_t CMD_REF_AdapterInit(void *data) {
|
||||
(void)data;
|
||||
return CMD_OK;
|
||||
}
|
||||
|
||||
int8_t CMD_REF_GetInput(void *data, CMD_RawInput_t *output) {
|
||||
CMD_RawInput_REF_t *ref = (CMD_RawInput_REF_t *)data;
|
||||
output->online[CMD_SRC_REF] = CMD_REF_IsOnline(ref);
|
||||
output->ref = *ref;
|
||||
return CMD_OK;
|
||||
}
|
||||
|
||||
bool CMD_REF_IsOnline(void *data) {
|
||||
CMD_RawInput_REF_t *ref = (CMD_RawInput_REF_t *)data;
|
||||
return !(ref->chassis.ref_status == REF_STATUS_OFFLINE&&
|
||||
ref->ai.ref_status == REF_STATUS_OFFLINE&&
|
||||
ref->cap.ref_status == REF_STATUS_OFFLINE&&
|
||||
ref->shoot.ref_status == REF_STATUS_OFFLINE);
|
||||
}
|
||||
|
||||
CMD_DEFINE_ADAPTER(REF, cmd_ref, CMD_SRC_REF, CMD_REF_AdapterInit, CMD_REF_GetInput, CMD_REF_IsOnline)
|
||||
|
||||
#endif /* CMD_ENABLE_SRC_REF */
|
||||
/* ========================================================================== */
|
||||
/* 适配器管理实现 */
|
||||
/* ========================================================================== */
|
||||
|
||||
@ -20,10 +20,21 @@
|
||||
|
||||
// 机器人参数配置
|
||||
Config_RobotParam_t robot_config = {
|
||||
.ref_screen={
|
||||
.width=1920,
|
||||
.height=1080,
|
||||
},
|
||||
.ai_param = {
|
||||
.can = BSP_FDCAN_2,
|
||||
.vision_id = 0x104,
|
||||
},
|
||||
.aimbot_param = {
|
||||
/* 下层板接收AI指令帧 ID（上层板发→下层板收） */
|
||||
.can = BSP_FDCAN_2,
|
||||
.cmd_id = 0x250u,
|
||||
/* 下层板反馈帧起始 ID（占连续6个ID:0x251-0x256） */
|
||||
.fb_base_id = 0x251u,
|
||||
},
|
||||
.imu_param = {
|
||||
.can=BSP_FDCAN_2,
|
||||
.accl_id=0x100,
|
||||
@ -196,11 +207,11 @@ Config_RobotParam_t robot_config = {
|
||||
},
|
||||
.mech_zero = {
|
||||
.yaw = 0.0f,
|
||||
.pit = 3.15f,
|
||||
.pit = 0.9f,
|
||||
},
|
||||
.travel = {
|
||||
.yaw = -1.0f,
|
||||
.pit = 0.6f,
|
||||
.pit = 0.65f,
|
||||
},
|
||||
.low_pass_cutoff_freq = {
|
||||
.out = -1.0f,
|
||||
@ -237,6 +248,13 @@ Config_RobotParam_t robot_config = {
|
||||
.threshold=310.0f,
|
||||
.suspectedTime=0.5f,
|
||||
},
|
||||
.heatControl={
|
||||
.enable=true,
|
||||
.nmax=2.0f, // 最大射频 Hz
|
||||
.Hwarn=200.0f, // 热量预警值
|
||||
.Hsatu=100.0f, // 热量饱和值
|
||||
.Hthres=50.0f, // 热量阈值
|
||||
},
|
||||
.motor={
|
||||
.fric = {
|
||||
{
|
||||
|
||||
@ -20,6 +20,7 @@ extern "C" {
|
||||
#include "component/PowerControl.h"
|
||||
#include "device/gimbal_imu.h"
|
||||
#include "module/vision_bridge.h"
|
||||
#include "module/aimbot.h"
|
||||
|
||||
typedef struct {
|
||||
Shoot_Params_t shoot_param;
|
||||
@ -29,6 +30,7 @@ typedef struct {
|
||||
CMD_Config_t cmd_param;
|
||||
GIMBAL_IMU_Param_t imu_param;
|
||||
AI_Param_t ai_param;
|
||||
Aimbot_Param_t aimbot_param; /* 下层板 ↔ 上层板 CAN 通信参数 */
|
||||
Referee_Screen_t ref_screen;
|
||||
} Config_RobotParam_t;
|
||||
|
||||
|
||||
@ -56,6 +56,9 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
|
||||
g->setpoint.eulr.yaw = g->feedback.motor.yaw.rotor_abs_angle;
|
||||
} else if (mode == GIMBAL_MODE_RELATIVE) {
|
||||
g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
|
||||
} else if (mode == GIMBAL_MODE_AI_CONTROL) {
|
||||
g->setpoint.eulr.yaw = g->feedback.motor.yaw.rotor_abs_angle;
|
||||
g->setpoint.eulr.pit = g->feedback.motor.pit.rotor_abs_angle;
|
||||
}
|
||||
|
||||
g->mode = mode;
|
||||
@ -237,7 +240,11 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
|
||||
CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI);
|
||||
// g->setpoint.eulr.pit = (g_cmd->delta_pit)/3.0f + 2.98f + 1/3.0f; //2.98f为机械零点位置
|
||||
|
||||
if (g_cmd->is_ai) {
|
||||
if (g_cmd->mode == GIMBAL_MODE_AI_CONTROL) {
|
||||
/* AI 模式：直接从指令获取角度设定值（每周期刷新） */
|
||||
g->setpoint.eulr.yaw = g_cmd->setpoint_yaw;
|
||||
g->setpoint.eulr.pit = g_cmd->setpoint_pit;
|
||||
} else if (g_cmd->is_ai) {
|
||||
g->setpoint.eulr.yaw = g_cmd->setpoint_yaw;
|
||||
g->setpoint.eulr.pit = g_cmd->setpoint_pit;
|
||||
}
|
||||
@ -247,7 +254,7 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
|
||||
case GIMBAL_MODE_RELAX:
|
||||
g->out.yaw = 0.0f;
|
||||
g->out.pit = 0.0f;
|
||||
break;
|
||||
break;
|
||||
|
||||
case GIMBAL_MODE_ABSOLUTE:
|
||||
yaw_omega_set_point = PID_Calc(&(g->pid.absolute.yaw_angle), g->setpoint.eulr.yaw,
|
||||
@ -281,6 +288,37 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
|
||||
g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw);
|
||||
g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit);
|
||||
break;
|
||||
|
||||
case GIMBAL_MODE_AI_CONTROL:
|
||||
/* 位置环 → 速度设定值 + 速度前馈 */
|
||||
yaw_omega_set_point = PID_Calc(&(g->pid.absolute.yaw_angle),
|
||||
g->setpoint.eulr.yaw,
|
||||
g->feedback.motor.yaw.rotor_abs_angle,
|
||||
0.0f, g->dt)
|
||||
+ g_cmd->ff_vel_yaw;
|
||||
|
||||
/* 速度环 + 加速度前馈 → yaw 输出 */
|
||||
g->out.yaw = PID_Calc(&(g->pid.absolute.yaw_omega), yaw_omega_set_point,
|
||||
g->feedback.imu.gyro.z, 0.0f, g->dt)
|
||||
+ g_cmd->ff_accl_yaw;
|
||||
|
||||
/* pit 位置环 → 速度设定值 + 速度前馈 */
|
||||
pit_omega_set_point = PID_Calc(&(g->pid.absolute.pit_angle),
|
||||
g->setpoint.eulr.pit,
|
||||
g->feedback.motor.pit.rotor_abs_angle,
|
||||
0.0f, g->dt)
|
||||
+ g_cmd->ff_vel_pit;
|
||||
|
||||
/* 速度环 + 加速度前馈 → pit 输出 */
|
||||
g->out.pit = PID_Calc(&(g->pid.absolute.pit_omega), pit_omega_set_point,
|
||||
g->feedback.imu.gyro.x, 0.0f, g->dt)
|
||||
+ g_cmd->ff_accl_pit;
|
||||
|
||||
/* 输出滤波 */
|
||||
g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw);
|
||||
g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit);
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
return 0;
|
||||
@ -310,6 +348,6 @@ void Gimbal_Output(Gimbal_t *g){
|
||||
* @param g 云台结构体
|
||||
* @param ui UI结构体
|
||||
*/
|
||||
void Gimbal_DumpUI(const Gimbal_t *g, Referee_GimbalUI_t *ui) {
|
||||
void Gimbal_DumpUI(const Gimbal_t *g, Gimbal_RefereeUI_t *ui) {
|
||||
ui->mode = g->mode;
|
||||
}
|
||||
|
||||
@ -29,22 +29,26 @@ typedef enum {
|
||||
GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
|
||||
GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
|
||||
GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
|
||||
GIMBAL_MODE_AI_CONTROL,
|
||||
GIMBAL_MODE_NUM
|
||||
} Gimbal_Mode_t;
|
||||
|
||||
/* UI 导出结构（供 referee 系统绘制） */
|
||||
typedef struct {
|
||||
Gimbal_Mode_t mode;
|
||||
} Referee_GimbalUI_t;
|
||||
|
||||
} Gimbal_RefereeUI_t;
|
||||
typedef struct {
|
||||
Gimbal_Mode_t mode;
|
||||
float delta_yaw;
|
||||
float delta_pit;
|
||||
float feedforward_pit;
|
||||
float feedforward_pit;
|
||||
float setpoint_yaw;
|
||||
float setpoint_pit;
|
||||
bool is_ai;
|
||||
/* GIMBAL_MODE_AI_CONTROL 速度/加速度前馈（来自 NUC） */
|
||||
float ff_vel_yaw; /* yaw 速度前馈（rad/s），叠加到内环角速度设定值 */
|
||||
float ff_vel_pit; /* pit 速度前馈（rad/s） */
|
||||
float ff_accl_yaw; /* yaw 加速度前馈（归一化），叠加到力矩输出 */
|
||||
float ff_accl_pit; /* pit 加速度前馈（归一化） */
|
||||
} Gimbal_CMD_t;
|
||||
|
||||
/* 软件限位 */
|
||||
@ -203,7 +207,7 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd);
|
||||
*/
|
||||
void Gimbal_Output(Gimbal_t *g);
|
||||
|
||||
void Gimbal_DumpUI(const Gimbal_t *g, Referee_GimbalUI_t *ui);
|
||||
void Gimbal_DumpUI(const Gimbal_t *g, Gimbal_RefereeUI_t *ui);
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -42,6 +42,12 @@ void Task(void *argument) {
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
#define MAX_FRIC_RPM 7000.0f
|
||||
#define MAX_TRIG_RPM 1500.0f//这里可能也会影响最高发射频率，待测试
|
||||
|
||||
/* 发射检测参数 */
|
||||
#define SHOT_DETECT_RPM_DROP_THRESHOLD 100.0f /* 摩擦轮转速下降阈值，单位rpm */
|
||||
#define SHOT_DETECT_SUSPECTED_TIME 0.0005f /* 发射嫌疑持续时间阈值，单位秒 */
|
||||
#define FRIC_READY_RPM_RATIO 0.95f /* 摩擦轮准备好的转速比例 */
|
||||
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
static bool last_firecmd;
|
||||
@ -183,6 +189,309 @@ int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed)
|
||||
return SHOOT_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief 更新热量控制数据（使用融合后的热量值）
|
||||
*
|
||||
* \param s 包含发射数据的结构体
|
||||
*
|
||||
* \return 函数运行结果
|
||||
*/
|
||||
static int8_t Shoot_UpdateHeatControl(Shoot_t *s)
|
||||
{
|
||||
if (s == NULL) {
|
||||
return SHOOT_ERR_NULL;
|
||||
}
|
||||
|
||||
/* 使用融合后的热量值计算剩余热量 */
|
||||
s->heatcontrol.Hres = s->heatcontrol.Hmax - s->heatcontrol.Hnow_fused;
|
||||
|
||||
/* 计算冷却射频 ncd = Hcd / Hgen */
|
||||
if (s->heatcontrol.Hgen > 0.0f) {
|
||||
s->heatcontrol.ncd = s->heatcontrol.Hcd / s->heatcontrol.Hgen;
|
||||
} else {
|
||||
s->heatcontrol.ncd = 0.0f;
|
||||
}
|
||||
|
||||
return SHOOT_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief 计算摩擦轮平均转速
|
||||
*
|
||||
* \param s 包含发射数据的结构体
|
||||
*
|
||||
* \return 平均转速
|
||||
*/
|
||||
static float Shoot_CalcAvgFricRpm(Shoot_t *s)
|
||||
{
|
||||
if (s == NULL) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
float sum = 0.0f;
|
||||
uint8_t fric_num = s->param->basic.fric_num;
|
||||
|
||||
for (int i = 0; i < fric_num; i++) {
|
||||
sum += fabs(s->var_fric.fil_rpm[i]);
|
||||
}
|
||||
|
||||
return (fric_num > 0) ? (sum / fric_num) : 0.0f;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief 发射检测：通过摩擦轮掉速检测发射
|
||||
*
|
||||
* \param s 包含发射数据的结构体
|
||||
*
|
||||
* \return 是否检测到掉速超过阈值
|
||||
*/
|
||||
static bool Shoot_DetectShotByRpmDrop(Shoot_t *s)
|
||||
{
|
||||
if (s == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
/* 只在READY和FIRE状态下检测，避免停机时误判 */
|
||||
if (s->running_state != SHOOT_STATE_READY &&
|
||||
s->running_state != SHOOT_STATE_FIRE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
/* 计算当前摩擦轮平均转速 */
|
||||
s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s);
|
||||
|
||||
/* 计算当前转速与目标转速的差值（掉速量） */
|
||||
s->heatcontrol.rpmDrop = s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm;
|
||||
|
||||
/* 判断是否发生明显掉速 */
|
||||
if (s->heatcontrol.rpmDrop > SHOT_DETECT_RPM_DROP_THRESHOLD) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* \brief 热量数据融合：用裁判系统数据修正自主估计
|
||||
*
|
||||
* \param s 包含发射数据的结构体
|
||||
*
|
||||
* \return 函数运行结果
|
||||
*
|
||||
* \note 裁判系统数据准确但更新慢，自主计算实时但可能误差累积
|
||||
* 只在裁判系统数据更新时才修正估计值，保证准确性和实时性
|
||||
*/
|
||||
static int8_t Shoot_FuseHeatData(Shoot_t *s)
|
||||
{
|
||||
if (s == NULL) {
|
||||
return SHOOT_ERR_NULL;
|
||||
}
|
||||
|
||||
/* 如果裁判系统数据有效（Hmax > 0） */
|
||||
if (s->heatcontrol.Hmax > 0.0f && s->heatcontrol.Hnow >= 0.0f) {
|
||||
/* 检测裁判系统数据是否有更新 */
|
||||
if (s->heatcontrol.Hnow != s->heatcontrol.Hnow_last) {
|
||||
/* 数据更新了，用裁判系统数据修正估计值 */
|
||||
s->heatcontrol.Hnow_estimated = s->heatcontrol.Hnow;
|
||||
s->heatcontrol.Hnow_last = s->heatcontrol.Hnow; /* 记录本次值 */
|
||||
}
|
||||
|
||||
/* 融合值就是裁判系统值（作为基准） */
|
||||
s->heatcontrol.Hnow_fused = s->heatcontrol.Hnow;
|
||||
} else {
|
||||
/* 裁判系统数据无效，仅使用自主估计值 */
|
||||
s->heatcontrol.Hnow_fused = s->heatcontrol.Hnow_estimated;
|
||||
}
|
||||
|
||||
return SHOOT_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief 计算可发射弹数
|
||||
*
|
||||
* \param s 包含发射数据的结构体
|
||||
*
|
||||
* \return 函数运行结果
|
||||
*/
|
||||
static int8_t Shoot_CalcAvailableShots(Shoot_t *s)
|
||||
{
|
||||
if (s == NULL) {
|
||||
return SHOOT_ERR_NULL;
|
||||
}
|
||||
|
||||
/* 计算剩余热量 */
|
||||
float heat_available = s->heatcontrol.Hmax - s->heatcontrol.Hnow_fused;
|
||||
|
||||
/* 计算可发射弹数 */
|
||||
if (s->heatcontrol.Hgen > 0.0f && heat_available > 0.0f) {
|
||||
s->heatcontrol.shots_available = (uint16_t)(heat_available / s->heatcontrol.Hgen);
|
||||
} else {
|
||||
s->heatcontrol.shots_available = 0;
|
||||
}
|
||||
|
||||
return SHOOT_OK;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* \brief 热量检测状态机
|
||||
*
|
||||
* \param s 包含发射数据的结构体
|
||||
*
|
||||
* \return 函数运行结果
|
||||
*/
|
||||
static int8_t Shoot_HeatDetectionFSM(Shoot_t *s)
|
||||
{
|
||||
if (s == NULL) {
|
||||
return SHOOT_ERR_NULL;
|
||||
}
|
||||
|
||||
switch (s->heatcontrol.detectState) {
|
||||
case SHOOT_HEAT_DETECT_IDLE:
|
||||
/* 停机状态：等待摩擦轮启动并加速到目标速度附近 */
|
||||
if (s->running_state == SHOOT_STATE_READY ||
|
||||
s->running_state == SHOOT_STATE_FIRE) {
|
||||
/* 计算当前平均转速 */
|
||||
s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s);
|
||||
|
||||
/* 检查摩擦轮是否达到目标转速的85%以上 */
|
||||
if (s->heatcontrol.avgFricRpm >= s->target_variable.fric_rpm * FRIC_READY_RPM_RATIO) {
|
||||
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY;
|
||||
s->heatcontrol.lastAvgFricRpm = s->heatcontrol.avgFricRpm;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case SHOOT_HEAT_DETECT_READY:
|
||||
/* 准备检测状态：监测摩擦轮掉速 */
|
||||
/* 检测掉速（当前转速低于目标转速超过阈值） */
|
||||
if (Shoot_DetectShotByRpmDrop(s)) {
|
||||
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_SUSPECTED;
|
||||
s->heatcontrol.detectTime = s->timer.now; /* 记录进入嫌疑状态的时间 */
|
||||
}
|
||||
|
||||
/* 如果摩擦轮停止 */
|
||||
if (s->running_state == SHOOT_STATE_IDLE) {
|
||||
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_IDLE;
|
||||
}
|
||||
break;
|
||||
|
||||
case SHOOT_HEAT_DETECT_SUSPECTED:
|
||||
/* 发射嫌疑状态：持续检测掉速 */
|
||||
/* 更新掉速量 */
|
||||
s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s);
|
||||
s->heatcontrol.rpmDrop = s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm;
|
||||
|
||||
/* 若掉速消失（转速恢复正常），回到准备状态 */
|
||||
if (s->heatcontrol.rpmDrop < SHOT_DETECT_RPM_DROP_THRESHOLD) {
|
||||
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY;
|
||||
}
|
||||
/* 若嫌疑状态持续超过阈值时间，确认发射 */
|
||||
else if ((s->timer.now - s->heatcontrol.detectTime) >= SHOT_DETECT_SUSPECTED_TIME) {
|
||||
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_CONFIRMED;
|
||||
}
|
||||
break;
|
||||
|
||||
case SHOOT_HEAT_DETECT_CONFIRMED:
|
||||
/* 确认发射状态：增加热量并返回准备状态 */
|
||||
/* 根据弹丸类型增加估计热量 */
|
||||
switch (s->param->basic.projectileType) {
|
||||
case SHOOT_PROJECTILE_17MM:
|
||||
s->heatcontrol.Hnow_estimated += 10.0f;
|
||||
break;
|
||||
case SHOOT_PROJECTILE_42MM:
|
||||
s->heatcontrol.Hnow_estimated += 100.0f;
|
||||
break;
|
||||
default:
|
||||
s->heatcontrol.Hnow_estimated += s->heatcontrol.Hgen;
|
||||
break;
|
||||
}
|
||||
|
||||
/* 限制估计热量不超过最大值 */
|
||||
if (s->heatcontrol.Hnow_estimated > s->heatcontrol.Hmax) {
|
||||
s->heatcontrol.Hnow_estimated = s->heatcontrol.Hmax;
|
||||
}
|
||||
|
||||
/* 增加发射计数 */
|
||||
s->heatcontrol.shots_detected++;
|
||||
|
||||
/* 返回准备检测状态 */
|
||||
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY;
|
||||
break;
|
||||
|
||||
default:
|
||||
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_IDLE;
|
||||
break;
|
||||
}
|
||||
|
||||
/* 善后工作：热量冷却（每个周期都执行） */
|
||||
if (s->heatcontrol.Hnow_estimated > 0.0f && s->heatcontrol.Hcd > 0.0f) {
|
||||
s->heatcontrol.Hnow_estimated -= s->heatcontrol.Hcd * s->timer.dt;
|
||||
if (s->heatcontrol.Hnow_estimated < 0.0f) {
|
||||
s->heatcontrol.Hnow_estimated = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
/* 数据融合 */
|
||||
Shoot_FuseHeatData(s);
|
||||
|
||||
/* 计算可发射弹数 */
|
||||
Shoot_CalcAvailableShots(s);
|
||||
|
||||
return SHOOT_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief 根据热量控制计算射频
|
||||
*
|
||||
* \param s 包含发射数据的结构体
|
||||
*
|
||||
* \return 计算出的射频值，单位Hz
|
||||
*/
|
||||
static float Shoot_CaluFreqByHeat(Shoot_t *s)
|
||||
{
|
||||
if (s == NULL) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
/* 如果热量控制未启用，返回配置的射频 */
|
||||
if (!s->param->heatControl.enable) {
|
||||
return s->param->basic.shot_freq;
|
||||
}
|
||||
|
||||
/* 检查Hmax和Hcd是否有效 */
|
||||
if (s->heatcontrol.Hmax <= 0.0f || s->heatcontrol.Hcd <= 0.0f) {
|
||||
return s->param->basic.shot_freq; /* 数据无效，使用默认射频 */
|
||||
}
|
||||
|
||||
float Hres = s->heatcontrol.Hres;
|
||||
float Hwarn = s->param->heatControl.Hwarn;
|
||||
float Hsatu = s->param->heatControl.Hsatu;
|
||||
float Hthres = s->param->heatControl.Hthres;
|
||||
float nmax = s->param->heatControl.nmax;
|
||||
float ncd = s->heatcontrol.ncd;
|
||||
|
||||
/* 剩余热量大于预警值：最大射频 */
|
||||
if (Hres > Hwarn) {
|
||||
return nmax;
|
||||
}
|
||||
/* 剩余热量在预警值和饱和值之间：线性映射 */
|
||||
else if (Hres > Hsatu) {
|
||||
/* 线性插值: n = ncd + (nmax - ncd) * (Hres - Hsatu) / (Hwarn - Hsatu) */
|
||||
float ratio = (Hres - Hsatu) / (Hwarn - Hsatu);
|
||||
return ncd + (nmax - ncd) * ratio;
|
||||
}
|
||||
/* 剩余热量在饱和值和阈值之间：冷却射频 */
|
||||
else if (Hres > Hthres) {
|
||||
return ncd;
|
||||
}
|
||||
/* 剩余热量小于阈值：停止射击 */
|
||||
else {
|
||||
return 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief 根据发射弹丸数量及发射频率计算拨弹电机目标角度
|
||||
*
|
||||
@ -196,10 +505,19 @@ int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
|
||||
if (s == NULL || s->var_trig.num_toShoot == 0) {
|
||||
return SHOOT_ERR_NULL;
|
||||
}
|
||||
|
||||
/* 更新热量控制数据 */
|
||||
Shoot_UpdateHeatControl(s);
|
||||
|
||||
/* 根据热量控制计算实际射频 */
|
||||
float actual_freq = Shoot_CaluFreqByHeat(s);
|
||||
|
||||
float dt = s->timer.now - s->var_trig.time_lastShoot;
|
||||
float dpos;
|
||||
dpos = CircleError(s->target_variable.trig_angle, s->var_trig.trig_agl, M_2PI);
|
||||
if(dt >= 1.0f/s->param->basic.shot_freq && cmd->firecmd && dpos<=1.0f)
|
||||
|
||||
/* 使用热量控制计算出的射频，而不是配置的固定射频 */
|
||||
if(actual_freq > 0.0f && dt >= 1.0f/actual_freq && cmd->firecmd && dpos<=1.0f)
|
||||
{
|
||||
s->var_trig.time_lastShoot=s->timer.now;
|
||||
CircleAdd(&s->target_variable.trig_angle, M_2PI/s->param->basic.num_trig_tooth, M_2PI);
|
||||
@ -623,6 +941,10 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
|
||||
|
||||
/* 归零变量 */
|
||||
memset(&s->var_trig,0,sizeof(s->var_trig));
|
||||
|
||||
/* 初始化热量控制 */
|
||||
memset(&s->heatcontrol, 0, sizeof(s->heatcontrol));
|
||||
|
||||
return SHOOT_OK;
|
||||
}
|
||||
|
||||
@ -644,6 +966,10 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
|
||||
s->timer.lask_wakeup = BSP_TIME_Get_us();
|
||||
Shoot_CaluTargetRPM(s,233);
|
||||
|
||||
/* 运行热量检测状态机 */
|
||||
Shoot_HeatDetectionFSM(s);
|
||||
|
||||
/* 运行卡弹检测状态机 */
|
||||
Shoot_JamDetectionFSM(s, cmd);
|
||||
// Shoot_CalufeedbackRPM(s);
|
||||
return SHOOT_OK;
|
||||
@ -655,7 +981,7 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
|
||||
* @param s 射击结构体
|
||||
* @param ui UI结构体
|
||||
*/
|
||||
void Shoot_DumpUI(Shoot_t *s, Referee_ShootUI_t *ui) {
|
||||
void Shoot_DumpUI(Shoot_t *s, Shoot_RefereeUI_t *ui) {
|
||||
ui->mode = s->mode;
|
||||
ui->fire = s->running_state;
|
||||
}
|
||||
|
||||
@ -30,6 +30,13 @@ typedef enum {
|
||||
SHOOT_JAMFSM_STATE_CONFIRMED, /* 确认状态 */
|
||||
SHOOT_JAMFSM_STATE_DEAL /* 处理状态 */
|
||||
}Shoot_JamDetectionFSM_State_t;
|
||||
|
||||
typedef enum {
|
||||
SHOOT_HEAT_DETECT_IDLE = 0, /* 停机状态 */
|
||||
SHOOT_HEAT_DETECT_READY, /* 准备检测状态 */
|
||||
SHOOT_HEAT_DETECT_SUSPECTED, /* 发射嫌疑状态 */
|
||||
SHOOT_HEAT_DETECT_CONFIRMED /* 确认发射状态 */
|
||||
}Shoot_HeatDetectionFSM_State_t;
|
||||
typedef enum {
|
||||
SHOOT_STATE_IDLE = 0,/* 熄火 */
|
||||
SHOOT_STATE_READY, /* 准备射击 */
|
||||
@ -48,7 +55,7 @@ typedef enum {
|
||||
typedef struct {
|
||||
Shoot_Mode_t mode;
|
||||
Shoot_Running_State_t fire;
|
||||
} Referee_ShootUI_t;
|
||||
} Shoot_RefereeUI_t;
|
||||
|
||||
typedef enum {
|
||||
SHOOT_PROJECTILE_17MM,
|
||||
@ -88,6 +95,36 @@ typedef struct {
|
||||
float lastTime;/* 用于记录怀疑状态或处理状态的开始时间 */
|
||||
Shoot_JamDetectionFSM_State_t fsmState; /* 卡弹检测状态机 */
|
||||
}Shoot_JamDetection_t;
|
||||
|
||||
typedef struct {
|
||||
/* 从裁判系统读取的常量 */
|
||||
float Hmax; // 热量上限
|
||||
float Hcd; // 热量冷却速度
|
||||
float Hgen; // 每发射一发产生的热量
|
||||
|
||||
/* 实时数据 */
|
||||
float Hnow; // 当前热量（从裁判系统实时读取）
|
||||
float Hnow_last; // 上次裁判系统热量值（用于检测更新）
|
||||
float Hres; // 剩余热量 (Hmax - Hnow)
|
||||
|
||||
/* 控制变量 */
|
||||
float ncd; // 冷却射频（消耗热量 = 自然恢复热量）
|
||||
|
||||
/* 发射检测状态机 */
|
||||
Shoot_HeatDetectionFSM_State_t detectState; // 检测状态
|
||||
float detectTime; // 检测计时器
|
||||
float avgFricRpm; // 摩擦轮平均转速
|
||||
float lastAvgFricRpm; // 上次摩擦轮平均转速
|
||||
float rpmDrop; // 转速下降量
|
||||
bool shotDetected; // 检测到发射标志
|
||||
|
||||
/* 自主热量估计 */
|
||||
float Hnow_estimated; // 估计的当前热量
|
||||
float Hnow_fused; // 融合后的热量值
|
||||
uint16_t shots_detected; // 检测到的发射数
|
||||
uint16_t shots_available;// 当前热量可发射弹数
|
||||
}Shoot_HeatControl_t;
|
||||
|
||||
typedef struct {
|
||||
float out_follow[MAX_FRIC_NUM];
|
||||
float out_err[MAX_FRIC_NUM];
|
||||
@ -120,6 +157,13 @@ typedef struct {
|
||||
float threshold; /* 卡弹检测阈值，单位A (dji2006建议设置为120A，dji3508建议设置为235A,根据实际测试调整)*/
|
||||
float suspectedTime;/* 卡弹怀疑时间，单位秒 */
|
||||
}jamDetection;/* 卡弹检测参数 */
|
||||
struct {
|
||||
bool enable; /* 是否启用热量控制 */
|
||||
float nmax;//最大射频
|
||||
float Hwarn;//热量预警值
|
||||
float Hsatu;//热量饱和值
|
||||
float Hthres;//热量阈值，超过这个值将无法射击
|
||||
}heatControl;/* 热量控制参数 */
|
||||
struct {
|
||||
Shoot_MOTOR_RM_Param_t fric[MAX_FRIC_NUM];
|
||||
MOTOR_RM_Param_t trig;
|
||||
@ -164,6 +208,7 @@ typedef struct {
|
||||
/* 控制信息*/
|
||||
Shoot_Running_State_t running_state; /* 运行状态机 */
|
||||
Shoot_JamDetection_t jamdetection; /* 卡弹检测控制信息 */
|
||||
Shoot_HeatControl_t heatcontrol; /* 热量控制信息 */
|
||||
Shoot_VARSForFricCtrl_t var_fric; /* 摩擦轮控制信息 */
|
||||
Shoot_VARSForTrigCtrl_t var_trig; /* 角度计算控制信息 */
|
||||
Shoot_Output_t output; /* 输出信息 */
|
||||
@ -245,7 +290,7 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd);
|
||||
* @param s 射击结构体
|
||||
* @param ui UI结构体
|
||||
*/
|
||||
void Shoot_DumpUI(Shoot_t *s, Referee_ShootUI_t *ui);
|
||||
void Shoot_DumpUI(Shoot_t *s, Shoot_RefereeUI_t *ui);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
|
||||
@ -1,29 +1,35 @@
|
||||
/*
|
||||
ai Task
|
||||
|
||||
通路：上位机 <--串口--> 上层板 <--CAN--> 下层板（本文件）
|
||||
下层板职责：
|
||||
1. 将云台/IMU 反馈数据通过 CAN 发送给上层板
|
||||
2. 从 CAN 解析上层板转发的 AI 控制指令
|
||||
3. 将解析结果写入 cmd.nuc 队列供 cmd task 消费
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
// #include "module/vision_bridge.h"
|
||||
// #include "component/ahrs/ahrs.h"
|
||||
// #include "module/gimbal.h"
|
||||
// #include "module/shoot.h"
|
||||
#include "module/aimbot.h"
|
||||
#include "module/config.h"
|
||||
#include "module/vision_bridge.h"
|
||||
#include "component/ahrs/ahrs.h"
|
||||
#include "module/gimbal.h"
|
||||
#include "device/referee_proto_types.h"
|
||||
/* USER INCLUDE END */
|
||||
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
/* USER STRUCT BEGIN */
|
||||
// AI_t ai;
|
||||
// AI_cmd_t aiToCmd;
|
||||
|
||||
// AI_RawData_t rawdata;
|
||||
// AHRS_Quaternion_t quat_from_imu;
|
||||
// Gimbal_Feedback_t rawdata_from_gimbal;
|
||||
static Aimbot_Param_t aimbot_can; /* CAN 通信参数（从 config 拷贝） */
|
||||
static Aimbot_FeedbackData_t aimbot_fb; /* 打包好的反馈数据 */
|
||||
static Aimbot_AIResult_t aimbot_cmd; /* 从 CAN 解析到的 AI 指令 */
|
||||
static Gimbal_Feedback_t ai_gimbal_fb; /* 来自云台任务的反馈 */
|
||||
static AHRS_Quaternion_t ai_imu_quat; /* 来自姿态估计任务的四元数 */
|
||||
static Referee_ForAI_t ai_ref; /* 裁判系统转发给 AI 的数据 */
|
||||
static AI_cmd_t ai_cmd_for_nuc; /* 转换后发往 cmd.nuc 的指令 */
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
@ -31,7 +37,6 @@
|
||||
void Task_ai(void *argument) {
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
|
||||
/* 计算任务运行到指定频率需要等待的tick数 */
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / AI_FREQ;
|
||||
|
||||
@ -39,33 +44,47 @@ void Task_ai(void *argument) {
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||
/* USER CODE INIT BEGIN */
|
||||
|
||||
aimbot_can = Config_GetRobotParam()->aimbot_param;
|
||||
Aimbot_Init(&aimbot_can);
|
||||
/* USER CODE INIT END */
|
||||
|
||||
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
|
||||
// osMessageQueueGet(task_runtime.msgq.ai.rawdata_FromGimbal, &rawdata_from_gimbal, NULL, 0);
|
||||
// osMessageQueueGet(task_runtime.msgq.ai.rawdata_FromIMU, &quat_from_imu, NULL, 0);
|
||||
// rawdata.mode=1;
|
||||
// rawdata.pitch=rawdata_from_gimbal.motor.pit.rotor_abs_angle;
|
||||
// rawdata.yaw=rawdata_from_gimbal.motor.yaw.rotor_abs_angle;
|
||||
// rawdata.pitch_vel=rawdata_from_gimbal.motor.pit.rotor_speed;
|
||||
// rawdata.yaw_vel=rawdata_from_gimbal.motor.yaw.rotor_speed;
|
||||
// rawdata.q[0]=quat_from_imu.q0;
|
||||
// rawdata.q[1]=quat_from_imu.q1;
|
||||
// rawdata.q[2]=quat_from_imu.q2;
|
||||
// rawdata.q[3]=quat_from_imu.q3;
|
||||
// AI_ParseForHost(&ai,&rawdata);
|
||||
// AI_StartSend(&ai);
|
||||
|
||||
// AI_StartReceiving(&ai);
|
||||
// AI_Get(&ai,&aiToCmd);
|
||||
// osMessageQueueReset(task_runtime.msgq.cmd.nuc);
|
||||
// osMessageQueuePut(task_runtime.msgq.cmd.nuc, &aiToCmd, 0, 0);
|
||||
|
||||
/* 读取裁判系统状态（非阻塞） */
|
||||
osMessageQueueGet(task_runtime.msgq.ai.ref, &ai_ref, NULL, 0);
|
||||
|
||||
/* 读取云台反馈数据（非阻塞） */
|
||||
osMessageQueueGet(task_runtime.msgq.ai.rawdata_FromGimbal, &ai_gimbal_fb, NULL, 0);
|
||||
|
||||
/* 读取 IMU 四元数（非阻塞） */
|
||||
osMessageQueueGet(task_runtime.msgq.ai.rawdata_FromIMU, &ai_imu_quat, NULL, 0);
|
||||
|
||||
/* 打包反馈并通过 CAN 发给上层板（6 帧）
|
||||
* bullet_speed / bullet_count 暂无独立队列，置零；
|
||||
* 如有裁判系统弹速信息可在此处填入。 */
|
||||
Aimbot_PackFeedback(&aimbot_fb, &ai_gimbal_fb, &ai_imu_quat,
|
||||
0.0f, 0u,
|
||||
(uint8_t)(ai_ref.ref_status == REF_STATUS_RUNNING ? 1u : 0u));
|
||||
Aimbot_SendFeedbackOnCAN(&aimbot_can, &aimbot_fb);
|
||||
|
||||
/* 解析上层板发来的 AI 指令（非阻塞） */
|
||||
if (Aimbot_ParseCmdFromCAN(&aimbot_can, &aimbot_cmd) == DEVICE_OK) {
|
||||
/* 将 Aimbot_AIResult_t 转换为 AI_cmd_t 并推送到 cmd.nuc 队列 */
|
||||
ai_cmd_for_nuc.mode = aimbot_cmd.mode;
|
||||
ai_cmd_for_nuc.gimbal.setpoint.yaw = aimbot_cmd.gimbal_t.setpoint.yaw;
|
||||
ai_cmd_for_nuc.gimbal.setpoint.pit = aimbot_cmd.gimbal_t.setpoint.pit;
|
||||
ai_cmd_for_nuc.gimbal.vel.yaw = aimbot_cmd.gimbal_t.vel.yaw;
|
||||
ai_cmd_for_nuc.gimbal.vel.pit = aimbot_cmd.gimbal_t.vel.pit;
|
||||
ai_cmd_for_nuc.gimbal.accl.yaw = aimbot_cmd.gimbal_t.accl.yaw;
|
||||
ai_cmd_for_nuc.gimbal.accl.pit = aimbot_cmd.gimbal_t.accl.pit;
|
||||
|
||||
osMessageQueueReset(task_runtime.msgq.cmd.nuc);
|
||||
osMessageQueuePut(task_runtime.msgq.cmd.nuc, &ai_cmd_for_nuc, 0, 0);
|
||||
}
|
||||
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
}
|
||||
@ -10,6 +10,7 @@
|
||||
#include "module/config.h"
|
||||
#include "device/supercap.h"
|
||||
#include "module/track.h"
|
||||
#include "device/referee_proto_types.h"
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
@ -21,9 +22,9 @@ Chassis_t chassis;
|
||||
Chassis_CMD_t chassis_cmd;
|
||||
Chassis_IMU_t chassis_imu;
|
||||
Track_CMD_t track_cmd;
|
||||
float max =50.0f;
|
||||
|
||||
Track_t track;
|
||||
Referee_ForChassis_t chassis_ref; /* 裁判系统底盘数据 */
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
@ -53,12 +54,17 @@ void Task_ctrl_chassis(void *argument) {
|
||||
osMessageQueueGet(task_runtime.msgq.chassis.yaw, &encoder_gimbalYawMotor, NULL, 0);
|
||||
osMessageQueueGet(task_runtime.msgq.chassis.cmd, &chassis_cmd, NULL, 0);
|
||||
osMessageQueueGet(task_runtime.msgq.track.cmd, &track_cmd, NULL, 0);
|
||||
osMessageQueueGet(task_runtime.msgq.chassis.ref, &chassis_ref, NULL, 0);
|
||||
chassis.feedback.encoder_gimbalYawMotor=encoder_gimbalYawMotor;
|
||||
|
||||
Chassis_UpdateFeedback(&chassis);
|
||||
Chassis_Control(&chassis, &chassis_cmd, osKernelGetTickCount());
|
||||
|
||||
// Chassis_Power_Control(&chassis,max);
|
||||
/* 功率限制：裁判系统在线时使用下发上限，否则使用保守默认值 */
|
||||
float power_limit = (chassis_ref.ref_status == REF_STATUS_RUNNING)
|
||||
? chassis_ref.chassis_power_limit
|
||||
: 500.0f;
|
||||
Chassis_Power_Control(&chassis, power_limit);
|
||||
|
||||
Chassis_Output(&chassis);
|
||||
|
||||
|
||||
@ -8,6 +8,7 @@
|
||||
/* USER INCLUDE BEGIN */
|
||||
#include "module/shoot.h"
|
||||
#include "module/config.h"
|
||||
#include "device/referee_proto_types.h"
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
@ -17,6 +18,7 @@
|
||||
/* USER STRUCT BEGIN */
|
||||
Shoot_t shoot;
|
||||
Shoot_CMD_t shoot_cmd;
|
||||
Referee_ForShoot_t shoot_ref; /* 裁判系统发射数据 */
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
@ -39,6 +41,16 @@ void Task_ctrl_shoot(void *argument) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
osMessageQueueGet(task_runtime.msgq.shoot.cmd, &shoot_cmd, NULL, 0);
|
||||
osMessageQueueGet(task_runtime.msgq.shoot.ref, &shoot_ref, NULL, 0);
|
||||
|
||||
/* 消费裁判系统发射数据：在线时更新热量控制参数 */
|
||||
if (shoot_ref.ref_status == REF_STATUS_RUNNING) {
|
||||
shoot.heatcontrol.Hmax = (float)shoot_ref.robot_status.shooter_barrel_heat_limit;
|
||||
shoot.heatcontrol.Hcd = (float)shoot_ref.robot_status.shooter_barrel_cooling_value;
|
||||
shoot.heatcontrol.Hnow = (float)shoot_ref.power_heat.shooter_42mm_barrel_heat;
|
||||
shoot.heatcontrol.Hgen = 100.0f; /* 42mm弹丸每发产生热量 */
|
||||
}
|
||||
|
||||
Shoot_UpdateFeedback(&shoot);
|
||||
Shoot_SetMode(&shoot,shoot_cmd.mode);
|
||||
Shoot_Control(&shoot,&shoot_cmd);
|
||||
|
||||
@ -14,6 +14,7 @@
|
||||
#include "module/chassis.h"
|
||||
#include "module/track.h"
|
||||
#include "device/referee.h"
|
||||
#include "module/vision_bridge.h"
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
@ -34,7 +35,10 @@ void Task_Init(void *argument) {
|
||||
|
||||
/* USER CODE INIT END */
|
||||
osKernelLock(); /* 锁定内核，防止任务切换 */
|
||||
|
||||
//Power-en
|
||||
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_13, GPIO_PIN_SET);
|
||||
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_14, GPIO_PIN_SET);
|
||||
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_15, GPIO_PIN_SET);
|
||||
/* 创建任务线程 */
|
||||
task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
|
||||
task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti);
|
||||
@ -44,8 +48,8 @@ void Task_Init(void *argument) {
|
||||
task_runtime.thread.ctrl_shoot = osThreadNew(Task_ctrl_shoot, NULL, &attr_ctrl_shoot);
|
||||
task_runtime.thread.cmd = osThreadNew(Task_cmd, NULL, &attr_cmd);
|
||||
task_runtime.thread.supercap = osThreadNew(Task_supercap, NULL, &attr_supercap);
|
||||
// task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai);
|
||||
task_runtime.thread.referee = osThreadNew(Task_referee, NULL, &attr_referee);
|
||||
task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai);
|
||||
// task_runtime.thread.referee = osThreadNew(Task_referee, NULL, &attr_referee);
|
||||
|
||||
// 创建消息队列
|
||||
/* USER MESSAGE BEGIN */
|
||||
@ -70,6 +74,8 @@ void Task_Init(void *argument) {
|
||||
task_runtime.msgq.ai.rawdata_FromGimbal = osMessageQueueNew(2u, sizeof(Gimbal_Feedback_t), NULL);
|
||||
task_runtime.msgq.ai.rawdata_FromIMU = osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);
|
||||
// task_runtime.msgq.ai.rawdata_FromShoot = osMessageQueueNew(2u, sizeof(), NULL);
|
||||
/* cmd.nuc: AI task -> cmd task 的指令队列 */
|
||||
task_runtime.msgq.cmd.nuc = osMessageQueueNew(2u, sizeof(AI_cmd_t), NULL);
|
||||
/* 裁判系统 */
|
||||
task_runtime.msgq.referee.tocmd.ai= osMessageQueueNew(2u, sizeof(Referee_ForAI_t), NULL);
|
||||
task_runtime.msgq.referee.tocmd.cap= osMessageQueueNew(2u, sizeof(Referee_ForCap_t), NULL);
|
||||
@ -81,10 +87,10 @@ void Task_Init(void *argument) {
|
||||
task_runtime.msgq.ai.ref = osMessageQueueNew(2u, sizeof(Referee_ForAI_t), NULL);
|
||||
task_runtime.msgq.cap.ref = osMessageQueueNew(2u, sizeof(Referee_ForCap_t), NULL);
|
||||
/* UI */
|
||||
task_runtime.msgq.referee.ui.tochassis =osMessageQueueNew(2u, sizeof(Referee_ChassisUI_t), NULL);
|
||||
task_runtime.msgq.referee.ui.tocap =osMessageQueueNew(2u, sizeof(Referee_CapUI_t), NULL);
|
||||
task_runtime.msgq.referee.ui.togimbal =osMessageQueueNew(2u, sizeof(Referee_GimbalUI_t), NULL);
|
||||
task_runtime.msgq.referee.ui.toshoot =osMessageQueueNew(2u, sizeof(Referee_ShootUI_t), NULL);
|
||||
task_runtime.msgq.referee.ui.tochassis =osMessageQueueNew(2u, sizeof(Chassis_RefereeUI_t), NULL);
|
||||
task_runtime.msgq.referee.ui.tocap =osMessageQueueNew(2u, sizeof(Cap_RefereeUI_t), NULL);
|
||||
task_runtime.msgq.referee.ui.togimbal =osMessageQueueNew(2u, sizeof(Gimbal_RefereeUI_t), NULL);
|
||||
task_runtime.msgq.referee.ui.toshoot =osMessageQueueNew(2u, sizeof(Shoot_RefereeUI_t), NULL);
|
||||
task_runtime.msgq.referee.ui.tocmd = osMessageQueueNew(2u, sizeof(bool), NULL);
|
||||
task_runtime.msgq.referee.ui.frcmd = osMessageQueueNew(2u, sizeof(Referee_UI_CMD_t), NULL);
|
||||
/* USER MESSAGE END */
|
||||
|
||||
@ -45,16 +45,16 @@ void Task_referee(void *argument) {
|
||||
/* 初始化裁判系统 */
|
||||
|
||||
Referee_Init(&ref, &ui, &Config_GetRobotParam()->ref_screen);
|
||||
|
||||
/* USER CODE INIT END */
|
||||
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
Referee_StartReceiving(&ref);
|
||||
|
||||
if (osThreadFlagsWait(SIGNAL_REFEREE_RAW_REDY, osFlagsWaitAll, 10) !=
|
||||
SIGNAL_REFEREE_RAW_REDY) {
|
||||
if (osKernelGetTickCount() - last_online_tick > 500)
|
||||
if (osKernelGetTickCount() - last_online_tick > 2500)
|
||||
Referee_HandleOffline(&ref);
|
||||
} else {
|
||||
Referee_Parse(&ref);
|
||||
@ -84,9 +84,11 @@ void Task_referee(void *argument) {
|
||||
|
||||
while (osMessageQueueGet(task_runtime.msgq.referee.ui.frcmd, &ref_cmd, NULL,
|
||||
0) == osOK) {
|
||||
ref_cmd=UI_AUTO_AIM_START;
|
||||
Referee_PraseCmd(&ui, ref_cmd);
|
||||
// Referee_StartSend(send_data, sizeof(send_data));
|
||||
// Referee_StartSend(send_data, sizeof(send_data));
|
||||
}
|
||||
|
||||
Referee_PackUI(&ui, &ref);
|
||||
}
|
||||
|
||||
@ -1,50 +1,64 @@
|
||||
/*
|
||||
supercap Task
|
||||
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
#include "device/supercap.h"
|
||||
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
/* USER STRUCT BEGIN */
|
||||
CAN_SuperCapTXDataTypeDef SuperCap_CanTX;
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
void Task_supercap(void *argument) {
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
|
||||
/* 计算任务运行到指定频率需要等待的tick数 */
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / SUPERCAP_FREQ;
|
||||
|
||||
osDelay(SUPERCAP_INIT_DELAY); /* 延时一段时间再开启任务 */
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||
/* USER CODE INIT BEGIN */
|
||||
SuperCap_Init();
|
||||
SuperCap_CanTX.Enable = 1 ; //超级电容使能。1使能，0失能
|
||||
SuperCap_CanTX.Charge = 1 ; //V1.1超级电容充电。1充电，0放电，在PLUS版本中，此标志位无效，超电的充放电是自动的
|
||||
SuperCap_CanTX.Powerlimit = 120 ; //裁判系统功率限制
|
||||
SuperCap_CanTX.ChargePower = 1 ; //V1.1超级电容充电功率，在PLUS版本中，此参数，超电的充电功率随着底盘功率变化
|
||||
/* USER CODE INIT END */
|
||||
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
SuperCap_Update();
|
||||
CAN_TX_SuperCapData(&SuperCap_CanTX);
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
}
|
||||
/*
|
||||
supercap Task
|
||||
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
#include "device/supercap.h"
|
||||
#include "device/referee_proto_types.h"
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
/* USER STRUCT BEGIN */
|
||||
CAN_SuperCapTXDataTypeDef SuperCap_CanTX;
|
||||
Referee_ForCap_t cap_ref; /* 裁判系统功率数据 */
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
void Task_supercap(void *argument) {
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
|
||||
/* 计算任务运行到指定频率需要等待的tick数 */
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / SUPERCAP_FREQ;
|
||||
|
||||
osDelay(SUPERCAP_INIT_DELAY); /* 延时一段时间再开启任务 */
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||
/* USER CODE INIT BEGIN */
|
||||
SuperCap_Init();
|
||||
|
||||
SuperCap_CanTX.Enable = 1 ; //超级电容使能。1使能，0失能
|
||||
|
||||
SuperCap_CanTX.Charge = 1 ; //V1.1超级电容充电。1充电，0放电，在PLUS版本中，此标志位无效，超电的充放电是自动的
|
||||
|
||||
SuperCap_CanTX.Powerlimit = 120 ; //裁判系统功率限制
|
||||
|
||||
SuperCap_CanTX.ChargePower = 1 ; //V1.1超级电容充电功率，在PLUS版本中，此参数，超电的充电功率随着底盘功率变化
|
||||
|
||||
/* USER CODE INIT END */
|
||||
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
/* 动态更新功率上限：裁判系统在线时使用下发值，否则回退到初始化值120W */
|
||||
osMessageQueueGet(task_runtime.msgq.cap.ref, &cap_ref, NULL, 0);
|
||||
if (cap_ref.ref_status == REF_STATUS_RUNNING && cap_ref.chassis_power_limit > 0.0f) {
|
||||
SuperCap_CanTX.Powerlimit = (uint8_t)cap_ref.chassis_power_limit;
|
||||
}
|
||||
|
||||
SuperCap_Update();
|
||||
|
||||
CAN_TX_SuperCapData(&SuperCap_CanTX);
|
||||
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -27,7 +27,7 @@ const osThreadAttr_t attr_blink = {
|
||||
const osThreadAttr_t attr_ctrl_chassis = {
|
||||
.name = "ctrl_chassis",
|
||||
.priority = osPriorityNormal,
|
||||
.stack_size = 512 * 16,
|
||||
.stack_size = 256 * 4,
|
||||
};
|
||||
const osThreadAttr_t attr_ctrl_gimbal = {
|
||||
.name = "ctrl_gimbal",
|
||||
@ -57,5 +57,5 @@ const osThreadAttr_t attr_ai = {
|
||||
const osThreadAttr_t attr_referee = {
|
||||
.name = "referee",
|
||||
.priority = osPriorityNormal,
|
||||
.stack_size = 256 * 4,
|
||||
.stack_size = 512 * 4, /* 增大栈：UIRefresh调用sin/cos(软浮点)栈消耗大 */
|
||||
};
|
||||
@ -7,7 +7,9 @@ CORTEX_M7.default_mode_Activation=0
|
||||
Dma.Request0=SPI2_RX
|
||||
Dma.Request1=SPI2_TX
|
||||
Dma.Request2=UART5_RX
|
||||
Dma.RequestsNb=3
|
||||
Dma.Request3=USART10_RX
|
||||
Dma.Request4=USART10_TX
|
||||
Dma.RequestsNb=5
|
||||
Dma.SPI2_RX.0.Direction=DMA_PERIPH_TO_MEMORY
|
||||
Dma.SPI2_RX.0.EventEnable=DISABLE
|
||||
Dma.SPI2_RX.0.FIFOMode=DMA_FIFOMODE_DISABLE
|
||||
@ -62,6 +64,42 @@ Dma.UART5_RX.2.SyncEnable=DISABLE
|
||||
Dma.UART5_RX.2.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
|
||||
Dma.UART5_RX.2.SyncRequestNumber=1
|
||||
Dma.UART5_RX.2.SyncSignalID=NONE
|
||||
Dma.USART10_RX.3.Direction=DMA_PERIPH_TO_MEMORY
|
||||
Dma.USART10_RX.3.EventEnable=DISABLE
|
||||
Dma.USART10_RX.3.FIFOMode=DMA_FIFOMODE_DISABLE
|
||||
Dma.USART10_RX.3.Instance=DMA2_Stream1
|
||||
Dma.USART10_RX.3.MemDataAlignment=DMA_MDATAALIGN_BYTE
|
||||
Dma.USART10_RX.3.MemInc=DMA_MINC_ENABLE
|
||||
Dma.USART10_RX.3.Mode=DMA_NORMAL
|
||||
Dma.USART10_RX.3.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
|
||||
Dma.USART10_RX.3.PeriphInc=DMA_PINC_DISABLE
|
||||
Dma.USART10_RX.3.Polarity=HAL_DMAMUX_REQ_GEN_RISING
|
||||
Dma.USART10_RX.3.Priority=DMA_PRIORITY_LOW
|
||||
Dma.USART10_RX.3.RequestNumber=1
|
||||
Dma.USART10_RX.3.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
|
||||
Dma.USART10_RX.3.SignalID=NONE
|
||||
Dma.USART10_RX.3.SyncEnable=DISABLE
|
||||
Dma.USART10_RX.3.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
|
||||
Dma.USART10_RX.3.SyncRequestNumber=1
|
||||
Dma.USART10_RX.3.SyncSignalID=NONE
|
||||
Dma.USART10_TX.4.Direction=DMA_MEMORY_TO_PERIPH
|
||||
Dma.USART10_TX.4.EventEnable=DISABLE
|
||||
Dma.USART10_TX.4.FIFOMode=DMA_FIFOMODE_DISABLE
|
||||
Dma.USART10_TX.4.Instance=DMA2_Stream2
|
||||
Dma.USART10_TX.4.MemDataAlignment=DMA_MDATAALIGN_BYTE
|
||||
Dma.USART10_TX.4.MemInc=DMA_MINC_ENABLE
|
||||
Dma.USART10_TX.4.Mode=DMA_NORMAL
|
||||
Dma.USART10_TX.4.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
|
||||
Dma.USART10_TX.4.PeriphInc=DMA_PINC_DISABLE
|
||||
Dma.USART10_TX.4.Polarity=HAL_DMAMUX_REQ_GEN_RISING
|
||||
Dma.USART10_TX.4.Priority=DMA_PRIORITY_LOW
|
||||
Dma.USART10_TX.4.RequestNumber=1
|
||||
Dma.USART10_TX.4.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
|
||||
Dma.USART10_TX.4.SignalID=NONE
|
||||
Dma.USART10_TX.4.SyncEnable=DISABLE
|
||||
Dma.USART10_TX.4.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
|
||||
Dma.USART10_TX.4.SyncRequestNumber=1
|
||||
Dma.USART10_TX.4.SyncSignalID=NONE
|
||||
FDCAN1.AutoRetransmission=ENABLE
|
||||
FDCAN1.CalculateBaudRateNominal=1000000
|
||||
FDCAN1.CalculateTimeBitNominal=1000
|
||||
@ -127,6 +165,7 @@ GPIO.groupedBy=Group By Peripherals
|
||||
KeepUserPlacement=false
|
||||
MMTAppRegionsCount=0
|
||||
MMTConfigApplied=false
|
||||
MMTSectionSuffix=_Section
|
||||
Mcu.CPN=STM32H723VGT6
|
||||
Mcu.Family=STM32H7
|
||||
Mcu.IP0=CORTEX_M7
|
||||
@ -192,6 +231,8 @@ MxDb.Version=DB.6.0.150
|
||||
NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
|
||||
NVIC.DMA1_Stream0_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
|
||||
NVIC.DMA1_Stream1_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
|
||||
NVIC.DMA2_Stream1_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
|
||||
NVIC.DMA2_Stream2_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true\:true
|
||||
NVIC.DMA2_Stream5_IRQn=true\:5\:0\:true\:false\:true\:false\:false\:true\:true
|
||||
NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
|
||||
NVIC.EXTI15_10_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
@ -217,6 +258,7 @@ NVIC.TIM2_IRQn=true\:15\:0\:false\:false\:true\:false\:false\:true\:true
|
||||
NVIC.TimeBase=TIM2_IRQn
|
||||
NVIC.TimeBaseIP=TIM2
|
||||
NVIC.UART5_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
NVIC.USART10_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
|
||||
PA13(JTMS/SWDIO).Mode=Serial_Wire
|
||||
PA13(JTMS/SWDIO).Signal=DEBUG_JTMS-SWDIO
|
||||
@ -326,6 +368,7 @@ ProjectManager.DeletePrevious=true
|
||||
ProjectManager.DeviceId=STM32H723VGTx
|
||||
ProjectManager.FirmwarePackage=STM32Cube FW_H7 V1.12.1
|
||||
ProjectManager.FreePins=false
|
||||
ProjectManager.FreePinsContext=
|
||||
ProjectManager.HalAssertFull=false
|
||||
ProjectManager.HeapSize=0x1000
|
||||
ProjectManager.KeepUserCode=true
|
||||
|
||||
@ -1,54 +1,63 @@
|
||||
|
||||
|
||||
|
||||
Breakpoint=D:/CUBEMX/hero/god-yuan-hero/User/module/gimbal.c:158:10, State=BP_STATE_DISABLED
|
||||
GraphedExpression="((((gimbal).feedback).imu).eulr).yaw", Color=#e56a6f, Show=0
|
||||
GraphedExpression="(cmd_ai).mode", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#35792b, Show=0
|
||||
GraphedExpression="((gimbal).out).yaw", Color=#769dda
|
||||
OpenDocument="motor_rm.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/device/motor_rm.c", Line=129
|
||||
OpenDocument="ctrl_chassis.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/ctrl_chassis.c", Line=10
|
||||
OpenDocument="ctrl_gimbal.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/ctrl_gimbal.c", Line=30
|
||||
OpenDocument="startup_stm32h723xx.s", FilePath="D:/CUBEMX/hero/god-yuan-hero/startup_stm32h723xx.s", Line=51
|
||||
OpenDocument="shoot.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/module/shoot.c", Line=33
|
||||
OpenDocument="ref_main.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/ref_main.c", Line=12
|
||||
OpenDocument="memcpy.c", FilePath="/build/gnu-tools-for-stm32_13.3.rel1.20250523-0900/src/newlib/newlib/libc/string/memcpy.c", Line=0
|
||||
OpenDocument="referee.h", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/device/referee.h", Line=0
|
||||
OpenDocument="referee.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/referee.c", Line=0
|
||||
OpenDocument="uart.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/bsp/uart.c", Line=119
|
||||
OpenDocument="supercap.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/device/supercap.c", Line=0
|
||||
OpenDocument="stm32h7xx_it.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Core/Src/stm32h7xx_it.c", Line=351
|
||||
OpenDocument="cmsis_os2.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c", Line=515
|
||||
OpenDocument="config.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/module/config.c", Line=466
|
||||
OpenDocument="ctrl_shoot.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/ctrl_shoot.c", Line=0
|
||||
OpenDocument="referee.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/device/referee.c", Line=21
|
||||
OpenDocument="main.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Core/Src/main.c", Line=60
|
||||
OpenDocument="stm32h7xx_hal_fdcan.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_fdcan.c", Line=5367
|
||||
OpenDocument="track.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/module/track.c", Line=87
|
||||
OpenDocument="fdcan.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/bsp/fdcan.c", Line=240
|
||||
OpenDocument="cmsis_gcc.h", FilePath="D:/CUBEMX/hero/god-yuan-hero/Drivers/CMSIS/Include/cmsis_gcc.h", Line=1193
|
||||
OpenDocument="startup_stm32h723xx.s", FilePath="D:/CUBEMX/hero/god-yuan-hero/startup_stm32h723xx.s", Line=48
|
||||
OpenDocument="referee_proto_types.h", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/device/referee_proto_types.h", Line=15
|
||||
OpenDocument="cmd.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/cmd.c", Line=51
|
||||
OpenDocument="cmd.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/module/cmd/cmd.c", Line=0
|
||||
OpenDocument="vision_bridge.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/module/vision_bridge.c", Line=78
|
||||
OpenDocument="tasks.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3637
|
||||
OpenDocument="ctrl_chassis.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/ctrl_chassis.c", Line=44
|
||||
OpenDocument="motor.h", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/device/motor.h", Line=20
|
||||
OpenDocument="filter.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/component/filter/filter.c", Line=42
|
||||
OpenDocument="filter.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/component/filter/filter.c", Line=54
|
||||
OpenDocument="user_math.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/component/user_math.c", Line=45
|
||||
OpenDocument="pid.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/component/pid.c", Line=61
|
||||
OpenDocument="gimbal.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/module/gimbal.c", Line=220
|
||||
OpenDocument="ctrl_gimbal.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/ctrl_gimbal.c", Line=16
|
||||
OpenDocument="main.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Core/Src/main.c", Line=60
|
||||
OpenDocument="tasks.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3655
|
||||
OpenDocument="motor_rm.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/device/motor_rm.c", Line=129
|
||||
OpenDocument="stm32h7xx_hal_uart_ex.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart_ex.c", Line=964
|
||||
OpenDocument="queue.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Middlewares/Third_Party/FreeRTOS/Source/queue.c", Line=1275
|
||||
OpenDocument="chassis.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/module/chassis.c", Line=213
|
||||
OpenDocument="stm32h7xx_hal_uart.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart.c", Line=3332
|
||||
OpenDocument="supercap.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/supercap.c", Line=27
|
||||
OpenDocument="atti_esti.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/atti_esti.c", Line=121
|
||||
OpenDocument="fdcan.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Core/Src/fdcan.c", Line=0
|
||||
OpenDocument="time.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/bsp/time.c", Line=17
|
||||
OpenToolbar="Debug", Floating=0, x=0, y=0
|
||||
OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=728, h=600, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
|
||||
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=326, h=930, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=728, h=329, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=728, h=594, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
|
||||
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=328, h=930, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=728, h=335, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Memory 1", DockArea=BOTTOM, x=0, y=0, w=239, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0xFFFF5088
|
||||
OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=1, w=728, h=600, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Functions", DockArea=LEFT, x=0, y=0, w=326, h=930, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=1, w=728, h=594, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Functions", DockArea=LEFT, x=0, y=0, w=328, h=930, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=866, h=525, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
|
||||
OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1453, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="1 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="21;187", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1232;-69", CodeGraphLegendShown=0, CodeGraphLegendPosition="1248;0"
|
||||
OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1453, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="5 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="21;187", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1157;-69", CodeGraphLegendShown=0, CodeGraphLegendPosition="1164;0"
|
||||
OpenWindow="Console", DockArea=BOTTOM, x=2, y=0, w=866, h=525, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
|
||||
TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[120;144;463]
|
||||
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[100;100]
|
||||
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;100;100;100;100;100;110;126;126]
|
||||
TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[120;144;464]
|
||||
TableHeader="Functions", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Address";"Size";"#Insts";"Source"], ColWidths=[1594;104;100;100;100]
|
||||
TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
|
||||
TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[]
|
||||
TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[215;100;100;100;1014]
|
||||
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[250;229;145;100]
|
||||
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ((((gimbal).feedback).imu).eulr).yaw";" (cmd_ai).mode";" ((gimbal).out).yaw"], ColWidths=[100;100;100;100;100]
|
||||
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;144;114;134;124;134;110;144;134]
|
||||
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;366]
|
||||
TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[215;100;100;100;1022]
|
||||
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[298;229;145;100]
|
||||
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;340]
|
||||
WatchedExpression="cmd", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="bmi088", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="chassis", RefreshRate=5, Window=Watched Data 1
|
||||
@ -61,4 +70,7 @@ WatchedExpression="imu_to_can", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="cmd_ai", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="ai", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="queue_list", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="imu_to_can", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="imu_to_can", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="ref", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="rxbuf", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="shoot_ref", RefreshRate=5, Window=Watched Data 1
|
||||
Loading…
Reference in New Issue
Block a user