212 lines
7.0 KiB
C
212 lines
7.0 KiB
C
/*
|
||
DR16接收机
|
||
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "rc.h"
|
||
|
||
#include <string.h>
|
||
|
||
#include "bsp_usart.h"
|
||
#include "error_detect.h"
|
||
|
||
|
||
//#define DR16_t
|
||
#define LD_t
|
||
|
||
#ifdef DR16
|
||
#define FRAME_LEN 36
|
||
|
||
#elif defined(LD_t)
|
||
|
||
#define FRAME_LEN 25
|
||
|
||
#endif
|
||
/* Private define ----------------------------------------------------------- */
|
||
|
||
#define RC_CH_VALUE_MIN ((uint16_t)364)
|
||
#define RC_CH_VALUE_MID ((uint16_t)1024)
|
||
#define RC_CH_VALUE_MAX ((uint16_t)1684)
|
||
|
||
static osThreadId_t thread_alert;
|
||
static uint8_t cbuf[FRAME_LEN]; //此处设置为两帧字节的长度 若为一帧会出现乱码的情况
|
||
|
||
/*通用，初始化，串口回调注册，dma数据接收*/
|
||
int8_t RC_SBUS_Init(void )
|
||
{
|
||
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
|
||
|
||
BSP_UART_RegisterCallback(BSP_UART_REMOTE, BSP_UART_IDLE_LINE_CB,
|
||
RC_SBUS_RxCpltCallback);
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
static void RC_SBUS_RxCpltCallback(void) {
|
||
osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY);
|
||
// detect_hook(DR16_TOE);
|
||
}
|
||
int8_t RC_SBUS_Restart(void) {
|
||
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_REMOTE));
|
||
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_REMOTE));
|
||
return DEVICE_OK;
|
||
}
|
||
int8_t RC_SBUS_StartDmaRecv(void) {
|
||
|
||
if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_REMOTE),
|
||
(uint8_t *)cbuf,
|
||
sizeof(cbuf)) == HAL_OK)
|
||
return DEVICE_OK;
|
||
return DEVICE_ERR;
|
||
}
|
||
bool RC_SBUS_WaitDmaCplt(uint32_t timeout) {
|
||
return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) ==
|
||
SIGNAL_DR16_RAW_REDY);
|
||
}
|
||
/*乐迪数据解析 */
|
||
int8_t LD_ParseRaw( RC_ctrl_t *rc_ctrl)
|
||
{
|
||
rc_ctrl->ch[0] = (cbuf[1] | (cbuf[2] << 8)) & 0x07ff; //!< Channel 0
|
||
rc_ctrl->ch[1] = ((cbuf[2] >> 3) | (cbuf[3] << 5)) & 0x07ff; //!< Channel 1
|
||
rc_ctrl->ch[2] = ((cbuf[3] >> 6) | (cbuf[4] << 2) | //!< Channel 2
|
||
(cbuf[5] << 10)) &0x07ff;
|
||
rc_ctrl->ch[3] = ((cbuf[5] >> 1) | (cbuf[6] << 7)) & 0x07ff; //!< Channel 3
|
||
|
||
rc_ctrl->sw[0] = ((int16_t)cbuf[6] >> 4 | ((int16_t)cbuf[7] << 4 )) & 0x07FF;
|
||
rc_ctrl->sw[1] = ((int16_t)cbuf[7] >> 7 | ((int16_t)cbuf[8] << 1 ) | (int16_t)cbuf[9] << 9 ) & 0x07FF;
|
||
rc_ctrl->sw[2] = ((int16_t)cbuf[9] >> 2 | ((int16_t)cbuf[10] << 6 )) & 0x07FF;;
|
||
rc_ctrl->sw[3] = ((int16_t)cbuf[10] >> 5 | ((int16_t)cbuf[11] << 3 )) & 0x07FF;
|
||
rc_ctrl->sw[4] = ((int16_t)cbuf[12] << 0 | ((int16_t)cbuf[13] << 8 )) & 0x07FF;
|
||
rc_ctrl->sw[5] = ((int16_t)cbuf[13] >> 3 | ((int16_t)cbuf[14] << 5 )) & 0x07FF;
|
||
rc_ctrl->sw[6] = ((int16_t)cbuf[14] >> 6 | ((int16_t)cbuf[15] << 2 ) | (int16_t)cbuf[16] << 10 ) & 0x07FF;
|
||
rc_ctrl->sw[7] = ((int16_t)cbuf[16] >> 1 | ((int16_t)cbuf[17] << 7 )) & 0x07FF;
|
||
|
||
rc_ctrl->ch[0] -= RC_CH_VALUE_MID;
|
||
rc_ctrl->ch[1] -= RC_CH_VALUE_MID;
|
||
rc_ctrl->ch[2] -= RC_CH_VALUE_MID;
|
||
rc_ctrl->ch[3] -= RC_CH_VALUE_MID;
|
||
|
||
|
||
rc_ctrl->ch[0] += 24; //y(-694,693)左右
|
||
rc_ctrl->ch[1] = -rc_ctrl->ch[1]-24; //x(-693,694)前后
|
||
rc_ctrl->ch[2] = -rc_ctrl->ch[2]+764; //m(95,1482)油门
|
||
rc_ctrl->ch[3] += 24; //w(-694,693)旋转
|
||
|
||
rc_ctrl->ch[1] = map_int(rc_ctrl->ch[1],-693,694,-700,700); //x映射到(-700,700)
|
||
rc_ctrl->ch[0] = map_int(rc_ctrl->ch[0],-694,693,-700,700); //y映射到(-700,700)
|
||
rc_ctrl->ch[3] = 0.5*(rc_ctrl->ch[3]); //w
|
||
|
||
// //死区(-30,30)
|
||
// if(rc_ctrl->ch[0]>-50&&rc_ctrl->ch[0]<50) rc_ctrl->ch[0]=0;
|
||
// if(rc_ctrl->ch[1]>-50&&rc_ctrl->ch[1]<50) rc_ctrl->ch[1]=0;
|
||
// if(rc_ctrl->ch[2]>-50&&rc_ctrl->ch[2]<50) rc_ctrl->ch[2]=0;
|
||
// if(rc_ctrl->ch[3]>-50&&rc_ctrl->ch[3]<50) rc_ctrl->ch[3]=0;
|
||
//
|
||
|
||
rc_ctrl->map_ch[0]=map_fp32(rc_ctrl->ch[0],-719,680,-1,1);
|
||
rc_ctrl->map_ch[1]=map_fp32(rc_ctrl->ch[1],-567,832,-1,1);
|
||
rc_ctrl->map_ch[2]=map_fp32(rc_ctrl->ch[2],95,1482,0,1);
|
||
rc_ctrl->map_ch[3]=map_fp32(rc_ctrl->ch[3],-317,375,-1,1);
|
||
|
||
rc_ctrl->map_ch[0]=expo_map(rc_ctrl->map_ch[0], 0.7f);
|
||
rc_ctrl->map_ch[1]=expo_map(rc_ctrl->map_ch[1],0.7f);
|
||
rc_ctrl->map_ch[2]=expo_map(rc_ctrl->map_ch[2],0.7f);
|
||
rc_ctrl->map_ch[3]=expo_map(rc_ctrl->map_ch[3],0.7f);
|
||
|
||
//死区(-30,30)
|
||
if(rc_ctrl->map_ch[0]>-0.05f&&rc_ctrl->map_ch[0]<0.05f) rc_ctrl->map_ch[0]=0;
|
||
if(rc_ctrl->map_ch[1]>-0.05f&&rc_ctrl->map_ch[1]<0.05f) rc_ctrl->map_ch[1]=0;
|
||
if(rc_ctrl->map_ch[2]>-0.05f&&rc_ctrl->map_ch[2]<0.05f) rc_ctrl->map_ch[2]=0;
|
||
if(rc_ctrl->map_ch[3]>-0.05f&&rc_ctrl->map_ch[3]<0.05f) rc_ctrl->map_ch[3]=0;
|
||
return 1;
|
||
|
||
}
|
||
/*dr16数据解析+校验 */
|
||
static bool DR16_DataCorrupted(const DR16_t *dr16) {
|
||
if (dr16 == NULL) return DEVICE_ERR_NULL;
|
||
|
||
if ((dr16->data.ch_r_x < RC_CH_VALUE_MIN) ||
|
||
(dr16->data.ch_r_x > RC_CH_VALUE_MAX))
|
||
return true;
|
||
|
||
if ((dr16->data.ch_r_y < RC_CH_VALUE_MIN) ||
|
||
(dr16->data.ch_r_y > RC_CH_VALUE_MAX))
|
||
return true;
|
||
|
||
if ((dr16->data.ch_l_x < RC_CH_VALUE_MIN) ||
|
||
(dr16->data.ch_l_x > RC_CH_VALUE_MAX))
|
||
return true;
|
||
|
||
if ((dr16->data.ch_l_y < 1000u) ||
|
||
(dr16->data.ch_l_y > RC_CH_VALUE_MAX))
|
||
return true;
|
||
|
||
if (dr16->data.sw_l == 0) return true;
|
||
|
||
if (dr16->data.sw_r == 0) return true;
|
||
|
||
return false;
|
||
}
|
||
int8_t DR16_ParseRaw(DR16_t *dr16)
|
||
{
|
||
if(dr16 ==NULL) return DEVICE_ERR_NULL;
|
||
dr16->data.ch_r_x = (cbuf[0] | (cbuf[1] <<8));
|
||
dr16->data.ch_r_y = ((cbuf[1] >> 3)| (cbuf[2] << 5));
|
||
dr16->data.ch_l_x = ((cbuf[2] >>6) | (cbuf[3] << 2) | (cbuf[4] <<10));
|
||
dr16->data.ch_l_y = ((cbuf[4] >>1) | (cbuf[5] <<7));
|
||
dr16->data.sw_r = ((cbuf[5] >>4));
|
||
dr16->data.sw_l = ((cbuf[5] >> 4) & 0x000C) >> 2;
|
||
|
||
return 1;
|
||
}
|
||
int8_t RC_ParseRC( DR16_t *dr16, RC_ctrl_t *rc_ctrl, CMD_RC_t *rc) {
|
||
if (dr16 == NULL) return DEVICE_ERR_NULL;
|
||
if (rc_ctrl == NULL) return DEVICE_ERR_NULL;
|
||
#ifdef DR16
|
||
/*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)(RC_CH_VALUE_MAX - RC_CH_VALUE_MIN);
|
||
|
||
rc->dr16.ch_r_x = 2 * ((float)dr16->data.ch_r_x - RC_CH_VALUE_MID) / full_range;
|
||
rc->dr16.ch_r_y = 2 * ((float)dr16->data.ch_r_y - RC_CH_VALUE_MID) / full_range;
|
||
rc->dr16.ch_l_x = 2 * ((float)dr16->data.ch_l_x - RC_CH_VALUE_MID) / full_range;
|
||
rc->dr16.ch_l_y = 2 * ((float)dr16->data.ch_l_y - RC_CH_VALUE_MID) / full_range;
|
||
|
||
rc->dr16.sw_l = (CMD_SwitchPos_t)dr16->data.sw_l;
|
||
rc->dr16.sw_r = (CMD_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(LD_t)
|
||
|
||
/*乐迪*/
|
||
rc->rc_type=RC_LD;
|
||
LD_ParseRaw(rc_ctrl);
|
||
|
||
memcpy(&rc->LD, rc_ctrl, sizeof(RC_ctrl_t));
|
||
#endif
|
||
return DEVICE_OK;
|
||
|
||
}
|
||
|
||
|
||
/*离线处理*/
|
||
int8_t DR16_HandleOffline(const DR16_t *dr16, CMD_RC_t *rc) {
|
||
if (dr16 == NULL) return DEVICE_ERR_NULL;
|
||
if (rc == NULL) return DEVICE_ERR_NULL;
|
||
|
||
(void)dr16;
|
||
memset(rc, 0, sizeof(*rc));
|
||
return 0;
|
||
}
|