267 lines
9.6 KiB
C
267 lines
9.6 KiB
C
#include "module/aimbot.h"
|
||
#include "device/device.h"
|
||
#include "bsp/uart.h"
|
||
#include "component/crc16.h"
|
||
#include <string.h>
|
||
|
||
/* =====================================================================
|
||
* FDCAN 帧索引（反馈方向：下层板?上层板）
|
||
* 注：上层板使用自己的IMU，下层板只发送射击数据与模式
|
||
* ===================================================================== */
|
||
#define FB_FRAME_DATA 0u /* 弹速(4B)+弹数(2B)+模式(1B)+保留(1B) */
|
||
|
||
/* =====================================================================
|
||
* UART 通信接口（上层板 <20>?上位<E4B88A>?PC<50>?
|
||
* ===================================================================== */
|
||
|
||
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->head[0] != 'M' || ai->head[1] != 'R') {
|
||
return DEVICE_ERR;
|
||
}
|
||
if (!CRC16_Verify((const uint8_t *)ai, sizeof(*ai))) {
|
||
return DEVICE_ERR;
|
||
}
|
||
result->mode = ai->mode;
|
||
result->gimbal_t.setpoint.yaw = ai->yaw;
|
||
result->gimbal_t.vel.yaw = ai->yaw_vel;
|
||
result->gimbal_t.accl.yaw = ai->yaw_acc;
|
||
result->gimbal_t.setpoint.pit = ai->pitch;
|
||
result->gimbal_t.vel.pit = ai->pitch_vel;
|
||
result->gimbal_t.accl.pit = ai->pitch_acc;
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 打包 MCU 数据（UART 发给上位机格式），修正了原始实现中的字段错误<E99499>?
|
||
* 四元数来<E695B0>?quat 参数；欧拉角、角速度来自 gimbal_fb->imu<6D>?
|
||
*/
|
||
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, uint8_t mode) {
|
||
if (mcu == NULL || gimbal_fb == NULL || quat == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
mcu->head[0] = 'M';
|
||
mcu->head[1] = 'R';
|
||
mcu->mode = mode;
|
||
mcu->q[0] = quat->q0;
|
||
mcu->q[1] = quat->q1;
|
||
mcu->q[2] = quat->q2;
|
||
mcu->q[3] = quat->q3;
|
||
mcu->yaw = gimbal_fb->imu.eulr.yaw;
|
||
mcu->yaw_vel = gimbal_fb->imu.gyro.z;
|
||
mcu->pitch = gimbal_fb->imu.eulr.pit;
|
||
mcu->pitch_vel = gimbal_fb->imu.gyro.x;
|
||
mcu->bullet_speed = bullet_speed;
|
||
mcu->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 通信接口（下层板 <20>?上层板）
|
||
* ===================================================================== */
|
||
|
||
/**
|
||
* @brief 初始<E5889D>?Aimbot CAN 通信：注册指令接收队列和反馈收发队列<E9989F>?
|
||
* 下层板只需注册 cmd_id；上层板只需注册 fb_base_id <20>?6 <20>?ID<49>?
|
||
* 本函数同时注册两侧所需 ID，上/下层板共用同一初始化流程即可<E58DB3>?
|
||
*/
|
||
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 <20>?Gimbal/IMU/Shoot 数据打包 CAN 反馈结构体<E69E84>?
|
||
/**
|
||
* @brief 【下层板】打包 CAN 反馈结构体。
|
||
*/
|
||
int8_t Aimbot_PackFeedback(Aimbot_FeedbackData_t *fb,
|
||
float bullet_speed, uint16_t bullet_count,
|
||
uint8_t mode) {
|
||
if (fb == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
fb->mode = mode;
|
||
fb->bullet_speed = bullet_speed;
|
||
fb->bullet_count = bullet_count;
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 【下层板】将反馈数据打成 1 个 CAN 标准帧发给上层板。
|
||
*
|
||
* 帧格式（每帧 8 字节）：
|
||
* 帧0: bullet_speed(float,4B) bullet_count(uint16,2B) mode(1B) rsv(1B)
|
||
*/
|
||
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: 弹速 + 弹数 + 模式 */
|
||
frame.id = param->fb_base_id + FB_FRAME_DATA;
|
||
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);
|
||
}
|
||
|
||
/**
|
||
* @brief 【下层板】从 CAN 队列中非阻塞地取出上层板发来<E58F91>?AI 指令并解析<E8A7A3>?
|
||
*
|
||
* 指令帧格式（8 字节，与 vision_bridge 一致）<E887B4>?
|
||
* data[0] : mode (1B)
|
||
* data[1..3.5] : yaw (28bit 有符号定点数<E782B9>?.1µrad/LSB)
|
||
* data[4.5..7] : pit (28bit 有符号定点数<E782B9>?.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;
|
||
int8_t ret = DEVICE_ERR;
|
||
|
||
if (BSP_FDCAN_GetMessage(param->can, param->cmd_id, &msg,
|
||
BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
|
||
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;
|
||
ret = DEVICE_OK;
|
||
}
|
||
|
||
if (BSP_FDCAN_GetMessage(param->can, param->cmd_id + 1, &msg,
|
||
BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
|
||
memcpy(&result->gimbal_t.vel.yaw, &msg.data[0], 4);
|
||
memcpy(&result->gimbal_t.vel.pit, &msg.data[4], 4);
|
||
ret = DEVICE_OK;
|
||
}
|
||
|
||
if (BSP_FDCAN_GetMessage(param->can, param->cmd_id + 2, &msg,
|
||
BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
|
||
memcpy(&result->gimbal_t.accl.yaw, &msg.data[0], 4);
|
||
memcpy(&result->gimbal_t.accl.pit, &msg.data[4], 4);
|
||
ret = DEVICE_OK;
|
||
}
|
||
|
||
return ret;
|
||
}
|
||
|
||
/**
|
||
* @brief 【上层板】将 AI 指令通过 CAN 发送给下层板（单帧 8 字节）<E88A82>?
|
||
*
|
||
* <20>?vision_bridge.c <20>?AI_SendCmdOnFDCAN 编码格式完全一致，
|
||
* 上层板可直接调用本函数替<E695B0>?vision_bridge 中的同名函数<E587BD>?
|
||
*/
|
||
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);
|
||
|
||
/* 第二帧：发速度 */
|
||
frame.id = param->cmd_id + 1;
|
||
memcpy(&frame.data[0], &cmd->gimbal_t.vel.yaw, 4);
|
||
memcpy(&frame.data[4], &cmd->gimbal_t.vel.pit, 4);
|
||
BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);
|
||
|
||
/* 第三帧：发加速度 */
|
||
frame.id = param->cmd_id + 2;
|
||
memcpy(&frame.data[0], &cmd->gimbal_t.accl.yaw, 4);
|
||
memcpy(&frame.data[4], &cmd->gimbal_t.accl.pit, 4);
|
||
BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);
|
||
}
|
||
|
||
/**
|
||
* @brief 【上层板】从 CAN 队列中非阻塞地解析下层板发来的反馈数据（共1帧）。
|
||
*
|
||
* @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: 弹速 + 弹数 + 模式 */
|
||
if (BSP_FDCAN_GetMessage(param->can,
|
||
param->fb_base_id + FB_FRAME_DATA,
|
||
&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];
|
||
}
|
||
|
||
return DEVICE_OK;
|
||
}
|
||
|