diff --git a/MDK-ARM/.vscode/keil-assistant.log b/MDK-ARM/.vscode/keil-assistant.log
index 6c4a5e0..7382f29 100644
--- a/MDK-ARM/.vscode/keil-assistant.log
+++ b/MDK-ARM/.vscode/keil-assistant.log
@@ -479,3 +479,5 @@
[info] Log at : 2025/6/27|16:40:58|GMT+0800
+[info] Log at : 2025/6/28|01:22:24|GMT+0800
+
diff --git a/MDK-ARM/.vscode/settings.json b/MDK-ARM/.vscode/settings.json
index 681d6c0..81c2fdd 100644
--- a/MDK-ARM/.vscode/settings.json
+++ b/MDK-ARM/.vscode/settings.json
@@ -97,6 +97,7 @@
"gimbal_task.h": "c",
"gimgal.h": "c",
"navi.h": "c",
- "up.h": "c"
+ "up.h": "c",
+ "bsp_usart.h": "c"
}
}
\ No newline at end of file
diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx
index ce2b613..eb773dd 100644
--- a/MDK-ARM/R2.uvoptx
+++ b/MDK-ARM/R2.uvoptx
@@ -158,47 +158,17 @@
0
1
- NUC_send,0x0A
+ tar_angle
1
1
- SendBuffer,0x10
+ huart->ErrorCode
2
1
- chassis,0x0A
-
-
- 3
- 1
- cmd_fromnuc,0x0A
-
-
- 4
- 1
- nucbuf,0x10
-
-
- 5
- 1
- UP,0x0A
-
-
- 6
- 1
- tar_angle
-
-
- 7
- 1
- up_cmd
-
-
- 8
- 1
- a,0x0A
+ error_code
diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf
index 3e4b0a7..cf40bd0 100644
Binary files a/MDK-ARM/R2/R2.axf and b/MDK-ARM/R2/R2.axf differ
diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c
index f24b59f..dc23a8f 100644
--- a/User/Module/Chassis.c
+++ b/User/Module/Chassis.c
@@ -32,9 +32,14 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
//
// c->sick_cali.sick_dis[i]=can->sickfed.raw_dis[i] / 10; //没使用原值
// }
- c->sick_cali.sick_dis[0]=can->sickfed.raw_dis[0] ;
- c->sick_cali.sick_dis[1]=can->sickfed.raw_dis[1]+50 ; //有点误差，手动补偿
- c->sick_cali.sick_dis[2]=can->sickfed.raw_dis[2] ;
+ c->sick_cali.sick_dis[0]=(fp32)(can->sickfed.raw_dis[0]) ;
+ c->sick_cali.sick_dis[1]=(fp32)(can->sickfed.raw_dis[1]+26) ; //有点误差，手动补偿
+ c->sick_cali.sick_dis[2]=(fp32)(can->sickfed.raw_dis[2] );
+
+// c->vofa_send[7] =c->sick_cali.sick_dis[0]-c->sick_cali.sick_dis[1];
+// 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;
}
@@ -60,7 +65,11 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre
LowPassFilter2p_Init(&(c->filled[0]), target_freq, 80.0f); // 角加速度滤波
LowPassFilter2p_Init(&(c->filled[1]), target_freq, 80.0f); // w滤波
LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f); // y滤波
- LowPassFilter2p_Init(&(c->filled[3]), target_freq, 80.0f); // x滤波
+ LowPassFilter2p_Init(&(c->filled[3]), target_freq, 80.0f);
+ LowPassFilter2p_Init(&(c->filled[4]), target_freq, 80.0f);
+ LowPassFilter2p_Init(&(c->filled[5]), target_freq, 80.0f);
+ LowPassFilter2p_Init(&(c->filled[6]), target_freq, 80.0f);
+ LowPassFilter2p_Init(&(c->filled[7]), target_freq, 80.0f);
c->sick_cali .sickparam=c->param ->sickparam ;
@@ -89,9 +98,7 @@ 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->move_vec.Vw = 0;
- c->move_vec.Vx = 0;
- c->move_vec.Vy = 0;
+
c->NUC_send.send[0] = 0;
c->sick_cali.is_reach = 0;
@@ -118,7 +125,9 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
c->NUC_send.send[0] = (c->sick_cali.is_reach == 1) ? 1 : 0;
break;
default:
-
+ c->move_vec.Vw = 0;
+ c->move_vec.Vx = 0;
+ c->move_vec.Vy = 0;
break;
}
break;
@@ -135,7 +144,12 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
// c->NUC_send.send[0] = 1;
break;
case AUTO_MID360_Pitch:
-
+ 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;
+ 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);
break;
case AUTO_MID360_Adjust:
c->move_vec.Vw = ctrl->Vw * 6000;
@@ -154,9 +168,9 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
case PB_UP:
case PB_MID:
case PB_DOWN:
- 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->Vw * 6000;
+ c->move_vec.Vx = ctrl->Vy * 6000;
+ c->move_vec.Vy = ctrl->Vx * 6000;
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);
@@ -171,7 +185,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
break;
default:
-
+
break;
}
@@ -189,7 +203,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
out->motor_CHASSIS3508.as_array[i] = c->final_out.final_3508out[i];
}
-// c->vofa_send[0] = (fp32)c->sick_cali.sick_dis[1] - c->sick_cali.sick_dis[2];
+
return CHASSIS_OK;
@@ -206,32 +220,41 @@ sick2,左1
*/
-
+fp32 diff_yaw,diff_y,diff_x;
int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
{
if (c == NULL) return CHASSIS_ERR_NULL;
if (ctrl == NULL) return CHASSIS_ERR_NULL;
fp32 delta_w,delta_x,delta_y;
- fp32 sick0 = c->sick_cali.sick_dis[0] ;
- fp32 sick1 = c->sick_cali.sick_dis[1];
- fp32 sick2 = c->sick_cali.sick_dis[2];
+ fp32 sick0 =LowPassFilter2p_Apply( &(c->filled[5]),c->sick_cali.sick_dis[0]) ;
+ fp32 sick1 =LowPassFilter2p_Apply( &(c->filled[6]), c->sick_cali.sick_dis[1]) ;
+ fp32 sick2 = LowPassFilter2p_Apply( &(c->filled[7]),c->sick_cali.sick_dis[2] );
const sickparam_t *param = &c->sick_cali.sickparam;
- fp32 diff_yaw = -(fp32)(sick1 - sick2);
- fp32 diff_y = -(fp32)(sick1 - c->sick_cali.sickparam.y_set);
- fp32 diff_x = (fp32)(sick0 - c->sick_cali.sickparam.x_set);
+ diff_yaw = -(fp32)(sick1 - sick2);
+ diff_y = -(fp32)(sick1 - c->sick_cali.sickparam.y_set);
+ diff_x = (fp32)(sick0 - c->sick_cali.sickparam.x_set);
- delta_w=(&c->pid.SickCaliWzInPID, diff_yaw, 0);
- delta_x=(&c->pid.SickCaliVxInPID, diff_x, 0);
- delta_y=(&c->pid.SickCaliVyInPID, diff_y,0);
+ delta_w=PID_calc(&c->pid.SickCaliWzOutPID, diff_yaw, 0);
+ delta_x=PID_calc(&c->pid.SickCaliVxOutPID, diff_x, 0);
+ delta_y=PID_calc(&c->pid.SickCaliVyOutPID, diff_y,0);
+
+ if(fabs(diff_yaw)>5){
+ c->move_vec.Vw=PID_calc(&c->pid.SickCaliWzInPID,-delta_w,0);
+// c->move_vec.Vw=delta_w;
+// c->move_vec.Vx=PID_calc(&c->pid.SickCaliVxInPID,delta_x,0);
+// c->move_vec.Vx=PID_calc(&c->pid.SickCaliVyInPID,delta_y,0);
+ }
+
+
static uint16_t reach_cnt = 0;
if (fabsf(diff_yaw) <= param->w_error &&
fabsf(diff_x) <= param->xy_error &&
fabsf(diff_y) <= param->xy_error) {
reach_cnt++;
- if (reach_cnt >= 50) {
+ if (reach_cnt >= 20) {
c->sick_cali.is_reach = 1;
@@ -252,7 +275,7 @@ int8_t Chassis_AngleCompensate(Chassis_t *c)
{
pian_angel=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
}
-
+
else {
cur_angle=AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
pian_angel=0;
diff --git a/User/Module/Chassis.h b/User/Module/Chassis.h
index 810a826..b2e31af 100644
--- a/User/Module/Chassis.h
+++ b/User/Module/Chassis.h
@@ -173,7 +173,7 @@ typedef struct{
fp32 vofa_send[8];
- LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
+ LowPassFilter2p_t filled[8]; /* 输出滤波器滤波器数组 */
struct {
int32_t sick_dis[4]; //获取到的sick激光值
diff --git a/User/Module/config.c b/User/Module/config.c
index 889ed2d..0ddc597 100644
--- a/User/Module/config.c
+++ b/User/Module/config.c
@@ -96,6 +96,14 @@ static const ConfigParam_t param ={
.Pitch_angle=66,
.go_up_speed =12,
.go_down_speed =5,
+ },
+ .MID360Cfg={
+
+ .go_release_pos=-200, // GO电机发射位置
+ .go_up_speed=20, // GO电机上升速度
+ .go_down_speed=10, // GO电机下降速度
+
+
},
},
.chassis = {/**/
@@ -123,19 +131,21 @@ static const ConfigParam_t param ={
.i_limit = 50.0f,
.out_limit =1000.0f,
},
+
+
.SickCali_WInparam = {
- .p = 0.0f,
- .i = 0.0f,
+ .p = 3.0f,
+ .i = 0.005f,
.d = 0.0f,
- .i_limit = 0.0f,
- .out_limit = 0.0f,
+ .i_limit = 500.0f,
+ .out_limit = 2000.0f,
},
.SickCali_WOutparam = {
- .p = 0.0f,
+ .p = 18.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
- .out_limit = 0.0f,
+ .out_limit = 1000.0f,
},
.SickCali_YInparam = {
.p = 0.0f,
@@ -145,11 +155,11 @@ static const ConfigParam_t param ={
.out_limit = 0.0f,
},
.SickCali_YOutparam = {
- .p = 0.0f,
- .i = 0.0f,
+ .p = 2.9f,
+ .i = 0.005f,
.d = 0.0f,
- .i_limit = 0.0f,
- .out_limit = 0.0f,
+ .i_limit = 500.0f,
+ .out_limit = 1000.0f,
},
.SickCali_XInparam = {
.p = 0.0f,
@@ -159,11 +169,11 @@ static const ConfigParam_t param ={
.out_limit = 0.0f,
},
.SickCali_XOutparam = {
- .p = 0.0f,
- .i = 0.0f,
+ .p = 2.9f,
+ .i = 0.0065f,
.d = 0.0f,
- .i_limit = 0.0f,
- .out_limit = 0.0f,
+ .i_limit = 500.0f,
+ .out_limit = 3000.0f,
},
.Chassis_AngleAdjust_param={
.p = 10.0f,
diff --git a/User/Module/up.c b/User/Module/up.c
index 888ea41..922e03a 100644
--- a/User/Module/up.c
+++ b/User/Module/up.c
@@ -54,7 +54,7 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
u->PassContext .is_init = 1;
}
u->LaunchContext.LaunchCfg = u->param->LaunchCfg;
-
+ u->MID360Context.MID360Cfg = u->param->MID360Cfg;
BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层
}
@@ -181,13 +181,16 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
/*部分数据更新*/
static int is_pitch=1;
- posss=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PitchContext.Curve);
+ posss=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3.4,4.2,&u->MID360Context.Curve);
// if (u->PitchContext.Curve == CURVE_58) {
// target->Pitch_angle = 58;
// } else {
// target->Pitch_angle = 66;
// }
-
+ u->vofa_send [2] = c->pos;
+ u->vofa_send [3] = LowPassFilter2p_Apply(&u->filled[0],c->pos);
+ u->vofa_send [4] =1;
+
switch (c->CMD_CtrlType )
{
case RCcontrol: //在手动模式下
@@ -205,8 +208,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
} //初始上电
//LaunchCfg->go_up_speed=15;
target->Pitch_angle =LaunchCfg->Pitch_angle;
-
-
+ target->go_pull_speed=LaunchCfg->go_up_speed;
target->Shoot_M2006_angle=LaunchCfg->m2006_init;
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新
u->LaunchContext.LaunchState = Launch_Stop;
@@ -235,54 +237,15 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
}
break;
case AUTO:
- /*自动下数据更新，根据距离切换俯仰角,如需自动调整，放在*/
-// if (u->PitchContext.Curve == CURVE_58) {
-// target->Pitch_angle = 58;
-// } else {
-// target->Pitch_angle = 66;
-// }
-
- switch(c-> CMD_mode)
- {
- case AUTO_MID360_Pitch:
-
- if (u->PitchContext .PitchState ==PITCH_PREPARE || u->PitchContext .PitchState ==PITCH_COMPLETE)
- {
- u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
- }
- /*根据距离实时计算所需pos*/
- if(c->pos){
- pitch_cfg ->go_release_pos=
- CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PitchContext.Curve);
- }
- else pitch_cfg ->go_release_pos=0 ;
- Pitch_Process(u,out,c);
- break ;
-
- case AUTO_MID360:
- target->Shoot_M2006_angle =LaunchCfg ->m2006_init ;
-
-
-
- break ;
- default:
- break;
-
- }
-
+ UP_AUTO_Control(u,out,c);
break ;
case PASS_BALL:
-// if (u->PitchContext.Curve == CURVE_58) {
-// target->Pitch_angle = 58;
-// } else {
-// target->Pitch_angle = 66;
-// }
Pass_Process(u,out,c);
break;
case RELAXED:
// target->go_shoot= pitch_cfg .
target->Pitch_angle = 58;
- target->go_shoot = -210.0f;
+ target->go_shoot = -190.0f;
// target->Pitch_angle = 58;
@@ -385,14 +348,14 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
PassCfg_t *PassCfg = &u->PassContext.PassCfg;
PassState_t *state = &u->PassContext.PassState;
up_motor_target_t *target=&u->motor_target ;
- LaunchContext_t *LaunchContext = &u->LaunchContext;
+ LaunchContext_t *LaunchContext = &u->LaunchContext;
Pass_Sequence_Check(u,c);
if(c->pos) //
{
PassCfg ->go_release_pos =
- CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PitchContext.Curve);
+ CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PassContext.Curve);
}
switch (*state) { //遥控器按键进行状态切换
@@ -426,3 +389,42 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
return 0;
}
+int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){
+ /*自动下数据更新，根据距离切换俯仰角,如需自动调整，放在*/
+ up_motor_target_t *target=&u->motor_target ;
+ LaunchContext_t *LaunchContext = &u->LaunchContext;
+ MID360Context_t *MID360Context=&u->MID360Context;
+ MID360Cfg_t *MID360Cfg = &u->MID360Context.MID360Cfg;
+ MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)+0.15,3.2,4.3,&u->MID360Context.Curve);
+ if (u->MID360Context.Curve == CURVE_58) {
+ target->Pitch_angle = 58;
+ } else {
+ target->Pitch_angle = 66;
+ }
+
+ LaunchContext->LaunchCfg.go_up_speed=MID360Cfg->go_up_speed;
+ LaunchContext->LaunchCfg.go_down_speed=MID360Cfg->go_down_speed;
+
+
+ switch(c-> CMD_mode)
+ {
+
+ case AUTO_MID360_Pitch:
+ if(MID360Context->IsLaunch==0){
+ MID360Context->IsLaunch=1;
+ LaunchContext->LaunchState = Launch_PREPARE;
+ }
+ Pitch_Launch_Sequence(u,LaunchContext,u->motorfeedback.go_data.Pos,MID360Cfg->go_release_pos,out);
+
+ break ;
+
+ case AUTO_MID360:
+ target->Shoot_M2006_angle= LaunchContext->LaunchCfg.m2006_init;
+ MID360Context->IsLaunch=0;
+ break ;
+ default:
+ break;
+
+ }
+
+ }
\ No newline at end of file
diff --git a/User/Module/up.h b/User/Module/up.h
index 599c0f8..d5c6fd9 100644
--- a/User/Module/up.h
+++ b/User/Module/up.h
@@ -51,6 +51,7 @@ typedef enum {
}LaunchState_t;
typedef struct {
+
LaunchCfg_t LaunchCfg;
LaunchState_t LaunchState;
} LaunchContext_t;
@@ -60,8 +61,7 @@ typedef struct {
typedef enum {
PITCH_PREPARE, // 准备阶段
- PITCH_START, //启动，拉扳机
-
+ PITCH_START, //启动，拉扳机
PITCH_WAIT, // 发射等待
PITCH_COMPLETE // 完成
@@ -79,7 +79,6 @@ typedef struct {
typedef struct {
PitchState_t PitchState;
PitchCfg_t PitchConfig;
- CurveType Curve; //当前函数，雷达距离->pos
uint8_t is_init ;
} PitchContext_t;
@@ -108,6 +107,26 @@ typedef struct {
uint8_t is_init ;
} PassContext_t;
+/*雷达自动*/
+
+typedef struct {
+
+ fp32 go_release_pos; // GO电机发射位置
+ fp32 go_up_speed; // GO电机上升速度
+ fp32 go_down_speed; // GO电机下降速度
+
+} MID360Cfg_t;
+
+typedef struct {
+ MID360Cfg_t MID360Cfg;
+
+ CurveType Curve; //当前函数，雷达距离->pos
+ int IsLaunch; //是否在发射过程
+} MID360Context_t;
+
+
+
+
typedef struct {
BMI088_t bmi088;
@@ -133,7 +152,7 @@ typedef struct
LaunchCfg_t LaunchCfg;
PitchCfg_t PitchCfg;
PassCfg_t PassCfg;
-
+ MID360Cfg_t MID360Cfg;
GO_MotorCmd_t go_cmd;
}UP_Param_t;
@@ -175,7 +194,9 @@ typedef struct{
/*投篮过程*/
PitchContext_t PitchContext;
/*传球过程*/
- PassContext_t PassContext;
+ PassContext_t PassContext;
+ /*自动过程*/
+ MID360Context_t MID360Context;
CMD_t *cmd;
struct{
@@ -232,6 +253,7 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c);
int8_t Pass_Sequence_Check(UP_t *u,CMD_t *c);
int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c);
int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out);
+int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c);
#endif
diff --git a/User/Module/up_utils.c b/User/Module/up_utils.c
index 9faeae8..b493bb4 100644
--- a/User/Module/up_utils.c
+++ b/User/Module/up_utils.c
@@ -62,25 +62,27 @@ int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, flo
}
-// 计算58度曲线
-static float curve_58(float d) {
- return 0.448f * d * d + 24.8604f * d - 177.99552f;
-}
-
-// 计算66度曲线
+// 计算66度曲线（偏上）
+// 计算66度曲线（偏上）
static float curve_66(float d) {
- return 8.84935f * d * d - 10.5424f * d - 126.791f;
+ return 4.0310f * d * d + 8.9026f * d -139.5156;
}
+// 计算58度曲线（偏下）
+// 计算58度曲线（偏下）
+static float curve_58(float d) {
+ return -1.9776f * d * d + 42.8499f * d - 204.2442f;
+}
/*
曲线切换,用于距离和pos拟合
迟滞区x-y
曲线x重合区，根据当前函数和变化方向切换
*/
+int abdddd=0;
float CurveChange(float d, float x, float y, CurveType *cs)
{
-
+ if (d<3.2) abdddd++;
if (*cs == CURVE_66) {
if (d > y) {
*cs = CURVE_58;
diff --git a/User/device/nuc.c b/User/device/nuc.c
index c093310..f7eb9f2 100644
--- a/User/device/nuc.c
+++ b/User/device/nuc.c
@@ -2,177 +2,181 @@
#include "crc16.h"
#include
#include "error_detect.h"
-static volatile uint32_t drop_message = 0;
+volatile uint32_t drop_message = 0;
static osThreadId_t thread_alert;
- uint8_t nucbuf[31];
- char SendBuffer[7];
+uint8_t nucbuf[25];
+char SendBuffer[7];
-
-static void NUC_IdleCallback(void) {
- osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY);
- detect_hook(NUC_TOE);
+static void NUC_CBLTCallback(void)
+{
+ osThreadFlagsSet(thread_alert, SIGNAL_NUC_RAW_REDY);
+ // detect_hook(NUC_TOE);
}
-int8_t NUC_Init(NUC_t *nuc){
- if(nuc == NULL) return DEVICE_ERR_NULL;
- if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL;
- BSP_UART_RegisterCallback(BSP_UART_NUC,BSP_UART_IDLE_LINE_CB,
- NUC_IdleCallback);
-
- return DEVICE_OK;
+static void NUC_ERRORCALLBACK(void)
+{
+ NUC_Restart();
+ uint32_t error_code = HAL_UART_GetError(BSP_UART_GetHandle(BSP_UART_NUC));
+ // osThreadFlagsSet(thread_alert, SIGNAL_NUC_ERROR);
}
-int8_t NUC_StartReceiving() {
- if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
+int8_t NUC_Init(NUC_t *nuc)
+{
+ if (nuc == NULL)
+ return DEVICE_ERR_NULL;
+ if ((thread_alert = osThreadGetId()) == NULL)
+ return DEVICE_ERR_NULL;
+ BSP_UART_RegisterCallback(BSP_UART_NUC, BSP_UART_RX_CPLT_CB,
+ NUC_CBLTCallback);
+ BSP_UART_RegisterCallback(BSP_UART_NUC, BSP_UART_ERROR_CB,
+ NUC_ERRORCALLBACK);
+ return DEVICE_OK;
+}
+int8_t NUC_StartReceiving()
+{
+ if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
(uint8_t *)nucbuf,
sizeof(nucbuf)) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
-int8_t NUC_StartSending(fp32 *data) {
+int8_t NUC_StartSending(fp32 *data)
+{
- union
- {
- float x[1];
- char data[4];
- }instance;
-
-
-// for (int i = 0; i < 1; i++) {
- instance.x[0] = data[0];
-// }
-
- SendBuffer[0] = 0xFC; //帧头
- SendBuffer[1] = 0x01; //控制帧
- for(int i = 2; i < 6; i++)
+ union
{
- SendBuffer[i] = instance.data[i-2];
- }
- SendBuffer[6] = 0xFD; //帧尾
+ float x[1];
+ char data[4];
+ } instance;
- if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
- (uint8_t *)SendBuffer,
- sizeof(SendBuffer)) == HAL_OK)
+ // for (int i = 0; i < 1; i++) {
+ instance.x[0] = data[0];
+ // }
+
+ SendBuffer[0] = 0xFC; // 帧头
+ SendBuffer[1] = 0x01; // 控制帧
+ for (int i = 2; i < 6; i++)
+ {
+ SendBuffer[i] = instance.data[i - 2];
+ }
+ SendBuffer[6] = 0xFD; // 帧尾
+
+ if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
+ (uint8_t *)SendBuffer,
+ sizeof(SendBuffer)) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
-int8_t NUC_Restart(void) {
+int8_t NUC_Restart(void)
+{
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_NUC));
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_NUC));
return DEVICE_OK;
}
-bool_t NUC_WaitDmaCplt(void) {
-
- return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,50) ==
+bool_t NUC_WaitDmaCplt(void)
+{
+ return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll, 20) ==
SIGNAL_NUC_RAW_REDY);
}
-
-int8_t NUC_RawParse(CMD_NUC_t *n){
- if(n ==NULL) return DEVICE_ERR_NULL;
- union
- {
- float x[5];
- char data[20];
- }instance;
- if(nucbuf[0]!=HEAD) goto error; //发送ID不是底盘
- else{
- n->status_fromnuc =nucbuf[1];
- n->ctrl_status =nucbuf[2];
+int8_t NUC_RawParse(CMD_NUC_t *n)
+{
+ if (n == NULL)
+ return DEVICE_ERR_NULL;
+ union
+ {
+ float x[5];
+ char data[20];
+ } instance;
+ if (nucbuf[0] != HEAD)
+ goto error; // 发送ID不是底盘
+ else
+ {
+ n->status_fromnuc = nucbuf[1];
+ n->ctrl_status = nucbuf[2];
switch (n->status_fromnuc)
{
- case VISION://控制帧0x02
- /* 协议格式
- 0xFF HEAD
- 0x02 控制帧
- 0x01 相机帧
- vx fp32
- vy fp32
- wz fp32
- 0xFE TAIL
- */
+ 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];//
+ 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;
- instance.data[3] = nucbuf[6];
- instance.data[2] = nucbuf[5];
- instance.data[1] = nucbuf[4];
- instance.data[0] = nucbuf[3];
+ case MID: // 控制帧0x08
+ /* 协议格式
+ 0xFF HEAD
+ 0x09 控制帧
+ 0x01 相机帧
+ vx fp32
+ vy fp32
+ wz fp32
+ 0xFE TAIL
+ */
+ if (nucbuf[24] != TAIL)
+ 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];//
+ 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]; //
- n->MID360.flag = nucbuf[23];//
-
-
-
-
-
-
-
-
-
break;
-
-
}
- return DEVICE_OK;
+ return DEVICE_OK;
}
- error:
- drop_message++;
- return DEVICE_ERR;
+error:
+ drop_message++;
+ NUC_Restart();
+ return DEVICE_ERR;
}
int8_t NUC_HandleOffline(CMD_NUC_t *cmd)
{
- if(cmd == NULL) return DEVICE_ERR_NULL;
-// memset(cmd, 0, sizeof(*cmd));
- return 0;
+ if (cmd == NULL)
+ return DEVICE_ERR_NULL;
+ // memset(cmd, 0, sizeof(*cmd));
+ return 0;
}
-
-
-
diff --git a/User/device/nuc.h b/User/device/nuc.h
index 3f7c0d1..fad848d 100644
--- a/User/device/nuc.h
+++ b/User/device/nuc.h
@@ -33,5 +33,7 @@ int8_t NUC_StartSending(fp32 *data) ;
bool_t NUC_WaitDmaCplt(void);
int8_t NUC_RawParse(CMD_NUC_t *n);
int8_t NUC_HandleOffline(CMD_NUC_t *cmd);
+int8_t NUC_Restart(void);
+
#endif
diff --git a/User/task/nuc_task.c b/User/task/nuc_task.c
index 3249396..29feee1 100644
--- a/User/task/nuc_task.c
+++ b/User/task/nuc_task.c
@@ -18,7 +18,7 @@ int a=0;
void Task_nuc(void *argument){
(void)argument; /**/
-// osDelay(TASK_INIT_DELAY_NUC);
+ osDelay(TASK_INIT_DELAY_NUC);
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_NUC;
NUC_Init(&nuc_raw);
@@ -35,8 +35,8 @@ void Task_nuc(void *argument){
#endif
a++;
NUC_StartReceiving();
- NUC_StartSending(NUC_send.send);
- NUC_StartSending (send);
+ // NUC_StartSending(NUC_send.send);
+ // NUC_StartSending (send);
if (NUC_WaitDmaCplt()){
NUC_RawParse(&cmd_fromnuc);
}
diff --git a/User/task/user_task.h b/User/task/user_task.h
index c24ab3b..81c836e 100644
--- a/User/task/user_task.h
+++ b/User/task/user_task.h
@@ -18,7 +18,7 @@
#define TASK_FREQ_UP (900u)
#define TASK_FREQ_CTRL_CMD (500u)
-#define TASK_FREQ_NUC (200u)
+#define TASK_FREQ_NUC (500u)
#define TASK_FREQ_CAN (1000u)
#define TASK_FREQ_RC (1000u)