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

协议修改

Browse Source
This commit is contained in:
ZHAISHUI04 2025-07-08 07:44:04 +08:00
parent 2403b3bf52
commit de7321138f
16 changed files with 114 additions and 234 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
MDK-ARM/.vscode/keil-assistant.log vendored
View File

@ -485,3 +485,5 @@
[info] Log at : 2025/7/1|14:01:22|GMT+0800 [info] Log at : 2025/7/1|14:01:22|GMT+0800
[info] Log at : 2025/7/7|02:36:22|GMT+0800

55
MDK-ARM/R2.uvoptx
View File

@ -134,13 +134,13 @@
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>DLGUARM</Key> <Key>CMSIS_AGDI</Key>
<Name></Name> <Name>-X"" -O206 -S8 -C0 -P00000000 -N00("") -D00(00000000) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>CMSIS_AGDI</Key> <Key>DLGUARM</Key>
<Name>-X"Any" -UAny -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name> <Name></Name>
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
@ -198,42 +198,7 @@
<Ww> <Ww>
<count>9</count> <count>9</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>NUC_send</ItemText> <ItemText>\\R2\../User/task/up_task.c\UP.MID360Context.IsLaunch</ItemText>
</Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>nuc_raw,0x0A</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>ang_58</ItemText>
</Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>ang_66</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_fromnuc,0x0A</ItemText>
</Ww>
<Ww>
<count>14</count>
<WinNumber>1</WinNumber>
<ItemText>Nuc,0x0A</ItemText>
</Ww>
<Ww>
<count>15</count>
<WinNumber>1</WinNumber>
<ItemText>rec_flag</ItemText>
</Ww>
<Ww>
<count>16</count>
<WinNumber>1</WinNumber>
<ItemText>ctrl</ItemText>
</Ww> </Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
@ -242,7 +207,7 @@
<DebugFlag> <DebugFlag>
<trace>0</trace> <trace>0</trace>
<periodic>1</periodic> <periodic>1</periodic>
<aLwin>1</aLwin> <aLwin>0</aLwin>
<aCover>0</aCover> <aCover>0</aCover>
<aSer1>0</aSer1> <aSer1>0</aSer1>
<aSer2>0</aSer2> <aSer2>0</aSer2>
@ -938,7 +903,7 @@
<Group> <Group>
<GroupName>User/bsp</GroupName> <GroupName>User/bsp</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@ -1218,7 +1183,7 @@
<Group> <Group>
<GroupName>User/Algorithm</GroupName> <GroupName>User/Algorithm</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@ -1390,7 +1355,7 @@
<Group> <Group>
<GroupName>Application/User/USB_DEVICE/App</GroupName> <GroupName>Application/User/USB_DEVICE/App</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@ -1434,7 +1399,7 @@
<Group> <Group>
<GroupName>Application/User/USB_DEVICE/Target</GroupName> <GroupName>Application/User/USB_DEVICE/Target</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>

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

Binary file not shown.

1
README.md
View File

@ -0,0 +1 @@
4.16 把整车用一个c板控制，添加主要代码，后续增加上层

40
User/Module/Chassis.c
View File

@ -35,10 +35,6 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
c->sick_cali.sick_dis[1]=(fp32)(can->sickfed.raw_dis[1]+30) ; //有点误差，手动补偿 c->sick_cali.sick_dis[1]=(fp32)(can->sickfed.raw_dis[1]+30) ; //有点误差，手动补偿
c->sick_cali.sick_dis[2]=(fp32)(can->sickfed.raw_dis[2] ); c->sick_cali.sick_dis[2]=(fp32)(can->sickfed.raw_dis[2] );
c->vofa_send[7] =c->sick_cali.sick_dis[1]-c->sick_cali.sick_dis[2];
c->vofa_send[6] = c->sick_cali.sick_dis[0];
c->vofa_send[5] =c->sick_cali.sick_dis[1];
c->vofa_send[4] =c->sick_cali.sick_dis[2];
return CHASSIS_OK; return CHASSIS_OK;
} }
@ -58,6 +54,12 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre
PID_init(&(c->pid.SickCaliVxOutPID), PID_POSITION, &(c->param->SickCali_XOutparam)); PID_init(&(c->pid.SickCaliVxOutPID), PID_POSITION, &(c->param->SickCali_XOutparam));
PID_init(&(c->pid.SickCaliVyInPID), PID_POSITION, &(c->param->SickCali_YInparam)); PID_init(&(c->pid.SickCaliVyInPID), PID_POSITION, &(c->param->SickCali_YInparam));
PID_init(&(c->pid.SickCaliVyOutPID), PID_POSITION, &(c->param->SickCali_YOutparam)); PID_init(&(c->pid.SickCaliVyOutPID), PID_POSITION, &(c->param->SickCali_YOutparam));
PID_init(&(c->pid.Mid360AngleInPID), PID_POSITION, &(c->param->Mid360AngleInPID_param));
PID_init(&(c->pid.Mid360AngleOutPID), PID_POSITION, &(c->param->Mid360AngleOutPID_param));
/*修正 */ /*修正 */
PID_init(&(c->pid.Chassis_AngleAdjust), PID_POSITION, &(c->param->Chassis_AngleAdjust_param)); PID_init(&(c->pid.Chassis_AngleAdjust), PID_POSITION, &(c->param->Chassis_AngleAdjust_param));
@ -98,15 +100,13 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z); c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z);
/*初始数据*/ /*初始数据*/
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
c->move_vec.Vw = 0;
c->to_nuc.send = 0; c->to_nuc.send = 0;
c->sick_cali.is_reach = 0; c->sick_cali.is_reach = 0;
c->move_vec.Vw=0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
c->vofa_send[0]= KalmanFilter(&c->extKalman[0] , c->sick_cali.sick_dis[0]);
c->vofa_send[1]= KalmanFilter(&c->extKalman[1] , c->sick_cali.sick_dis[1]);
c->vofa_send[2]= KalmanFilter(&c->extKalman[2] , c->sick_cali.sick_dis[2]);
@ -131,7 +131,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
break; break;
case Chassis_Adjust_Sick: case Chassis_Adjust_Sick:
// sick_calibration(c, ctrl, out); sick_calibration(c, ctrl, out);
// c->to_nuc.send = (c->sick_cali.is_reach == 1) ? 1 : 0; // c->to_nuc.send = (c->sick_cali.is_reach == 1) ? 1 : 0;
break; break;
@ -141,22 +141,13 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
case AUTO: case AUTO:
switch (c->chassis_ctrl.mode) { switch (c->chassis_ctrl.mode) {
case AUTO_MID360: case AUTO_MID360:
// c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000;
// c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000;
// c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
c->move_vec.Vw =-ctrl->cmd_MID360.posw * 3000;
abs_limit_fp(&c->move_vec.Vx, 5000.0f); c->move_vec.Vw = -ctrl->cmd_MID360.angle * 3000;
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f); abs_limit_fp(&c->move_vec.Vw, 5000.0f);
break; break;
case AUTO_MID360_Pitch: case AUTO_MID360_Pitch:
// c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000; c->move_vec.Vw = -ctrl->cmd_MID360.angle * 3000;
// c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000;
// c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f); abs_limit_fp(&c->move_vec.Vw, 5000.0f);
break; break;
case REPOSITION: case REPOSITION:
@ -178,9 +169,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
break ; break ;
case PB_MID: case PB_MID:
case PB_DOWN: case PB_DOWN:
c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000; c->move_vec.Vw = -ctrl->cmd_MID360.angle * 1000;
c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000;
c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
abs_limit_fp(&c->move_vec.Vx, 5000.0f); abs_limit_fp(&c->move_vec.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f); abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f); abs_limit_fp(&c->move_vec.Vw, 5000.0f);
@ -314,3 +303,4 @@ int8_t Chassis_AngleCompensate(Chassis_t *c)
} }

10
User/Module/Chassis.h
View File

@ -90,6 +90,12 @@ typedef struct
pid_param_t Chassis_AngleAdjust_param; pid_param_t Chassis_AngleAdjust_param;
pid_param_t Mid360AngleInPID_param;
pid_param_t Mid360AngleOutPID_param;
sickparam_t sickparam; sickparam_t sickparam;
}Chassis_Param_t; }Chassis_Param_t;
@ -163,6 +169,10 @@ typedef struct{
pid_type_def SickCaliVyInPID; pid_type_def SickCaliVyInPID;
pid_type_def SickCaliVyOutPID; pid_type_def SickCaliVyOutPID;
pid_type_def Mid360AngleInPID;
pid_type_def Mid360AngleOutPID;
}pid; }pid;

21
User/Module/config.c
View File

@ -235,6 +235,27 @@ static const ConfigParam_t param ={
.out_limit =1000.0f, .out_limit =1000.0f,
}, },
.Mid360AngleInPID_param={
.p = 0.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit =1000.0f,
},
.Mid360AngleOutPID_param={
.p = 10.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit =1000.0f,
},
.sickparam={ .sickparam={
.w_error=2, .w_error=2,
.xy_error=3, .xy_error=3,

16
User/Module/up.c
View File

@ -181,10 +181,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
/*部分数据更新*/ /*部分数据更新*/
static int is_pitch=1; static int is_pitch=1;
posss=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3.4,4.2,&u->MID360Context.Curve); posss=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis),3.4,4.2,&u->MID360Context.Curve);
u->vofa_send[0] = u->MID360Context.MID360Cfg.go_release_pos;
u->vofa_send[1] = u->motorfeedback.go_data.Pos;
u->vofa_send[2] = c->pos;
switch (c->CMD_CtrlType ) switch (c->CMD_CtrlType )
{ {
@ -364,7 +361,7 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
PassCfg ->go_release_pos = PassCfg ->go_release_pos =
CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PassContext.Curve); CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis),3,4,&u->PassContext.Curve);
switch (*state) { //遥控器按键进行状态切换 switch (*state) { //遥控器按键进行状态切换
case PASS_STOP: case PASS_STOP:
@ -403,14 +400,7 @@ int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){
LaunchContext_t *LaunchContext = &u->LaunchContext; LaunchContext_t *LaunchContext = &u->LaunchContext;
MID360Context_t *MID360Context=&u->MID360Context; MID360Context_t *MID360Context=&u->MID360Context;
MID360Cfg_t *MID360Cfg = &u->MID360Context.MID360Cfg; MID360Cfg_t *MID360Cfg = &u->MID360Context.MID360Cfg;
if(u->MID360Context.Curve==CURVE_58){ MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis),3.2,4.3,&u->MID360Context.Curve);
MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)-0.25,3.2,4.3,&u->MID360Context.Curve);
}
else {
MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)-0.05,3.2,4.3,&u->MID360Context.Curve);
}
if (u->MID360Context.Curve == CURVE_58) { if (u->MID360Context.Curve == CURVE_58) {
target->Pitch_angle = 58; target->Pitch_angle = 58;
} else { } else {

10
User/Module/up_utils.c
View File

@ -65,7 +65,7 @@ int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, flo
} }
// 计算66度曲线（偏上）
// 计算66度曲线（偏上） // 计算66度曲线（偏上）
static float curve_66(float d) { static float curve_66(float d) {
return 3.7028f * d * d + 11.2126f * d -142.9446f; return 3.7028f * d * d + 11.2126f * d -142.9446f;
@ -81,15 +81,11 @@ static float curve_58(float d) {
迟滞区x-y 迟滞区x-y
曲线x重合区，根据当前函数和变化方向切换 曲线x重合区，根据当前函数和变化方向切换
*/ */
int abdddd=0;
fp32 ang_58;
fp32 ang_66;
float CurveChange(float d, float x, float y, CurveType *cs) float CurveChange(float d, float x, float y, CurveType *cs)
{ {
if (d<3.2) abdddd++;
if (*cs == CURVE_66) { if (*cs == CURVE_66) {
if (d > y) { if (d > y) {
*cs = CURVE_58; *cs = CURVE_58;
} }
} else { // CURVE_58 } else { // CURVE_58
@ -100,10 +96,8 @@ float CurveChange(float d, float x, float y, CurveType *cs)
// 根据当前曲线返回结果 // 根据当前曲线返回结果
if (*cs == CURVE_58) { if (*cs == CURVE_58) {
ang_58=d;
return curve_58(d); return curve_58(d);
} else { } else {
ang_66=d;
return curve_66(d); return curve_66(d);
} }
} }

21
User/bsp/protocol.h
View File

@ -37,19 +37,11 @@ typedef struct __attribute__((packed)) {
typedef struct __attribute__((packed)) { typedef struct __attribute__((packed)) {
Protocol_ID_t recv_id;//作为帧头使用确认通信ID正确 Protocol_ID_t recv_id;//作为帧头使用确认通信ID正确
uint8_t status; /* 控制命令 */
struct __attribute__((packed)) { struct __attribute__((packed)) {
float posy; /*pitch轴*/
float posx; /*yaw轴*/
char cmd;
float dis; /*距离*/
}pick;
struct __attribute__((packed)) {
float vx; /* x轴移动速度 */
float vy; /* y轴移动速度 */
float wz; /* z轴转动速度 */ float wz; /* z轴转动速度 */
float dis;
}navi; }navi;
} Protocol_DownDataChassis_t; } Protocol_DownDataChassis_t;
@ -83,17 +75,17 @@ typedef struct __attribute__((packed)) {
typedef struct __attribute__((packed)) typedef struct __attribute__((packed))
{ {
fp32 status; uint8_t status;
} Protocol_UpDataCMD_t; } Protocol_UpDataCMD_t;
/* 视觉 -> 电控 上层机构数据结构体*/ /* 视觉 -> 电控 上层机构数据结构体*/
typedef struct __attribute__((packed)) { typedef struct __attribute__((packed)) {
float angle; // 角度信息
uint8_t status; /* 控制命令 */ float dis; // 位移信息
} Protocol_DownDataUpper_t; } Protocol_DownDataUpper_t;
typedef struct __attribute__((packed)) { typedef struct __attribute__((packed)) {
Protocol_UpDataMCU_t data; Protocol_UpDataMCU_t data;
} Protocol_UpPackageMCU_t; } Protocol_UpPackageMCU_t;
@ -106,6 +98,7 @@ typedef struct __attribute__((packed)) {
typedef struct __attribute__((packed)) { typedef struct __attribute__((packed)) {
Protocol_DownDataUpper_t data; Protocol_DownDataUpper_t data;
} Protocol_DownPackageUpper_t; } Protocol_DownPackageUpper_t;
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

35
User/device/cmd.c
View File

@ -89,42 +89,23 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
if(cmd == NULL) return -1; if(cmd == NULL) return -1;
if(n == NULL) return -1; if(n == NULL) return -1;
cmd->cmd_status = n->status_fromnuc;
cmd->raw_status = n->ctrl_status;
for (int i = 0; i < 7; ++i) for (int i = 0; i < 7; ++i)
{ // 从最低位到最高位遍历 { // 从最低位到最高位遍历
cmd->status[i] = ((cmd->raw_status) & (1 << i)) ? 1 : 0; cmd->status[i] = ((cmd->raw_status) & (1 << i)) ? 1 : 0;
} }
switch(cmd->cmd_status){
case MID:
cmd->cmd_MID360.posx = n->MID360.vx;
cmd->cmd_MID360.posy = n->MID360.vy;
cmd->cmd_MID360.posw = n->MID360.wz;
cmd->pos =n->MID360 .pos ;
break;
case VISION :
cmd ->cmd_pick .posx =n->camera.data1 ;
cmd ->cmd_pick .posy =n->camera.data2 ;
cmd ->cmd_pick .posw =n->camera.data3 ;
break;
default:
cmd->cmd_MID360.posx = 0;
cmd->cmd_MID360.posy = 0;
cmd->cmd_MID360.posw = 0;
cmd->pos =0 ;
break ;
cmd->cmd_MID360.angle = n->MID360.angle;
cmd->cmd_MID360.dis = n->MID360.dis;
}
return 0; return 0;
} }

15
User/device/cmd.h
View File

@ -42,19 +42,12 @@ typedef enum{
Dribbl_transfer Dribbl_transfer
}CMD_mode_t; }CMD_mode_t;
typedef struct { typedef struct {
uint8_t status_fromnuc;
uint8_t ctrl_status;
struct struct
{ {
fp32 vx;
fp32 vy;
fp32 wz;
fp32 pos;
fp32 angle; fp32 angle;
fp32 dis;
char flag;
}MID360; }MID360;
struct struct
@ -149,9 +142,9 @@ typedef struct {
}cmd_pick; }cmd_pick;
struct struct
{ {
fp32 posx; fp32 angle;
fp32 posy; fp32 dis;
fp32 posw;
}cmd_MID360; }cmd_MID360;

89
User/device/nuc.c
View File

@ -6,7 +6,7 @@ volatile uint32_t drop_message = 0;
static osThreadId_t thread_alert; static osThreadId_t thread_alert;
uint8_t nucbuf[25]; uint8_t nucbuf[10];
char SendBuffer[7]; char SendBuffer[7];
static void NUC_CBLTCallback(void) static void NUC_CBLTCallback(void)
@ -58,86 +58,29 @@ int8_t NUC_RawParse(CMD_NUC_t *n, NUC_t *nuc)
return DEVICE_ERR_NULL; return DEVICE_ERR_NULL;
union union
{ {
float x[5]; float x[2];
char data[20]; char data[8];
} instance; } instance;
if (nucbuf[0] != HEAD) if (nucbuf[0] != HEAD)
goto error; // 发送ID不是底盘 goto error; // 发送ID不是底盘
else else
{ {
n->status_fromnuc = nucbuf[1]; if (nucbuf[9] != TAIL)
n->ctrl_status = nucbuf[2];
switch (n->status_fromnuc)
{
case VISION: // 控制帧0x02
/* 协议格式
0xFF HEAD
0x02 控制帧
0x01 相机帧
vx fp32
vy fp32
wz fp32
0xFE TAIL
*/
instance.data[3] = nucbuf[3];
instance.data[2] = nucbuf[4];
instance.data[1] = nucbuf[5];
instance.data[0] = nucbuf[6];
n->camera.data1 = instance.x[0]; //
instance.data[7] = nucbuf[7];
instance.data[6] = nucbuf[8];
instance.data[5] = nucbuf[9];
instance.data[4] = nucbuf[10];
n->camera.data2 = instance.x[1]; //
instance.data[11] = nucbuf[11];
instance.data[10] = nucbuf[12];
instance.data[9] = nucbuf[13];
instance.data[8] = nucbuf[14];
n->camera.data3 = instance.x[2]; //
break;
case MID: // 控制帧0x08
/* 协议格式
0xFF HEAD
0x09 控制帧
0x01 相机帧
vx fp32
vy fp32
wz fp32
0xFE TAIL
*/
if (nucbuf[24] != TAIL)
goto error; goto error;
instance.data[3] = nucbuf[6];
instance.data[2] = nucbuf[5];
instance.data[1] = nucbuf[4];
instance.data[0] = nucbuf[3];
n->MID360.vx = instance.x[0]; //
instance.data[7] = nucbuf[10];
instance.data[6] = nucbuf[9];
instance.data[5] = nucbuf[8];
instance.data[4] = nucbuf[7];
n->MID360.vy = instance.x[1]; //
instance.data[11] = nucbuf[14];
instance.data[10] = nucbuf[13];
instance.data[9] = nucbuf[12];
instance.data[8] = nucbuf[11];
n->MID360.wz = instance.x[2]; //
instance.data[15] = nucbuf[18];
instance.data[14] = nucbuf[17];
instance.data[13] = nucbuf[16];
instance.data[12] = nucbuf[15];
n->MID360.pos = instance.x[3]; //
instance.data[19] = nucbuf[22];
instance.data[18] = nucbuf[21];
instance.data[17] = nucbuf[20];
instance.data[16] = nucbuf[19];
n->MID360.angle = instance.x[4]; //
n->MID360.flag = nucbuf[23]; //
break; instance.data[3] = nucbuf[4];
} instance.data[2] = nucbuf[3];
instance.data[1] = nucbuf[2];
instance.data[0] = nucbuf[1];
n->MID360.angle = instance.x[0]; //
instance.data[7] = nucbuf[8];
instance.data[6] = nucbuf[7];
instance.data[5] = nucbuf[6];
instance.data[4] = nucbuf[5];
n->MID360.dis = instance.x[1]; //
nuc->unc_online = true; // 设置为在线状态 nuc->unc_online = true; // 设置为在线状态
return DEVICE_OK; return DEVICE_OK;
} }

12
User/task/cmd_task.c
View File

@ -16,7 +16,7 @@ static CMD_NUC_t Nuc;
#endif #endif
int rec_flag=0;
void Task_cmd(void *argument){ void Task_cmd(void *argument){
(void)argument; /*未使用传递参数 消除警告*/ (void)argument; /*未使用传递参数 消除警告*/
@ -40,14 +40,12 @@ void Task_cmd(void *argument){
/*将各任务接收到的原始数据解析为通用的控制命令*/ /*将各任务接收到的原始数据解析为通用的控制命令*/
if((osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK)){ //nuc /*注意，不能将nuc和码盘导航一块使用*/
}
else if(osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK) //nuc
{
memset(&Nuc, 0, sizeof(CMD_NUC_t));
}
CMD_ParseNuc(&cmd,&Nuc); CMD_ParseNuc(&cmd,&Nuc);
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);

1
User/task/nuc_task.c
View File

@ -40,7 +40,6 @@ void Task_nuc(void *argument){
#ifdef DEBUG #ifdef DEBUG
task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId()); task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId());
#endif #endif
a++;
NUC_StartReceiving(); NUC_StartReceiving();
if (NUC_WaitDmaCplt()){ if (NUC_WaitDmaCplt()){
NUC_RawParse(&cmd_fromnuc, &nuc_raw); NUC_RawParse(&cmd_fromnuc, &nuc_raw);

4
User/task/user_task.h
View File

@ -17,10 +17,10 @@
#define TASK_FREQ_CHASSIS (900u) #define TASK_FREQ_CHASSIS (900u)
#define TASK_FREQ_UP (900u) #define TASK_FREQ_UP (900u)
#define TASK_FREQ_CTRL_CMD (100u) #define TASK_FREQ_CTRL_CMD (500u)
#define TASK_FREQ_NUC (100u) #define TASK_FREQ_NUC (100u)
#define TASK_FREQ_CAN (1000u) #define TASK_FREQ_CAN (1000u)
#define TASK_FREQ_RC (1000u) #define TASK_FREQ_RC (100u)
#define TASK_FREQ_ERROR_DTC (3u) #define TASK_FREQ_ERROR_DTC (3u)