63 lines
2.0 KiB
C
63 lines
2.0 KiB
C
#include "vision_bridge.h"
|
|
#include "device/device.h"
|
|
#include "bsp/uart.h"
|
|
#include "component/crc16.h"
|
|
#include <string.h>
|
|
|
|
int8_t AI_Init(AI_t *ai) {
|
|
if (ai == NULL) return DEVICE_ERR_NULL;
|
|
memset(ai, 0, sizeof(AI_t));
|
|
return 0;
|
|
}
|
|
|
|
int8_t AI_StartReceiving(AI_t *ai) {
|
|
if (BSP_UART_Receive(BSP_UART_AI,(uint8_t *)&ai->fromhost,sizeof(ai->fromhost), true)==HAL_OK) {
|
|
return DEVICE_OK;}
|
|
return DEVICE_ERR;
|
|
}
|
|
|
|
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=17.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_StartSend(AI_t *ai) {
|
|
if (BSP_UART_Transmit(BSP_UART_AI,(uint8_t *)&ai->tohost,sizeof(ai->tohost), true)==HAL_OK)
|
|
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;
|
|
}
|