126 lines
3.2 KiB
C
126 lines
3.2 KiB
C
#include "device/ai.h"
|
|
#include "device/device.h"
|
|
#include "bsp/uart.h"
|
|
#include "component/crc16.h"
|
|
#include "device/referee.h"
|
|
|
|
static int8_t Package_BuildAndVerify(uint8_t *raw, uint16_t len, uint8_t id) {
|
|
uint16_t *crc = NULL;
|
|
|
|
if (raw == NULL || len <= sizeof(uint16_t)) {
|
|
return DEVICE_ERR;
|
|
}
|
|
|
|
raw[0] = id;
|
|
crc = (uint16_t *)(void *)(raw + len - sizeof(uint16_t));
|
|
*crc = CRC16_Calc(raw, len - sizeof(uint16_t), CRC16_INIT);
|
|
|
|
if (CRC16_Verify(raw, len) != true) {
|
|
return DEVICE_ERR;
|
|
}
|
|
|
|
return DEVICE_OK;
|
|
}
|
|
|
|
static int8_t Package_Check(const uint8_t *raw, uint16_t len, uint8_t expected_id) {
|
|
if (raw == NULL || len <= sizeof(uint16_t)) {
|
|
return DEVICE_ERR;
|
|
}
|
|
|
|
if (raw[0] != expected_id) {
|
|
return DEVICE_ERR;
|
|
}
|
|
|
|
if (CRC16_Verify(raw, len) != true) {
|
|
return DEVICE_ERR;
|
|
}
|
|
|
|
return DEVICE_OK;
|
|
}
|
|
|
|
|
|
int8_t AI_StartReceiving(PackageAI_t *ai) {
|
|
if (BSP_UART_Receive(BSP_UART_1,(uint8_t *)ai,sizeof(*ai), true)==HAL_OK) {
|
|
return DEVICE_OK;}
|
|
return DEVICE_ERR;
|
|
}
|
|
|
|
int8_t AI_Get_NUC(PackageAI_t *ai,AI_result_t* result) {
|
|
if (ai == NULL || result == NULL) {
|
|
return DEVICE_ERR;
|
|
}
|
|
|
|
if (Package_Check((const uint8_t *)ai, sizeof(*ai), ID_AI) != DEVICE_OK) {
|
|
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;
|
|
}
|
|
|
|
|
|
int8_t MCU_Send(PackageMCU_t* mcu,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat ){
|
|
(void)quat;
|
|
if (mcu == NULL || motor == NULL) {
|
|
return DEVICE_ERR;
|
|
}
|
|
|
|
mcu->data.mode=1;
|
|
mcu->data.q[0]=motor->imu.quat.q0;
|
|
mcu->data.q[1]=motor->imu.quat.q1;
|
|
mcu->data.q[2]=motor->imu.quat.q2;
|
|
mcu->data.q[3]=motor->imu.quat.q3;
|
|
mcu->data.yaw=motor->imu.eulr.yaw;
|
|
mcu->data.yaw_vel=motor->imu.gyro.z;
|
|
mcu->data.pitch=motor->imu.eulr.rol;
|
|
mcu->data.pitch_vel=motor->imu.gyro.x;
|
|
mcu->data.bullet_count=0;
|
|
mcu->data.bullet_speed=22;
|
|
|
|
return Package_BuildAndVerify((uint8_t *)mcu, sizeof(*mcu), ID_MCU);
|
|
}
|
|
|
|
int8_t REF_Send(PackageReferee_t *referee,Referee_RobotStatus_t* robot_status,Referee_GameStatus_t *game_status) {
|
|
if (referee == NULL || robot_status == NULL || game_status == NULL) {
|
|
return DEVICE_ERR;
|
|
}
|
|
|
|
referee->data.remain_hp=robot_status->current_HP;
|
|
referee->data.game_progress = game_status->game_progress & 0x0F;
|
|
referee->data.stage_remain_time= game_status->stage_remain_time;
|
|
|
|
return Package_BuildAndVerify((uint8_t *)referee, sizeof(*referee), ID_REF);
|
|
}
|
|
|
|
int8_t REF_StartSend(PackageReferee_t *referee) {
|
|
if (referee == NULL) {
|
|
return DEVICE_ERR;
|
|
}
|
|
|
|
if(BSP_UART_Transmit(BSP_UART_1, (uint8_t *)referee, sizeof(*referee), true)==HAL_OK)
|
|
return DEVICE_OK;
|
|
return DEVICE_ERR;
|
|
}
|
|
|
|
|
|
int8_t MCU_StartSend(PackageMCU_t *mcu) {
|
|
if(BSP_UART_Transmit(BSP_UART_1, (uint8_t *)mcu, sizeof(*mcu), true)==HAL_OK)
|
|
return DEVICE_OK;
|
|
return DEVICE_ERR;
|
|
|
|
}
|
|
|
|
|
|
|
|
|