diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx index 7c29790..7424a0e 100644 --- a/MDK-ARM/R2.uvoptx +++ b/MDK-ARM/R2.uvoptx @@ -153,112 +153,7 @@ 0 1 - chassis - - - 1 - 1 - ctrl - - - 2 - 1 - dr16,0x0A - - - 3 - 1 - UP_t,0x0A - - - 4 - 1 - UP,0x0A - - - 5 - 1 - can,0x0A - - - 6 - 1 - up_cmd,0x0A - - - 7 - 1 - rc_ctrl - - - 8 - 1 - up_cmd - - - 9 - 1 - cmd,0x0A - - - 10 - 1 - ctrl,0x0A - - - 11 - 1 - ctrl_up,0x0A - - - 12 - 1 - task_runtime,0x0A - - - 13 - 1 - can,0x0A - - - 14 - 1 - can_rx,0x0A - - - 15 - 1 - can_out,0x0A - - - 16 - 1 - gcan - - - 17 - 1 - raw_tx,0x0A - - - 18 - 1 - raw_rx2,0x0A - - - 19 - 1 - can_rx,0x0A - - - 20 - 1 - can_out - - - 21 - 1 - UP_CAN_out,0x0A + rc_ctrl,0x0A @@ -1222,8 +1117,8 @@ 0 0 0 - ..\User\task\dr16_task.c - dr16_task.c + ..\User\task\up_task.c + up_task.c 0 0 @@ -1234,20 +1129,8 @@ 0 0 0 - ..\User\task\r12ds_task.c - r12ds_task.c - 0 - 0 - - - 8 - 73 - 1 - 0 - 0 - 0 - ..\User\task\up_task.c - up_task.c + ..\User\task\rc_task.c + rc_task.c 0 0 @@ -1261,7 +1144,7 @@ 0 9 - 74 + 73 1 0 0 @@ -1273,7 +1156,7 @@ 9 - 75 + 74 1 0 0 @@ -1285,7 +1168,7 @@ 9 - 76 + 75 1 0 0 @@ -1297,7 +1180,7 @@ 9 - 77 + 76 1 0 0 @@ -1309,7 +1192,7 @@ 9 - 78 + 77 1 0 0 @@ -1321,7 +1204,7 @@ 9 - 79 + 78 1 0 0 @@ -1341,7 +1224,7 @@ 0 10 - 80 + 79 1 0 0 @@ -1353,7 +1236,7 @@ 10 - 81 + 80 1 0 0 @@ -1365,7 +1248,7 @@ 10 - 82 + 81 1 0 0 @@ -1377,7 +1260,7 @@ 10 - 83 + 82 1 0 0 @@ -1389,7 +1272,7 @@ 10 - 84 + 83 1 0 0 @@ -1401,31 +1284,7 @@ 10 - 85 - 1 - 0 - 0 - 0 - ..\User\device\dr16.c - dr16.c - 0 - 0 - - - 10 - 86 - 1 - 0 - 0 - 0 - ..\User\device\r12ds.c - r12ds.c - 0 - 0 - - - 10 - 87 + 84 1 0 0 @@ -1435,17 +1294,29 @@ 0 0 + + 10 + 85 + 1 + 0 + 0 + 0 + ..\User\device\rc.c + rc.c + 0 + 0 + Application/User/USB_DEVICE/App - 0 + 1 0 0 0 11 - 88 + 86 1 0 0 @@ -1457,7 +1328,7 @@ 11 - 89 + 87 1 0 0 @@ -1469,7 +1340,7 @@ 11 - 90 + 88 1 0 0 @@ -1483,13 +1354,13 @@ Application/User/USB_DEVICE/Target - 0 + 1 0 0 0 12 - 91 + 89 1 0 0 @@ -1503,13 +1374,13 @@ Middlewares/USB_Device_Library - 0 + 1 0 0 0 13 - 92 + 90 1 0 0 @@ -1521,7 +1392,7 @@ 13 - 93 + 91 1 0 0 @@ -1533,7 +1404,7 @@ 13 - 94 + 92 1 0 0 @@ -1545,7 +1416,7 @@ 13 - 95 + 93 1 0 0 diff --git a/MDK-ARM/R2.uvprojx b/MDK-ARM/R2.uvprojx index f9d4bc2..ee3c96d 100644 --- a/MDK-ARM/R2.uvprojx +++ b/MDK-ARM/R2.uvprojx @@ -1228,21 +1228,16 @@ 1 ..\User\task\error_detect_task.c - - dr16_task.c - 1 - ..\User\task\dr16_task.c - - - r12ds_task.c - 1 - ..\User\task\r12ds_task.c - up_task.c 1 ..\User\task\up_task.c + + rc_task.c + 1 + ..\User\task\rc_task.c + @@ -1308,21 +1303,16 @@ 1 ..\User\device\vofa.c - - dr16.c - 1 - ..\User\device\dr16.c - - - r12ds.c - 1 - ..\User\device\r12ds.c - GO_M8010_6_Driver.c 1 ..\User\device\GO_M8010_6_Driver.c + + rc.c + 1 + ..\User\device\rc.c + diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf index 3d40118..801082c 100644 Binary files a/MDK-ARM/R2/R2.axf and b/MDK-ARM/R2/R2.axf differ diff --git a/User/Algorithm/user_math.c b/User/Algorithm/user_math.c index 3d163b4..52b02c4 100644 --- a/User/Algorithm/user_math.c +++ b/User/Algorithm/user_math.c @@ -27,7 +27,10 @@ { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } - +// 对摇杆输出做指数曲线映射，提升小范围操控精度 +float expo_map(float value, float expo_factor) { + return value * (1 - expo_factor) + value * fabs(value) * expo_factor; +} int Setint_Deadzone(int value,int deadzone) { if(value>=-deadzone&&value<=deadzone) diff --git a/User/Algorithm/user_math.h b/User/Algorithm/user_math.h index 766dc9a..8021299 100644 --- a/User/Algorithm/user_math.h +++ b/User/Algorithm/user_math.h @@ -159,5 +159,5 @@ PolarCoordinate_t addPolarVectors(PolarCoordinate_t v1, PolarCoordinate_t v2); uint8_t average(uint8_t arr[], uint8_t n); int abs_value(int num); - +float expo_map(float value, float expo_factor) ; #endif diff --git a/User/bsp/bsp_usart.h b/User/bsp/bsp_usart.h index cef04b6..4974127 100644 --- a/User/bsp/bsp_usart.h +++ b/User/bsp/bsp_usart.h @@ -7,7 +7,6 @@ #define SBUS_RX_BUF_NUM 50u - #define RC_FRAME_LENGTH 25u typedef enum { diff --git a/User/device/LD_remote.c b/User/device/LD_remote.c deleted file mode 100644 index 14c70bf..0000000 --- a/User/device/LD_remote.c +++ /dev/null @@ -1,31 +0,0 @@ -#include "LD_remote.h" -#include - -#include "bsp_usart.h" -#include "error_detect.h" - -static uint8_t sbus_rx_buf_new[SBUS_RX_BUF_NUM]; // SBUS 数据缓冲区 -static RC_Ctrl_New_t rc_ctrl_new; // SBUS 数据结构 - -/* SBUS 数据解析 */ -static void sbus_to_rc_new(volatile const uint8_t *sbus_buf) { - if (sbus_buf == NULL) return; - - rc_ctrl_new.ch[0] = (sbus_buf[1] | (sbus_buf[2] << 8)) & 0x07FF; - rc_ctrl_new.ch[1] = ((sbus_buf[2] >> 3) | (sbus_buf[3] << 5)) & 0x07FF; - rc_ctrl_new.ch[2] = ((sbus_buf[3] >> 6) | (sbus_buf[4] << 2) | (sbus_buf[5] << 10)) & 0x07FF; - rc_ctrl_new.ch[3] = ((sbus_buf[5] >> 1) | (sbus_buf[6] << 7)) & 0x07FF; - - rc_ctrl_new.sw[0] = ((sbus_buf[6] >> 4) | (sbus_buf[7] << 4)) & 0x07FF; - rc_ctrl_new.sw[1] = ((sbus_buf[7] >> 7) | (sbus_buf[8] << 1) | (sbus_buf[9] << 9)) & 0x07FF; -} - -/* 初始化函数 */ -void RC_New_Init(void) { - HAL_UARTEx_ReceiveToIdle_DMA(&huart3, sbus_rx_buf_new, SBUS_RX_BUF_NUM); -} - -/* 获取数据指针 */ -const RC_Ctrl_New_t* RC_New_GetData(void) { - return &rc_ctrl_new; -} \ No newline at end of file diff --git a/User/device/LD_remote.h b/User/device/LD_remote.h deleted file mode 100644 index 5ee6770..0000000 --- a/User/device/LD_remote.h +++ /dev/null @@ -1,21 +0,0 @@ -#ifndef LD_H -#define LD_H -#include "user_math.h" - - -#include "main.h" - -/* SBUS ݽṹ */ -typedef struct { - int16_t ch[4]; // 4 ͨ - int16_t sw[8]; // 8 -} RC_Ctrl_New_t; - -/* */ -void RC_New_Init(void); -const RC_Ctrl_New_t* RC_New_GetData(void); - - - -#endif - diff --git a/User/device/cmd.c b/User/device/cmd.c index d73c2f7..316793a 100644 --- a/User/device/cmd.c +++ b/User/device/cmd.c @@ -25,41 +25,18 @@ return 0; } - - static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) { - /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */ -// RC_dr16 , -// RC_r12ds -// - - /*乐迪反馈值转换(-1 -- 1 )范围*/ - -// if(rc->ch_x<0) cmd->Vx =map_fp32(rc->ch_x,-337,0,-1,0); -// else cmd->Vx =map_fp32(rc->ch_x,0,310,0,1); -// -// -// if(rc->ch_y<0) cmd->Vy =map_fp32(rc->ch_y,-260,0,-1,0); -// else cmd->Vy =map_fp32(rc->ch_y,0,312,0,1); - -// if(rc->ch_w<0) cmd->Vw =map_fp32(rc->ch_w,-308,0,-1,0); -// else cmd->Vw =map_fp32(rc->ch_w,0,291,0,1); -// cmd->key_ctrl_l = rc->key [0]; -// cmd->key_ctrl_r = rc->key [1]; -// + 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->Vx = rc->ch_r_x; - cmd->Vy = rc->ch_r_y; - cmd->Vw = rc->ch_l_x; - - cmd->poscamear = rc->ch_l_y; - - cmd->key_ctrl_l = rc->sw_l; - cmd->key_ctrl_r = rc->sw_r ; + cmd->key_ctrl_l = rc->dr16.sw_l; + cmd->key_ctrl_r = rc->dr16.sw_r ; } @@ -80,7 +57,7 @@ int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc){ if (rc == NULL) return -1; /*c当rc丢控时，恢复机器人至默认状态 */ - if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) { + if ((rc->dr16.sw_l == CMD_SW_ERR) || (rc->dr16.sw_r == CMD_SW_ERR)) { CMD_RcLostLogic(cmd); } else { CMD_RcLogic(rc, cmd); @@ -123,69 +100,52 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){ -/* - 遥控器模式重新分配 - 这一部分没有设置具体的模式名，后期根据需要修改 - 遥控器模式(RC): - 左按键 --- 右按键 - mode1 - up no_mode - mode2 - - - mode3 - down no_mode - mode4 - - mid auto_navi(0x09)雷达导航 - -*/ /*遥控器，上下层通用，按键控制，统一到cmd*/ int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) { if(cmd == NULL) return -1; - cmd->Vx = rc->ch_r_x; - cmd->Vy = rc->ch_r_y; - cmd->Vw = rc->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->ch_l_y; + cmd->poscamear = rc->dr16.ch_l_y; - cmd->key_ctrl_l = rc->sw_l; - cmd->key_ctrl_r = rc->sw_r ; + cmd->key_ctrl_l = rc->dr16.sw_l; + cmd->key_ctrl_r = rc->dr16.sw_r ; - if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) { + if ((rc->dr16.sw_l == CMD_SW_ERR) || (rc->dr16.sw_r == CMD_SW_ERR)) { cmd->CMD_CtrlType =RELAXED; } - else if(rc->sw_l==CMD_SW_UP) + else if(rc->dr16.sw_l==CMD_SW_UP) { cmd ->CMD_CtrlType =RCcontrol; - if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_mode =Pitch; //左上，右上，投篮，设置好的 + if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Pitch; //左上，右上，投篮，设置好的 - if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式 + if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式 - if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左上，右上，手动调整 + if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左上，右上，手动调整 } - else if(rc->sw_l==CMD_SW_MID) + else if(rc->dr16.sw_l==CMD_SW_MID) { cmd ->CMD_CtrlType =AUTO; - if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_mode =AUTO_NAVI; //左中，右中，雷达 + if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =AUTO_NAVI; //左中，右中，雷达 - if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左中，右中，无模式 + if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左中，右中，无模式 - if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =AUTO_PICK; //左中，右下，视觉 + if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =AUTO_PICK; //左中，右下，视觉 } - else if(rc->sw_l==CMD_SW_DOWN) + else if(rc->dr16.sw_l==CMD_SW_DOWN) { cmd ->CMD_CtrlType =RCcontrol; - if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左下，右上，投篮 - if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左下，右中，无模式 - if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左下，右上，无模式 + if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左下，右上，投篮 + if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左下，右中，无模式 + if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左下，右上，无模式 } diff --git a/User/device/cmd.h b/User/device/cmd.h index 986e80d..9efab0d 100644 --- a/User/device/cmd.h +++ b/User/device/cmd.h @@ -54,23 +54,36 @@ typedef enum { CMD_SW_MID = 3, CMD_SW_DOWN = 2, } CMD_SwitchPos_t; - +typedef enum{ + RC_DR16, + RC_LD +}rc_type_t; /*遥控器值，使用CMD_RcLogic函数传入CMD_t结构体*/ typedef struct { + + rc_type_t rc_type; + + struct { float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */ float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */ float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */ float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */ - float ch_res; /* 第五通道值 */ - CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */ CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */ - uint16_t key; /* 按键值 */ uint16_t res; /* 保留，未启用 */ + }__attribute__((packed))dr16; + + struct { + fp32 map_ch[4]; + int16_t ch[4]; + int16_t sw[8]; + + }__attribute__((packed))LD; + } __attribute__((packed))CMD_RC_t; diff --git a/User/device/dr16.c b/User/device/dr16.c deleted file mode 100644 index 04454d2..0000000 --- a/User/device/dr16.c +++ /dev/null @@ -1,136 +0,0 @@ -/* - DR16接收机 - -*/ - -/* Includes ----------------------------------------------------------------- */ -#include "dr16.h" - -#include - -#include "bsp_usart.h" -#include "error_detect.h" - - - - -/* Private define ----------------------------------------------------------- */ -#define DR16_CH_VALUE_MIN (364u) -#define DR16_CH_VALUE_MID (1024u) -#define DR16_CH_VALUE_MAX (1684u) - -/* Private macro ------------------------------------------------------------ */ -/* Private typedef ---------------------------------------------------------- */ -/* Private variables -------------------------------------------------------- */ - -static osThreadId_t thread_alert; -uint8_t cbuf[36]; //此处设置为两帧字节的长度 若为一帧会出现乱码的情况 -/* Private function -------------------------------------------------------- */ -static void DR16_RxCpltCallback(void) { - osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY); -// detect_hook(DR16_TOE); -} - -static bool DR16_DataCorrupted(const DR16_t *dr16) { - if (dr16 == NULL) return DEVICE_ERR_NULL; - - if ((dr16->data.ch_r_x < DR16_CH_VALUE_MIN) || - (dr16->data.ch_r_x > DR16_CH_VALUE_MAX)) - return true; - - if ((dr16->data.ch_r_y < DR16_CH_VALUE_MIN) || - (dr16->data.ch_r_y > DR16_CH_VALUE_MAX)) - return true; - - if ((dr16->data.ch_l_x < DR16_CH_VALUE_MIN) || - (dr16->data.ch_l_x > DR16_CH_VALUE_MAX)) - return true; - - if ((dr16->data.ch_l_y < 1000u) || - (dr16->data.ch_l_y > DR16_CH_VALUE_MAX)) - return true; - - if (dr16->data.sw_l == 0) return true; - - if (dr16->data.sw_r == 0) return true; - - return false; -} -/* Exported functions ------------------------------------------------------- */ -int8_t DR16_Init(DR16_t *dr16) { - if (dr16 == NULL) return DEVICE_ERR_NULL; - if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL; - - BSP_UART_RegisterCallback(BSP_UART_REMOTE, BSP_UART_IDLE_LINE_CB, - DR16_RxCpltCallback); - return DEVICE_OK; -} - -int8_t DR16_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 DR16_StartDmaRecv(DR16_t *dr16) { - 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 DR16_WaitDmaCplt(uint32_t timeout) { - return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) == - SIGNAL_DR16_RAW_REDY); -} - -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 DR16_ParseRC(const DR16_t *dr16, CMD_RC_t *rc) { - if (dr16 == NULL) return DEVICE_ERR_NULL; - - if (DR16_DataCorrupted(dr16)) { - return DEVICE_ERR; - } else { - memset(rc, 0, sizeof(*rc)); - } - float full_range = (float)(DR16_CH_VALUE_MAX - DR16_CH_VALUE_MIN); - - rc->ch_r_x = 2 * ((float)dr16->data.ch_r_x - DR16_CH_VALUE_MID) / full_range; - rc->ch_r_y = 2 * ((float)dr16->data.ch_r_y - DR16_CH_VALUE_MID) / full_range; - rc->ch_l_x = 2 * ((float)dr16->data.ch_l_x - DR16_CH_VALUE_MID) / full_range; - rc->ch_l_y = 2 * ((float)dr16->data.ch_l_y - DR16_CH_VALUE_MID) / full_range; - - rc->sw_l = (CMD_SwitchPos_t)dr16->data.sw_l; - rc->sw_r = (CMD_SwitchPos_t)dr16->data.sw_r; - - rc->key = dr16->data.key; - -// rc->ch_res = ((float)dr16->data.res - DR16_CH_VALUE_MID) / full_range; - 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; -} - - - - diff --git a/User/device/motor_control.c b/User/device/motor_control.c deleted file mode 100644 index 560da9f..0000000 --- a/User/device/motor_control.c +++ /dev/null @@ -1,7 +0,0 @@ -#include "motor_control.h" -#include "pid.h" -#include "user_math.h" -#include "device\device.h" -#include "can_use.h" - - diff --git a/User/device/motor_control.h b/User/device/motor_control.h deleted file mode 100644 index 2bf0589..0000000 --- a/User/device/motor_control.h +++ /dev/null @@ -1,23 +0,0 @@ -#ifndef _MOTOR_CONTROL_H_ -#define _MOTOR_CONTROL_H_ - -#include "struct_typedef.h" -#include "pid.h" - -/*󽮵*/ -typedef struct -{ - uint16_t ecd; - int16_t speed_rpm; - int16_t given_current; - uint16_t temperate; - int16_t last_ecd; - int16_t round_cnt; - int16_t total_angle; - uint16_t offset_ecd; - uint32_t msg_cnt; -} DJmotor_measure_t; - - - -#endif \ No newline at end of file diff --git a/User/device/r12ds.c b/User/device/r12ds.c deleted file mode 100644 index e5bbe97..0000000 --- a/User/device/r12ds.c +++ /dev/null @@ -1,192 +0,0 @@ -/* - 乐迪r12ds遥控器。 - 需要根据实际的遥控器通道修改解析的函数 该遥控器采用sbus 用的uart3 - -*/ - - -/* Includes ----------------------------------------------------------------- */ -#include "r12ds.h" -#include "main.h" -#include "error_detect.h" -#include -#include "cmd.h" -#include "user_math.h" - -#ifdef r12ds_t -extern UART_HandleTypeDef huart3; -extern DMA_HandleTypeDef hdma_usart3_rx; - - - -osThreadId_t thread_alert; - -int buf0ready =0; /*不起作用*/ -int buf1ready =0; - -/* Private function -------------------------------------------------------- */ -static void R12DS_IdleCallback(void) { - static uint16_t this_time_rx_len = 0; - if( (hdma_usart3_rx.Instance->CR & DMA_SxCR_CT) == RESET) - { - //current memory buffer used is memory0 - - //disable dma to change dma register - __HAL_DMA_DISABLE(&hdma_usart3_rx); - - //get received data length, length = set_data_length - remain_length - this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR; - - //reset set_data_length - hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM; - - //change memory0 to memory1 - hdma_usart3_rx.Instance->CR |= DMA_SxCR_CT; - - //enable dma - __HAL_DMA_ENABLE(&hdma_usart3_rx); - - //1 frame length is correct data - if(this_time_rx_len == RC_FRAME_LENGTH) - { -// osThreadFlagsSet(thread_alert,SIGNAL_R12DS_BUF0_REDY); - buf0ready = 1; -// detect_hook(R12DS_TOE); - } - } - else - { - __HAL_DMA_DISABLE(&hdma_usart3_rx); - - this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR; - - hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM; - - //change memory1 to memory0 - DMA1_Stream1->CR &= ~(DMA_SxCR_CT); - - __HAL_DMA_ENABLE(&hdma_usart3_rx); - - if(this_time_rx_len == RC_FRAME_LENGTH) - { -// osThreadFlagsSet(thread_alert,SIGNAL_R12DS_BUF1_REDY); - buf1ready = 1; -// detect_hook(R12DS_TOE); - } - } -} - -/* Exported functions ------------------------------------------------------- */ - - -int8_t R12ds_DMA_Init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num) -{ - if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL; - //enable the dma transfer for the receiver request - SET_BIT(huart3.Instance->CR3, USART_CR3_DMAR); - - //enable idle interrupt - __HAL_UART_ENABLE_IT(&huart3, UART_IT_IDLE); - - //disable dma, to change the dma register - __HAL_DMA_DISABLE(&hdma_usart3_rx); - - //disable dma again but why? - //what's the condition? - while(hdma_usart3_rx.Instance->CR & DMA_SxCR_EN) - { - __HAL_DMA_DISABLE(&hdma_usart3_rx); - } - - //?? - hdma_usart3_rx.Instance->PAR = (uint32_t) & (USART3->DR); - - //memory buffer 1 - hdma_usart3_rx.Instance->M0AR = (uint32_t)(rx1_buf); - - //momory buffer 2 - hdma_usart3_rx.Instance->M1AR = (uint32_t)(rx2_buf); - - //data length - hdma_usart3_rx.Instance->NDTR = dma_buf_num; - - //enable double memory buffer - SET_BIT(hdma_usart3_rx.Instance->CR, DMA_SxCR_DBM); - - - //enable dma - __HAL_DMA_ENABLE(&hdma_usart3_rx); - - BSP_UART_RegisterCallback(BSP_UART_REMOTE, BSP_UART_IDLE_LINE_CB, - R12DS_IdleCallback); - - return 1; -} - - - -int8_t sbus_to_rc(volatile const uint8_t *sbus_buf, CMD_RC_t *rc_ctrl) -{ - if (sbus_buf == NULL || rc_ctrl == NULL) - { - return 0; - } - - - rc_ctrl->ch_x = (sbus_buf[1] | (sbus_buf[2] << 8)) & 0x07ff; //Channel 1 x - rc_ctrl->ch_y = ((sbus_buf[2] >> 3) | (sbus_buf[3] << 5)) & 0x07ff; //Channel 2 mul - rc_ctrl->mul= ((sbus_buf[3] >> 6) | (sbus_buf[4] << 2) | //Channel 3 y - (sbus_buf[5] << 10)) &0x07ff; - rc_ctrl->ch_w = ((sbus_buf[5] >> 1) | (sbus_buf[6] << 7)) & 0x07ff; //Channel 4 w - - rc_ctrl->key[0] = ((int16_t)sbus_buf[6] >> 4 | ((int16_t)sbus_buf[7] << 4 )) & 0x07FF; //Channel 5 - rc_ctrl->key[1] = ((int16_t)sbus_buf[7] >> 7 | ((int16_t)sbus_buf[8] << 1 ) | (int16_t)sbus_buf[9] << 9 ) & 0x07FF; //Channel 6 - rc_ctrl->key[2] = ((int16_t)sbus_buf[9] >> 2 | ((int16_t)sbus_buf[10] << 6 )) & 0x07FF;; //Channel 7 - rc_ctrl->key[3] = ((int16_t)sbus_buf[10] >> 5 | ((int16_t)sbus_buf[11] << 3 )) & 0x07FF; //Channel 8 - rc_ctrl->key[4] = ((int16_t)sbus_buf[12] << 0 | ((int16_t)sbus_buf[13] << 8 )) & 0x07FF; //Channel 9 - rc_ctrl->key[5] = ((int16_t)sbus_buf[13] >> 3 | ((int16_t)sbus_buf[14] << 5 )) & 0x07FF; //Channel 10 - rc_ctrl->key[6] = ((int16_t)sbus_buf[14] >> 6 | ((int16_t)sbus_buf[15] << 2 ) | (int16_t)sbus_buf[16] << 10 ) & 0x07FF; //Channel 11 - rc_ctrl->key[7] = ((int16_t)sbus_buf[16] >> 1 | ((int16_t)sbus_buf[17] << 7 )) & 0x07FF; //Channel 12 - - rc_ctrl->ch_y -= RC_CH_VALUE_OFFSET; - rc_ctrl->ch_x -= RC_CH_VALUE_OFFSET; - rc_ctrl->mul -= RC_CH_VALUE_OFFSET; - rc_ctrl->ch_w -= RC_CH_VALUE_OFFSET; - - - - rc_ctrl->ch_y -= 4; //y(-694,693) - rc_ctrl->ch_x += 3; //x(-693,694) - rc_ctrl->mul = rc_ctrl->mul; //m(-518,843) - rc_ctrl->ch_w += 4; //w(-694,693) - - rc_ctrl->key[2] = map_fp32(rc_ctrl->key[2],306,1694,1694,306); - rc_ctrl->key[3] = map_fp32(rc_ctrl->key[3],306,1694,1694,306); - rc_ctrl->ch_x = map_fp32(rc_ctrl->ch_x,700,-800,-700,700); //x - rc_ctrl->ch_y = map_fp32(rc_ctrl->ch_y,-900,796,700,-700); //y - rc_ctrl->mul = map_fp32(rc_ctrl->mul,-632,901,25,0); //m - rc_ctrl->ch_w = map_fp32(rc_ctrl->ch_w,-820,780,-700,700); - rc_ctrl->ch_w = 0.5f*(rc_ctrl->ch_w); - rc_ctrl->ch_x = -0.5f*(rc_ctrl->ch_x); - rc_ctrl->ch_y = 0.5f*(rc_ctrl->ch_y); -// -//̀死区(-5,5) - if(rc_ctrl->ch_y>-15&&rc_ctrl->ch_y<15) rc_ctrl->ch_y=0; - if(rc_ctrl->ch_x>-15&&rc_ctrl->ch_x<15) rc_ctrl->ch_x=0; - if(rc_ctrl->mul>=0&&rc_ctrl->mul<=3) rc_ctrl->mul=0; - if(rc_ctrl->ch_w>-15&&rc_ctrl->ch_w<15) rc_ctrl->ch_w=0; - - - if( (((int16_t)sbus_buf[16] >> 1 | ((int16_t)sbus_buf[17] << 7 )) & 0x07FF )<380) rc_ctrl->offline =1; //遥控器掉线 - else rc_ctrl->offline =0; - - - return 0; -} - - - - - -#endif - diff --git a/User/device/r12ds.h b/User/device/r12ds.h deleted file mode 100644 index 111c3bc..0000000 --- a/User/device/r12ds.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef _R12DS_H -#define _R12DS_H - -#include -#include "device.h" -#include "bsp_usart.h" -#include "cmd.h" - - -#ifdef r12ds_t - -#define RC_FRAME_LENGTH 25u - - -#define RC_CH_VALUE_OFFSET ((uint16_t)1024) - - - -int8_t R12ds_DMA_Init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num); - -int8_t sbus_to_rc(volatile const uint8_t *sbus_buf,CMD_RC_t *rc_ctrl); - - - - - -#endif - -#endif diff --git a/User/device/rc.c b/User/device/rc.c new file mode 100644 index 0000000..8c15501 --- /dev/null +++ b/User/device/rc.c @@ -0,0 +1,211 @@ +/* + DR16接收机 + +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "rc.h" + +#include + +#include "bsp_usart.h" +#include "error_detect.h" + + +#define DR16 +//#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; +} diff --git a/User/device/dr16.h b/User/device/rc.h similarity index 56% rename from User/device/dr16.h rename to User/device/rc.h index 2b99679..f39fe24 100644 --- a/User/device/dr16.h +++ b/User/device/rc.h @@ -1,9 +1,9 @@ -#ifndef DR16_h -#define DR16_h +#ifndef RC_h +#define RC_h /* Includes ----------------------------------------------------------------- */ #include - +#include "bsp_usart.h" #include "cmd.h" #include "user_math.h" #include "device.h" @@ -11,11 +11,6 @@ /* Exported constants ------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */ - - - - - typedef __packed struct { uint16_t ch_r_x : 11; @@ -37,17 +32,39 @@ typedef struct { DR16_Data_t data; } DR16_t; -/* Exported functions prototypes -------------------------------------------- */ -int8_t DR16_Init(DR16_t *dr16); -int8_t DR16_Restart(void); -int8_t DR16_StartDmaRecv(DR16_t *dr16); -bool DR16_WaitDmaCplt(uint32_t timeout); + +//#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; + + + + + + + + +int8_t RC_SBUS_Init(void ); +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); +static bool DR16_DataCorrupted(const DR16_t *dr16) ; int8_t DR16_ParseRaw(DR16_t *dr16); -int8_t DR16_ParseRC(const DR16_t *dr16, CMD_RC_t *rc); -int8_t DR16_HandleOffline(const DR16_t *dr16, CMD_RC_t *rc); +int8_t RC_ParseRC( DR16_t *dr16, RC_ctrl_t *rc_ctrl, CMD_RC_t *rc) ; +int8_t DR16_HandleOffline(const DR16_t *dr16, CMD_RC_t *rc) ; #endif - - diff --git a/User/device/remote_control.c b/User/device/remote_control.c deleted file mode 100644 index aa5e3ed..0000000 --- a/User/device/remote_control.c +++ /dev/null @@ -1,185 +0,0 @@ -/** - ****************************(C) COPYRIGHT 2019 DJI**************************** - * @file remote_control.c/h - * @brief ңңͨSBUSЭ鴫䣬DMA䷽ʽԼCPU - * ԴôڿжͬʱṩһЩDMA - * ķʽ֤Ȳεȶԡ - * @note ͨжfreeRTOS - * @history - * Version Date Author Modification - * V1.0.0 Dec-01-2019 RM 1. - * - @verbatim - ============================================================================== - - ============================================================================== - @endverbatim - ****************************(C) COPYRIGHT 2019 DJI**************************** - */ - -#include "remote_control.h" -#include "main.h" - - -extern UART_HandleTypeDef huart3; -extern DMA_HandleTypeDef hdma_usart3_rx; - -static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl); - -RC_ctrl_t rc_ctrl; -static uint8_t sbus_rx_buf[2][RC_FRAME_LENGTH]; -uint8_t remote_ready = 0;//ң׼ - -void remote_control_init(void) -{ - RC_init(sbus_rx_buf[0], sbus_rx_buf[1], RC_FRAME_LENGTH); -} - - -//ж -void USART3_IRQHandler(void) -{ - //have received data ??? - if(huart3.Instance->SR & UART_FLAG_RXNE) - { - __HAL_UART_CLEAR_FEFLAG(&huart3); - } - else if(USART3->SR & UART_FLAG_IDLE) - { - - static uint16_t this_time_rx_len = 0; - __HAL_UART_CLEAR_PEFLAG(&huart3); - - if( (hdma_usart3_rx.Instance->CR & DMA_SxCR_CT) == RESET) - { - //current memory buffer used is memory0 - - //disable dma to change dma register - __HAL_DMA_DISABLE(&hdma_usart3_rx); - - //get received data length, length = set_data_length - remain_length - this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR; - - //reset set_data_length - hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM; - - //change memory0 to memory1 - hdma_usart3_rx.Instance->CR |= DMA_SxCR_CT; - - //enable dma - __HAL_DMA_ENABLE(&hdma_usart3_rx); - - //1 frame length is correct data - if(this_time_rx_len == RC_FRAME_LENGTH) - { - sbus_to_rc(sbus_rx_buf[0], &rc_ctrl); - } - } - else - { - __HAL_DMA_DISABLE(&hdma_usart3_rx); - - this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR; - - hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM; - - //change memory1 to memory0 - DMA1_Stream1->CR &= ~(DMA_SxCR_CT); - - __HAL_DMA_ENABLE(&hdma_usart3_rx); - - if(this_time_rx_len == RC_FRAME_LENGTH) - { - sbus_to_rc(sbus_rx_buf[1], &rc_ctrl); - } - } - } -} - -static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl) -{ - if (sbus_buf == NULL || rc_ctrl == NULL) - { - return; - } - - rc_ctrl->ch[1] = (sbus_buf[1] | (sbus_buf[2] << 8)) & 0x07ff; //Channel 1 - rc_ctrl->ch[2] = ((sbus_buf[2] >> 3) | (sbus_buf[3] << 5)) & 0x07ff; //Channel 2 - rc_ctrl->ch[0] = ((sbus_buf[3] >> 6) | (sbus_buf[4] << 2) | //Channel 3 - (sbus_buf[5] << 10)) &0x07ff; - rc_ctrl->ch[3] = ((sbus_buf[5] >> 1) | (sbus_buf[6] << 7)) & 0x07ff; //Channel 4 - - rc_ctrl->sw[0] = ((int16_t)sbus_buf[6] >> 4 | ((int16_t)sbus_buf[7] << 4 )) & 0x07FF; //Channel 5 - rc_ctrl->sw[1] = ((int16_t)sbus_buf[7] >> 7 | ((int16_t)sbus_buf[8] << 1 ) | (int16_t)sbus_buf[9] << 9 ) & 0x07FF; //Channel 6 - rc_ctrl->sw[2] = ((int16_t)sbus_buf[9] >> 2 | ((int16_t)sbus_buf[10] << 6 )) & 0x07FF;; //Channel 7 - rc_ctrl->sw[3] = ((int16_t)sbus_buf[10] >> 5 | ((int16_t)sbus_buf[11] << 3 )) & 0x07FF; //Channel 8 - rc_ctrl->sw[4] = ((int16_t)sbus_buf[12] << 0 | ((int16_t)sbus_buf[13] << 8 )) & 0x07FF; //Channel 9 - rc_ctrl->sw[5] = ((int16_t)sbus_buf[13] >> 3 | ((int16_t)sbus_buf[14] << 5 )) & 0x07FF; //Channel 10 - rc_ctrl->sw[6] = ((int16_t)sbus_buf[14] >> 6 | ((int16_t)sbus_buf[15] << 2 ) | (int16_t)sbus_buf[16] << 10 ) & 0x07FF; //Channel 11 - rc_ctrl->sw[7] = ((int16_t)sbus_buf[16] >> 1 | ((int16_t)sbus_buf[17] << 7 )) & 0x07FF; //Channel 12 - - rc_ctrl->ch[0] -= RC_CH_VALUE_OFFSET; - rc_ctrl->ch[1] -= RC_CH_VALUE_OFFSET; - rc_ctrl->ch[2] -= RC_CH_VALUE_OFFSET; - rc_ctrl->ch[3] -= RC_CH_VALUE_OFFSET; - - -// //ң - rc_ctrl->ch[0] += 20; //y(-694,693) - rc_ctrl->ch[1] += 20; //x(-693,694) - rc_ctrl->ch[2] = -rc_ctrl->ch[2]+144; //m(-518,843) - rc_ctrl->ch[3] += 4; //w(-694,693) - - rc_ctrl->sw[2] = map(rc_ctrl->sw[2],306,1694,1694,306); - rc_ctrl->sw[3] = map(rc_ctrl->sw[3],306,1694,1694,306); -// rc_ctrl->ch[1] = map(rc_ctrl->ch[1],656,-656,-700,700); //x - rc_ctrl->ch[0] = map(rc_ctrl->ch[0],-800,796,700,-700); //y - rc_ctrl->ch[2] = map(rc_ctrl->ch[2],-632,901,25,0); //m - rc_ctrl->ch[3] = map(rc_ctrl->ch[3],-820,780,-700,700); - rc_ctrl->ch[3] = 0.5*(rc_ctrl->ch[3]); - rc_ctrl->ch[1] = 0.5*(rc_ctrl->ch[1]); - rc_ctrl->ch[0] = 0.5*(rc_ctrl->ch[0]); -// - //(-30,30) - if(rc_ctrl->ch[0]>-14&&rc_ctrl->ch[0]<10) rc_ctrl->ch[0]=0; - if(rc_ctrl->ch[1]>-30&&rc_ctrl->ch[1]<20) rc_ctrl->ch[1]=0; - if(rc_ctrl->ch[2]>=0&&rc_ctrl->ch[2]<=3) rc_ctrl->ch[2]=0; - if(rc_ctrl->ch[3]>-22&&rc_ctrl->ch[3]<22) rc_ctrl->ch[3]=0; - - remote_ready = 1; -} - - - -int map(int x, int in_min, int in_max, int out_min, int out_max) //ӳ亯 -{ - return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; -} - - -/* - - - - -306 306 - sw[] sw[7] -1694 1694 - -306 306 -sw[6] sw[4] -1694 1694 - -306 306 306 306 -sw[0] sw[2] sw[1]:306-1694 sw[5]:306-1694 sw[]1000 sw[3] -1694 1694 1694 1694 - 710 - 688 1425 - | | - | | - 54 -616------ch[3]770 -354---------ch[0] 339 0 - | | - | | - ch[2] ch[1] - _699 38 -*/ diff --git a/User/device/remote_control.h b/User/device/remote_control.h deleted file mode 100644 index 5971266..0000000 --- a/User/device/remote_control.h +++ /dev/null @@ -1,42 +0,0 @@ -/** - ****************************(C) COPYRIGHT 2016 DJI**************************** - * @file remote_control.c/h - * @brief ңңͨSBUSЭ鴫䣬DMA䷽ʽԼCPU - * ԴôڿжͬʱṩһЩDMA - * ķʽ֤Ȳεȶԡ - * @note - * @history - * Version Date Author Modification - * V1.0.0 Dec-26-2018 RM 1. - * - @verbatim - ============================================================================== - - ============================================================================== - @endverbatim - ****************************(C) COPYRIGHT 2016 DJI**************************** - */ -#ifndef REMOTE_CONTROL_H -#define REMOTE_CONTROL_H -#include "struct_typedef.h" -#include "bsp_rc.h" - -#define SBUS_RX_BUF_NUM 50u - -#define RC_FRAME_LENGTH 25u - -#define RC_CH_VALUE_OFFSET ((uint16_t)1024) - -typedef struct -{ - int16_t ch[4]; - int16_t sw[8]; - -}__attribute__((packed)) RC_ctrl_t; - -extern void remote_control_init(void); -static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl); -int map(int x, int in_min, int in_max, int out_min, int out_max); //ӳ亯 - - -#endif diff --git a/User/task/error_detect_task.c b/User/task/error_detect_task.c index f8e8992..8ace272 100644 --- a/User/task/error_detect_task.c +++ b/User/task/error_detect_task.c @@ -51,7 +51,6 @@ */ #include "error_detect.h" #include "user_task.h" -#include "r12ds.h" #include "nuc.h" /** * @brief init error_list, assign offline_time, online_time, priority. diff --git a/User/task/init.c b/User/task/init.c index 7300d1a..e63b787 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -35,10 +35,8 @@ void Task_Init(void *argument) { osThreadNew(Task_AttiEsti, NULL, &attr_atti_esti); task_runtime.thread.chassis = osThreadNew(Task_Chassis, NULL, &attr_chassis); -// task_runtime.thread.r12ds = -// osThreadNew(Task_r12ds,NULL,&attr_r12ds); - task_runtime.thread.dr16 = - osThreadNew(Task_dr16,NULL,&attr_dr16); + task_runtime.thread.rc = + osThreadNew(Task_rc,NULL,&attr_rc); task_runtime.thread.can = osThreadNew(Task_can,NULL,&attr_can); diff --git a/User/task/r12ds_task.c b/User/task/r12ds_task.c deleted file mode 100644 index 3e5aa3a..0000000 --- a/User/task/r12ds_task.c +++ /dev/null @@ -1,82 +0,0 @@ -#include "r12ds.h" -#include "cmsis_os.h" -#include "user_task.h" -#include - - -#ifdef r12ds_t - -#ifdef DEBUG - -CMD_RC_t cmd_rc; - -uint8_t sbus_rx_buf[2][RC_FRAME_LENGTH]; - -#else - -static CMD_RC_t cmd_rc; - -#endif - - - -CMD_RC_t *R12ds_DataGet(){ - return &cmd_rc; -} - - -void R12ds_HandleOffline(void) { - CMD_RC_t *rc; - rc = R12ds_DataGet(); - memset(rc, 0, sizeof(rc->ch_x) + sizeof(rc->mul) + sizeof(rc->ch_y) + sizeof(rc->ch_w) + sizeof(rc->key)); -} - -#endif - -void Task_r12ds(void *argument) -{ - (void)argument; - - - const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_R12DS; -#ifdef r12ds_t - - R12ds_DMA_Init(sbus_rx_buf[0],sbus_rx_buf[1],50); -#endif - - uint32_t tick = osKernelGetTickCount(); - - - while (1) - { - -#ifdef r12ds_t - -#ifdef DEBUG - - task_runtime.stack_water_mark.r12ds = - osThreadGetStackSpace(osThreadGetId()); - -#endif - - sbus_to_rc(sbus_rx_buf[0],&cmd_rc); - - if(cmd_rc.offline ==1) - { - R12ds_HandleOffline(); - } - - osMessageQueueReset(task_runtime.msgq.cmd.raw.rc); - osMessageQueuePut(task_runtime.msgq.cmd.raw.rc,(&cmd_rc),0,0); -#endif - - tick += delay_tick; /* 计算下一个唤醒时*/ - - osDelay(10); - - } - - - - - } diff --git a/User/task/r12ds_task.h b/User/task/r12ds_task.h deleted file mode 100644 index ebf5d49..0000000 --- a/User/task/r12ds_task.h +++ /dev/null @@ -1,16 +0,0 @@ -#ifndef _R12DS_TASK_H -#define _R12DS_TASK_H - - - - - - - - - - - - - -#endif \ No newline at end of file diff --git a/User/task/dr16_task.c b/User/task/rc_task.c similarity index 68% rename from User/task/dr16_task.c rename to User/task/rc_task.c index d465e15..5584765 100644 --- a/User/task/dr16_task.c +++ b/User/task/rc_task.c @@ -5,26 +5,24 @@ /* Includes ----------------------------------------------------------------- */ #include -#include "dr16.h" +#include "rc.h" #include "user_task.h" /* Private typedef ---------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ - - - #ifdef DEBUG DR16_t dr16; CMD_RC_t cmd_rc; +RC_ctrl_t ld; #else static DR16_t dr16; static CMD_RC_t cmd_rc; + static RC_ctrl_t cc; + #endif - - /* Private function --------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */ @@ -33,31 +31,22 @@ static CMD_RC_t cmd_rc; * * \param argument 未使用 */ -void Task_dr16(void *argument) { +void Task_rc(void *argument) { (void)argument; /* 未使用，消除警告 */ - const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_DR16; - uint32_t tick = osKernelGetTickCount(); - - - - DR16_Init(&dr16); /* 初始化dr16 */ + RC_SBUS_Init(); /* 初始化 */ while (1) { - - #ifdef DEBUG /* */ - task_runtime.stack_water_mark.dr16 = osThreadGetStackSpace(osThreadGetId()); + task_runtime.stack_water_mark.rc = osThreadGetStackSpace(osThreadGetId()); #endif - /* 开启DMA */ - DR16_StartDmaRecv(&dr16); + RC_SBUS_StartDmaRecv(); - if (DR16_WaitDmaCplt(30)) { - /* 转换 */ - DR16_ParseRaw(&dr16); - DR16_ParseRC(&dr16, &cmd_rc); + if (RC_SBUS_WaitDmaCplt(30)) { + + RC_ParseRC(&dr16,&ld, &cmd_rc); } else { /* 处理遥控器离线 */ DR16_HandleOffline(&dr16, &cmd_rc); @@ -65,10 +54,4 @@ void Task_dr16(void *argument) { osMessageQueueReset(task_runtime.msgq.cmd.raw.rc); osMessageQueuePut(task_runtime.msgq.cmd.raw.rc, &cmd_rc, 0, 0); } - - - tick += delay_tick; /* 计算下一个唤醒时*/ - - osDelay(10); - } diff --git a/User/task/user_task.c b/User/task/user_task.c index 569ce05..8c37e77 100644 --- a/User/task/user_task.c +++ b/User/task/user_task.c @@ -58,7 +58,7 @@ const osThreadAttr_t attr_error_detect = { .stack_size = 128 *4, }; -const osThreadAttr_t attr_dr16 = { +const osThreadAttr_t attr_rc = { .name = "dr16", .priority = osPriorityRealtime, .stack_size = 128 *4, diff --git a/User/task/user_task.h b/User/task/user_task.h index 02fdf1a..2108081 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -21,7 +21,7 @@ #define TASK_FREQ_NUC (500u) #define TASK_FREQ_CAN (1000u) #define TASK_FREQ_R12DS (1000u) -#define TASK_FREQ_DR16 (1000u) +#define TASK_FREQ_RC (1000u) #define TASK_FREQ_ERROR_DTC (3u) @@ -40,8 +40,7 @@ typedef struct { osThreadId_t chassis; osThreadId_t up; - osThreadId_t dr16; - osThreadId_t r12ds; + osThreadId_t rc; osThreadId_t can; osThreadId_t cmd; osThreadId_t nuc; @@ -111,8 +110,7 @@ typedef struct { UBaseType_t can; UBaseType_t atti_esti; - UBaseType_t dr16; - UBaseType_t r12ds; + UBaseType_t rc; UBaseType_t cmd; UBaseType_t nuc; @@ -124,8 +122,7 @@ typedef struct { float chassis; float can; float atti_esti; - float r12ds; - float dr16; + float rc; float up; float cmd; @@ -137,8 +134,7 @@ typedef struct { float chassis; float can; float atti_esti; - float r12ds; - float dr16; + float rc; float up; float cmd; @@ -166,7 +162,7 @@ extern const osThreadAttr_t attr_nuc; extern const osThreadAttr_t attr_error_detect; -extern const osThreadAttr_t attr_dr16; +extern const osThreadAttr_t attr_rc; //extern const osThreadAttr_t attr_r12ds; @@ -179,17 +175,13 @@ void Task_Chassis(void *argument); void Task_up(void *argument); - void Task_can(void *argument); void Task_cmd(void *argument); void Task_nuc(void *argument); - void Task_error_detect(void *argument); -void Task_r12ds(void *argument); - -void Task_dr16(void *argument); +void Task_rc(void *argument); #endif