遥控器类型在cmd.h里面修改
This commit is contained in:
parent
cfe106fa85
commit
8c5f3de185
@ -32,7 +32,6 @@
|
|||||||
/* USER CODE BEGIN Includes */
|
/* USER CODE BEGIN Includes */
|
||||||
#include "bsp_delay.h"
|
#include "bsp_delay.h"
|
||||||
#include "bsp_can.h"
|
#include "bsp_can.h"
|
||||||
#include "Action.h"
|
|
||||||
//#include "bsp_dwt.h"
|
//#include "bsp_dwt.h"
|
||||||
/* USER CODE END Includes */
|
/* USER CODE END Includes */
|
||||||
|
|
||||||
|
|||||||
@ -1144,30 +1144,6 @@
|
|||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>..\User\task\action_task.c</PathWithFileName>
|
|
||||||
<FilenameWithoutPath>action_task.c</FilenameWithoutPath>
|
|
||||||
<RteFlg>0</RteFlg>
|
|
||||||
<bShared>0</bShared>
|
|
||||||
</File>
|
|
||||||
<File>
|
|
||||||
<GroupNumber>8</GroupNumber>
|
|
||||||
<FileNumber>72</FileNumber>
|
|
||||||
<FileType>1</FileType>
|
|
||||||
<tvExp>0</tvExp>
|
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
|
||||||
<bDave2>0</bDave2>
|
|
||||||
<PathWithFileName>..\User\task\navi_task.c</PathWithFileName>
|
|
||||||
<FilenameWithoutPath>navi_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\r12ds_task.c</PathWithFileName>
|
<PathWithFileName>..\User\task\r12ds_task.c</PathWithFileName>
|
||||||
<FilenameWithoutPath>r12ds_task.c</FilenameWithoutPath>
|
<FilenameWithoutPath>r12ds_task.c</FilenameWithoutPath>
|
||||||
<RteFlg>0</RteFlg>
|
<RteFlg>0</RteFlg>
|
||||||
@ -1183,7 +1159,7 @@
|
|||||||
<RteFlg>0</RteFlg>
|
<RteFlg>0</RteFlg>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>9</GroupNumber>
|
<GroupNumber>9</GroupNumber>
|
||||||
<FileNumber>74</FileNumber>
|
<FileNumber>72</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
@ -1195,7 +1171,7 @@
|
|||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>9</GroupNumber>
|
<GroupNumber>9</GroupNumber>
|
||||||
<FileNumber>75</FileNumber>
|
<FileNumber>73</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
@ -1207,7 +1183,7 @@
|
|||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>9</GroupNumber>
|
<GroupNumber>9</GroupNumber>
|
||||||
<FileNumber>76</FileNumber>
|
<FileNumber>74</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
@ -1219,7 +1195,7 @@
|
|||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>9</GroupNumber>
|
<GroupNumber>9</GroupNumber>
|
||||||
<FileNumber>77</FileNumber>
|
<FileNumber>75</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
@ -1231,7 +1207,7 @@
|
|||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>9</GroupNumber>
|
<GroupNumber>9</GroupNumber>
|
||||||
<FileNumber>78</FileNumber>
|
<FileNumber>76</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
@ -1243,7 +1219,7 @@
|
|||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>9</GroupNumber>
|
<GroupNumber>9</GroupNumber>
|
||||||
<FileNumber>79</FileNumber>
|
<FileNumber>77</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
@ -1253,18 +1229,6 @@
|
|||||||
<RteFlg>0</RteFlg>
|
<RteFlg>0</RteFlg>
|
||||||
<bShared>0</bShared>
|
<bShared>0</bShared>
|
||||||
</File>
|
</File>
|
||||||
<File>
|
|
||||||
<GroupNumber>9</GroupNumber>
|
|
||||||
<FileNumber>80</FileNumber>
|
|
||||||
<FileType>1</FileType>
|
|
||||||
<tvExp>0</tvExp>
|
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
|
||||||
<bDave2>0</bDave2>
|
|
||||||
<PathWithFileName>..\User\Algorithm\navi.c</PathWithFileName>
|
|
||||||
<FilenameWithoutPath>navi.c</FilenameWithoutPath>
|
|
||||||
<RteFlg>0</RteFlg>
|
|
||||||
<bShared>0</bShared>
|
|
||||||
</File>
|
|
||||||
</Group>
|
</Group>
|
||||||
|
|
||||||
<Group>
|
<Group>
|
||||||
@ -1275,7 +1239,7 @@
|
|||||||
<RteFlg>0</RteFlg>
|
<RteFlg>0</RteFlg>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>10</GroupNumber>
|
<GroupNumber>10</GroupNumber>
|
||||||
<FileNumber>81</FileNumber>
|
<FileNumber>78</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
@ -1287,7 +1251,7 @@
|
|||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>10</GroupNumber>
|
<GroupNumber>10</GroupNumber>
|
||||||
<FileNumber>82</FileNumber>
|
<FileNumber>79</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
@ -1299,7 +1263,7 @@
|
|||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>10</GroupNumber>
|
<GroupNumber>10</GroupNumber>
|
||||||
<FileNumber>83</FileNumber>
|
<FileNumber>80</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
@ -1311,7 +1275,7 @@
|
|||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>10</GroupNumber>
|
<GroupNumber>10</GroupNumber>
|
||||||
<FileNumber>84</FileNumber>
|
<FileNumber>81</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
@ -1323,7 +1287,7 @@
|
|||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>10</GroupNumber>
|
<GroupNumber>10</GroupNumber>
|
||||||
<FileNumber>85</FileNumber>
|
<FileNumber>82</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
@ -1335,7 +1299,7 @@
|
|||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>10</GroupNumber>
|
<GroupNumber>10</GroupNumber>
|
||||||
<FileNumber>86</FileNumber>
|
<FileNumber>83</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
@ -1347,19 +1311,7 @@
|
|||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>10</GroupNumber>
|
<GroupNumber>10</GroupNumber>
|
||||||
<FileNumber>87</FileNumber>
|
<FileNumber>84</FileNumber>
|
||||||
<FileType>1</FileType>
|
|
||||||
<tvExp>0</tvExp>
|
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
|
||||||
<bDave2>0</bDave2>
|
|
||||||
<PathWithFileName>..\User\device\Action.c</PathWithFileName>
|
|
||||||
<FilenameWithoutPath>Action.c</FilenameWithoutPath>
|
|
||||||
<RteFlg>0</RteFlg>
|
|
||||||
<bShared>0</bShared>
|
|
||||||
</File>
|
|
||||||
<File>
|
|
||||||
<GroupNumber>10</GroupNumber>
|
|
||||||
<FileNumber>88</FileNumber>
|
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
@ -1379,7 +1331,7 @@
|
|||||||
<RteFlg>0</RteFlg>
|
<RteFlg>0</RteFlg>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>11</GroupNumber>
|
<GroupNumber>11</GroupNumber>
|
||||||
<FileNumber>89</FileNumber>
|
<FileNumber>85</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
@ -1391,7 +1343,7 @@
|
|||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>11</GroupNumber>
|
<GroupNumber>11</GroupNumber>
|
||||||
<FileNumber>90</FileNumber>
|
<FileNumber>86</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
@ -1403,7 +1355,7 @@
|
|||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>11</GroupNumber>
|
<GroupNumber>11</GroupNumber>
|
||||||
<FileNumber>91</FileNumber>
|
<FileNumber>87</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
@ -1423,7 +1375,7 @@
|
|||||||
<RteFlg>0</RteFlg>
|
<RteFlg>0</RteFlg>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>12</GroupNumber>
|
<GroupNumber>12</GroupNumber>
|
||||||
<FileNumber>92</FileNumber>
|
<FileNumber>88</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
@ -1443,7 +1395,7 @@
|
|||||||
<RteFlg>0</RteFlg>
|
<RteFlg>0</RteFlg>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>13</GroupNumber>
|
<GroupNumber>13</GroupNumber>
|
||||||
<FileNumber>93</FileNumber>
|
<FileNumber>89</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
@ -1455,7 +1407,7 @@
|
|||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>13</GroupNumber>
|
<GroupNumber>13</GroupNumber>
|
||||||
<FileNumber>94</FileNumber>
|
<FileNumber>90</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
@ -1467,7 +1419,7 @@
|
|||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>13</GroupNumber>
|
<GroupNumber>13</GroupNumber>
|
||||||
<FileNumber>95</FileNumber>
|
<FileNumber>91</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
@ -1479,7 +1431,7 @@
|
|||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>13</GroupNumber>
|
<GroupNumber>13</GroupNumber>
|
||||||
<FileNumber>96</FileNumber>
|
<FileNumber>92</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
|
|||||||
@ -1228,16 +1228,6 @@
|
|||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<FilePath>..\User\task\dr16_task.c</FilePath>
|
<FilePath>..\User\task\dr16_task.c</FilePath>
|
||||||
</File>
|
</File>
|
||||||
<File>
|
|
||||||
<FileName>action_task.c</FileName>
|
|
||||||
<FileType>1</FileType>
|
|
||||||
<FilePath>..\User\task\action_task.c</FilePath>
|
|
||||||
</File>
|
|
||||||
<File>
|
|
||||||
<FileName>navi_task.c</FileName>
|
|
||||||
<FileType>1</FileType>
|
|
||||||
<FilePath>..\User\task\navi_task.c</FilePath>
|
|
||||||
</File>
|
|
||||||
<File>
|
<File>
|
||||||
<FileName>r12ds_task.c</FileName>
|
<FileName>r12ds_task.c</FileName>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
@ -1278,11 +1268,6 @@
|
|||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<FilePath>..\User\Algorithm\pid.c</FilePath>
|
<FilePath>..\User\Algorithm\pid.c</FilePath>
|
||||||
</File>
|
</File>
|
||||||
<File>
|
|
||||||
<FileName>navi.c</FileName>
|
|
||||||
<FileType>1</FileType>
|
|
||||||
<FilePath>..\User\Algorithm\navi.c</FilePath>
|
|
||||||
</File>
|
|
||||||
</Files>
|
</Files>
|
||||||
</Group>
|
</Group>
|
||||||
<Group>
|
<Group>
|
||||||
@ -1318,11 +1303,6 @@
|
|||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<FilePath>..\User\device\dr16.c</FilePath>
|
<FilePath>..\User\device\dr16.c</FilePath>
|
||||||
</File>
|
</File>
|
||||||
<File>
|
|
||||||
<FileName>Action.c</FileName>
|
|
||||||
<FileType>1</FileType>
|
|
||||||
<FilePath>..\User\device\Action.c</FilePath>
|
|
||||||
</File>
|
|
||||||
<File>
|
<File>
|
||||||
<FileName>r12ds.c</FileName>
|
<FileName>r12ds.c</FileName>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
|
|||||||
Binary file not shown.
@ -1,216 +0,0 @@
|
|||||||
#include "navi.h"
|
|
||||||
|
|
||||||
|
|
||||||
//导航初始化一系列参数
|
|
||||||
int8_t Action_init(ops_t *o , const ops_param_t *param , Action_POS_t *pos)
|
|
||||||
{
|
|
||||||
if(o ==NULL) return DEVICE_ERR;
|
|
||||||
if(param == NULL) return DEVICE_ERR;
|
|
||||||
// while(!Action_ready)
|
|
||||||
// {
|
|
||||||
// TIM4->CCR3 = 9999;//等待全场定位初始化
|
|
||||||
// }
|
|
||||||
|
|
||||||
// TIM4->CCR3 = 0;
|
|
||||||
HAL_GPIO_WritePin(LED_G_GPIO_Port,LED_G_Pin,GPIO_PIN_SET);//接收到码盘数据后亮灯指示
|
|
||||||
//
|
|
||||||
|
|
||||||
// osDelay(5);
|
|
||||||
|
|
||||||
o->param = param;
|
|
||||||
/*全场定位pid初始化*/
|
|
||||||
//全场定位跑路径的xy方向速度环pid
|
|
||||||
PID_init(&o->pid_PosSpeed_x, PID_POSITION,(&o->param->ops_pid.pid_PosSpeed_x_param));
|
|
||||||
PID_init(&o->pid_PosSpeed_y, PID_POSITION,(&o->param->ops_pid.pid_PosSpeed_y_param));
|
|
||||||
/* 初始化角度纠正PID */
|
|
||||||
PID_init(&o->pid_OutAngle,PID_POSITION,(&o->param->ops_pid.pid_OutAngle_param));//外环
|
|
||||||
PID_init(&o->pid_InnerAngle,PID_POSITION,(&o->param->ops_pid.pid_InnerAngle_param));//内环
|
|
||||||
//底盘xy方向位置环pid初始化
|
|
||||||
//内环
|
|
||||||
PID_init(&o->pid_pos_x_inner,PID_POSITION,(&o->param->ops_pid.pid_pos_x_inner_param));
|
|
||||||
PID_init(&o->pid_pos_y_inner,PID_POSITION,(&o->param->ops_pid.pid_pos_y_inner_param));
|
|
||||||
//外环
|
|
||||||
PID_init(&o->pid_pos_x_out,PID_POSITION,(&o->param->ops_pid.pid_pos_x_out_param));
|
|
||||||
PID_init(&o->pid_pos_y_out,PID_POSITION,(&o->param->ops_pid.pid_pos_y_out_param));
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//底盘路径速度pid初始化
|
|
||||||
PID_init(&o->path_speed_pid,PID_POSITION,(&o->param->ops_pid.path_speed_pid_param));
|
|
||||||
//全场定位底盘坐标初始化
|
|
||||||
o->chassis_pos->pos_x=0;
|
|
||||||
o->chassis_pos->pos_y=0;
|
|
||||||
o->chassis_pos->pos_lastX=0;
|
|
||||||
o->chassis_pos->pos_lastY=0;
|
|
||||||
o->chassis_map =param->path ;
|
|
||||||
o->chassis_pos = pos;
|
|
||||||
|
|
||||||
|
|
||||||
//设置全场定位允许的误差范围
|
|
||||||
o->state.mistake = POS_ALLOW_MISTAKE;
|
|
||||||
o->state .angle_mistake =POS_ALLOW_ANGLE_MISTAKE ;
|
|
||||||
o->state.moveState = START;
|
|
||||||
o->state.points_num = param->path_num;
|
|
||||||
//标志位初始化
|
|
||||||
o->POS_IS_CPT = NO;
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
return DEVICE_OK;
|
|
||||||
}
|
|
||||||
//利用C板imu纠正角度
|
|
||||||
fp32 ops9_AngleCorr(ops_t *o,fp32 hope_angle)
|
|
||||||
{
|
|
||||||
fp32 delta_angle,delta_w;
|
|
||||||
//外环角度纠正
|
|
||||||
delta_angle = PID_calc(&o->pid_OutAngle,o->ops_imu_pos.yaw,hope_angle);
|
|
||||||
//内环速度纠正
|
|
||||||
delta_w = -PID_calc(&o->pid_InnerAngle,o->ops_gyro.z,delta_angle);
|
|
||||||
return delta_w;
|
|
||||||
}
|
|
||||||
|
|
||||||
//底盘xy速度计算(全场定位)
|
|
||||||
void POS_chassis_set(ops_t *o,fp32 vx_set, fp32 vy_set, fp32 yaw_angle_set){
|
|
||||||
|
|
||||||
|
|
||||||
o->final_out.vx = (PID_calc(&o->pid_PosSpeed_x, o->chassis_pos->pos_Vx,vx_set));//根据遥控器的方向选择正负
|
|
||||||
o->final_out.vy = PID_calc(&o->pid_PosSpeed_y, o->chassis_pos->pos_Vy,vy_set);
|
|
||||||
o->final_out.yaw_angle = ops9_AngleCorr(o,yaw_angle_set);
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
//定点控制
|
|
||||||
void hold_point(ops_t *o,point_t p,fp32 yaw_angle_set)
|
|
||||||
{
|
|
||||||
fp32 delta_x,delta_y;
|
|
||||||
fp32 delta_vx,delta_vy;
|
|
||||||
fp32 vx_set,vy_set;//x,y,w目标速度
|
|
||||||
|
|
||||||
//x
|
|
||||||
delta_x = PID_calc(&o->pid_pos_x_out,o->chassis_pos->pos_x,p.x);
|
|
||||||
delta_vx = PID_calc(&o->pid_pos_x_inner,o->chassis_pos->pos_Vx,delta_x);
|
|
||||||
|
|
||||||
//y
|
|
||||||
delta_y = PID_calc(&o->pid_pos_y_out,o->chassis_pos->pos_y,p.y);
|
|
||||||
delta_vy = PID_calc(&o->pid_pos_y_inner,o->chassis_pos->pos_Vy,delta_y);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
vx_set = delta_vx;
|
|
||||||
vy_set = delta_vy;
|
|
||||||
|
|
||||||
POS_chassis_set(o,vx_set,vy_set,yaw_angle_set);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//判断是否到达函数(全场定位)，最最后判断
|
|
||||||
bool isArrive(point_t p,ops_t *o)
|
|
||||||
{
|
|
||||||
uint16_t xArrive = 0, yArrive = 0, zArrive=0;
|
|
||||||
|
|
||||||
xArrive = abs_float_double(p.x,o->chassis_pos->pos_x) < o->state.mistake ? 1:0;
|
|
||||||
yArrive = abs_float_double(p.y,o->chassis_pos->pos_y) < o->state.mistake ? 1:0;
|
|
||||||
|
|
||||||
if(xArrive && yArrive) return true;
|
|
||||||
else return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//寻迹，跑点函数
|
|
||||||
int8_t go_path(ops_t *o,CMD_ACTION_t *ops_out)
|
|
||||||
{
|
|
||||||
static fp32 distance;//两点之间的距离差
|
|
||||||
static fp32 chassis_speed_set;//底盘速度设置
|
|
||||||
static fp32 SIN,COS;//方向解算
|
|
||||||
static fp32 world_vx,world_vy;//x,y方向分解速度
|
|
||||||
static fp32 chassis_vx,chassis_vy;//底盘xy分速度
|
|
||||||
static int cnt =0 ;//计数标志位
|
|
||||||
|
|
||||||
static int flag_update_num;//记录flag更新，防止cnt一直变化
|
|
||||||
|
|
||||||
/*数据同步*/
|
|
||||||
o->current_x =o->chassis_pos ->pos_x ;
|
|
||||||
o->current_y =o->chassis_pos ->pos_y ;
|
|
||||||
o->next_mapx = o->chassis_map[cnt].x;
|
|
||||||
o->next_mapy =o->chassis_map[cnt].y;
|
|
||||||
|
|
||||||
|
|
||||||
o->Navi_Mode .Trig_flag = ops_out ->flag ;
|
|
||||||
|
|
||||||
if(o == NULL) return DEVICE_ERR;
|
|
||||||
if (ops_out == NULL) return DEVICE_ERR;
|
|
||||||
|
|
||||||
// static fp32 yaw_angle_set = 0;
|
|
||||||
if(o->POS_IS_CPT == NO)//路径未跑完
|
|
||||||
{
|
|
||||||
if(o->state.moveState ==START)
|
|
||||||
{
|
|
||||||
//更新路径状态
|
|
||||||
o->state.moveState =MOVING;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
distance = sqrt((o->chassis_map[cnt].x - o->chassis_pos->pos_x)*(o->chassis_map[cnt].x - o->chassis_pos->pos_x)
|
|
||||||
+(o->chassis_map[cnt].y - o->chassis_pos->pos_y)*(o->chassis_map[cnt].y - o->chassis_pos->pos_y));
|
|
||||||
|
|
||||||
chassis_speed_set = PID_calc(&o->path_speed_pid,-distance,0);//此处由pid计算距离得出相应的速度
|
|
||||||
//速度分解
|
|
||||||
SIN = (o->chassis_map[cnt].y - o->chassis_pos->pos_y)/distance;
|
|
||||||
COS = (o->chassis_map[cnt].x - o->chassis_pos->pos_x)/distance;
|
|
||||||
|
|
||||||
world_vx = chassis_speed_set * COS;
|
|
||||||
world_vy = chassis_speed_set * SIN;
|
|
||||||
|
|
||||||
// chassis_vx = world_vx;
|
|
||||||
// chassis_vy = world_vy;
|
|
||||||
//世界坐标转机器坐标系转换
|
|
||||||
|
|
||||||
chassis_vx = -world_vy*sin(o->ops_imu_pos.yaw) + world_vx*cos(o->ops_imu_pos.yaw);
|
|
||||||
chassis_vy = world_vy*cos(o->ops_imu_pos.yaw) + world_vx*sin(o->ops_imu_pos.yaw);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
/*下面的逻辑按需更改，根据cnt计数器自动跑点，遥控器触发跑点等等*/
|
|
||||||
|
|
||||||
|
|
||||||
if(isArrive(o->chassis_map[cnt],o)) //判断是否到达(全场定位)，只涉及xy的判断
|
|
||||||
{
|
|
||||||
/*到达点后，这里应该增加cnt增加或减小的触发方式*/
|
|
||||||
|
|
||||||
if (o->Navi_Mode .Trig_flag != flag_update_num) { // 只有flag发生变化时才更新cnt
|
|
||||||
if (o->Navi_Mode .Trig_flag==1) cnt++; }
|
|
||||||
else if (o->Navi_Mode .Trig_flag == -1) cnt--;
|
|
||||||
flag_update_num = o->Navi_Mode .Trig_flag; // 更新prev_flag为当前的flag状态
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
o->POS_IS_CPT = NO;
|
|
||||||
hold_point(o,o->chassis_map[cnt],o->chassis_map[cnt].angle); //调用该函数使临近误差值快速响应
|
|
||||||
}
|
|
||||||
if(cnt>3)//根据点数更改
|
|
||||||
{
|
|
||||||
o->POS_IS_CPT =YES;
|
|
||||||
//到达目的地
|
|
||||||
o->final_out .vx =0;
|
|
||||||
o->final_out .vy =0;
|
|
||||||
o->final_out.yaw_angle=0;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
ops_out->out.Vx= o->final_out .vx ;
|
|
||||||
ops_out->out.Vy = o->final_out .vy ;
|
|
||||||
ops_out->out.Vw = o->final_out.yaw_angle;
|
|
||||||
|
|
||||||
return DEVICE_OK;
|
|
||||||
}
|
|
||||||
@ -1,108 +0,0 @@
|
|||||||
#ifndef NAVI_H
|
|
||||||
#define NAVI_H
|
|
||||||
|
|
||||||
|
|
||||||
#include "Action.h"
|
|
||||||
#include "pid.h"
|
|
||||||
#include "ahrs.h"
|
|
||||||
#include "bmi088.h"
|
|
||||||
#include "map.h"
|
|
||||||
|
|
||||||
typedef enum {
|
|
||||||
AUTO_MODE, // 自动跑点模式
|
|
||||||
MANUAL_MODE , // 控制模式
|
|
||||||
NO_MODE
|
|
||||||
} Navi_Mode_t;
|
|
||||||
|
|
||||||
typedef struct {
|
|
||||||
|
|
||||||
Navi_Mode_t mode;
|
|
||||||
|
|
||||||
int Trig_flag;//触发沿，初始值为零，两种控制模式下，这个值变为1，会使cnt+1，为-1，使cnt-1
|
|
||||||
|
|
||||||
} Navi_COT_MODE_t;
|
|
||||||
|
|
||||||
|
|
||||||
typedef struct{
|
|
||||||
float pos_x;
|
|
||||||
float pos_y;
|
|
||||||
float last_pos_x;
|
|
||||||
float last_pos_y;
|
|
||||||
}chassis_position_t;
|
|
||||||
|
|
||||||
typedef enum{
|
|
||||||
YES,
|
|
||||||
NO,
|
|
||||||
NEXT1,
|
|
||||||
NEXT2
|
|
||||||
}is_cpt_e;
|
|
||||||
typedef struct{
|
|
||||||
pid_param_t pid_PosSpeed_x_param;
|
|
||||||
pid_param_t pid_PosSpeed_y_param;
|
|
||||||
pid_param_t pid_pos_x_out_param;
|
|
||||||
pid_param_t pid_pos_x_inner_param;
|
|
||||||
pid_param_t pid_pos_y_out_param;
|
|
||||||
pid_param_t pid_pos_y_inner_param;
|
|
||||||
pid_param_t pid_OutAngle_param;
|
|
||||||
pid_param_t pid_InnerAngle_param;
|
|
||||||
pid_param_t path_speed_pid_param;
|
|
||||||
|
|
||||||
}ops_pid_param_t;
|
|
||||||
|
|
||||||
typedef struct {
|
|
||||||
ops_pid_param_t ops_pid;
|
|
||||||
const point_t *path;
|
|
||||||
int8_t path_num;
|
|
||||||
}ops_param_t;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
typedef struct {
|
|
||||||
Action_POS_t *chassis_pos;
|
|
||||||
fp32 current_x;
|
|
||||||
fp32 current_y;
|
|
||||||
|
|
||||||
fp32 next_mapx;
|
|
||||||
fp32 next_mapy;
|
|
||||||
const point_t *chassis_map;
|
|
||||||
const ops_param_t *param;
|
|
||||||
PathState_t state;
|
|
||||||
Navi_COT_MODE_t Navi_Mode;
|
|
||||||
|
|
||||||
AHRS_Eulr_t ops_imu_pos;
|
|
||||||
AHRS_Gyro_t ops_gyro;
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
pid_type_def pid_PosSpeed_x;
|
|
||||||
pid_type_def pid_PosSpeed_y;
|
|
||||||
pid_type_def pid_pos_x_out;
|
|
||||||
pid_type_def pid_pos_x_inner;
|
|
||||||
pid_type_def pid_pos_y_out;
|
|
||||||
pid_type_def pid_pos_y_inner;
|
|
||||||
pid_type_def pid_OutAngle;
|
|
||||||
pid_type_def pid_InnerAngle;
|
|
||||||
pid_type_def path_speed_pid;
|
|
||||||
|
|
||||||
|
|
||||||
is_cpt_e POS_IS_CPT;
|
|
||||||
|
|
||||||
struct
|
|
||||||
{
|
|
||||||
fp32 vx;
|
|
||||||
fp32 vy;
|
|
||||||
fp32 yaw_angle;
|
|
||||||
}final_out;
|
|
||||||
|
|
||||||
}ops_t;
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
int8_t Action_init(ops_t *o,const ops_param_t *param,Action_POS_t *pos);
|
|
||||||
|
|
||||||
int8_t go_path(ops_t *o,CMD_ACTION_t *ops_out);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
@ -1,6 +1,5 @@
|
|||||||
#include "Chassis.h"
|
#include "Chassis.h"
|
||||||
#include "gpio.h"
|
#include "gpio.h"
|
||||||
#include "Action.h"
|
|
||||||
#include "user_math.h"
|
#include "user_math.h"
|
||||||
#include "bsp_buzzer.h"
|
#include "bsp_buzzer.h"
|
||||||
#include "bsp_delay.h"
|
#include "bsp_delay.h"
|
||||||
@ -113,16 +112,11 @@ int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq)
|
|||||||
void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算
|
void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算
|
||||||
{
|
{
|
||||||
c->hopemotorout.OmniSpeedOut[3] = -Vx+Vy+Vw;//右前
|
c->hopemotorout.OmniSpeedOut[3] = -Vx+Vy+Vw;//右前
|
||||||
c->hopemotorout.OmniSpeedOut[2] = -Vx-Vy+Vw;//右后
|
c->hopemotorout.OmniSpeedOut[1] = -Vx-Vy+Vw;//右后
|
||||||
c->hopemotorout.OmniSpeedOut[1] = Vx-Vy+Vw;//左后
|
c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw;//左后
|
||||||
c->hopemotorout.OmniSpeedOut[0] = Vx+Vy+Vw;//左前
|
c->hopemotorout.OmniSpeedOut[0] = Vx+Vy+Vw;//左前
|
||||||
}
|
}
|
||||||
|
|
||||||
// v[0] = (vx_set+vy_set+vw_set)*mul + gyro_z;//左前
|
|
||||||
// v[1] = (-vx_set+vy_set+vw_set)*mul + gyro_z;//右前
|
|
||||||
// v[2] = (-vx_set-vy_set+vw_set)*mul + gyro_z;//右后
|
|
||||||
// v[3] = (vx_set-vy_set+vw_set)*mul + gyro_z;//左后
|
|
||||||
|
|
||||||
//bool isArrive(fp32 real_pos,fp32 target_pos,fp32 mistake)
|
//bool isArrive(fp32 real_pos,fp32 target_pos,fp32 mistake)
|
||||||
//{
|
//{
|
||||||
// uint16_t xArrive = 0, yArrive = 0;
|
// uint16_t xArrive = 0, yArrive = 0;
|
||||||
|
|||||||
@ -31,7 +31,6 @@
|
|||||||
#include "can_use.h"
|
#include "can_use.h"
|
||||||
#include "cmd.h"
|
#include "cmd.h"
|
||||||
#include "filter.h"
|
#include "filter.h"
|
||||||
#include "Action.h"
|
|
||||||
|
|
||||||
#define CHASSIS_OK (0)
|
#define CHASSIS_OK (0)
|
||||||
#define CHASSIS_ERR (-1)
|
#define CHASSIS_ERR (-1)
|
||||||
@ -125,7 +124,6 @@ typedef struct{
|
|||||||
const Chassis_Param_t *param; //一些固定的参数
|
const Chassis_Param_t *param; //一些固定的参数
|
||||||
|
|
||||||
ChassisImu_t pos088; //088的实时姿态
|
ChassisImu_t pos088; //088的实时姿态
|
||||||
Action_POS_t Action_pos;
|
|
||||||
CMD_Chassis_CtrlType_e ctrl;
|
CMD_Chassis_CtrlType_e ctrl;
|
||||||
CMD_Chassis_mode_e mode;
|
CMD_Chassis_mode_e mode;
|
||||||
|
|
||||||
|
|||||||
@ -193,93 +193,7 @@ static const ConfigParam_t param_chassis ={
|
|||||||
|
|
||||||
},
|
},
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/*码盘导航*/
|
|
||||||
|
|
||||||
.ops={
|
|
||||||
|
|
||||||
/*导航点*/
|
|
||||||
.path=map_point,
|
|
||||||
.path_num = point_num ,
|
|
||||||
.ops_pid={
|
|
||||||
/*路径速度*/
|
|
||||||
.path_speed_pid_param=
|
|
||||||
{
|
|
||||||
.p = 15.1f,
|
|
||||||
.i = 0.02f,
|
|
||||||
.d = 3.2f,
|
|
||||||
.i_limit = 200.0f,
|
|
||||||
.out_limit =6000.0f,
|
|
||||||
|
|
||||||
|
|
||||||
},
|
|
||||||
.pid_PosSpeed_x_param={
|
|
||||||
.p =1.0f,
|
|
||||||
.i =0.001f,
|
|
||||||
.d =0.0f,
|
|
||||||
.i_limit =15.0f,
|
|
||||||
.out_limit =4000.0f,
|
|
||||||
},
|
|
||||||
.pid_PosSpeed_y_param={
|
|
||||||
.p =1.0f,
|
|
||||||
.i =0.001f,
|
|
||||||
.d =0.0f,
|
|
||||||
.i_limit =15.0f,
|
|
||||||
.out_limit =4000.0f,
|
|
||||||
},
|
|
||||||
.pid_pos_x_out_param={
|
|
||||||
.p =1.0f,
|
|
||||||
.i =0.001f,
|
|
||||||
.d =0.0f,
|
|
||||||
.i_limit =15.0f,
|
|
||||||
.out_limit =4000.0f,
|
|
||||||
},
|
|
||||||
.pid_pos_x_inner_param={
|
|
||||||
.p =2.0f,
|
|
||||||
.i =0.001f,
|
|
||||||
.d =0.0f,
|
|
||||||
.i_limit =15.0f,
|
|
||||||
.out_limit =4000.0f,
|
|
||||||
},
|
|
||||||
.pid_pos_y_out_param={
|
|
||||||
.p =1.0f,
|
|
||||||
.i =0.001f,
|
|
||||||
.d =0.0f,
|
|
||||||
.i_limit =15.0f,
|
|
||||||
.out_limit =4000.0f,
|
|
||||||
},
|
|
||||||
.pid_pos_y_inner_param={
|
|
||||||
.p =2.0f,
|
|
||||||
.i =0.001f,
|
|
||||||
.d =0.0f,
|
|
||||||
.i_limit =15.0f,
|
|
||||||
.out_limit =4000.0f,
|
|
||||||
},
|
|
||||||
/*码盘导航陀螺仪角度纠正内外环*/
|
|
||||||
.pid_OutAngle_param={
|
|
||||||
.p = 5.0f,
|
|
||||||
.i = 0.0f,
|
|
||||||
.d = 0.0f,
|
|
||||||
.i_limit = 0.0f,
|
|
||||||
.out_limit =100.0f,
|
|
||||||
},
|
|
||||||
.pid_InnerAngle_param={
|
|
||||||
.p = 100.0f,
|
|
||||||
.i = 0.0f,
|
|
||||||
.d = 0.0f,
|
|
||||||
.i_limit = 0.0f,
|
|
||||||
.out_limit =1000.0f,
|
|
||||||
},
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
},
|
|
||||||
|
|
||||||
|
|
||||||
},
|
|
||||||
|
|
||||||
.can = {
|
.can = {
|
||||||
.pitch6020 = BSP_CAN_1,
|
.pitch6020 = BSP_CAN_1,
|
||||||
.motor3508 = BSP_CAN_1,
|
.motor3508 = BSP_CAN_1,
|
||||||
@ -293,11 +207,7 @@ const ConfigParam_t *Config_ChassisGet(void)
|
|||||||
return ¶m_chassis;
|
return ¶m_chassis;
|
||||||
}
|
}
|
||||||
|
|
||||||
///*获取导航地图*/
|
|
||||||
//void set_ops_path(ConfigParam_t *config, const point_t *path, int8_t path_num) {
|
|
||||||
// config->ops.path = path;
|
|
||||||
// config->ops.path_num = path_num;
|
|
||||||
//}
|
|
||||||
/**
|
/**
|
||||||
* \brief 从Flash读取配置信息
|
* \brief 从Flash读取配置信息
|
||||||
*
|
*
|
||||||
|
|||||||
@ -4,13 +4,11 @@
|
|||||||
#include "Chassis.h"
|
#include "Chassis.h"
|
||||||
#include "can_use.h"
|
#include "can_use.h"
|
||||||
#include "ahrs.h"
|
#include "ahrs.h"
|
||||||
#include "navi.h"
|
|
||||||
#include "map.h"
|
#include "map.h"
|
||||||
|
|
||||||
|
|
||||||
typedef struct{
|
typedef struct{
|
||||||
Chassis_Param_t chassis; /**/
|
Chassis_Param_t chassis; /**/
|
||||||
ops_param_t ops;
|
|
||||||
CAN_Params_t can;
|
CAN_Params_t can;
|
||||||
AHRS_Eulr_t mech_zero[4];
|
AHRS_Eulr_t mech_zero[4];
|
||||||
|
|
||||||
|
|||||||
@ -1,53 +0,0 @@
|
|||||||
#include "bsp_action.h"
|
|
||||||
#include "main.h"
|
|
||||||
|
|
||||||
|
|
||||||
extern UART_HandleTypeDef huart1;
|
|
||||||
extern DMA_HandleTypeDef hdma_usart1_rx;
|
|
||||||
void RS232_init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num)
|
|
||||||
{
|
|
||||||
|
|
||||||
//enable the dma transfer for the receiver request
|
|
||||||
SET_BIT(huart1.Instance->CR3, USART_CR3_DMAR);
|
|
||||||
|
|
||||||
//enable idle interrupt
|
|
||||||
__HAL_UART_ENABLE_IT(&huart1, UART_IT_IDLE);
|
|
||||||
|
|
||||||
//disable dma, to change the dma register
|
|
||||||
__HAL_DMA_DISABLE(&hdma_usart1_rx);
|
|
||||||
|
|
||||||
//disable dma again but why?
|
|
||||||
//what's the condition?
|
|
||||||
while(hdma_usart1_rx.Instance->CR & DMA_SxCR_EN)
|
|
||||||
{
|
|
||||||
__HAL_DMA_DISABLE(&hdma_usart1_rx);
|
|
||||||
}
|
|
||||||
|
|
||||||
//??
|
|
||||||
hdma_usart1_rx.Instance->PAR = (uint32_t) & (USART1->DR);
|
|
||||||
|
|
||||||
//memory buffer 1
|
|
||||||
hdma_usart1_rx.Instance->M0AR = (uint32_t)(rx1_buf);
|
|
||||||
|
|
||||||
//momory buffer 2
|
|
||||||
hdma_usart1_rx.Instance->M1AR = (uint32_t)(rx2_buf);
|
|
||||||
|
|
||||||
//data length
|
|
||||||
hdma_usart1_rx.Instance->NDTR = dma_buf_num;
|
|
||||||
|
|
||||||
//enable double memory buffer
|
|
||||||
SET_BIT(hdma_usart1_rx.Instance->CR, DMA_SxCR_DBM);
|
|
||||||
|
|
||||||
|
|
||||||
//enable dma
|
|
||||||
__HAL_DMA_ENABLE(&hdma_usart1_rx);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@ -1,8 +0,0 @@
|
|||||||
#ifndef BSP_ACTION_H
|
|
||||||
#define BSP_ACTION_H
|
|
||||||
#include "struct_typedef.h"
|
|
||||||
|
|
||||||
void RS232_init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num);
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
@ -1,203 +0,0 @@
|
|||||||
|
|
||||||
/**
|
|
||||||
* 东北大学ACTION码盘驱动
|
|
||||||
*
|
|
||||||
* 本驱动采用ops_9定位系统，负责收发码盘的原始数据，并在任务中解析出相应的位置坐标。
|
|
||||||
* 解析后的位置坐标将被交给导航处理模块，生成期望的运动向量。
|
|
||||||
*
|
|
||||||
* 使用手册：
|
|
||||||
* 请参阅相关文档了解如何使用本驱动。
|
|
||||||
*
|
|
||||||
* 注意：
|
|
||||||
* 本驱动仅适用于东北大学ACTION码盘。
|
|
||||||
|
|
||||||
*删去Action_HandleOffline错误处理函数中对结构体清0的函数
|
|
||||||
|
|
||||||
*/
|
|
||||||
|
|
||||||
/* Includes ----------------------------------------------------------------- */
|
|
||||||
#include "tim.h"
|
|
||||||
#include "Action.h"
|
|
||||||
#include <string.h>
|
|
||||||
|
|
||||||
static osThreadId_t thread_alert;
|
|
||||||
|
|
||||||
|
|
||||||
uint8_t rxbuf[RS232_FRAME_LENGTH];
|
|
||||||
|
|
||||||
static void Ops10msTimerCallback(void *arg){
|
|
||||||
(void)arg;
|
|
||||||
osThreadFlagsSet(thread_alert,SIGNAL_OPSTIMER_REDY);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Private function -------------------------------------------------------- */
|
|
||||||
static void ACTION_IdleCallback(void) {
|
|
||||||
osThreadFlagsSet(thread_alert,SIGNAL_ACTION_RAW_REDY);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Exported functions ------------------------------------------------------- */
|
|
||||||
int8_t ACTIONRECV_Init(Action_POS_t *pos){
|
|
||||||
if(pos == NULL) return DEVICE_ERR_NULL;
|
|
||||||
|
|
||||||
pos->Action_ready =0;//码盘校准标志位初始化
|
|
||||||
|
|
||||||
if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL;
|
|
||||||
pos->action_timer_Id =
|
|
||||||
osTimerNew(Ops10msTimerCallback, osTimerPeriodic,NULL,NULL);
|
|
||||||
|
|
||||||
osTimerStart(pos->action_timer_Id,10);//此处ticks 理解为1个tick为1ms 此处为每10ms触发一次回调函数
|
|
||||||
|
|
||||||
BSP_UART_RegisterCallback(BSP_UART_ACTION,BSP_UART_IDLE_LINE_CB,
|
|
||||||
ACTION_IdleCallback);
|
|
||||||
|
|
||||||
return DEVICE_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t ACTION_StartReceiving() {
|
|
||||||
if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_ACTION),
|
|
||||||
(uint8_t *)rxbuf,
|
|
||||||
sizeof(rxbuf)) == HAL_OK)
|
|
||||||
return DEVICE_OK;
|
|
||||||
return DEVICE_ERR;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool_t ACTION_WaitDmaCplt(void) {
|
|
||||||
return(osThreadFlagsWait(SIGNAL_ACTION_RAW_REDY, osFlagsWaitAll,400) ==
|
|
||||||
SIGNAL_ACTION_RAW_REDY);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/* *
|
|
||||||
|
|
||||||
|
|
||||||
*/
|
|
||||||
int8_t ACTION_Parse(Action_POS_t *pos)
|
|
||||||
{
|
|
||||||
if (pos == NULL) return DEVICE_ERR_NULL;
|
|
||||||
|
|
||||||
static union
|
|
||||||
{
|
|
||||||
fp32 pos_data[3];
|
|
||||||
char rxbuff[12];
|
|
||||||
} pos_rxbuf;
|
|
||||||
|
|
||||||
|
|
||||||
if (rxbuf[0] == 0x0D && rxbuf[1] == 0x0A)
|
|
||||||
{
|
|
||||||
if (rxbuf[26] == 0x0A && rxbuf[27] == 0x0D)
|
|
||||||
{
|
|
||||||
|
|
||||||
pos_rxbuf.rxbuff[0]=rxbuf[2];
|
|
||||||
pos_rxbuf.rxbuff[1]=rxbuf[3];
|
|
||||||
pos_rxbuf.rxbuff[2]=rxbuf[4];
|
|
||||||
pos_rxbuf.rxbuff[3]=rxbuf[5];
|
|
||||||
|
|
||||||
|
|
||||||
pos_rxbuf.rxbuff[4]=rxbuf[14];
|
|
||||||
pos_rxbuf.rxbuff[5]=rxbuf[15];
|
|
||||||
pos_rxbuf.rxbuff[6]=rxbuf[16];
|
|
||||||
pos_rxbuf.rxbuff[7]=rxbuf[17];
|
|
||||||
|
|
||||||
|
|
||||||
pos_rxbuf.rxbuff[8]=rxbuf[18];
|
|
||||||
pos_rxbuf.rxbuff[9]=rxbuf[19];
|
|
||||||
pos_rxbuf.rxbuff[10]=rxbuf[20];
|
|
||||||
pos_rxbuf.rxbuff[11]=rxbuf[21];
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// 数据解析
|
|
||||||
pos->pos_yaw = pos_rxbuf.pos_data[0];
|
|
||||||
// 按照安装方向决定正负号
|
|
||||||
pos->pos_x = pos_rxbuf.pos_data[1];
|
|
||||||
pos->pos_y = pos_rxbuf.pos_data[2];
|
|
||||||
|
|
||||||
pos ->Action_ready =1;//码盘校准完成
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
return DEVICE_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
//该函数用来计算速度度(利用获取的位置来计算相应的速度)
|
|
||||||
//此处获取的是真实的位置(mm)后每10ms做一次微分处理
|
|
||||||
int8_t ACTION_DataRefresh(Action_POS_t *pos){
|
|
||||||
if (pos == NULL) return DEVICE_ERR_NULL;
|
|
||||||
|
|
||||||
if (osThreadFlagsGet() & SIGNAL_OPSTIMER_REDY){
|
|
||||||
osThreadFlagsClear(SIGNAL_OPSTIMER_REDY);
|
|
||||||
//计算每毫秒的速度 该函数每10ms调用
|
|
||||||
pos->pos_Vx = (pos->pos_x - pos->pos_lastX) / 10;
|
|
||||||
pos->pos_Vy = (pos->pos_y - pos->pos_lastY) / 10;
|
|
||||||
|
|
||||||
pos->pos_lastX = pos->pos_x;
|
|
||||||
pos->pos_lastY = pos->pos_y;
|
|
||||||
}
|
|
||||||
return DEVICE_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
int8_t Action_HandleOffline(Action_POS_t *pos) {
|
|
||||||
if (pos == NULL) return DEVICE_ERR_NULL;
|
|
||||||
|
|
||||||
(void)pos;
|
|
||||||
|
|
||||||
// memset(pos, 0, sizeof(*pos));
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* 将字符串拼接 */
|
|
||||||
void Strcat(char str1[], char str2[], uint8_t num)
|
|
||||||
{
|
|
||||||
int i = 0, j = 0;
|
|
||||||
|
|
||||||
while (str1[i] != '\0')
|
|
||||||
i++;
|
|
||||||
|
|
||||||
for (j = 0; j < num; j++)
|
|
||||||
{
|
|
||||||
str1[i++] = str2[j];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/* 该部分函数用于码盘的重定位系统 */
|
|
||||||
/* ----------------------------- */
|
|
||||||
|
|
||||||
/* 手动标定，用于校正 */
|
|
||||||
void ACT_Calibration(void)
|
|
||||||
{
|
|
||||||
HAL_UART_Transmit(&huart1, (uint8_t *)"ACTR", 4, 100);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* 清零 */
|
|
||||||
/* 将当前位置设置为 (0, 0) */
|
|
||||||
void ACT_ZeroClear(void)
|
|
||||||
{
|
|
||||||
HAL_UART_Transmit(&huart1, (uint8_t *)"ACT0", 4, 100);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* 更新 XY 坐标 */
|
|
||||||
/* 将当前位置更新为指定的 X 和 Y */
|
|
||||||
void ACT_UpdateXY(float pos_x, float pos_y)
|
|
||||||
{
|
|
||||||
char update_xy[12] = "ACTD"; // 命令前缀为 "ACTD"
|
|
||||||
static union
|
|
||||||
{
|
|
||||||
float XY[2]; // 两个浮点数表示 X 和 Y 坐标
|
|
||||||
char data[8]; // 将浮点数视为字节数据
|
|
||||||
} set;
|
|
||||||
|
|
||||||
set.XY[0] = pos_x;
|
|
||||||
set.XY[1] = pos_y;
|
|
||||||
|
|
||||||
Strcat(update_xy, set.data, 8);
|
|
||||||
|
|
||||||
HAL_UART_Transmit(&huart1, (uint8_t *)update_xy, sizeof(update_xy), 100);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@ -1,59 +0,0 @@
|
|||||||
#ifndef _ACTION_H_
|
|
||||||
#define _ACTION_H_
|
|
||||||
|
|
||||||
#define RS232_FRAME_LENGTH 28u
|
|
||||||
#define REF_LEN_RX_BUFF 0xFF
|
|
||||||
|
|
||||||
#include <cmsis_os2.h>
|
|
||||||
#include "device.h"
|
|
||||||
#include "bsp_usart.h"
|
|
||||||
#include "cmd.h"
|
|
||||||
|
|
||||||
|
|
||||||
typedef struct
|
|
||||||
{
|
|
||||||
osTimerId_t action_timer_Id;
|
|
||||||
/*原始数据*/
|
|
||||||
fp32 pos_x;
|
|
||||||
fp32 pos_y;
|
|
||||||
fp32 pos_yaw; // 航向角速度
|
|
||||||
/*处理数据*/
|
|
||||||
fp32 pos_Vx;
|
|
||||||
fp32 pos_Vy;
|
|
||||||
fp32 pos_lastX;
|
|
||||||
fp32 pos_lastY;
|
|
||||||
|
|
||||||
int Action_ready;//码盘校准标志位
|
|
||||||
|
|
||||||
|
|
||||||
}Action_POS_t;
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
int8_t ACTIONRECV_Init(Action_POS_t*pos);
|
|
||||||
|
|
||||||
int8_t ACTION_StartReceiving(void);
|
|
||||||
|
|
||||||
bool_t ACTION_WaitDmaCplt(void);
|
|
||||||
|
|
||||||
int8_t ACTION_Parse(Action_POS_t*pos);
|
|
||||||
|
|
||||||
int8_t ACTION_DataRefresh(Action_POS_t*pos);
|
|
||||||
|
|
||||||
int8_t Action_HandleOffline(Action_POS_t *pos);
|
|
||||||
|
|
||||||
void ACT_UpdateXY(float pos_x,float pos_y);
|
|
||||||
|
|
||||||
void ACT_ZeroClear(void);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
@ -13,6 +13,17 @@
|
|||||||
|
|
||||||
/* Private function -------------------------------------------------------- */
|
/* Private function -------------------------------------------------------- */
|
||||||
/*Export function --------------------------------------------------------------*/
|
/*Export function --------------------------------------------------------------*/
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef dr16_t
|
||||||
|
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
|
||||||
|
#endif
|
||||||
int8_t CMD_Init(CMD_t *cmd){
|
int8_t CMD_Init(CMD_t *cmd){
|
||||||
/*若主结构体为空 自动返回错误 */
|
/*若主结构体为空 自动返回错误 */
|
||||||
if(cmd == NULL) return-1;
|
if(cmd == NULL) return-1;
|
||||||
@ -23,12 +34,14 @@ int8_t CMD_Init(CMD_t *cmd){
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
float aaaa;
|
|
||||||
|
|
||||||
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) {
|
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) {
|
||||||
|
|
||||||
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
|
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef r12ds_t
|
||||||
|
/*乐迪反馈值转换(-1 -- 1 )范围*/
|
||||||
if(rc->ch_x<0) cmd->Vx =map_fp32(rc->ch_x,-337,0,-1,0);
|
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);
|
else cmd->Vx =map_fp32(rc->ch_x,0,310,0,1);
|
||||||
|
|
||||||
@ -42,7 +55,25 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) {
|
|||||||
|
|
||||||
cmd->key_ctrl_l = rc->key [0];
|
cmd->key_ctrl_l = rc->key [0];
|
||||||
cmd->key_ctrl_r = rc->key [1];
|
cmd->key_ctrl_r = rc->key [1];
|
||||||
|
#elif defined(dr16_t)
|
||||||
|
|
||||||
|
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 ;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -59,12 +90,15 @@ static void CMD_RcLostLogic(CMD_t *cmd){
|
|||||||
int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc){
|
int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc){
|
||||||
if (cmd == NULL) return -1;
|
if (cmd == NULL) return -1;
|
||||||
if (rc == NULL) return -1;
|
if (rc == NULL) return -1;
|
||||||
|
#ifdef dr16_t
|
||||||
/*c当rc丢控时，恢复机器人至默认状态 */
|
/*c当rc丢控时，恢复机器人至默认状态 */
|
||||||
// if ((rc->ch_w == CMD_SW_ERR) || (rc->ch_y == CMD_SW_ERR)) {
|
if ((rc->ch_w == CMD_SW_ERR) || (rc->ch_y == CMD_SW_ERR)) {
|
||||||
// CMD_RcLostLogic(cmd);
|
CMD_RcLostLogic(cmd);
|
||||||
// } else {
|
} else {
|
||||||
CMD_RcLogic(rc, cmd);
|
CMD_RcLogic(rc, cmd);
|
||||||
// }
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -174,27 +208,4 @@ int8_t CMD_CtrlSet(CMD_t *cmd) {
|
|||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
int8_t CMD_ParseAction(CMD_t *cmd,CMD_ACTION_t *act)
|
|
||||||
{
|
|
||||||
if(cmd == NULL) return -1;
|
|
||||||
if(act == NULL) return -1;
|
|
||||||
|
|
||||||
if(cmd->key_ctrl_l ==CMD_SW_MID )
|
|
||||||
{
|
|
||||||
if(cmd->key_ctrl_r == CMD_SW_UP)
|
|
||||||
act->flag =1;
|
|
||||||
|
|
||||||
if(cmd->key_ctrl_r == CMD_SW_DOWN )
|
|
||||||
act->flag =-1;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
cmd->C_navi .vx =act ->out .Vx ;
|
|
||||||
cmd ->C_navi .vy =act ->out .Vy ;
|
|
||||||
cmd ->C_navi .wz =act ->out .Vw ;
|
|
||||||
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|||||||
@ -24,6 +24,14 @@
|
|||||||
|
|
||||||
#define MID (0x09)
|
#define MID (0x09)
|
||||||
|
|
||||||
|
|
||||||
|
/*选择遥控器，else为r12ds*/
|
||||||
|
//#define dr16_t
|
||||||
|
#define r12ds_t
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
typedef enum{
|
typedef enum{
|
||||||
RC,//遥控器控制，左按键上
|
RC,//遥控器控制，左按键上
|
||||||
MID_NAVI,//雷达导航
|
MID_NAVI,//雷达导航
|
||||||
@ -63,54 +71,46 @@ typedef enum {
|
|||||||
CMD_SW_DOWN = 2,
|
CMD_SW_DOWN = 2,
|
||||||
} CMD_SwitchPos_t;
|
} CMD_SwitchPos_t;
|
||||||
|
|
||||||
|
typedef enum
|
||||||
|
{
|
||||||
|
RC_dr16 ,
|
||||||
|
RC_r12ds
|
||||||
|
}remote_type;
|
||||||
|
|
||||||
/*遥控器值，使用CMD_RcLogic函数传入CMD_t结构体*/
|
/*遥控器值，使用CMD_RcLogic函数传入CMD_t结构体*/
|
||||||
typedef struct {
|
typedef struct {
|
||||||
// float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
|
|
||||||
// float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
|
remote_type RC_type;
|
||||||
// float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
|
#ifdef dr16_t
|
||||||
// float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */
|
|
||||||
|
float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
|
||||||
|
float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
|
||||||
|
float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
|
||||||
|
float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */
|
||||||
|
|
||||||
// float ch_res; /* 第五通道值 */
|
float ch_res; /* 第五通道值 */
|
||||||
|
|
||||||
// CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */
|
CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */
|
||||||
// CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */
|
CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */
|
||||||
|
|
||||||
|
|
||||||
// uint16_t key; /* 按键值 */
|
uint16_t key; /* 按键值 */
|
||||||
|
|
||||||
// uint16_t res; /* 保留，未启用 */
|
uint16_t res; /* 保留，未启用 */
|
||||||
|
|
||||||
|
#else
|
||||||
int16_t ch_x; // X轴（Channel 1）
|
int16_t ch_x; // X轴（Channel 1）
|
||||||
int16_t mul; // MUL（Channel 2）
|
int16_t mul; // MUL（Channel 2）
|
||||||
int16_t ch_y; // Y轴（Channel 3）
|
int16_t ch_y; // Y轴（Channel 3）
|
||||||
int16_t ch_w; // W轴（Channel 4）
|
int16_t ch_w; // W轴（Channel 4）
|
||||||
int16_t key[8]; // 按键通道（Channel 5-12）
|
int16_t key[8]; // 按键通道（Channel 5-12）
|
||||||
|
|
||||||
int16_t offline;
|
int16_t offline;
|
||||||
|
#endif
|
||||||
} __attribute__((packed))CMD_RC_t;
|
} __attribute__((packed))CMD_RC_t;
|
||||||
|
|
||||||
typedef struct
|
|
||||||
{
|
|
||||||
fp32 Vx;
|
|
||||||
fp32 Vy;
|
|
||||||
fp32 Vw;
|
|
||||||
}OpsHopeVector_t;
|
|
||||||
|
|
||||||
|
|
||||||
typedef struct
|
|
||||||
{
|
|
||||||
fp32 x;
|
|
||||||
fp32 y;
|
|
||||||
fp32 z;
|
|
||||||
int cnt_point;//计数点
|
|
||||||
}ops_point;
|
|
||||||
|
|
||||||
|
|
||||||
typedef struct {
|
|
||||||
|
|
||||||
OpsHopeVector_t out;
|
|
||||||
|
|
||||||
int flag;//传递flag触发标志位
|
|
||||||
|
|
||||||
} CMD_ACTION_t;
|
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
uint8_t status_fromnuc;
|
uint8_t status_fromnuc;
|
||||||
@ -121,12 +121,7 @@ typedef struct {
|
|||||||
fp32 vy;
|
fp32 vy;
|
||||||
fp32 wz;
|
fp32 wz;
|
||||||
}navi;
|
}navi;
|
||||||
struct
|
|
||||||
{
|
|
||||||
fp32 posx;
|
|
||||||
fp32 posy;
|
|
||||||
fp32 posw;
|
|
||||||
}pick;
|
|
||||||
struct
|
struct
|
||||||
{
|
{
|
||||||
fp32 angle;
|
fp32 angle;
|
||||||
@ -175,11 +170,8 @@ int8_t CMD_Init(CMD_t *cmd);
|
|||||||
|
|
||||||
int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc);
|
int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc);
|
||||||
|
|
||||||
int8_t CMD_ParseAction(CMD_t *cmd,CMD_ACTION_t *act);
|
|
||||||
|
|
||||||
int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n);
|
int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n);
|
||||||
|
|
||||||
|
|
||||||
int8_t CMD_SwitchStatus(CMD_t *cmd);
|
int8_t CMD_SwitchStatus(CMD_t *cmd);
|
||||||
|
|
||||||
int8_t CMD_CtrlSet(CMD_t *cmd);
|
int8_t CMD_CtrlSet(CMD_t *cmd);
|
||||||
|
|||||||
@ -1,135 +1,137 @@
|
|||||||
///*
|
/*
|
||||||
// DR16接收机
|
DR16接收机
|
||||||
|
|
||||||
//*/
|
*/
|
||||||
|
|
||||||
///* Includes ----------------------------------------------------------------- */
|
/* Includes ----------------------------------------------------------------- */
|
||||||
//#include "dr16.h"
|
#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;
|
|
||||||
//}
|
|
||||||
|
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
#include "bsp_usart.h"
|
||||||
|
#include "error_detect.h"
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef dr16_t
|
||||||
|
|
||||||
|
/* 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|||||||
@ -11,6 +11,11 @@
|
|||||||
/* Exported constants ------------------------------------------------------- */
|
/* Exported constants ------------------------------------------------------- */
|
||||||
/* Exported macro ----------------------------------------------------------- */
|
/* Exported macro ----------------------------------------------------------- */
|
||||||
/* Exported types ----------------------------------------------------------- */
|
/* Exported types ----------------------------------------------------------- */
|
||||||
|
|
||||||
|
#ifdef dr16_t
|
||||||
|
|
||||||
|
|
||||||
|
#define dr16_t
|
||||||
typedef __packed struct
|
typedef __packed struct
|
||||||
{
|
{
|
||||||
uint16_t ch_r_x : 11;
|
uint16_t ch_r_x : 11;
|
||||||
@ -44,3 +49,4 @@ int8_t DR16_HandleOffline(const DR16_t *dr16, CMD_RC_t *rc);
|
|||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#endif
|
||||||
@ -12,6 +12,8 @@
|
|||||||
#include <string.h>
|
#include <string.h>
|
||||||
#include "cmd.h"
|
#include "cmd.h"
|
||||||
#include "user_math.h"
|
#include "user_math.h"
|
||||||
|
|
||||||
|
#ifdef r12ds_t
|
||||||
extern UART_HandleTypeDef huart3;
|
extern UART_HandleTypeDef huart3;
|
||||||
extern DMA_HandleTypeDef hdma_usart3_rx;
|
extern DMA_HandleTypeDef hdma_usart3_rx;
|
||||||
|
|
||||||
@ -121,11 +123,6 @@ int8_t R12ds_DMA_Init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num)
|
|||||||
return 1;
|
return 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;
|
|
||||||
//}
|
|
||||||
|
|
||||||
|
|
||||||
int8_t sbus_to_rc(volatile const uint8_t *sbus_buf, CMD_RC_t *rc_ctrl)
|
int8_t sbus_to_rc(volatile const uint8_t *sbus_buf, CMD_RC_t *rc_ctrl)
|
||||||
{
|
{
|
||||||
@ -180,5 +177,5 @@ int8_t sbus_to_rc(volatile const uint8_t *sbus_buf, CMD_RC_t *rc_ctrl)
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|||||||
@ -6,6 +6,9 @@
|
|||||||
#include "bsp_usart.h"
|
#include "bsp_usart.h"
|
||||||
#include "cmd.h"
|
#include "cmd.h"
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef r12ds_t
|
||||||
|
|
||||||
#define RC_FRAME_LENGTH 25u
|
#define RC_FRAME_LENGTH 25u
|
||||||
|
|
||||||
|
|
||||||
@ -21,8 +24,8 @@ int8_t sbus_to_rc(volatile const uint8_t *sbus_buf,CMD_RC_t *rc_ctrl);
|
|||||||
//void R12ds_HandleOffline(void);
|
//void R12ds_HandleOffline(void);
|
||||||
|
|
||||||
|
|
||||||
// int map_int(int x, int in_min, int in_max, int out_min, int out_max) ;
|
|
||||||
|
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|||||||
@ -1,75 +0,0 @@
|
|||||||
/*
|
|
||||||
码盘解析任务
|
|
||||||
|
|
||||||
处理码盘测量的实际距离
|
|
||||||
|
|
||||||
将需要的数据共享给其他的线程
|
|
||||||
|
|
||||||
*/
|
|
||||||
#include "action_task.h"
|
|
||||||
#include "Action.h"
|
|
||||||
#include "user_task.h"
|
|
||||||
|
|
||||||
|
|
||||||
#ifdef DEBUG
|
|
||||||
|
|
||||||
Action_POS_t pos;
|
|
||||||
#else
|
|
||||||
|
|
||||||
static Action_POS pos;
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
void Task_action(void *argument)
|
|
||||||
{
|
|
||||||
(void)argument;
|
|
||||||
// osDelay(TASK_INIT_DELAY_ACTION);
|
|
||||||
|
|
||||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_ACTION;
|
|
||||||
|
|
||||||
|
|
||||||
ACTIONRECV_Init(&pos);
|
|
||||||
|
|
||||||
|
|
||||||
uint32_t tick = osKernelGetTickCount(); /* 获取当前控制任务运行频率的tick*/
|
|
||||||
|
|
||||||
while (1)
|
|
||||||
{
|
|
||||||
#ifdef DEBUG
|
|
||||||
/* 记录任务使用的的栈空闄*/
|
|
||||||
task_runtime.stack_water_mark.action =
|
|
||||||
osThreadGetStackSpace(osThreadGetId());
|
|
||||||
#endif
|
|
||||||
ACTION_StartReceiving();
|
|
||||||
ACTION_DataRefresh(&pos);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/* 使用下面的错误处理函数遇到一些问题
|
|
||||||
每10ms置标志位， ACTION_WaitDmaCplt 判断此标志位，通过后会执行速度计算
|
|
||||||
根据计算，任务运行频率大于中断产生频率，每一次运行判断标志位不通过，会导致结构体内数据清0的操作
|
|
||||||
目前的处理方法：注释掉Action_HandleOffline函数中，对结构体数据清零的函数memset
|
|
||||||
|
|
||||||
|
|
||||||
*/
|
|
||||||
|
|
||||||
if(ACTION_WaitDmaCplt()){
|
|
||||||
|
|
||||||
ACTION_Parse(&pos);
|
|
||||||
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
Action_HandleOffline(&pos);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
//将解算后的码盘位置值放入消息队列供其他任务使用
|
|
||||||
osMessageQueueReset(task_runtime.msgq.cmd.raw.Action);
|
|
||||||
osMessageQueuePut(task_runtime.msgq.cmd.raw.Action,(&pos),0,0);
|
|
||||||
|
|
||||||
|
|
||||||
tick += delay_tick; /* 计算下一个唤醒时刻*/
|
|
||||||
osDelayUntil(tick);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
@ -1 +0,0 @@
|
|||||||
|
|
||||||
@ -13,8 +13,6 @@
|
|||||||
static CAN_t can;
|
static CAN_t can;
|
||||||
|
|
||||||
|
|
||||||
#define remote_mode dr16 //遥控器选择
|
|
||||||
|
|
||||||
#ifdef DEBUG
|
#ifdef DEBUG
|
||||||
|
|
||||||
CAN_Output_t out;
|
CAN_Output_t out;
|
||||||
@ -70,9 +68,7 @@ void Task_Chassis(void *argument)
|
|||||||
osMessageQueueGet(task_runtime.msgq.imu.accl, &chassis.pos088.bmi088.accl,NULL, 0);
|
osMessageQueueGet(task_runtime.msgq.imu.accl, &chassis.pos088.bmi088.accl,NULL, 0);
|
||||||
/*can上设备数据获取*/
|
/*can上设备数据获取*/
|
||||||
osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0);
|
osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0);
|
||||||
|
|
||||||
osMessageQueueGet(task_runtime.msgq.cmd.raw.Action, &chassis.Action_pos , NULL, 0);
|
|
||||||
|
|
||||||
/*底盘控制信息获取,目前dji遥控器*/
|
/*底盘控制信息获取,目前dji遥控器*/
|
||||||
osMessageQueueGet(task_runtime.msgq.cmd.chassis,&ctrl, NULL, 0);
|
osMessageQueueGet(task_runtime.msgq.cmd.chassis,&ctrl, NULL, 0);
|
||||||
|
|
||||||
|
|||||||
@ -6,7 +6,6 @@
|
|||||||
CMD_t cmd;
|
CMD_t cmd;
|
||||||
CMD_RC_t rc_ctrl;
|
CMD_RC_t rc_ctrl;
|
||||||
CMD_NUC_t Nuc;
|
CMD_NUC_t Nuc;
|
||||||
CMD_ACTION_t cmd_ops_out;
|
|
||||||
|
|
||||||
#else
|
#else
|
||||||
|
|
||||||
@ -52,8 +51,6 @@ void Task_cmd(void *argument){
|
|||||||
if(osMessageQueueGet(task_runtime.msgq.cmd.raw.rc, &rc_ctrl, 0, 0) == osOK)//遥控器
|
if(osMessageQueueGet(task_runtime.msgq.cmd.raw.rc, &rc_ctrl, 0, 0) == osOK)//遥控器
|
||||||
CMD_ParseRc(&cmd, &rc_ctrl);
|
CMD_ParseRc(&cmd, &rc_ctrl);
|
||||||
|
|
||||||
if(osMessageQueueGet(task_runtime.msgq.cmd.raw .ops_9_out , &cmd_ops_out, 0, 0) == osOK)//码盘
|
|
||||||
CMD_ParseAction (&cmd ,&cmd_ops_out);
|
|
||||||
osKernelUnlock(); /* 同上 解锁RTOS内核 */
|
osKernelUnlock(); /* 同上 解锁RTOS内核 */
|
||||||
|
|
||||||
/*将需要与其他任务共享的数据放到消息队列里 此处主要分享给底盘 后续会添加和上层机构的通信 */
|
/*将需要与其他任务共享的数据放到消息队列里 此处主要分享给底盘 后续会添加和上层机构的通信 */
|
||||||
|
|||||||
@ -1,104 +1,62 @@
|
|||||||
///*
|
/*
|
||||||
// DR16接收机通信任务（支持DR16和SBUS双协议）
|
DR16接收机通信任务
|
||||||
//*/
|
*/
|
||||||
|
|
||||||
///* Includes ----------------------------------------------------------------- */
|
/* Includes ----------------------------------------------------------------- */
|
||||||
//#include <string.h>
|
#include <string.h>
|
||||||
//#include "dr16.h"
|
|
||||||
//#include "LD_remote.h" // 新增 SBUS 支持
|
|
||||||
//#include "user_task.h"
|
|
||||||
|
|
||||||
///* Private typedef ---------------------------------------------------------- */
|
#include "dr16.h"
|
||||||
///* Private define ----------------------------------------------------------- */
|
#include "user_task.h"
|
||||||
//#define PROTOCOL_DR16 0
|
|
||||||
//#define PROTOCOL_SBUS 1
|
|
||||||
|
|
||||||
|
/* Private typedef ---------------------------------------------------------- */
|
||||||
|
/* Private define ----------------------------------------------------------- */
|
||||||
/* Private macro ------------------------------------------------------------ */
|
/* Private macro ------------------------------------------------------------ */
|
||||||
///* Private variables -------------------------------------------------------- */
|
/* Private variables -------------------------------------------------------- */
|
||||||
//#ifdef DEBUG
|
|
||||||
//DR16_t dr16;
|
|
||||||
//CMD_RC_t cmd_rc;
|
|
||||||
//#else
|
|
||||||
//static DR16_t dr16;
|
|
||||||
//static CMD_RC_t cmd_rc;
|
|
||||||
//#endif
|
|
||||||
|
|
||||||
//static uint8_t protocol_type = PROTOCOL_DR16; // 当前协议类型
|
#ifdef dr16
|
||||||
// uint8_t sbus_rx_buf[36]; // SBUS 数据缓冲区
|
|
||||||
|
|
||||||
///* Private function --------------------------------------------------------- */
|
#ifdef DEBUG
|
||||||
///**
|
DR16_t dr16;
|
||||||
// * \brief 协议识别函数
|
CMD_RC_t cmd_rc;
|
||||||
// *
|
#else
|
||||||
// * \param buf 接收到的数据缓冲区
|
static DR16_t dr16;
|
||||||
// * \return uint8_t 协议类型：PROTOCOL_DR16 或 PROTOCOL_SBUS
|
static CMD_RC_t cmd_rc;
|
||||||
// */
|
#endif
|
||||||
//static uint8_t Detect_Protocol(const uint8_t *buf) {
|
|
||||||
// if (buf[0] == 0x0F && buf[24] == 0x00) { // SBUS 协议特征
|
|
||||||
// return PROTOCOL_SBUS;
|
|
||||||
// }
|
|
||||||
// return PROTOCOL_DR16; // 默认 DR16
|
|
||||||
//}
|
|
||||||
|
|
||||||
///**
|
#endif
|
||||||
// * \brief 统一回调函数
|
|
||||||
// */
|
|
||||||
//static void UART_RxCpltCallback(void) {
|
|
||||||
// if (Detect_Protocol(sbus_rx_buf) == PROTOCOL_SBUS) {
|
|
||||||
// protocol_type = PROTOCOL_SBUS;
|
|
||||||
// } else {
|
|
||||||
// protocol_type = PROTOCOL_DR16;
|
|
||||||
// }
|
|
||||||
// osThreadFlagsSet(osThreadGetId(), SIGNAL_DR16_RAW_REDY);
|
|
||||||
//}
|
|
||||||
|
|
||||||
///* Exported functions ------------------------------------------------------- */
|
/* Private function --------------------------------------------------------- */
|
||||||
|
/* Exported functions ------------------------------------------------------- */
|
||||||
|
|
||||||
///**
|
/**
|
||||||
// * \brief dr16接收机任务（支持DR16和SBUS双协议）
|
* \brief dr16接收机
|
||||||
// *
|
*
|
||||||
// * \param argument 未使用
|
* \param argument 未使用
|
||||||
// */
|
*/
|
||||||
//void Task_dr16(void *argument) {
|
void Task_dr16(void *argument) {
|
||||||
// (void)argument; /* 未使用，消除警告 */
|
(void)argument; /* 未使用，消除警告 */
|
||||||
|
#ifdef dr16
|
||||||
|
|
||||||
// DR16_Init(&dr16); /* 初始化 DR16 */
|
DR16_Init(&dr16); /* 初始化dr16 */
|
||||||
// RC_New_Init(); /* 初始化 SBUS */
|
|
||||||
|
|
||||||
// while (1) {
|
while (1) {
|
||||||
//#ifdef DEBUG
|
#ifdef DEBUG
|
||||||
// /* 调试信息：堆栈水位 */
|
/* */
|
||||||
// task_runtime.stack_water_mark.dr16 = osThreadGetStackSpace(osThreadGetId());
|
task_runtime.stack_water_mark.dr16 = osThreadGetStackSpace(osThreadGetId());
|
||||||
//#endif
|
#endif
|
||||||
|
/* 开启DMA */
|
||||||
|
DR16_StartDmaRecv(&dr16);
|
||||||
|
|
||||||
// /* 开启 DMA 接收 */
|
if (DR16_WaitDmaCplt(30)) {
|
||||||
// if (protocol_type == PROTOCOL_DR16) {
|
/* 转换 */
|
||||||
// DR16_StartDmaRecv(&dr16);
|
DR16_ParseRaw(&dr16);
|
||||||
// } else {
|
DR16_ParseRC(&dr16, &cmd_rc);
|
||||||
// HAL_UARTEx_ReceiveToIdle_DMA(&huart3, sbus_rx_buf, sizeof(sbus_rx_buf));
|
} else {
|
||||||
// }
|
/* 处理遥控器离线 */
|
||||||
|
DR16_HandleOffline(&dr16, &cmd_rc);
|
||||||
// /* 等待 DMA 完成 */
|
}
|
||||||
// if (DR16_WaitDmaCplt(30)) {
|
osMessageQueueReset(task_runtime.msgq.cmd.raw.rc);
|
||||||
// if (protocol_type == PROTOCOL_DR16) {
|
osMessageQueuePut(task_runtime.msgq.cmd.raw.rc, &cmd_rc, 0, 0);
|
||||||
// /* DR16 数据处理 */
|
}
|
||||||
// DR16_ParseRaw(&dr16);
|
#endif
|
||||||
// DR16_ParseRC(&dr16, &cmd_rc);
|
}
|
||||||
// } else {
|
|
||||||
// /* SBUS 数据处理 */
|
|
||||||
// const RC_Ctrl_New_t *sbus_data = RC_New_GetData();
|
|
||||||
// cmd_rc.ch_r_x = sbus_data->ch[0] / 1000.0f; // 映射到 [-1, 1]
|
|
||||||
// cmd_rc.ch_r_y = sbus_data->ch[1] / 1000.0f;
|
|
||||||
// cmd_rc.ch_l_x = sbus_data->ch[2] / 1000.0f;
|
|
||||||
// cmd_rc.ch_l_y = sbus_data->ch[3] / 1000.0f;
|
|
||||||
// }
|
|
||||||
// } else {
|
|
||||||
// /* 处理遥控器离线 */
|
|
||||||
// DR16_HandleOffline(&dr16, &cmd_rc);
|
|
||||||
// }
|
|
||||||
|
|
||||||
// /* 发送数据到消息队列 */
|
|
||||||
// osMessageQueueReset(task_runtime.msgq.cmd.raw.rc);
|
|
||||||
// osMessageQueuePut(task_runtime.msgq.cmd.raw.rc, &cmd_rc, 0, 0);
|
|
||||||
// }
|
|
||||||
//}
|
|
||||||
|
|||||||
@ -11,7 +11,6 @@
|
|||||||
#include "task\user_task.h"
|
#include "task\user_task.h"
|
||||||
#include "can_use.h"
|
#include "can_use.h"
|
||||||
#include "cmd.h"
|
#include "cmd.h"
|
||||||
#include "Action.h"
|
|
||||||
|
|
||||||
/* Private typedef ---------------------------------------------------------- */
|
/* Private typedef ---------------------------------------------------------- */
|
||||||
/* Private define ----------------------------------------------------------- */
|
/* Private define ----------------------------------------------------------- */
|
||||||
@ -36,8 +35,6 @@ void Task_Init(void *argument) {
|
|||||||
osThreadNew(Task_AttiEsti, NULL, &attr_atti_esti);
|
osThreadNew(Task_AttiEsti, NULL, &attr_atti_esti);
|
||||||
task_runtime.thread.chassis =
|
task_runtime.thread.chassis =
|
||||||
osThreadNew(Task_Chassis, NULL, &attr_chassis);
|
osThreadNew(Task_Chassis, NULL, &attr_chassis);
|
||||||
// task_runtime.thread.dr16 =
|
|
||||||
// osThreadNew(Task_dr16,NULL,&attr_dr16);
|
|
||||||
task_runtime.thread.r12ds =
|
task_runtime.thread.r12ds =
|
||||||
osThreadNew(Task_r12ds,NULL,&attr_r12ds);
|
osThreadNew(Task_r12ds,NULL,&attr_r12ds);
|
||||||
|
|
||||||
@ -47,13 +44,7 @@ void Task_Init(void *argument) {
|
|||||||
osThreadNew(Task_cmd,NULL,&attr_cmd);
|
osThreadNew(Task_cmd,NULL,&attr_cmd);
|
||||||
task_runtime.thread.nuc =
|
task_runtime.thread.nuc =
|
||||||
osThreadNew(Task_nuc,NULL,&attr_nuc);
|
osThreadNew(Task_nuc,NULL,&attr_nuc);
|
||||||
|
|
||||||
task_runtime.thread.navi =
|
|
||||||
osThreadNew(Task_Navi,NULL,&attr_navi);
|
|
||||||
|
|
||||||
task_runtime.thread.action_ops=
|
|
||||||
osThreadNew(Task_action,NULL,&attr_ops_9pos);
|
|
||||||
|
|
||||||
task_runtime.thread.error_detect =
|
task_runtime.thread.error_detect =
|
||||||
osThreadNew(Task_error_detect,NULL,&attr_error_detect);
|
osThreadNew(Task_error_detect,NULL,&attr_error_detect);
|
||||||
|
|
||||||
@ -88,13 +79,6 @@ void Task_Init(void *argument) {
|
|||||||
osMessageQueueNew(3u, sizeof(CMD_t), NULL);
|
osMessageQueueNew(3u, sizeof(CMD_t), NULL);
|
||||||
task_runtime.msgq.cmd.raw.nuc =
|
task_runtime.msgq.cmd.raw.nuc =
|
||||||
osMessageQueueNew(3u,sizeof(CMD_NUC_t), NULL);
|
osMessageQueueNew(3u,sizeof(CMD_NUC_t), NULL);
|
||||||
task_runtime.msgq.cmd.raw.Action =
|
|
||||||
osMessageQueueNew(3u,sizeof(Action_POS_t), NULL);
|
|
||||||
task_runtime.msgq.cmd.raw.ops_9_out =
|
|
||||||
osMessageQueueNew(3u,sizeof(CMD_ACTION_t), NULL);
|
|
||||||
|
|
||||||
// task_runtime.mutex.imu_eulr =
|
|
||||||
// xSemaphoreCreateMutex();
|
|
||||||
|
|
||||||
osKernelUnlock();
|
osKernelUnlock();
|
||||||
osThreadTerminate(osThreadGetId()); /* 结束自身 */
|
osThreadTerminate(osThreadGetId()); /* 结束自身 */
|
||||||
|
|||||||
@ -1,68 +0,0 @@
|
|||||||
#include "user_task.h"
|
|
||||||
#include "navi.h"
|
|
||||||
#include "map.h"
|
|
||||||
|
|
||||||
#ifdef DEBUG
|
|
||||||
ops_t o;
|
|
||||||
Action_POS_t pos_get;
|
|
||||||
CMD_ACTION_t ops_out;//经过导航算法解算后输出的期望控制值
|
|
||||||
|
|
||||||
#else
|
|
||||||
|
|
||||||
static ops_t o;
|
|
||||||
static Action_POS_t pos_get;
|
|
||||||
static CMD_ACTION_t ops_out;
|
|
||||||
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
void Task_Navi(void *argument){
|
|
||||||
|
|
||||||
(void)argument;
|
|
||||||
|
|
||||||
const uint32_t delay_tick = osKernelGetTickFreq() /TASK_FREQ_NAVI;
|
|
||||||
|
|
||||||
Action_init(&o,&(task_runtime.config.chassis_config->ops),&pos_get);
|
|
||||||
|
|
||||||
|
|
||||||
uint32_t tick = osKernelGetTickCount(); /* 获取当前控制任务运行频率的tick*/
|
|
||||||
|
|
||||||
while (1)
|
|
||||||
{
|
|
||||||
#ifdef DEBUG
|
|
||||||
/* 记录任务使用的的栈空间 */
|
|
||||||
task_runtime.stack_water_mark.action =
|
|
||||||
osThreadGetStackSpace(osThreadGetId());
|
|
||||||
#endif
|
|
||||||
osMessageQueueGet(task_runtime.msgq.imu.gyro,&o.ops_gyro,NULL,0);
|
|
||||||
|
|
||||||
osMessageQueueGet(task_runtime.msgq.imu.eulr,&o.ops_imu_pos,NULL,0);
|
|
||||||
|
|
||||||
osMessageQueueGet(task_runtime.msgq.cmd .raw .Action ,&pos_get,NULL,0);
|
|
||||||
|
|
||||||
go_path(&o,&ops_out);
|
|
||||||
|
|
||||||
//将解算后的导航期望运动值放入消息队列供其他任务使用
|
|
||||||
osMessageQueueReset(task_runtime.msgq.cmd.raw.ops_9_out);
|
|
||||||
osMessageQueuePut(task_runtime.msgq.cmd.raw.ops_9_out,(&ops_out),0,0);
|
|
||||||
|
|
||||||
tick += delay_tick; /* 计算下一个唤醒时刻*/
|
|
||||||
osDelayUntil(tick);
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@ -1 +0,0 @@
|
|||||||
#include "navi.h"
|
|
||||||
@ -3,6 +3,9 @@
|
|||||||
#include "user_task.h"
|
#include "user_task.h"
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef r12ds_t
|
||||||
|
|
||||||
#ifdef DEBUG
|
#ifdef DEBUG
|
||||||
|
|
||||||
CMD_RC_t cmd_rc;
|
CMD_RC_t cmd_rc;
|
||||||
@ -20,6 +23,8 @@ static CMD_RC_t cmd_rc;
|
|||||||
CMD_RC_t *R12ds_DataGet(){
|
CMD_RC_t *R12ds_DataGet(){
|
||||||
return &cmd_rc;
|
return &cmd_rc;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
//void R12ds_HandleOffline(void) {
|
//void R12ds_HandleOffline(void) {
|
||||||
// CMD_RC_t *rc;
|
// CMD_RC_t *rc;
|
||||||
// rc = R12ds_DataGet();
|
// rc = R12ds_DataGet();
|
||||||
@ -27,10 +32,15 @@ CMD_RC_t *R12ds_DataGet(){
|
|||||||
// memset(rc, 0, sizeof(*rc));
|
// memset(rc, 0, sizeof(*rc));
|
||||||
// rc->offline = 1;//用作遥控器断电后急停使用
|
// rc->offline = 1;//用作遥控器断电后急停使用
|
||||||
//}
|
//}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
void Task_r12ds(void *argument)
|
void Task_r12ds(void *argument)
|
||||||
{
|
{
|
||||||
(void)argument;
|
(void)argument;
|
||||||
|
|
||||||
|
#ifdef r12ds_t
|
||||||
|
|
||||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_R12DS;
|
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_R12DS;
|
||||||
R12ds_DMA_Init(sbus_rx_buf[0],sbus_rx_buf[1],50);
|
R12ds_DMA_Init(sbus_rx_buf[0],sbus_rx_buf[1],50);
|
||||||
|
|
||||||
@ -61,6 +71,7 @@ void Task_r12ds(void *argument)
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@ -25,11 +25,6 @@ const osThreadAttr_t attr_atti_esti = {
|
|||||||
.priority = osPriorityRealtime,
|
.priority = osPriorityRealtime,
|
||||||
.stack_size = 256 * 4,
|
.stack_size = 256 * 4,
|
||||||
};
|
};
|
||||||
const osThreadAttr_t attr_ops_9pos = {
|
|
||||||
.name = "action",
|
|
||||||
.priority = osPriorityRealtime,
|
|
||||||
.stack_size = 256 *4,
|
|
||||||
};
|
|
||||||
const osThreadAttr_t attr_chassis = {
|
const osThreadAttr_t attr_chassis = {
|
||||||
.name = "chassis",
|
.name = "chassis",
|
||||||
.priority = osPriorityAboveNormal,
|
.priority = osPriorityAboveNormal,
|
||||||
@ -65,14 +60,10 @@ const osThreadAttr_t attr_error_detect = {
|
|||||||
.stack_size = 128 *4,
|
.stack_size = 128 *4,
|
||||||
};
|
};
|
||||||
|
|
||||||
//const osThreadAttr_t attr_dr16 = {
|
const osThreadAttr_t attr_dr16 = {
|
||||||
// .name = "dr16",
|
.name = "dr16",
|
||||||
// .priority = osPriorityRealtime,
|
|
||||||
// .stack_size = 128 *4,
|
|
||||||
//};
|
|
||||||
|
|
||||||
const osThreadAttr_t attr_navi = {
|
|
||||||
.name = "navi",
|
|
||||||
.priority = osPriorityRealtime,
|
.priority = osPriorityRealtime,
|
||||||
.stack_size = 128 *4,
|
.stack_size = 128 *4,
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -18,17 +18,13 @@
|
|||||||
#define TASK_FREQ_CTRL_CMD (500u)
|
#define TASK_FREQ_CTRL_CMD (500u)
|
||||||
#define TASK_FREQ_NUC (500u)
|
#define TASK_FREQ_NUC (500u)
|
||||||
#define TASK_FREQ_CAN (1000u)
|
#define TASK_FREQ_CAN (1000u)
|
||||||
#define TASK_FREQ_NAVI (500u)
|
|
||||||
#define TASK_FREQ_R12DS (1000u)
|
#define TASK_FREQ_R12DS (1000u)
|
||||||
#define TASK_FREQ_ACTION (500u)
|
#define TASK_FREQ_dr16 (1000u)
|
||||||
|
|
||||||
#define TASK_FREQ_ERROR_DTC (3u)
|
#define TASK_FREQ_ERROR_DTC (3u)
|
||||||
|
|
||||||
#define DEBUG
|
#define DEBUG
|
||||||
|
|
||||||
|
|
||||||
#define TASK_INIT_DELAY_ACTION (400u)
|
|
||||||
|
|
||||||
#define TASK_INIT_DELAY_NUC (500u)
|
#define TASK_INIT_DELAY_NUC (500u)
|
||||||
|
|
||||||
/* Exported defines --------------------------------------------------------- */
|
/* Exported defines --------------------------------------------------------- */
|
||||||
@ -40,17 +36,14 @@ typedef struct {
|
|||||||
struct {
|
struct {
|
||||||
osThreadId_t atti_esti;
|
osThreadId_t atti_esti;
|
||||||
osThreadId_t chassis;
|
osThreadId_t chassis;
|
||||||
// osThreadId_t dr16;
|
osThreadId_t dr16;
|
||||||
|
|
||||||
osThreadId_t r12ds;
|
osThreadId_t r12ds;
|
||||||
|
|
||||||
osThreadId_t action_ops;
|
|
||||||
|
|
||||||
osThreadId_t ai;
|
osThreadId_t ai;
|
||||||
osThreadId_t can;
|
osThreadId_t can;
|
||||||
osThreadId_t cmd;
|
osThreadId_t cmd;
|
||||||
osThreadId_t nuc;
|
osThreadId_t nuc;
|
||||||
osThreadId_t navi;
|
|
||||||
osThreadId_t error_detect;
|
osThreadId_t error_detect;
|
||||||
} thread;
|
} thread;
|
||||||
|
|
||||||
@ -67,8 +60,6 @@ typedef struct {
|
|||||||
osMessageQueueId_t rc;
|
osMessageQueueId_t rc;
|
||||||
osMessageQueueId_t nuc;
|
osMessageQueueId_t nuc;
|
||||||
osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/
|
osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/
|
||||||
osMessageQueueId_t Action;
|
|
||||||
osMessageQueueId_t ops_9_out;
|
|
||||||
|
|
||||||
|
|
||||||
}raw;
|
}raw;
|
||||||
@ -105,13 +96,11 @@ typedef struct {
|
|||||||
UBaseType_t chassis;
|
UBaseType_t chassis;
|
||||||
UBaseType_t can;
|
UBaseType_t can;
|
||||||
UBaseType_t atti_esti;
|
UBaseType_t atti_esti;
|
||||||
// UBaseType_t dr16;
|
UBaseType_t dr16;
|
||||||
UBaseType_t r12ds;
|
UBaseType_t r12ds;
|
||||||
|
|
||||||
UBaseType_t action;
|
|
||||||
UBaseType_t cmd;
|
UBaseType_t cmd;
|
||||||
UBaseType_t nuc;
|
UBaseType_t nuc;
|
||||||
UBaseType_t navi;
|
|
||||||
UBaseType_t error_detect;
|
UBaseType_t error_detect;
|
||||||
|
|
||||||
} stack_water_mark; /* stack使用 */
|
} stack_water_mark; /* stack使用 */
|
||||||
@ -123,8 +112,6 @@ typedef struct {
|
|||||||
float r12ds;
|
float r12ds;
|
||||||
float cmd;
|
float cmd;
|
||||||
float nuc;
|
float nuc;
|
||||||
float action;
|
|
||||||
float navi;
|
|
||||||
float error_detect;
|
float error_detect;
|
||||||
} freq; /* 任务运行频率 */
|
} freq; /* 任务运行频率 */
|
||||||
|
|
||||||
@ -132,12 +119,9 @@ typedef struct {
|
|||||||
float chassis;
|
float chassis;
|
||||||
float can;
|
float can;
|
||||||
float atti_esti;
|
float atti_esti;
|
||||||
float ai;
|
|
||||||
float r12ds;
|
float r12ds;
|
||||||
float cmd;
|
float cmd;
|
||||||
float nuc;
|
float nuc;
|
||||||
float navi;
|
|
||||||
float action;
|
|
||||||
float error_detect;
|
float error_detect;
|
||||||
} last_up_time; /* 任务上一次运行的时间 */
|
} last_up_time; /* 任务上一次运行的时间 */
|
||||||
#endif
|
#endif
|
||||||
@ -155,8 +139,6 @@ extern const osThreadAttr_t attr_can;
|
|||||||
|
|
||||||
extern const osThreadAttr_t attr_cmd;
|
extern const osThreadAttr_t attr_cmd;
|
||||||
|
|
||||||
extern const osThreadAttr_t attr_ops_9pos;
|
|
||||||
|
|
||||||
extern const osThreadAttr_t attr_nuc;
|
extern const osThreadAttr_t attr_nuc;
|
||||||
|
|
||||||
extern const osThreadAttr_t attr_error_detect;
|
extern const osThreadAttr_t attr_error_detect;
|
||||||
@ -164,7 +146,6 @@ extern const osThreadAttr_t attr_error_detect;
|
|||||||
//extern const osThreadAttr_t attr_dr16;
|
//extern const osThreadAttr_t attr_dr16;
|
||||||
|
|
||||||
extern const osThreadAttr_t attr_r12ds;
|
extern const osThreadAttr_t attr_r12ds;
|
||||||
extern const osThreadAttr_t attr_navi;
|
|
||||||
|
|
||||||
/* Exported functions prototypes -------------------------------------------- */
|
/* Exported functions prototypes -------------------------------------------- */
|
||||||
void Task_Init(void *argument);
|
void Task_Init(void *argument);
|
||||||
@ -179,14 +160,10 @@ void Task_cmd(void *argument);
|
|||||||
|
|
||||||
void Task_nuc(void *argument);
|
void Task_nuc(void *argument);
|
||||||
|
|
||||||
void Task_action(void *argument);
|
|
||||||
|
|
||||||
void Task_Navi(void *argument);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void Task_error_detect(void *argument);
|
void Task_error_detect(void *argument);
|
||||||
|
|
||||||
void Task_r12ds(void *argument);
|
void Task_r12ds(void *argument);
|
||||||
|
|
||||||
//void Task_dr16(void *argument);
|
void Task_dr16(void *argument);
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user