diff --git a/CMakeLists.txt b/CMakeLists.txt index 5b2e137..d340a72 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 diff --git a/Core/Inc/stm32h7xx_it.h b/Core/Inc/stm32h7xx_it.h index 360c432..920363c 100644 --- a/Core/Inc/stm32h7xx_it.h +++ b/Core/Inc/stm32h7xx_it.h @@ -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 */ diff --git a/Core/Src/dma.c b/Core/Src/dma.c index 8cf8599..9e093ba 100644 --- a/Core/Src/dma.c +++ b/Core/Src/dma.c @@ -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); diff --git a/Core/Src/stm32h7xx_it.c b/Core/Src/stm32h7xx_it.c index 4b34eea..44298d4 100644 --- a/Core/Src/stm32h7xx_it.c +++ b/Core/Src/stm32h7xx_it.c @@ -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. */ diff --git a/Core/Src/usart.c b/Core/Src/usart.c index 34de662..95513fc 100644 --- a/Core/Src/usart.c +++ b/Core/Src/usart.c @@ -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 */ diff --git a/User/bsp/fdcan.c b/User/bsp/fdcan.c index 8944cb9..7f3425e 100644 --- a/User/bsp/fdcan.c +++ b/User/bsp/fdcan.c @@ -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; } diff --git a/User/bsp/uart.h b/User/bsp/uart.h index 4ac2ab3..5e270f5 100644 --- a/User/bsp/uart.h +++ b/User/bsp/uart.h @@ -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; diff --git a/User/device/device.h b/User/device/device.h index c03ccfb..6954037 100644 --- a/User/device/device.h +++ b/User/device/device.h @@ -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 */ diff --git a/User/device/motor_dm.c b/User/device/motor_dm.c index 24e41ee..bbbef2c 100644 --- a/User/device/motor_dm.c +++ b/User/device/motor_dm.c @@ -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); } diff --git a/User/device/referee.c b/User/device/referee.c index f6b4dca..aac7039 100644 --- a/User/device/referee.c +++ b/User/device/referee.c @@ -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: diff --git a/User/device/referee.h b/User/device/referee.h index 4537285..ff1919b 100644 --- a/User/device/referee.h +++ b/User/device/referee.h @@ -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; diff --git a/User/device/supercap.h b/User/device/supercap.h index 5cb600c..07a638c 100644 --- a/User/device/supercap.h +++ b/User/device/supercap.h @@ -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 } diff --git a/User/module/aimbot.c b/User/module/aimbot.c new file mode 100644 index 0000000..ebaffee --- /dev/null +++ b/User/module/aimbot.c @@ -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 + +/* ===================================================================== + * 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; +} + diff --git a/User/module/aimbot.h b/User/module/aimbot.h new file mode 100644 index 0000000..a4335ba --- /dev/null +++ b/User/module/aimbot.h @@ -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 + +// 数据包 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 diff --git a/User/module/chassis.c b/User/module/chassis.c index 9ea2afe..30ea340 100644 --- a/User/module/chassis.c +++ b/User/module/chassis.c @@ -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; } diff --git a/User/module/chassis.h b/User/module/chassis.h index 24416aa..e666ed7 100644 --- a/User/module/chassis.h +++ b/User/module/chassis.h @@ -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 } diff --git a/User/module/cmd/cmd.c b/User/module/cmd/cmd.c index 9ca682f..4a180b0 100644 --- a/User/module/cmd/cmd.c +++ b/User/module/cmd/cmd.c @@ -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; } } diff --git a/User/module/cmd/cmd_adapter.c b/User/module/cmd/cmd_adapter.c index 584e5e5..ebc8d79 100644 --- a/User/module/cmd/cmd_adapter.c +++ b/User/module/cmd/cmd_adapter.c @@ -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 */ /* ========================================================================== */ /* 适配器管理实现 */ /* ========================================================================== */ diff --git a/User/module/config.c b/User/module/config.c index 12fc0cc..9ca2c83 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -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 = { { diff --git a/User/module/config.h b/User/module/config.h index 923bd20..f2f0723 100644 --- a/User/module/config.h +++ b/User/module/config.h @@ -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; diff --git a/User/module/gimbal.c b/User/module/gimbal.c index 37da408..add7754 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -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; } diff --git a/User/module/gimbal.h b/User/module/gimbal.h index 204f453..cfede90 100644 --- a/User/module/gimbal.h +++ b/User/module/gimbal.h @@ -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 diff --git a/User/module/shoot.c b/User/module/shoot.c index 8a719fc..8111aeb 100644 --- a/User/module/shoot.c +++ b/User/module/shoot.c @@ -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; } diff --git a/User/module/shoot.h b/User/module/shoot.h index 15f868d..d98e66e 100644 --- a/User/module/shoot.h +++ b/User/module/shoot.h @@ -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 diff --git a/User/task/ai.c b/User/task/ai.c index f7c4611..7b7f2ca 100644 --- a/User/task/ai.c +++ b/User/task/ai.c @@ -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); /* 运行结束,等待下一次唤醒 */ } - } \ No newline at end of file diff --git a/User/task/ctrl_chassis.c b/User/task/ctrl_chassis.c index b491fae..6dbe54a 100644 --- a/User/task/ctrl_chassis.c +++ b/User/task/ctrl_chassis.c @@ -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); diff --git a/User/task/ctrl_shoot.c b/User/task/ctrl_shoot.c index f5b2a44..8d9bb30 100644 --- a/User/task/ctrl_shoot.c +++ b/User/task/ctrl_shoot.c @@ -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); diff --git a/User/task/init.c b/User/task/init.c index baeba7e..338e726 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -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 */ diff --git a/User/task/referee.c b/User/task/ref_main.c similarity index 95% rename from User/task/referee.c rename to User/task/ref_main.c index b64899c..d38fcda 100644 --- a/User/task/referee.c +++ b/User/task/ref_main.c @@ -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); } diff --git a/User/task/supercap.c b/User/task/supercap.c index a955d90..aefb6e9 100644 --- a/User/task/supercap.c +++ b/User/task/supercap.c @@ -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); /* 运行结束,等待下一次唤醒 */ + } + +} diff --git a/User/task/user_task.c b/User/task/user_task.c index 3af2890..131074b 100644 --- a/User/task/user_task.c +++ b/User/task/user_task.c @@ -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(软浮点)栈消耗大 */ }; \ No newline at end of file diff --git a/hero_DM02.ioc b/hero_DM02.ioc index e964dcd..f1d129b 100644 --- a/hero_DM02.ioc +++ b/hero_DM02.ioc @@ -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 diff --git a/ozone/hero.jdebug.user b/ozone/hero.jdebug.user index dfef6dc..8094ebc 100644 --- a/ozone/hero.jdebug.user +++ b/ozone/hero.jdebug.user @@ -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 \ No newline at end of file +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 \ No newline at end of file