66 lines
1.8 KiB
C
66 lines
1.8 KiB
C
#include "device/ai.h"
|
|
#include "device/device.h"
|
|
#include "bsp/uart.h"
|
|
#include "component/crc16.h"
|
|
|
|
|
|
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->id==ID_AI){
|
|
// if(CRC16_Verify((const uint8_t*)&(ai), sizeof(&ai))==true)
|
|
// {
|
|
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) {
|
|
|
|
mcu->id=ID_MCU;
|
|
mcu->data.mode=0;
|
|
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;
|
|
|
|
mcu->crc16=CRC16_Calc(((const uint8_t*)&(mcu)),sizeof(&mcu)-sizeof(uint16_t), CRC16_INIT );
|
|
if(CRC16_Verify(((const uint8_t*)&(mcu)), sizeof(&mcu))!=true){
|
|
return DEVICE_ERR;
|
|
}
|
|
return DEVICE_OK;
|
|
}
|
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
|
|
|
|