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

Compare commits

base: LYL:eb6f11b02c4c23722f14e8967a2cdd7400c6b913
Branches Tags
LYL:main
...
compare: LYL:a70e50b82219a008e46aeb8a3e27009e5ec0c8d4
Branches Tags
LYL:main

2 Commits
eb6f11b02c ... a70e50b822

Author SHA1 Message Date
ZHAISHUI04
a70e50b822 . 2025-04-25 19:50:22 +08:00
ZHAISHUI04
f82b125112 适配乐迪 2025-04-25 19:35:10 +08:00
26 changed files with 364 additions and 1090 deletions
Show Stats Expand all files Collapse all files

209
MDK-ARM/R2.uvoptx
View File

@ -153,112 +153,7 @@
<Ww>
<count>0</count>
<WinNumber>1</WinNumber>
<ItemText>chassis</ItemText>
</Ww>
<Ww>
<count>1</count>
<WinNumber>1</WinNumber>
<ItemText>ctrl</ItemText>
</Ww>
<Ww>
<count>2</count>
<WinNumber>1</WinNumber>
<ItemText>dr16,0x0A</ItemText>
</Ww>
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>UP_t,0x0A</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>UP,0x0A</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>can,0x0A</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>up_cmd,0x0A</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>rc_ctrl</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>up_cmd</ItemText>
</Ww>
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>cmd,0x0A</ItemText>
</Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>ctrl,0x0A</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>ctrl_up,0x0A</ItemText>
</Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>task_runtime,0x0A</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>can,0x0A</ItemText>
</Ww>
<Ww>
<count>14</count>
<WinNumber>1</WinNumber>
<ItemText>can_rx,0x0A</ItemText>
</Ww>
<Ww>
<count>15</count>
<WinNumber>1</WinNumber>
<ItemText>can_out,0x0A</ItemText>
</Ww>
<Ww>
<count>16</count>
<WinNumber>1</WinNumber>
<ItemText>gcan</ItemText>
</Ww>
<Ww>
<count>17</count>
<WinNumber>1</WinNumber>
<ItemText>raw_tx,0x0A</ItemText>
</Ww>
<Ww>
<count>18</count>
<WinNumber>1</WinNumber>
<ItemText>raw_rx2,0x0A</ItemText>
</Ww>
<Ww>
<count>19</count>
<WinNumber>1</WinNumber>
<ItemText>can_rx,0x0A</ItemText>
</Ww>
<Ww>
<count>20</count>
<WinNumber>1</WinNumber>
<ItemText>can_out</ItemText>
</Ww>
<Ww>
<count>21</count>
<WinNumber>1</WinNumber>
<ItemText>UP_CAN_out,0x0A</ItemText>
<ItemText>rc_ctrl,0x0A</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
@ -1222,8 +1117,8 @@
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\task\dr16_task.c</PathWithFileName>
<FilenameWithoutPath>dr16_task.c</FilenameWithoutPath>
<PathWithFileName>..\User\task\up_task.c</PathWithFileName>
<FilenameWithoutPath>up_task.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
@ -1234,20 +1129,8 @@
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\task\r12ds_task.c</PathWithFileName>
<FilenameWithoutPath>r12ds_task.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>73</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\task\up_task.c</PathWithFileName>
<FilenameWithoutPath>up_task.c</FilenameWithoutPath>
<PathWithFileName>..\User\task\rc_task.c</PathWithFileName>
<FilenameWithoutPath>rc_task.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
@ -1261,7 +1144,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>74</FileNumber>
<FileNumber>73</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1273,7 +1156,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>75</FileNumber>
<FileNumber>74</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1285,7 +1168,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>76</FileNumber>
<FileNumber>75</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1297,7 +1180,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>77</FileNumber>
<FileNumber>76</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1309,7 +1192,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>78</FileNumber>
<FileNumber>77</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1321,7 +1204,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>79</FileNumber>
<FileNumber>78</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1341,7 +1224,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>80</FileNumber>
<FileNumber>79</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1353,7 +1236,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>81</FileNumber>
<FileNumber>80</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1365,7 +1248,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>82</FileNumber>
<FileNumber>81</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1377,7 +1260,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>83</FileNumber>
<FileNumber>82</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1389,7 +1272,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>84</FileNumber>
<FileNumber>83</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1401,31 +1284,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>85</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\device\dr16.c</PathWithFileName>
<FilenameWithoutPath>dr16.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>86</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\device\r12ds.c</PathWithFileName>
<FilenameWithoutPath>r12ds.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>87</FileNumber>
<FileNumber>84</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1435,17 +1294,29 @@
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>85</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\device\rc.c</PathWithFileName>
<FilenameWithoutPath>rc.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group>
<GroupName>Application/User/USB_DEVICE/App</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>88</FileNumber>
<FileNumber>86</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1457,7 +1328,7 @@
</File>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>89</FileNumber>
<FileNumber>87</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1469,7 +1340,7 @@
</File>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>90</FileNumber>
<FileNumber>88</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1483,13 +1354,13 @@
<Group>
<GroupName>Application/User/USB_DEVICE/Target</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
<File>
<GroupNumber>12</GroupNumber>
<FileNumber>91</FileNumber>
<FileNumber>89</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1503,13 +1374,13 @@
<Group>
<GroupName>Middlewares/USB_Device_Library</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>92</FileNumber>
<FileNumber>90</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1521,7 +1392,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>93</FileNumber>
<FileNumber>91</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1533,7 +1404,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>94</FileNumber>
<FileNumber>92</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1545,7 +1416,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>95</FileNumber>
<FileNumber>93</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>

30
MDK-ARM/R2.uvprojx
View File

@ -1228,21 +1228,16 @@
<FileType>1</FileType>
<FilePath>..\User\task\error_detect_task.c</FilePath>
</File>
<File>
<FileName>dr16_task.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\dr16_task.c</FilePath>
</File>
<File>
<FileName>r12ds_task.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\r12ds_task.c</FilePath>
</File>
<File>
<FileName>up_task.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\up_task.c</FilePath>
</File>
<File>
<FileName>rc_task.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\rc_task.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -1308,21 +1303,16 @@
<FileType>1</FileType>
<FilePath>..\User\device\vofa.c</FilePath>
</File>
<File>
<FileName>dr16.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\dr16.c</FilePath>
</File>
<File>
<FileName>r12ds.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\r12ds.c</FilePath>
</File>
<File>
<FileName>GO_M8010_6_Driver.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\GO_M8010_6_Driver.c</FilePath>
</File>
<File>
<FileName>rc.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\rc.c</FilePath>
</File>
</Files>
</Group>
<Group>

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

Binary file not shown.

5
User/Algorithm/user_math.c
View File

@ -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)

2
User/Algorithm/user_math.h
View File

@ -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

1
User/bsp/bsp_usart.h
View File

@ -7,7 +7,6 @@
#define SBUS_RX_BUF_NUM 50u
#define RC_FRAME_LENGTH 25u
typedef enum {

31
User/device/LD_remote.c
View File

@ -1,31 +0,0 @@
#include "LD_remote.h"
#include <string.h>
#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;
}

21
User/device/LD_remote.h
View File

@ -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

90
User/device/cmd.c
View File

@ -29,37 +29,16 @@ 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 +59,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 +102,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; //左下，右上，无模式
}

21
User/device/cmd.h
View File

@ -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;

136
User/device/dr16.c
View File

@ -1,136 +0,0 @@
/*
DR16接收机
*/
/* Includes ----------------------------------------------------------------- */
#include "dr16.h"
#include <string.h>
#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;
}

7
User/device/motor_control.c
View File

@ -1,7 +0,0 @@
#include "motor_control.h"
#include "pid.h"
#include "user_math.h"
#include "device\device.h"
#include "can_use.h"

23
User/device/motor_control.h
View File

@ -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

192
User/device/r12ds.c
View File

@ -1,192 +0,0 @@
/*
乐迪r12ds遥控器。
需要根据实际的遥控器通道修改解析的函数 该遥控器采用sbus 用的uart3
*/
/* Includes ----------------------------------------------------------------- */
#include "r12ds.h"
#include "main.h"
#include "error_detect.h"
#include <string.h>
#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

29
User/device/r12ds.h
View File

@ -1,29 +0,0 @@
#ifndef _R12DS_H
#define _R12DS_H
#include <cmsis_os2.h>
#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

211
User/device/rc.c Normal file
View File

@ -0,0 +1,211 @@
/*
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;
}

49
User/device/dr16.h → User/device/rc.h
View File

@ -1,9 +1,9 @@
#ifndef DR16_h
#define DR16_h
#ifndef RC_h
#define RC_h
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#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 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

185
User/device/remote_control.c
View File

@ -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
*/

42
User/device/remote_control.h
View File

@ -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

1
User/task/error_detect_task.c
View File

@ -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.

6
User/task/init.c
View File

@ -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);

82
User/task/r12ds_task.c
View File

@ -1,82 +0,0 @@
#include "r12ds.h"
#include "cmsis_os.h"
#include "user_task.h"
#include <string.h>
#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);
}
}

16
User/task/r12ds_task.h
View File

@ -1,16 +0,0 @@
#ifndef _R12DS_TASK_H
#define _R12DS_TASK_H
#endif

39
User/task/dr16_task.c → User/task/rc_task.c
View File

@ -5,26 +5,24 @@
/* Includes ----------------------------------------------------------------- */
#include <string.h>
#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);
}

2
User/task/user_task.c
View File

@ -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,

22
User/task/user_task.h
View File

@ -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