339 lines
13 KiB
C
339 lines
13 KiB
C
#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;
|
||
}
|
||
|