122 lines
4.7 KiB
C
122 lines
4.7 KiB
C
/* Includes ----------------------------------------------------------------- */
|
|
#include "AT9S_Pro.h"
|
|
|
|
#include <string.h>
|
|
#include "bsp/uart.h"
|
|
#include "device/device.h"
|
|
|
|
/* Private define ----------------------------------------------------------- */
|
|
#define DEAD_AREA 0.025f
|
|
/* Private variables -------------------------------------------------------- */
|
|
static osThreadId_t thread_alert;
|
|
static bool inited = false;
|
|
|
|
/* Private function --------------------------------------------------------- */
|
|
static void AT9S_RxCpltCallback(void)
|
|
{
|
|
osThreadFlagsSet(thread_alert, SIGNAL_AT9S_RAW_REDY);
|
|
}
|
|
|
|
/* Exported functions ------------------------------------------------------- */
|
|
int8_t AT9S_Init(AT9S_t *at9s)
|
|
{
|
|
if (at9s == 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_AT9S_PRO, BSP_UART_RX_CPLT_CB,
|
|
AT9S_RxCpltCallback);
|
|
|
|
inited = true;
|
|
return DEVICE_OK;
|
|
}
|
|
|
|
int8_t AT9S_Restart(void)
|
|
{
|
|
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_AT9S_PRO));
|
|
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_AT9S_PRO));
|
|
return DEVICE_OK;
|
|
}
|
|
|
|
int8_t AT9S_StartDmaRecv(uint8_t *cmd_buffer)
|
|
{
|
|
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AT9S_PRO),
|
|
cmd_buffer,
|
|
AT9S_FRAME_LEN) == HAL_OK)
|
|
return DEVICE_OK;
|
|
return DEVICE_ERR;
|
|
}
|
|
|
|
bool AT9S_WaitDmaCplt(uint32_t timeout)
|
|
{
|
|
return (osThreadFlagsWait(SIGNAL_AT9S_RAW_REDY,
|
|
osFlagsWaitAll, timeout) == SIGNAL_AT9S_RAW_REDY);
|
|
}
|
|
AT9S_Raw_t r;
|
|
/* 解析 25 字节原始帧并填充结构体 */
|
|
void AT9S_ParseRaw(const uint8_t raw[AT9S_FRAME_LEN], AT9S_t *out)
|
|
{
|
|
// AT9S_Raw_t r;
|
|
|
|
/* 摇杆 */
|
|
r.ch[0] = ((raw[1] | raw[2] << 8) & 0x07FF);
|
|
r.ch[1] = ((raw[2] >> 3 | raw[3] << 5) & 0x07FF);
|
|
r.ch[2] = ((raw[3] >> 6 | raw[4] << 2 | raw[5] << 10) & 0x07FF);
|
|
r.ch[3] = ((raw[5] >> 1 | raw[6] << 7) & 0x07FF);
|
|
|
|
/* 开关/旋钮 */
|
|
r.sw[0] = ((raw[6] >> 4 | raw[7] << 4) & 0x07FF);
|
|
r.sw[1] = ((raw[7] >> 7 | raw[8] << 1 | raw[9] << 9) & 0x07FF);
|
|
r.sw[2] = ((raw[9] >> 2 | raw[10]<< 6) & 0x07FF);
|
|
r.sw[3] = ((raw[10]>> 5 | raw[11]<< 3) & 0x07FF);
|
|
r.sw[4] = ((raw[12] | raw[13]<< 8) & 0x07FF);
|
|
r.sw[5] = ((raw[13]>> 3 | raw[14]<< 5) & 0x07FF);
|
|
r.sw[6] = ((raw[14]>> 6 | raw[15]<< 2 | raw[16]<< 10) & 0x07FF);
|
|
r.sw[7] = ((raw[16]>> 1 | raw[17]<< 7) & 0x07FF);
|
|
|
|
/* 开关映射 */
|
|
#define MAP_SWITCH(v) \
|
|
((v) > 300 && (v) < 500) ? AT9S_CMD_SW_DOWN : \
|
|
((v) >= 500 && (v) < 1500) ? AT9S_CMD_SW_MID : \
|
|
((v) >= 1500 && (v) < 1700) ? AT9S_CMD_SW_UP : AT9S_CMD_SW_ERR
|
|
|
|
#define MAP_SWITCH_RESERVE(v) \
|
|
((v) > 300 && (v) < 500) ? AT9S_CMD_SW_UP : \
|
|
((v) >= 500 && (v) < 1500) ? AT9S_CMD_SW_MID : \
|
|
((v) >= 1500 && (v) < 1700) ? AT9S_CMD_SW_DOWN : AT9S_CMD_SW_ERR
|
|
|
|
out->data.rc.ch_l_x = 2.0f*(r.ch[3]-209.0f)/(1596.0f-209.0f)-1.0f;
|
|
out->data.rc.ch_l_y = 2.0f*(r.ch[2]-178.0f)/(1575.0f-198.0f)-1.0f;
|
|
out->data.rc.ch_r_x = 2.0f*(r.ch[0]-210.0f)/(1596.0f-210.0f)-1.0f;
|
|
out->data.rc.ch_r_y = 2.0f*(r.ch[1]-221.0f)/(1604.0f-217.0f)-1.0f;
|
|
|
|
if(fabs(out->data.rc.ch_l_x)<=DEAD_AREA)out->data.rc.ch_l_x = 0;
|
|
if(fabs(out->data.rc.ch_l_y)<=4*DEAD_AREA)out->data.rc.ch_l_y = 0;
|
|
if(fabs(out->data.rc.ch_r_x)<=DEAD_AREA)out->data.rc.ch_r_x = 0;
|
|
if(fabs(out->data.rc.ch_r_y)<=DEAD_AREA)out->data.rc.ch_r_y = 0;
|
|
|
|
if(fabs(out->data.rc.ch_l_x)>=1.0f)out->data.rc.ch_l_x= out->data.rc.ch_l_x>0?1.0f:-1.0f;
|
|
if(fabs(out->data.rc.ch_l_y)>=1.0f)out->data.rc.ch_l_y= out->data.rc.ch_l_y>0?1.0f:-1.0f;
|
|
if(fabs(out->data.rc.ch_r_x)>=1.0f)out->data.rc.ch_r_x= out->data.rc.ch_r_x>0?1.0f:-1.0f;
|
|
if(fabs(out->data.rc.ch_r_y)>=1.0f)out->data.rc.ch_r_y= out->data.rc.ch_r_y>0?1.0f:-1.0f;
|
|
// out->data.ch_r_x = r.ch[0];
|
|
// out->data.ch_r_y = r.ch[1];
|
|
// out->data.ch_l_y = r.ch[2];
|
|
// out->data.ch_l_x = r.ch[3];
|
|
|
|
out->data.rc.key_A = MAP_SWITCH_RESERVE(r.sw[0]);
|
|
out->data.rc.key_B = MAP_SWITCH_RESERVE(r.sw[1]);
|
|
out->data.rc.key_C = MAP_SWITCH_RESERVE(r.sw[2]);
|
|
out->data.rc.key_D = MAP_SWITCH(r.sw[3]);
|
|
out->data.rc.key_E = MAP_SWITCH(r.sw[4]);
|
|
out->data.rc.key_F = MAP_SWITCH(r.sw[5]);
|
|
out->data.rc.key_G = MAP_SWITCH_RESERVE(r.sw[6]);
|
|
out->data.rc.key_H = MAP_SWITCH(r.sw[7]);
|
|
// out->knob_left = MAP_SWITCH(r.sw[6]);
|
|
// out->knob_right = MAP_SWITCH(r.sw[7]);
|
|
|
|
#undef MAP_SWITCH
|
|
//check online.
|
|
out->header.online = (r.sw[7] != 300&&r.ch[0] != 1000);
|
|
}
|