151 lines
5.4 KiB
C
151 lines
5.4 KiB
C
/*
|
|
AI
|
|
*/
|
|
|
|
/* Includes ----------------------------------------------------------------- */
|
|
#include "ai.h"
|
|
|
|
#include <string.h>
|
|
|
|
#include "bsp\delay.h"
|
|
#include "bsp\uart.h"
|
|
#include "component\crc16.h"
|
|
#include "component\crc8.h"
|
|
#include "component\user_math.h"
|
|
|
|
/* Private define ----------------------------------------------------------- */
|
|
/* Private macro ------------------------------------------------------------ */
|
|
/* Private typedef ---------------------------------------------------------- */
|
|
/* Private variables -------------------------------------------------------- */
|
|
static volatile uint32_t drop_message = 0;
|
|
|
|
static osThreadId_t thread_alert;
|
|
|
|
static bool inited = false;
|
|
|
|
/* Private function -------------------------------------------------------- */
|
|
|
|
static void Ai_RxCpltCallback(void) {
|
|
osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
|
|
}
|
|
|
|
/* Exported functions ------------------------------------------------------- */
|
|
int8_t AI_Init(AI_t *ai) {
|
|
if (ai == NULL) return DEVICE_ERR_NULL;
|
|
if (inited) return DEVICE_ERR_INITED;
|
|
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
|
|
|
|
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB,
|
|
Ai_RxCpltCallback);
|
|
inited = true;
|
|
|
|
ai->ai_online = false;
|
|
return 0;
|
|
}
|
|
|
|
int8_t AI_Restart(void) {
|
|
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_AI));
|
|
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_AI));
|
|
return DEVICE_OK;
|
|
}
|
|
|
|
int8_t AI_StartReceiving(AI_t *ai) {
|
|
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI),
|
|
ai->form_host.rx_buffer,
|
|
sizeof(ai->form_host.rx_buffer)) == HAL_OK)
|
|
return DEVICE_OK;
|
|
return DEVICE_ERR;
|
|
}
|
|
|
|
bool AI_WaitDmaCplt(void) {
|
|
return (osThreadFlagsWait(SIGNAL_AI_RAW_REDY, osFlagsWaitAll, 0) ==
|
|
SIGNAL_AI_RAW_REDY);
|
|
}
|
|
|
|
int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host) {
|
|
(void)cmd_host;
|
|
ai->ai_online = true;
|
|
if (!CRC16_Verify(ai->form_host.rx_buffer, sizeof(ai->form_host.rx_buffer)))
|
|
goto error;
|
|
memcpy(&(ai->form_host.DownPackage), ai->form_host.rx_buffer, sizeof(ai->form_host.DownPackage));
|
|
// cmd_host->gimbal_delta.pit = ai->form_host.data.gimbal.pit;
|
|
// cmd_host->gimbal_delta.yaw = ai->form_host.data.gimbal.yaw;
|
|
// cmd_host->gimbal_delta.rol = ai->form_host.data.gimbal.rol;
|
|
// cmd_host->fire = (ai->form_host.data.notice & AI_NOTICE_FIRE);
|
|
// cmd_host->chassis_move_vec.vx = ai->form_host.data.chassis_move_vec.vx;
|
|
// cmd_host->chassis_move_vec.vy = ai->form_host.data.chassis_move_vec.vy;
|
|
// cmd_host->chassis_move_vec.wz = ai->form_host.data.chassis_move_vec.wz;
|
|
// float yaw_out = PID_Calc(&ai->pid[0], 0.0f, ai->form_host.DownPackage.data.gimbal.yaw, 0.0f , 1.0/10.0f);
|
|
// float pit_out = PID_Calc(&ai->pid[1], 0.0f, ai->form_host.DownPackage.data.gimbal.pit, 0.0f , 1.0/10.0f);
|
|
cmd_host->gimbal_delta.pit = ai->form_host.DownPackage.data.gimbal.pit;
|
|
cmd_host->gimbal_delta.yaw = ai->form_host.DownPackage.data.gimbal.yaw;
|
|
// cmd_host->gimbal_delta.pit = pit_out;
|
|
// cmd_host->gimbal_delta.yaw = yaw_out;
|
|
cmd_host->gimbal_delta.rol = ai->form_host.DownPackage.data.gimbal.rol;
|
|
cmd_host->fire = (ai->form_host.DownPackage.data.notice & AI_NOTICE_FIRE);
|
|
cmd_host->chassis_move_vec.vx = ai->form_host.DownPackage.data.chassis_move_vec.vx;
|
|
cmd_host->chassis_move_vec.vy = ai->form_host.DownPackage.data.chassis_move_vec.vy;
|
|
cmd_host->chassis_move_vec.wz = ai->form_host.DownPackage.data.chassis_move_vec.wz;
|
|
|
|
return DEVICE_OK;
|
|
|
|
error:
|
|
drop_message++;
|
|
return DEVICE_ERR;
|
|
}
|
|
|
|
int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) {
|
|
if (ai == NULL) return DEVICE_ERR_NULL;
|
|
if (cmd_host == NULL) return DEVICE_ERR_NULL;
|
|
ai->ai_online = false;
|
|
memset(ai->form_host.rx_buffer, 0, sizeof(ai->form_host.rx_buffer));
|
|
memset(&(ai->form_host.DownPackage), 0, sizeof(ai->form_host.DownPackage));
|
|
memset(cmd_host, 0, sizeof(*cmd_host));
|
|
return 0;
|
|
}
|
|
|
|
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat) {
|
|
ai->to_host.mcu.id = AI_ID_MCU;
|
|
memcpy((void *)&(ai->to_host.mcu.package.data.quat), (const void *)quat,
|
|
sizeof(*quat));
|
|
ai->to_host.mcu.package.data.notice = 0;
|
|
if (ai->status == AI_STATUS_AUTOAIM)
|
|
ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOAIM;
|
|
else if (ai->status == AI_STATUS_HITSWITCH)
|
|
ai->to_host.mcu.package.data.notice |= AI_NOTICE_HITBUFF;
|
|
else if (ai->status == AI_STATUS_AUTOMATIC)
|
|
ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOMATIC;
|
|
|
|
ai->to_host.mcu.package.crc16 = CRC16_Calc(
|
|
(const uint8_t *)&(ai->to_host.mcu.package),
|
|
sizeof(ai->to_host.mcu.package) - sizeof(uint16_t), CRC16_INIT);
|
|
return DEVICE_OK;
|
|
}
|
|
|
|
int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref) {
|
|
(void)ref;
|
|
ai->to_host.ref.id = AI_ID_REF;
|
|
ai->to_host.ref.package.crc16 = CRC16_Calc(
|
|
(const uint8_t *)&(ai->to_host.ref.package),
|
|
sizeof(ai->to_host.ref.package) - sizeof(uint16_t), CRC16_INIT);
|
|
return DEVICE_OK;
|
|
}
|
|
|
|
int8_t AI_StartSend(AI_t *ai, bool ref_update) {
|
|
if (ref_update) {
|
|
if (HAL_UART_Transmit_DMA(
|
|
BSP_UART_GetHandle(BSP_UART_AI), (uint8_t *)&(ai->to_host),
|
|
sizeof(ai->to_host.ref) + sizeof(ai->to_host.mcu)) == HAL_OK)
|
|
return DEVICE_OK;
|
|
else
|
|
return DEVICE_ERR;
|
|
} else {
|
|
if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_AI),
|
|
(uint8_t *)&(ai->to_host.mcu),
|
|
sizeof(ai->to_host.mcu)) == HAL_OK)
|
|
return DEVICE_OK;
|
|
else
|
|
return DEVICE_ERR;
|
|
}
|
|
}
|