LYL/R2_NEW
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

增加乐迪的离线处理

Browse Source
This commit is contained in:
ZHAISHUI04 2025-04-25 23:25:19 +08:00
parent 9df55fdc08
commit 5ff6e56d70
7 changed files with 166 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
MDK-ARM/R2.uvoptx
View File

@ -155,6 +155,11 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>rc_ctrl,0x0A</ItemText> <ItemText>rc_ctrl,0x0A</ItemText>
</Ww> </Ww>
<Ww>
<count>1</count>
<WinNumber>1</WinNumber>
<ItemText>LD_raw,0x0A</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>

BIN
MDK-ARM/R2/R2.axf
View File

Binary file not shown.

14
User/device/cmd.c
View File

@ -28,7 +28,7 @@ return 0;
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) { static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) {
if(rc->rc_type==RC_DR16){
cmd->Vx = rc->dr16.ch_r_x; cmd->Vx = rc->dr16.ch_r_x;
cmd->Vy = rc->dr16.ch_r_y; cmd->Vy = rc->dr16.ch_r_y;
cmd->Vw = rc->dr16.ch_l_x; cmd->Vw = rc->dr16.ch_l_x;
@ -37,7 +37,16 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) {
cmd->key_ctrl_l = rc->dr16.sw_l; cmd->key_ctrl_l = rc->dr16.sw_l;
cmd->key_ctrl_r = rc->dr16.sw_r ; cmd->key_ctrl_r = rc->dr16.sw_r ;
}
if(rc->rc_type==RC_LD){
cmd->Vx = rc->LD.ch_r_x;
cmd->Vy = rc->LD.ch_r_y ;
cmd->Vw = rc->LD .ch_l_x ;
cmd->poscamear = rc->LD.ch_l_y;
}
} }
@ -59,7 +68,8 @@ int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc){
/*c当rc丢控时，恢复机器人至默认状态 */ /*c当rc丢控时，恢复机器人至默认状态 */
if ((rc->dr16.sw_l == CMD_SW_ERR) || (rc->dr16.sw_r == CMD_SW_ERR)) { if ((rc->dr16.sw_l == CMD_SW_ERR) || (rc->dr16.sw_r == CMD_SW_ERR)) {
CMD_RcLostLogic(cmd); CMD_RcLostLogic(cmd);
} else { }
else {
CMD_RcLogic(rc, cmd); CMD_RcLogic(rc, cmd);
} }
return 0; return 0;

48
User/device/cmd.h
View File

@ -2,7 +2,10 @@
#define _CMD_H #define _CMD_H
#include "struct_typedef.h" #include "struct_typedef.h"
#include "device.h" #include "device.h"
#include "cmd.h"
#include "gpio.h"
#include "user_math.h"
#include <string.h>
#define MID (0x09) #define MID (0x09)
#define PICK (0x06) #define PICK (0x06)
@ -56,7 +59,9 @@ typedef enum {
} CMD_SwitchPos_t; } CMD_SwitchPos_t;
typedef enum{ typedef enum{
RC_DR16, RC_DR16,
RC_LD RC_LD ,
Control_loss
}rc_type_t; }rc_type_t;
/*遥控器值，使用CMD_RcLogic函数传入CMD_t结构体*/ /*遥控器值，使用CMD_RcLogic函数传入CMD_t结构体*/
typedef struct { typedef struct {
@ -78,9 +83,22 @@ typedef struct {
}__attribute__((packed))dr16; }__attribute__((packed))dr16;
struct { struct {
fp32 map_ch[4]; fp32 ch_r_x ;
int16_t ch[4]; fp32 ch_r_y ;
int16_t sw[8]; fp32 ch_l_x ;
fp32 ch_l_y ;
int16_t key_A ;
// int16_t key_B ;//无
int16_t key_C ;
// int16_t key_D ;//无
int16_t key_E ;
int16_t key_F;
int16_t key_G ;
int16_t key_H ;
int16_t knob_left;//左旋钮
int16_t knob_right;//右旋钮
}__attribute__((packed))LD; }__attribute__((packed))LD;
@ -89,6 +107,9 @@ typedef struct {
typedef struct { typedef struct {
CMD_mode_t CMD_mode;
CMD_CtrlType_t CMD_CtrlType;
uint8_t cmd_status; uint8_t cmd_status;
uint8_t raw_status; uint8_t raw_status;
uint8_t status[7]; uint8_t status[7];
@ -116,28 +137,11 @@ typedef struct {
CMD_mode_t CMD_mode;
CMD_CtrlType_t CMD_CtrlType;
} CMD_t; } CMD_t;
/*nuc数据统一到cmd*/
/*
该任务用来处理来自不同控制方式下传回的期望的控制指令在此处统一为通用格式的控制指令发送给其他任务
*/
/* Includes ----------------------------------------------------------------- */
#include "cmd.h"
#include "gpio.h"
#include "user_math.h"
#include <string.h>
/* Private function -------------------------------------------------------- */
/*Export function --------------------------------------------------------------*/
int8_t CMD_Init(CMD_t *cmd); int8_t CMD_Init(CMD_t *cmd);

132
User/device/rc.c
View File

@ -12,8 +12,8 @@
#include "error_detect.h" #include "error_detect.h"
#define DR16 //#define DR16
//#define LD_t #define LD_t
#ifdef DR16 #ifdef DR16
#define FRAME_LEN 36 #define FRAME_LEN 36
@ -64,60 +64,76 @@ bool RC_SBUS_WaitDmaCplt(uint32_t timeout) {
SIGNAL_DR16_RAW_REDY); SIGNAL_DR16_RAW_REDY);
} }
/*乐迪数据解析 */ /*乐迪数据解析 */
int8_t LD_ParseRaw( RC_ctrl_t *rc_ctrl) int8_t LD_ParseRaw( LD_raw_t *raw, LD_Data_t *LD)
{ {
rc_ctrl->ch[0] = (cbuf[1] | (cbuf[2] << 8)) & 0x07ff; //!< Channel 0 raw->ch[0] = (cbuf[1] | (cbuf[2] << 8)) & 0x07ff; //!< Channel 0
rc_ctrl->ch[1] = ((cbuf[2] >> 3) | (cbuf[3] << 5)) & 0x07ff; //!< Channel 1 raw->ch[1] = ((cbuf[2] >> 3) | (cbuf[3] << 5)) & 0x07ff; //!< Channel 1
rc_ctrl->ch[2] = ((cbuf[3] >> 6) | (cbuf[4] << 2) | //!< Channel 2 raw->ch[2] = ((cbuf[3] >> 6) | (cbuf[4] << 2) | //!< Channel 2
(cbuf[5] << 10)) &0x07ff; (cbuf[5] << 10)) &0x07ff;
rc_ctrl->ch[3] = ((cbuf[5] >> 1) | (cbuf[6] << 7)) & 0x07ff; //!< Channel 3 raw->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; raw->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; raw->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;; raw->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; raw->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; raw->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; raw->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; raw->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; raw->sw[7] = ((int16_t)cbuf[16] >> 1 | ((int16_t)cbuf[17] << 7 )) & 0x07FF;
rc_ctrl->ch[0] -= RC_CH_VALUE_MID; raw->ch[0] -= RC_CH_VALUE_MID;
rc_ctrl->ch[1] -= RC_CH_VALUE_MID; raw->ch[1] -= RC_CH_VALUE_MID;
rc_ctrl->ch[2] -= RC_CH_VALUE_MID; raw->ch[2] -= RC_CH_VALUE_MID;
rc_ctrl->ch[3] -= RC_CH_VALUE_MID; raw->ch[3] -= RC_CH_VALUE_MID;
rc_ctrl->ch[0] += 24; //y(-694,693)左右 raw->ch[0] += 24; //y(-694,693)左右
rc_ctrl->ch[1] = -rc_ctrl->ch[1]-24; //x(-693,694)前后 raw->ch[1] = -raw->ch[1]-24; //x(-693,694)前后
rc_ctrl->ch[2] = -rc_ctrl->ch[2]+764; //m(95,1482)油门 raw->ch[2] = -raw->ch[2]+764; //m(95,1482)油门
rc_ctrl->ch[3] += 24; //w(-694,693)旋转 raw->ch[3] += 24; //w(-694,693)旋转
rc_ctrl->ch[1] = map_int(rc_ctrl->ch[1],-693,694,-700,700); //x映射到(-700,700) raw->ch[1] = map_int(raw->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) raw->ch[0] = map_int(raw->ch[0],-694,693,-700,700); //y映射到(-700,700)
rc_ctrl->ch[3] = 0.5*(rc_ctrl->ch[3]); //w raw->ch[3] = 0.5*(raw->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); raw->map_ch[0]=map_fp32(raw->ch[0],-719,680,-1,1);
rc_ctrl->map_ch[1]=map_fp32(rc_ctrl->ch[1],-567,832,-1,1); raw->map_ch[1]=map_fp32(raw->ch[1],-567,832,-1,1);
rc_ctrl->map_ch[2]=map_fp32(rc_ctrl->ch[2],95,1482,0,1); raw->map_ch[2]=map_fp32(raw->ch[2],95,1482,0,1);
rc_ctrl->map_ch[3]=map_fp32(rc_ctrl->ch[3],-317,375,-1,1); raw->map_ch[3]=map_fp32(raw->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); raw->map_ch[0]=expo_map(raw->map_ch[0], 0.7f);
rc_ctrl->map_ch[2]=expo_map(rc_ctrl->map_ch[2],0.7f); raw->map_ch[1]=expo_map(raw->map_ch[1],0.7f);
rc_ctrl->map_ch[3]=expo_map(rc_ctrl->map_ch[3],0.7f); raw->map_ch[2]=expo_map(raw->map_ch[2],0.7f);
raw->map_ch[3]=expo_map(raw->map_ch[3],0.7f);
//死区(-30,30) //死区(-30,30)
if(rc_ctrl->map_ch[0]>-0.05f&&rc_ctrl->map_ch[0]<0.05f) rc_ctrl->map_ch[0]=0; if(raw->map_ch[0]>-0.05f&&raw->map_ch[0]<0.05f) raw->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(raw->map_ch[1]>-0.05f&&raw->map_ch[1]<0.05f) raw->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(raw->map_ch[2]>-0.05f&&raw->map_ch[2]<0.05f) raw->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; if(raw->map_ch[3]>-0.05f&&raw->map_ch[3]<0.05f) raw->map_ch[3]=0;
/*dr16采用的是复制内存，这里因为内存顺序不同没用*/
LD->ch_l_x= raw->map_ch[3];
LD->ch_l_y =raw->map_ch[2];
LD->ch_r_x= raw->map_ch[0];
LD->ch_r_y =raw->map_ch[1];
LD->key_A =raw->sw[0];
// LD->key_B =rc_ctrl->sw[0]
LD->key_C =raw->sw[2];
// LD->key_D =rc_ctrl->sw[0]
LD->key_E =raw->sw[5];
LD->key_F =raw->sw[3];
LD->key_G =raw->sw[7];
LD->key_H =raw->sw[4];
LD->knob_left=raw->sw[1];
LD->knob_right =raw->sw[6];
return 1; return 1;
} }
@ -159,9 +175,9 @@ int8_t DR16_ParseRaw(DR16_t *dr16)
return 1; return 1;
} }
int8_t RC_ParseRC( DR16_t *dr16, RC_ctrl_t *rc_ctrl, CMD_RC_t *rc) { int8_t RC_ParseRC( DR16_t *dr16,LD_raw_t *LD_raw, LD_Data_t *LD, CMD_RC_t *rc) {
if (dr16 == NULL) return DEVICE_ERR_NULL; if (dr16 == NULL) return DEVICE_ERR_NULL;
if (rc_ctrl == NULL) return DEVICE_ERR_NULL; if (LD == NULL) return DEVICE_ERR_NULL;
#ifdef DR16 #ifdef DR16
/*DR16部分*/ /*DR16部分*/
rc->rc_type=RC_DR16; rc->rc_type=RC_DR16;
@ -191,21 +207,37 @@ int8_t RC_ParseRC( DR16_t *dr16, RC_ctrl_t *rc_ctrl, CMD_RC_t *rc) {
/*乐迪*/ /*乐迪*/
rc->rc_type=RC_LD; rc->rc_type=RC_LD;
LD_ParseRaw(rc_ctrl); LD_ParseRaw(LD_raw,LD);
memcpy(&rc->LD, LD, sizeof(LD_Data_t));
/*离线处理，和dr16位置不同*/
memcpy(&rc->LD, rc_ctrl, sizeof(RC_ctrl_t)); if(LD_raw->sw[7] != 393)
{
LD_HandleOffline(LD,rc);
memset(cbuf, 0, sizeof(cbuf)); //有时候会出现消息数组错位，所以直接清空，在离线和指定按键不对的情况下，原数据不可信
}
#endif #endif
return DEVICE_OK; return DEVICE_OK;
} }
/*离线处理*/ /*dr16离线处理*/
int8_t DR16_HandleOffline(const DR16_t *dr16, CMD_RC_t *rc) { int8_t DR16_HandleOffline(const DR16_t *dr16, CMD_RC_t *rc) {
if (dr16 == NULL) return DEVICE_ERR_NULL; if (dr16 == NULL) return DEVICE_ERR_NULL;
if (rc == NULL) return DEVICE_ERR_NULL; if (rc == NULL) return DEVICE_ERR_NULL;
rc->rc_type =Control_loss ;
(void)dr16; (void)dr16;
memset(rc, 0, sizeof(*rc)); memset(&rc->dr16 , 0, sizeof(DR16_t));
return 0; return 0;
} }
int8_t LD_HandleOffline(const LD_Data_t *LD, CMD_RC_t *rc) {
if (LD == NULL) return DEVICE_ERR_NULL;
if (rc == NULL) return DEVICE_ERR_NULL;
rc->rc_type =Control_loss ;
memset(&rc->LD , 0, sizeof(LD_Data_t));
return 0;
}

36
User/device/rc.h
View File

@ -32,23 +32,36 @@ typedef struct {
DR16_Data_t data; DR16_Data_t data;
} DR16_t; } DR16_t;
/*原始数据*/
//#define SBUS_RX_BUF_NUM 25u
//#define RC_FRAME_LENGTH 18u
typedef __packed struct typedef __packed struct
{ {
fp32 map_ch[4]; fp32 map_ch[4];
int16_t ch[4]; int16_t ch[4];
int16_t sw[8]; int16_t sw[8];
} RC_ctrl_t; } LD_raw_t;
typedef __packed struct
{
fp32 ch_r_x ;
fp32 ch_r_y ;
fp32 ch_l_x ;
fp32 ch_l_y ;
int16_t key_A ;
// int16_t key_B ;//无
int16_t key_C ;
// int16_t key_D ;//无
int16_t key_E ;
int16_t key_F;
int16_t key_G ;
int16_t key_H ;
int16_t knob_left;//左旋钮
int16_t knob_right;//右旋钮
} LD_Data_t;
@ -60,11 +73,12 @@ static void RC_SBUS_RxCpltCallback(void) ;
int8_t RC_SBUS_Restart(void) ; int8_t RC_SBUS_Restart(void) ;
int8_t RC_SBUS_StartDmaRecv(void) ; int8_t RC_SBUS_StartDmaRecv(void) ;
bool RC_SBUS_WaitDmaCplt(uint32_t timeout) ; bool RC_SBUS_WaitDmaCplt(uint32_t timeout) ;
int8_t LD_ParseRaw( RC_ctrl_t *rc_ctrl); int8_t LD_ParseRaw( LD_raw_t *raw, LD_Data_t *LD);
static bool DR16_DataCorrupted(const DR16_t *dr16) ; static bool DR16_DataCorrupted(const DR16_t *dr16) ;
int8_t DR16_ParseRaw(DR16_t *dr16); int8_t DR16_ParseRaw(DR16_t *dr16);
int8_t RC_ParseRC( DR16_t *dr16, RC_ctrl_t *rc_ctrl, CMD_RC_t *rc) ; int8_t RC_ParseRC( DR16_t *dr16,LD_raw_t *LD_raw, LD_Data_t *LD, CMD_RC_t *rc) ;
int8_t DR16_HandleOffline(const DR16_t *dr16, CMD_RC_t *rc) ; int8_t DR16_HandleOffline(const DR16_t *dr16, CMD_RC_t *rc) ;
int8_t LD_HandleOffline(const LD_Data_t *LD, CMD_RC_t *rc) ;
#endif #endif

8
User/task/rc_task.c
View File

@ -15,7 +15,8 @@
#ifdef DEBUG #ifdef DEBUG
DR16_t dr16; DR16_t dr16;
CMD_RC_t cmd_rc; CMD_RC_t cmd_rc;
RC_ctrl_t ld; LD_raw_t LD_raw;
LD_Data_t LD;
#else #else
static DR16_t dr16; static DR16_t dr16;
static CMD_RC_t cmd_rc; static CMD_RC_t cmd_rc;
@ -46,10 +47,11 @@ void Task_rc(void *argument) {
if (RC_SBUS_WaitDmaCplt(30)) { if (RC_SBUS_WaitDmaCplt(30)) {
RC_ParseRC(&dr16,&ld, &cmd_rc); RC_ParseRC(&dr16,&LD_raw,&LD, &cmd_rc);
} else { } else {
/* 处理遥控器离线 */ /* 处理dr16遥控器离线 ,乐迪不可用*/
DR16_HandleOffline(&dr16, &cmd_rc); DR16_HandleOffline(&dr16, &cmd_rc);
LD_HandleOffline(&LD,&cmd_rc);
} }
osMessageQueueReset(task_runtime.msgq.cmd.raw.rc); osMessageQueueReset(task_runtime.msgq.cmd.raw.rc);
osMessageQueuePut(task_runtime.msgq.cmd.raw.rc, &cmd_rc, 0, 0); osMessageQueuePut(task_runtime.msgq.cmd.raw.rc, &cmd_rc, 0, 0);