diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx
index 7424a0e..aeb487c 100644
--- a/MDK-ARM/R2.uvoptx
+++ b/MDK-ARM/R2.uvoptx
@@ -155,6 +155,11 @@
1
rc_ctrl,0x0A
+
+ 1
+ 1
+ LD_raw,0x0A
+
0
diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf
index 801082c..9d5cef0 100644
Binary files a/MDK-ARM/R2/R2.axf and b/MDK-ARM/R2/R2.axf differ
diff --git a/User/device/cmd.c b/User/device/cmd.c
index 316793a..2778cec 100644
--- a/User/device/cmd.c
+++ b/User/device/cmd.c
@@ -28,17 +28,26 @@ return 0;
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->Vy = rc->dr16.ch_r_y;
+ cmd->Vw = rc->dr16.ch_l_x;
- cmd->Vx = rc->dr16.ch_r_x;
- cmd->Vy = rc->dr16.ch_r_y;
- cmd->Vw = rc->dr16.ch_l_x;
+ cmd->poscamear = rc->dr16.ch_l_y;
- cmd->poscamear = rc->dr16.ch_l_y;
+ cmd->key_ctrl_l = rc->dr16.sw_l;
+ cmd->key_ctrl_r = rc->dr16.sw_r ;
+ }
+ if(rc->rc_type==RC_LD){
- cmd->key_ctrl_l = rc->dr16.sw_l;
- cmd->key_ctrl_r = rc->dr16.sw_r ;
+ 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丢控时，恢复机器人至默认状态 */
if ((rc->dr16.sw_l == CMD_SW_ERR) || (rc->dr16.sw_r == CMD_SW_ERR)) {
CMD_RcLostLogic(cmd);
- } else {
+ }
+ else {
CMD_RcLogic(rc, cmd);
}
return 0;
diff --git a/User/device/cmd.h b/User/device/cmd.h
index 9efab0d..0bc6543 100644
--- a/User/device/cmd.h
+++ b/User/device/cmd.h
@@ -2,7 +2,10 @@
#define _CMD_H
#include "struct_typedef.h"
#include "device.h"
-
+#include "cmd.h"
+#include "gpio.h"
+#include "user_math.h"
+#include
#define MID (0x09)
#define PICK (0x06)
@@ -56,7 +59,9 @@ typedef enum {
} CMD_SwitchPos_t;
typedef enum{
RC_DR16,
- RC_LD
+ RC_LD ,
+ Control_loss
+
}rc_type_t;
/*遥控器值，使用CMD_RcLogic函数传入CMD_t结构体*/
typedef struct {
@@ -78,9 +83,22 @@ typedef struct {
}__attribute__((packed))dr16;
struct {
- fp32 map_ch[4];
- int16_t ch[4];
- int16_t sw[8];
+ 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;//右旋钮
+
}__attribute__((packed))LD;
@@ -88,7 +106,10 @@ typedef struct {
} __attribute__((packed))CMD_RC_t;
typedef struct {
-
+
+ CMD_mode_t CMD_mode;
+ CMD_CtrlType_t CMD_CtrlType;
+
uint8_t cmd_status;
uint8_t raw_status;
uint8_t status[7];
@@ -115,29 +136,12 @@ typedef struct {
}cmd_MID360;
-
- CMD_mode_t CMD_mode;
- CMD_CtrlType_t CMD_CtrlType;
+
} CMD_t;
-/*nuc数据统一到cmd*/
-/*
-该任务用来处理来自不同控制方式下传回的期望的控制指令在此处统一为通用格式的控制指令发送给其他任务
-*/
-
-
-
-/* Includes ----------------------------------------------------------------- */
-#include "cmd.h"
-#include "gpio.h"
-#include "user_math.h"
-#include
-
-/* Private function -------------------------------------------------------- */
-/*Export function --------------------------------------------------------------*/
int8_t CMD_Init(CMD_t *cmd);
diff --git a/User/device/rc.c b/User/device/rc.c
index 8c15501..7000eb8 100644
--- a/User/device/rc.c
+++ b/User/device/rc.c
@@ -12,8 +12,8 @@
#include "error_detect.h"
-#define DR16
-//#define LD_t
+//#define DR16
+#define LD_t
#ifdef DR16
#define FRAME_LEN 36
@@ -64,61 +64,77 @@ bool RC_SBUS_WaitDmaCplt(uint32_t timeout) {
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
- 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
+ raw->ch[0] = (cbuf[1] | (cbuf[2] << 8)) & 0x07ff; //!< Channel 0
+ raw->ch[1] = ((cbuf[2] >> 3) | (cbuf[3] << 5)) & 0x07ff; //!< Channel 1
+ raw->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
+ 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;
- 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;
+ raw->sw[0] = ((int16_t)cbuf[6] >> 4 | ((int16_t)cbuf[7] << 4 )) & 0x07FF;
+ raw->sw[1] = ((int16_t)cbuf[7] >> 7 | ((int16_t)cbuf[8] << 1 ) | (int16_t)cbuf[9] << 9 ) & 0x07FF;
+ raw->sw[2] = ((int16_t)cbuf[9] >> 2 | ((int16_t)cbuf[10] << 6 )) & 0x07FF;;
+ raw->sw[3] = ((int16_t)cbuf[10] >> 5 | ((int16_t)cbuf[11] << 3 )) & 0x07FF;
+ raw->sw[4] = ((int16_t)cbuf[12] << 0 | ((int16_t)cbuf[13] << 8 )) & 0x07FF;
+ raw->sw[5] = ((int16_t)cbuf[13] >> 3 | ((int16_t)cbuf[14] << 5 )) & 0x07FF;
+ raw->sw[6] = ((int16_t)cbuf[14] >> 6 | ((int16_t)cbuf[15] << 2 ) | (int16_t)cbuf[16] << 10 ) & 0x07FF;
+ raw->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;
+ raw->ch[0] -= RC_CH_VALUE_MID;
+ raw->ch[1] -= RC_CH_VALUE_MID;
+ raw->ch[2] -= RC_CH_VALUE_MID;
+ raw->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)旋转
+ raw->ch[0] += 24; //y(-694,693)左右
+ raw->ch[1] = -raw->ch[1]-24; //x(-693,694)前后
+ raw->ch[2] = -raw->ch[2]+764; //m(95,1482)油门
+ raw->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
+ raw->ch[1] = map_int(raw->ch[1],-693,694,-700,700); //x映射到(-700,700)
+ raw->ch[0] = map_int(raw->ch[0],-694,693,-700,700); //y映射到(-700,700)
+ 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);
- 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);
+ raw->map_ch[0]=map_fp32(raw->ch[0],-719,680,-1,1);
+ raw->map_ch[1]=map_fp32(raw->ch[1],-567,832,-1,1);
+ raw->map_ch[2]=map_fp32(raw->ch[2],95,1482,0,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);
- 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);
+ /*非线性映射*/
+ raw->map_ch[0]=expo_map(raw->map_ch[0], 0.7f);
+ raw->map_ch[1]=expo_map(raw->map_ch[1],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)
- 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;
+ if(raw->map_ch[0]>-0.05f&&raw->map_ch[0]<0.05f) raw->map_ch[0]=0;
+ if(raw->map_ch[1]>-0.05f&&raw->map_ch[1]<0.05f) raw->map_ch[1]=0;
+ if(raw->map_ch[2]>-0.05f&&raw->map_ch[2]<0.05f) raw->map_ch[2]=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;
}
/*dr16数据解析+校验 */
@@ -159,9 +175,9 @@ int8_t DR16_ParseRaw(DR16_t *dr16)
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 (rc_ctrl == NULL) return DEVICE_ERR_NULL;
+ if (LD == NULL) return DEVICE_ERR_NULL;
#ifdef DR16
/*DR16部分*/
rc->rc_type=RC_DR16;
@@ -190,22 +206,38 @@ int8_t RC_ParseRC( DR16_t *dr16, RC_ctrl_t *rc_ctrl, CMD_RC_t *rc) {
#elif defined(LD_t)
/*乐迪*/
- rc->rc_type=RC_LD;
- LD_ParseRaw(rc_ctrl);
-
- memcpy(&rc->LD, rc_ctrl, sizeof(RC_ctrl_t));
+ rc->rc_type=RC_LD;
+ LD_ParseRaw(LD_raw,LD);
+ memcpy(&rc->LD, LD, sizeof(LD_Data_t));
+ /*离线处理，和dr16位置不同*/
+
+ if(LD_raw->sw[7] != 393)
+ {
+ LD_HandleOffline(LD,rc);
+ memset(cbuf, 0, sizeof(cbuf)); //有时候会出现消息数组错位，所以直接清空，在离线和指定按键不对的情况下，原数据不可信
+ }
#endif
return DEVICE_OK;
}
-/*离线处理*/
+/*dr16离线处理*/
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;
+ rc->rc_type =Control_loss ;
(void)dr16;
- memset(rc, 0, sizeof(*rc));
+ memset(&rc->dr16 , 0, sizeof(DR16_t));
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;
+
+}
\ No newline at end of file
diff --git a/User/device/rc.h b/User/device/rc.h
index f39fe24..43665fe 100644
--- a/User/device/rc.h
+++ b/User/device/rc.h
@@ -32,23 +32,36 @@ typedef struct {
DR16_Data_t data;
} DR16_t;
-
-
-//#define SBUS_RX_BUF_NUM 25u
-
-//#define RC_FRAME_LENGTH 18u
-
-
-
+/*ԭʼ*/
typedef __packed struct
{
fp32 map_ch[4];
int16_t ch[4];
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_StartDmaRecv(void) ;
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) ;
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 LD_HandleOffline(const LD_Data_t *LD, CMD_RC_t *rc) ;
#endif
diff --git a/User/task/rc_task.c b/User/task/rc_task.c
index 5584765..b34b109 100644
--- a/User/task/rc_task.c
+++ b/User/task/rc_task.c
@@ -15,7 +15,8 @@
#ifdef DEBUG
DR16_t dr16;
CMD_RC_t cmd_rc;
-RC_ctrl_t ld;
+LD_raw_t LD_raw;
+LD_Data_t LD;
#else
static DR16_t dr16;
static CMD_RC_t cmd_rc;
@@ -46,10 +47,11 @@ void Task_rc(void *argument) {
if (RC_SBUS_WaitDmaCplt(30)) {
- RC_ParseRC(&dr16,&ld, &cmd_rc);
+ RC_ParseRC(&dr16,&LD_raw,&LD, &cmd_rc);
} else {
- /* 处理遥控器离线 */
+ /* 处理dr16遥控器离线 ,乐迪不可用*/
DR16_HandleOffline(&dr16, &cmd_rc);
+ LD_HandleOffline(&LD,&cmd_rc);
}
osMessageQueueReset(task_runtime.msgq.cmd.raw.rc);
osMessageQueuePut(task_runtime.msgq.cmd.raw.rc, &cmd_rc, 0, 0);