/* DR16接收机 */ /* Includes ----------------------------------------------------------------- */ #include "dr16.h" #include #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; //!< Switch left rc_ctrl->sw[1] = ((int16_t)cbuf[7] >> 7 | ((int16_t)cbuf[8] << 1 ) | (int16_t)cbuf[9] << 9 ) & 0x07FF; //!< Switch right rc_ctrl->sw[2] = ((int16_t)cbuf[9] >> 2 | ((int16_t)cbuf[10] << 6 )) & 0x07FF;; //!< Mouse X axis rc_ctrl->sw[3] = ((int16_t)cbuf[10] >> 5 | ((int16_t)cbuf[11] << 3 )) & 0x07FF; //!< Mouse Y axis rc_ctrl->sw[4] = ((int16_t)cbuf[12] << 0 | ((int16_t)cbuf[13] << 8 )) & 0x07FF; //!< Mouse Z axis rc_ctrl->sw[5] = ((int16_t)cbuf[13] >> 3 | ((int16_t)cbuf[14] << 5 )) & 0x07FF; //!< Mouse Left Is Press ? rc_ctrl->sw[6] = ((int16_t)cbuf[14] >> 6 | ((int16_t)cbuf[15] << 2 ) | (int16_t)cbuf[16] << 10 ) & 0x07FF; //!< Mouse Right Is Press ? rc_ctrl->sw[7] = ((int16_t)cbuf[16] >> 1 | ((int16_t)cbuf[17] << 7 )) & 0x07FF; //!< KeyBoard value 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.05&&rc_ctrl->map_ch[0]<0.05) rc_ctrl->map_ch[0]=0; if(rc_ctrl->map_ch[1]>-0.05&&rc_ctrl->map_ch[1]<0.05) rc_ctrl->map_ch[1]=0; if(rc_ctrl->map_ch[2]>-0.05&&rc_ctrl->map_ch[2]<0.05) rc_ctrl->map_ch[2]=0; if(rc_ctrl->map_ch[3]>-0.05&&rc_ctrl->map_ch[3]<0.05) rc_ctrl->map_ch[3]=0; } /*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; }