120 lines
4.0 KiB
C
120 lines
4.0 KiB
C
#include "vision_bridge.h"
|
||
#include "device/device.h"
|
||
#include "bsp/uart.h"
|
||
#include "component/crc16.h"
|
||
#include <string.h>
|
||
#include "bsp/fdcan.h"
|
||
|
||
#define AI_CMD_CAN_DLC (8u) /* 1字节mode + 3.5字节yaw + 3.5字节pit */
|
||
#define AI_CMD_ANGLE_SCALE (10000000.0f) /* 0.0000001 rad per LSB */
|
||
|
||
int8_t AI_Init(AI_t *ai, AI_Param_t *param) {
|
||
if (ai == NULL) return DEVICE_ERR_NULL;
|
||
|
||
BSP_FDCAN_Init();
|
||
|
||
memset(ai, 0, sizeof(AI_t));
|
||
|
||
ai->param = param;
|
||
BSP_FDCAN_RegisterId(ai->param->can, ai->param->vision_id, 3);
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
int8_t AI_ParseForHost(AI_t* ai, AI_RawData_t* raw_data){
|
||
ai->tohost.head[0]='M';
|
||
ai->tohost.head[1]='R';
|
||
ai->tohost.mode=raw_data->mode;
|
||
ai->tohost.pitch=raw_data->pitch;
|
||
ai->tohost.yaw=raw_data->yaw;
|
||
ai->tohost.pitch_vel=raw_data->pitch_vel;
|
||
ai->tohost.yaw_vel=raw_data->yaw_vel;
|
||
ai->tohost.q[0]=raw_data->q[0];
|
||
ai->tohost.q[1]=raw_data->q[1];
|
||
ai->tohost.q[2]=raw_data->q[2];
|
||
ai->tohost.q[3]=raw_data->q[3];
|
||
ai->tohost.bullet_count=10;
|
||
ai->tohost.bullet_speed=10.5;
|
||
|
||
ai->tohost.crc16=CRC16_Calc(((const uint8_t*)&(ai->tohost)),sizeof(ai->tohost)-sizeof(uint16_t), CRC16_INIT );
|
||
if(CRC16_Verify(((const uint8_t*)&(ai->tohost)), sizeof(ai->tohost))!=true){
|
||
return DEVICE_ERR;
|
||
}
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
int8_t AI_Get(AI_t *ai, AI_cmd_t* outcmd) {
|
||
if(ai->fromhost.head[0]!='M'&&ai->fromhost.head[1]!='R'){
|
||
return DEVICE_ERR;
|
||
}
|
||
// CRC16_Calc(&ai->fromhost,sizeof(ai->fromhost),ai->fromhost.crc16);
|
||
if(CRC16_Verify((const uint8_t*)&(ai->fromhost), sizeof(ai->fromhost))!=true){
|
||
return DEVICE_ERR;
|
||
}
|
||
outcmd->gimbal.setpoint.pit = ai->fromhost.pitch;
|
||
outcmd->gimbal.setpoint.yaw = ai->fromhost.yaw;
|
||
outcmd->mode = ai->fromhost.mode;
|
||
outcmd->gimbal.accl.pit = ai->fromhost.pitch_acc;
|
||
outcmd->gimbal.vel.pit = ai->fromhost.pitch_vel;
|
||
outcmd->gimbal.accl.yaw = ai->fromhost.yaw_acc;
|
||
outcmd->gimbal.vel.yaw = ai->fromhost.yaw_vel;
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
|
||
/* 打包并通过 CAN2 发送AI命令给下层板（mode + yaw + pit） */
|
||
void AI_SendCmdOnFDCAN(AI_t *ai, const AI_cmd_t *cmd) {
|
||
if (cmd == NULL) return;
|
||
|
||
/* 将float角度转换为int32_t定点数（0.0000001 rad/LSB） */
|
||
const int32_t yaw = (int32_t)(cmd->gimbal.setpoint.yaw * AI_CMD_ANGLE_SCALE);
|
||
const int32_t pit = (int32_t)(cmd->gimbal.setpoint.pit * AI_CMD_ANGLE_SCALE);
|
||
|
||
BSP_FDCAN_StdDataFrame_t frame = {0};
|
||
frame.id = ai->param->vision_id;
|
||
frame.dlc = AI_CMD_CAN_DLC;
|
||
|
||
/* mode(1字节) + yaw(3.5字节) + pit(3.5字节) */
|
||
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);
|
||
|
||
(void)BSP_FDCAN_TransmitStdDataFrame(ai->param->can, &frame);
|
||
}
|
||
|
||
/* 从CAN消息解析AI命令 (mode + yaw + pit) */
|
||
void AI_ParseCmdFromCan(AI_t *ai, AI_cmd_t *cmd) {
|
||
|
||
if (cmd == NULL) {
|
||
return;
|
||
}
|
||
BSP_FDCAN_Message_t msg;
|
||
if (BSP_FDCAN_GetMessage(ai->param->can, ai->param->vision_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) != 0) {
|
||
return;
|
||
}
|
||
|
||
/* 解析mode (1字节) */
|
||
cmd->mode = msg.data[0];
|
||
|
||
/* 解析yaw (3.5字节) */
|
||
int32_t yaw_raw = (int32_t)((msg.data[1] << 20) | (msg.data[2] << 12) | (msg.data[3] << 4) | ((msg.data[4] >> 4) & 0xF));
|
||
if (yaw_raw & 0x08000000) yaw_raw |= 0xF0000000;
|
||
cmd->gimbal.setpoint.yaw = (float)yaw_raw / AI_CMD_ANGLE_SCALE;
|
||
|
||
/* 解析pit (3.5字节) */
|
||
int32_t pit_raw = (int32_t)(((msg.data[4] & 0xF) << 24) | (msg.data[5] << 16) | (msg.data[6] << 8) | msg.data[7]);
|
||
if (pit_raw & 0x08000000) pit_raw |= 0xF0000000;
|
||
cmd->gimbal.setpoint.pit = (float)pit_raw / AI_CMD_ANGLE_SCALE;
|
||
|
||
/* 其他字段根据需要可以初始化为0 */
|
||
cmd->gimbal.vel.yaw = 0.0f;
|
||
cmd->gimbal.vel.pit = 0.0f;
|
||
cmd->gimbal.accl.yaw = 0.0f;
|
||
cmd->gimbal.accl.pit = 0.0f;
|
||
} |