199 lines
7.7 KiB
C
199 lines
7.7 KiB
C
/* Includes ----------------------------------------------------------------- */
|
||
#include "device/ET16s.h"
|
||
#include "device/device.h"
|
||
#include <string.h>
|
||
//#include "error_detect.h"
|
||
#include "bsp/uart.h"
|
||
#include "component/calc_lib.h"
|
||
|
||
#define R12DS
|
||
//#define DR16
|
||
/*
|
||
两个遥控器都是
|
||
x
|
||
|
|
||
|
|
||
———y
|
||
*/
|
||
#ifdef DR16
|
||
|
||
#define FRAME_LEN 36
|
||
|
||
#endif
|
||
|
||
#ifdef R12DS
|
||
|
||
#define FRAME_LEN 25u
|
||
|
||
#endif
|
||
/* Private define ----------------------------------------------------------- */
|
||
#define REMOTE_CH_VALUE_MIN (364u)
|
||
#define REMOTE_CH_VALUE_MID (1024u)
|
||
#define REMOTE_CH_VALUE_MAX (1684u)
|
||
|
||
/* Private macro ------------------------------------------------------------ */
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private variables -------------------------------------------------------- */
|
||
|
||
|
||
static osThreadId_t thread_alert;
|
||
//uint8_t cbuf[FRAME_LEN]; //此处设置为两帧字节的长度 若为一帧会出现乱码的情况
|
||
//uint8_t cbuf[sizeof(LD_raw_t)];
|
||
uint8_t cbuf[25u];
|
||
/* Private function -------------------------------------------------------- */
|
||
int8_t REMOTE_Restart(void) {
|
||
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_3));
|
||
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_3));
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
static void REMOTE_RxCpltCallback(void) {
|
||
osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY);
|
||
// detect_hook(DR16_TOE);
|
||
}
|
||
//遥控器初始化
|
||
int8_t REMOTE_Init(void) {
|
||
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
|
||
|
||
BSP_UART_RegisterCallback(BSP_UART_3,BSP_UART_RX_CPLT_CB,
|
||
REMOTE_RxCpltCallback);
|
||
memset(cbuf, 0, sizeof(cbuf));//初始化清空数据包
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
int8_t REMOTE_StartDmaRecv(void) {
|
||
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_3),
|
||
(uint8_t *)cbuf,
|
||
sizeof(cbuf)) == HAL_OK)
|
||
return DEVICE_OK;
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
bool REMOTE_WaitDmaCplt(uint32_t timeout) {
|
||
return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) ==
|
||
SIGNAL_DR16_RAW_REDY);
|
||
}
|
||
|
||
|
||
|
||
/*乐迪用的按键封装 */
|
||
ET16S_SwitchPos_t Keymap(int16_t value) {
|
||
return (value == 353) ? ET16S_SW_UP :
|
||
(value == 1024) ? ET16S_SW_MID :
|
||
(value == 1695) ? ET16S_SW_DOWN : ET16S_SW_ERR;
|
||
}
|
||
|
||
int8_t ET16s_ParseRaw( ET16s_t *et16s){
|
||
//ET16s
|
||
if(et16s==NULL)return ET16s_ERR_NULL;
|
||
et16s->header.online = true;
|
||
et16s->raw_data.ch[0] = (cbuf[1] | (cbuf[2] << 8)) & 0x07ff; //Channel 1
|
||
et16s->raw_data.ch[3] = ((cbuf[2] >> 3) | (cbuf[3] << 5)) & 0x07ff; //Channel 2
|
||
et16s->raw_data.ch[1] = ((cbuf[3] >> 6) | (cbuf[4] << 2) | //Channel 3
|
||
(cbuf[5] << 10)) &0x07ff;
|
||
et16s->raw_data.ch[2] = ((cbuf[5] >> 1) | (cbuf[6] << 7)) & 0x07ff; //Channel 4
|
||
|
||
et16s->raw_data.sw[6] = ((int16_t)cbuf[6] >> 4 | ((int16_t)cbuf[7] << 4 )) & 0x07FF; //Channel 5
|
||
et16s->raw_data.sw[0] = ((int16_t)cbuf[7] >> 7 | ((int16_t)cbuf[8] << 1 ) | (int16_t)cbuf[9] << 9 ) & 0x07FF; //Channel 6
|
||
et16s->raw_data.sw[5] = ((int16_t)cbuf[9] >> 2 | ((int16_t)cbuf[10] << 6 )) & 0x07FF;; //Channel 7
|
||
et16s->raw_data.sw[4] = ((int16_t)cbuf[10] >> 5 | ((int16_t)cbuf[11] << 3 )) & 0x07FF; //Channel 8
|
||
et16s->raw_data.sw[3] = ((int16_t)cbuf[12] << 0 | ((int16_t)cbuf[13] << 8 )) & 0x07FF; //Channel 9
|
||
et16s->raw_data.sw[2] = ((int16_t)cbuf[13] >> 3 | ((int16_t)cbuf[14] << 5 )) & 0x07FF; //Channel 10
|
||
et16s->raw_data.sw[1] = ((int16_t)cbuf[14] >> 6 | ((int16_t)cbuf[15] << 2 ) | (int16_t)cbuf[16] << 10 ) & 0x07FF; //Channel 11
|
||
et16s->raw_data.sw[7] = ((int16_t)cbuf[16] >> 1 | ((int16_t)cbuf[17] << 7 )) & 0x07FF;
|
||
|
||
et16s->raw_data.sw[6] = ((int16_t)cbuf[6] >> 4 | ((int16_t)cbuf[7] << 4 )) & 0x07FF; //Channel 5
|
||
et16s->raw_data.sw[0] = ((int16_t)cbuf[7] >> 7 | ((int16_t)cbuf[8] << 1 ) | (int16_t)cbuf[9] << 9 ) & 0x07FF; //Channel 6
|
||
et16s->raw_data.sw[5] = ((int16_t)cbuf[9] >> 2 | ((int16_t)cbuf[10] << 6 )) & 0x07FF;; //Channel 7
|
||
et16s->raw_data.sw[4] = ((int16_t)cbuf[10] >> 5 | ((int16_t)cbuf[11] << 3 )) & 0x07FF; //Channel 8
|
||
et16s->raw_data.sw[3] = ((int16_t)cbuf[12] << 0 | ((int16_t)cbuf[13] << 8 )) & 0x07FF; //Channel 9
|
||
et16s->raw_data.sw[2] = ((int16_t)cbuf[13] >> 3 | ((int16_t)cbuf[14] << 5 )) & 0x07FF; //Channel 10
|
||
et16s->raw_data.sw[1] = ((int16_t)cbuf[14] >> 6 | ((int16_t)cbuf[15] << 2 ) | (int16_t)cbuf[16] << 10 ) & 0x07FF; //Channel 11
|
||
et16s->raw_data.sw[7] = ((int16_t)cbuf[16] >> 1 | ((int16_t)cbuf[17] << 7 )) & 0x07FF;
|
||
|
||
et16s->raw_data.ch[1] = map_fp32( et16s->raw_data.ch[1],353,1695,-1,1); //lx
|
||
et16s->raw_data.ch[0] = map_fp32( et16s->raw_data.ch[0],353,1695,-1,1); //ly
|
||
et16s->raw_data.ch[2] = map_fp32( et16s->raw_data.ch[2],353,1695,-1,1); //rx
|
||
et16s->raw_data.ch[3] = map_fp32( et16s->raw_data.ch[3],353,1695,-1,1); //ry
|
||
|
||
if( et16s->raw_data.ch[0]>-0.01&&et16s->raw_data.ch[0]<=0.01) et16s->raw_data.ch[0]=0;
|
||
if( et16s->raw_data.ch[1]>-0.01&&et16s->raw_data.ch[1]<=0.01) et16s->raw_data.ch[1]=0;
|
||
if( et16s->raw_data.ch[2]>-0.01&&et16s->raw_data.ch[2]<=0.01) et16s->raw_data.ch[2]=0;
|
||
if( et16s->raw_data.ch[3]>-0.01&&et16s->raw_data.ch[3]<=0.01) et16s->raw_data.ch[3]=0;
|
||
return ET16s_OK;
|
||
|
||
}
|
||
|
||
int8_t ET16S_ParseRC(ET16s_t *et16s) {
|
||
if (et16s == NULL) return DEVICE_ERR_NULL;
|
||
|
||
#ifdef DR16
|
||
rc->rc_type=RC_DR16;
|
||
DR16_ParseRaw(dr16);
|
||
if (DR16_DataCorrupted(dr16)) {
|
||
return DEVICE_ERR;
|
||
} else {
|
||
memset(rc, 0, sizeof(*rc));
|
||
}
|
||
|
||
float full_range = (float)(REMOTE_CH_VALUE_MAX - REMOTE_CH_VALUE_MIN);
|
||
|
||
rc->DJ.ch_r_x = 2 * ((float)dr16->data.ch_r_y - REMOTE_CH_VALUE_MID) / full_range;
|
||
rc->DJ.ch_r_y = 2 * ((float)dr16->data.ch_r_x - REMOTE_CH_VALUE_MID) / full_range;
|
||
rc->DJ.ch_l_x = 2 * ((float)dr16->data.ch_l_y - REMOTE_CH_VALUE_MID) / full_range;
|
||
rc->DJ.ch_l_y = 2 * ((float)dr16->data.ch_l_x - REMOTE_CH_VALUE_MID) / full_range;
|
||
|
||
rc->DJ.sw_l = (ET16S_SwitchPos_t)dr16->data.sw_l;
|
||
rc->DJ.sw_r = (ET16S_SwitchPos_t)dr16->data.sw_r;
|
||
|
||
// rc->dr16.key = dr16->data.key;
|
||
|
||
// rc->ch_res = ((float)dr16->data.res - DR16_CH_VALUE_MID) / full_range;
|
||
|
||
#elif defined(R12DS)
|
||
|
||
/*天地飞*/
|
||
et16s->rc_type=RC_ET16s;
|
||
ET16s_ParseRaw(et16s);
|
||
|
||
et16s->ET16s.ch_r_x = et16s->raw_data.ch[2] ;
|
||
et16s->ET16s.ch_r_y = et16s->raw_data.ch[3] ;
|
||
et16s->ET16s.ch_l_x = et16s->raw_data.ch[0] ;
|
||
et16s->ET16s.ch_l_y = et16s->raw_data.ch[1] ;
|
||
|
||
et16s->ET16s.key_A = Keymap(et16s->raw_data.sw[0]);
|
||
et16s->ET16s.key_B = Keymap(et16s->raw_data.sw[1]);
|
||
et16s->ET16s.key_C = Keymap(et16s->raw_data.sw[2]);
|
||
et16s->ET16s.key_D = Keymap(et16s->raw_data.sw[3]);
|
||
et16s->ET16s.key_E = Keymap(et16s->raw_data.sw[4]);
|
||
et16s->ET16s.key_F = Keymap(et16s->raw_data.sw[5]);
|
||
et16s->ET16s.key_G = Keymap(et16s->raw_data.sw[6]);
|
||
et16s->ET16s.key_H = Keymap(et16s->raw_data.sw[7]);
|
||
// rc->ET16s.knob_left = ET16s->sw[7]; //200 330 479 629 778 928 1075 1224 1373 1522 1670 1800
|
||
/* 通道5出了bug，只能这样解决 */
|
||
// switch(et16s->raw_data.sw[4]){
|
||
// case 353:
|
||
// et16s->ET16s.key_E=ET16S_SW_UP;
|
||
// break;
|
||
// case 1024:
|
||
// et16s->ET16s.key_E=ET16S_SW_MID;
|
||
// break;
|
||
// case 1695:
|
||
// et16s->ET16s.key_E=ET16S_SW_DOWN;
|
||
// break;
|
||
// }
|
||
#endif
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
|
||
|
||
int8_t ET16s_HandleOffline(ET16s_t *rc) {
|
||
|
||
if (rc == NULL) return DEVICE_ERR_NULL;
|
||
rc->rc_type =Control_loss ;
|
||
rc->header.online = false;
|
||
memset(&rc->ET16s , 0, sizeof(ET16s_raw_t));
|
||
return 0;
|
||
}
|