82 lines
2.2 KiB
C
82 lines
2.2 KiB
C
#include "ai.h"
|
|
#include "bsp/uart.h"
|
|
#include "component/crc16.h"
|
|
#include "module/gimbal.h"
|
|
|
|
|
|
void ai_init(AI_t* ai){
|
|
ai->RX.head[0]="M";
|
|
ai->RX.head[0]="R";
|
|
ai->TX.head[0]="M";
|
|
ai->TX.head[0]="R";
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
int8_t AI_StartReceiving(AI_t *ai) {
|
|
if (BSP_UART_Receive(BSP_UART_NUC,&ai->RX,sizeof(ai->RX), false))
|
|
return DEVICE_OK;
|
|
return DEVICE_ERR;
|
|
}
|
|
|
|
int8_t AI_Get_NUC(AI_t *ai,AI_cmd_t* ai_cmd) {
|
|
if(ai->RX.head[0]!='M'&&ai->RX.head[1]!='R'){
|
|
return DEVICE_ERR;
|
|
}
|
|
// CRC16_Calc(&ai->RX,sizeof(ai->RX),ai->RX.crc16);
|
|
// if(CRC16_Verify((const uint8_t*)&(ai->RX), sizeof(ai->RX.crc16))!=true){
|
|
// return DEVICE_ERR;
|
|
// }
|
|
ai_cmd->gimbal_t.setpoint.pit=ai->RX.pitch;
|
|
ai_cmd->gimbal_t.setpoint.yaw=ai->RX.yaw;
|
|
ai_cmd->mode=ai->RX.mode;
|
|
ai_cmd->gimbal_t.accl.pit=ai->RX.pitch_acc;
|
|
ai_cmd->gimbal_t.vel.pit=ai->RX.pitch_vel;
|
|
ai_cmd->gimbal_t.accl.yaw=ai->RX.yaw_acc;
|
|
ai_cmd->gimbal_t.vel.yaw=ai->RX.yaw_vel;
|
|
return DEVICE_OK;
|
|
}
|
|
|
|
|
|
int8_t AI_ParseHost(AI_t *ai,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat) {
|
|
|
|
ai->TX.head[0]='M';
|
|
ai->TX.head[1]='R';
|
|
// ai->TX.mode=2;
|
|
ai->TX.pitch=motor->motor.pitch_4310_motor_feedback.rotor_abs_angle;
|
|
ai->TX.yaw=motor->motor.yaw_4310_motor_feedback.rotor_abs_angle;
|
|
ai->TX.pitch_vel=motor->motor.pitch_4310_motor_feedback.rotor_speed;
|
|
ai->TX.yaw_vel=motor->motor.yaw_4310_motor_feedback.rotor_speed;
|
|
ai->TX.q[0]=quat->q1;
|
|
ai->TX.q[1]=quat->q2;
|
|
ai->TX.q[2]=quat->q3;
|
|
ai->TX.q[3]=quat->q0;
|
|
|
|
ai->TX.bullet_count=10;
|
|
ai->TX.bullet_speed=10;
|
|
|
|
ai->TX.crc16=CRC16_Calc(((const uint8_t*)&(ai->TX)),sizeof(ai->TX)-sizeof(uint16_t),ai->TX.crc16);
|
|
if(CRC16_Verify(((const uint8_t*)&(ai->TX)), sizeof(ai->TX))!=true){
|
|
return DEVICE_ERR;
|
|
}
|
|
return DEVICE_OK;
|
|
}
|
|
|
|
int8_t AI_StartSend(AI_t *ai) {
|
|
// if (ref_update) {
|
|
if (BSP_UART_Transmit(BSP_UART_NUC,&ai->TX,sizeof(ai->TX), false))
|
|
return DEVICE_OK;
|
|
// else
|
|
// return DEVICE_ERR;
|
|
// } else {
|
|
// if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_AI),
|
|
// (uint8_t *)&(ai->TX),
|
|
// sizeof(ai->TX)) == HAL_OK)
|
|
// return DEVICE_OK;
|
|
// else
|
|
// return DEVICE_ERR;
|
|
// }
|
|
}
|