From 5333918f7be71700662b9cce5eedccea9eb6626a Mon Sep 17 00:00:00 2001 From: xxxxm <2389287465@qq.com> Date: Fri, 20 Mar 2026 03:52:29 +0800 Subject: [PATCH 1/3] =?UTF-8?q?add-cmd=E5=88=B0=E5=BA=95=E8=87=AA=E8=B5=B7?= =?UTF-8?q?=E5=92=8Crecover=EF=BC=8C=E5=B7=A6=E6=8B=A8=E6=9D=86=E6=9C=80?= =?UTF-8?q?=E4=B8=8B=E6=8A=A2=E5=8D=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/cmd/cmd.c | 17 ++++++++++++----- User/module/config.c | 4 ++-- User/module/gimbal.h | 2 +- 3 files changed, 15 insertions(+), 8 deletions(-) diff --git a/User/module/cmd/cmd.c b/User/module/cmd/cmd.c index 82f1dd3..90d5e1f 100644 --- a/User/module/cmd/cmd.c +++ b/User/module/cmd/cmd.c @@ -4,6 +4,7 @@ #include "cmd.h" #include "bsp/time.h" #include "module/cmd/cmd_types.h" +#include "module/gimbal.h" #include "module/shoot.h" #include #include @@ -70,7 +71,7 @@ static void CMD_RC_BuildGimbalCmd(CMD_t *ctx) { #if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_SHOOT static void CMD_RC_BuildShootCmd(CMD_t *ctx) { if (ctx->input.online[CMD_SRC_RC]) { - ctx->output.shoot.cmd.mode = SHOOT_MODE_CONTINUE; + ctx->output.shoot.cmd.mode = SHOOT_MODE_SINGLE; } else { ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE; } @@ -648,16 +649,22 @@ int8_t CMD_Arbitrate(CMD_t *ctx) { if (ctx->input.online[CMD_SRC_NUC]) { if (ctx->active_source==CMD_SRC_RC) { if (ctx->input.rc.sw[0] == CMD_SW_DOWN) { - ctx->output.gimbal.source = CMD_SRC_NUC; - ctx->output.shoot.source = CMD_SRC_NUC; + //用遥控器调自瞄的时候打开下面两行 + // ctx->output.gimbal.source = CMD_SRC_NUC; + // ctx->output.shoot.source = CMD_SRC_NUC; #if CMD_ENABLE_MODULE_REFUI ctx->output.refui.source = CMD_SRC_NUC; #endif - } } } +} #endif - + if (ctx->input.rc.sw[0] == CMD_SW_DOWN) { + ctx->output.balance_chassis.source = CMD_SRC_RC; + ctx->output.gimbal.source = CMD_SRC_RC; + ctx->output.balance_chassis.cmd.mode = CHASSIS_MODE_LEG_TEST; + ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RECOVER; + } return CMD_OK; } diff --git a/User/module/config.c b/User/module/config.c index 1e52b66..4127d44 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -538,14 +538,14 @@ Config_RobotParam_t robot_config = { .gimbal_sw_up = GIMBAL_MODE_RELAX, .gimbal_sw_mid = GIMBAL_MODE_ABSOLUTE, // .gimbal_sw_down = GIMBAL_MODE_RECOVER, - .gimbal_sw_down = GIMBAL_MODE_AI_CONTROL, + .gimbal_sw_down = GIMBAL_MODE_RECOVER, #endif #if CMD_ENABLE_MODULE_BALANCE_CHASSIS .balance_sw_up = CHASSIS_MODE_RELAX, // .balance_sw_mid = CHASSIS_MODE_RELAX, // .balance_sw_down = CHASSIS_MODE_RELAX, .balance_sw_mid =CHASSIS_MODE_WHELL_LEG_BALANCE, - .balance_sw_down = CHASSIS_MODE_WHELL_LEG_BALANCE, + .balance_sw_down = CHASSIS_MODE_LEG_TEST, #endif }, }, diff --git a/User/module/gimbal.h b/User/module/gimbal.h index b111c7f..86f2eb9 100644 --- a/User/module/gimbal.h +++ b/User/module/gimbal.h @@ -80,7 +80,7 @@ typedef struct { KPID_Params_t yaw_omega; /* yaw轴角速度环PID参数 */ KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */ KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */ - } aimbot; + } aimbot; struct { KPID_Params_t yaw_angle; /* recover yaw位置环PID参数 */ KPID_Params_t pit_angle; /* recover pitch位置环PID参数 */ From 08036a7b1f5c464bac2b076cb26a80504ceef74a Mon Sep 17 00:00:00 2001 From: xxxxm <2389287465@qq.com> Date: Fri, 20 Mar 2026 15:01:51 +0800 Subject: [PATCH 2/3] fix-heatctrl --- User/module/config.c | 10 +++---- User/module/shoot.c | 67 ++++++++++++++++++++++++++++++++++++++++---- User/module/shoot.h | 14 ++++----- 3 files changed, 74 insertions(+), 17 deletions(-) diff --git a/User/module/config.c b/User/module/config.c index 4127d44..eca48af 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -151,11 +151,11 @@ Config_RobotParam_t robot_config = { }, .heatControl={ .enable=true, - .safe_shots=5, // 安全出弹余量 - .nmax=15.0f, // 最大射频 Hz - .Hwarn=200.0f, // 热量预警值 - .Hsatu=100.0f, // 热量饱和值 - .Hthres=50.0f, // 热量阈值 + .safe_shots=0.05f, // 安全出弹余量比例 + .nmax=18.0f, // 最大射频 Hz + .Hwarn=0.70f, // 预警阈值比例（70%） + .Hsatu=0.40f, // 饱和阈值比例（40%） + .Hthres=0.08f, // 停射阈值比例（8%） }, .motor={ .fric = { diff --git a/User/module/shoot.c b/User/module/shoot.c index 886ce4f..e596451 100644 --- a/User/module/shoot.c +++ b/User/module/shoot.c @@ -276,6 +276,52 @@ static bool Shoot_DetectShotByRpmDrop(Shoot_t *s) return false; } + +static float Shoot_ResolveHeatThreshold(float cfg, float Hmax) +{ + if (cfg <= 0.0f) { + return 0.0f; + } + + if (cfg <= 1.0f) { + return cfg * Hmax; + } + + return cfg; +} + +static uint16_t Shoot_ResolveSafeShots(Shoot_t *s) +{ + if (s == NULL) { + return 0U; + } + + float cfg = s->param->heatControl.safe_shots; + if (cfg <= 0.0f) { + return 0U; + } + + if (cfg <= 1.0f) { + if (s->heatcontrol.Hmax <= 0.0f || s->heatcontrol.Hgen <= 0.0f) { + return 0U; + } + + float total_shots = s->heatcontrol.Hmax / s->heatcontrol.Hgen; + float safe = ceilf(cfg * total_shots); + if (safe < 1.0f) { + safe = 1.0f; + } + if (safe > (float)UINT16_MAX) { + safe = (float)UINT16_MAX; + } + return (uint16_t)safe; + } + + if (cfg > (float)UINT16_MAX) { + cfg = (float)UINT16_MAX; + } + return (uint16_t)cfg; +} /** @@ -487,11 +533,21 @@ static float Shoot_CaluFreqByHeat(Shoot_t *s) } float Hres = s->heatcontrol.Hres; - float Hwarn = s->param->heatControl.Hwarn; - float Hsatu = s->param->heatControl.Hsatu; - float Hthres = s->param->heatControl.Hthres; + float Hwarn = Shoot_ResolveHeatThreshold(s->param->heatControl.Hwarn, s->heatcontrol.Hmax); + float Hsatu = Shoot_ResolveHeatThreshold(s->param->heatControl.Hsatu, s->heatcontrol.Hmax); + float Hthres = Shoot_ResolveHeatThreshold(s->param->heatControl.Hthres, s->heatcontrol.Hmax); float nmax = s->param->heatControl.nmax; float ncd = s->heatcontrol.ncd; + + if (Hwarn <= 0.0f) { + Hwarn = s->heatcontrol.Hmax * 0.7f; + } + if (Hsatu <= 0.0f || Hsatu >= Hwarn) { + Hsatu = Hwarn * 0.5f; + } + if (Hthres <= 0.0f || Hthres >= Hsatu) { + Hthres = fmaxf(Hsatu * 0.5f, s->heatcontrol.Hgen); + } /* 剩余热量大于预警值：最大射频 */ if (Hres > Hwarn) { @@ -532,10 +588,11 @@ int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd) /* 根据热量控制计算实际射频 */ float actual_freq = Shoot_CaluFreqByHeat(s); + uint16_t safe_shots = Shoot_ResolveSafeShots(s); /* 检查可发射弹丸数是否满足安全余量 */ if (s->param->heatControl.enable && - s->heatcontrol.shots_available <= s->param->heatControl.safe_shots) { + s->heatcontrol.shots_available <= safe_shots) { actual_freq = 0.0f; /* 禁止发弹 */ } @@ -929,7 +986,7 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq) KPID_MODE_CALC_D, target_freq, ¶m->pid.fric_follow); - PID_Init(&s->pid.fric_err[i], + PID_Init(&s->pid.fric_err[i], KPID_MODE_CALC_D, target_freq, ¶m->pid.fric_err); diff --git a/User/module/shoot.h b/User/module/shoot.h index 2773d8a..abd9c9a 100644 --- a/User/module/shoot.h +++ b/User/module/shoot.h @@ -13,8 +13,8 @@ extern "C" { #include "component/pid.h" #include "device/motor_rm.h" /* Exported constants ------------------------------------------------------- */ -#define MAX_FRIC_NUM 2 -#define MAX_NUM_MULTILEVEL 1 /* 多级发射级数 */ +#define MAX_FRIC_NUM 6 +#define MAX_NUM_MULTILEVEL 2 /* 多级发射级数 */ #define SHOOT_OK (0) /* 运行正常 */ #define SHOOT_ERR_NULL (-1) /* 运行时发现NULL指针 */ @@ -102,7 +102,7 @@ typedef struct { }Shoot_JamDetection_t; typedef struct { - bool ref_online; /* 裁判系统数据是否在线 */ + bool ref_online; /* 裁判系统在线标志：在线才启用热量限制 */ /* 从裁判系统读取的常量 */ float Hmax; // 热量上限 float Hcd; // 热量冷却速度 @@ -165,11 +165,11 @@ typedef struct { }jamDetection;/* 卡弹检测参数 */ struct { bool enable; /* 是否启用热量控制 */ - uint16_t safe_shots;/* 安全余量，当shots_available小于等于该值时禁止发弹 */ + float safe_shots;/* 安全余量：<=1按总可发弹数比例，>1按绝对发弹数 */ float nmax;//最大射频 - float Hwarn;//热量预警值 - float Hsatu;//热量饱和值 - float Hthres;//热量阈值，超过这个值将无法射击 + float Hwarn;//预警阈值：<=1按Hmax比例，>1按绝对热量 + float Hsatu;//饱和阈值：<=1按Hmax比例，>1按绝对热量 + float Hthres;//停射阈值：<=1按Hmax比例，>1按绝对热量 }heatControl;/* 热量控制参数 */ struct { Shoot_MOTOR_RM_Param_t fric[MAX_FRIC_NUM]; From dd6c48c75cc7e280074e7a540dea0841420a3b46 Mon Sep 17 00:00:00 2001 From: xxxxm <2389287465@qq.com> Date: Sat, 21 Mar 2026 02:04:44 +0800 Subject: [PATCH 3/3] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E7=83=AD=E9=87=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/cmd/cmd.c | 9 ++++++++- User/module/config.c | 14 +++++++------- User/module/shoot.c | 43 +++++++++++++++++++++--------------------- User/module/shoot.h | 4 ++-- User/task/ctrl_shoot.c | 30 +++++++++++++++++++++++++---- 5 files changed, 64 insertions(+), 36 deletions(-) diff --git a/User/module/cmd/cmd.c b/User/module/cmd/cmd.c index 958027a..c08b5ac 100644 --- a/User/module/cmd/cmd.c +++ b/User/module/cmd/cmd.c @@ -71,7 +71,7 @@ static void CMD_RC_BuildGimbalCmd(CMD_t *ctx) { #if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_SHOOT static void CMD_RC_BuildShootCmd(CMD_t *ctx) { if (ctx->input.online[CMD_SRC_RC]) { - ctx->output.shoot.cmd.mode = SHOOT_MODE_SINGLE; + ctx->output.shoot.cmd.mode = SHOOT_MODE_CONTINUE; } else { ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE; } @@ -406,6 +406,13 @@ static void CMD_PC_BuildBalanceChassisCmd(CMD_t *ctx) { ctx->output.balance_chassis.cmd.move_vec.vy = 0.0f; ctx->output.balance_chassis.cmd.move_vec.wz = 0.0f; + ctx->output.balance_chassis.cmd.height += (float)ctx->input.pc.mouse.z * sens->mouse_sens * 0.001f * ctx->timer.dt; + if (ctx->output.balance_chassis.cmd.height > 1.0f) { + ctx->output.balance_chassis.cmd.height = 1.0f; + } else if (ctx->output.balance_chassis.cmd.height < 0.2f) { + ctx->output.balance_chassis.cmd.height = 0.2f; + } + ctx->output.balance_chassis.cmd.jump_trigger = false; ctx->output.balance_chassis.cmd.mode = CHASSIS_MODE_WHELL_LEG_BALANCE; CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_BALANCE_CHASSIS); diff --git a/User/module/config.c b/User/module/config.c index eca48af..07104e4 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -146,16 +146,16 @@ Config_RobotParam_t robot_config = { }, .jamDetection={ .enable=true, - .threshold=200.0f, + .threshold=90.0f, .suspectedTime=0.5f, }, .heatControl={ .enable=true, - .safe_shots=0.05f, // 安全出弹余量比例 - .nmax=18.0f, // 最大射频 Hz - .Hwarn=0.70f, // 预警阈值比例（70%） - .Hsatu=0.40f, // 饱和阈值比例（40%） - .Hthres=0.08f, // 停射阈值比例（8%） + .safe_shots=0.10f, // 安全出弹余量比例 + .nmax=15.0f, // 最大射频 Hz + .Hwarn=0.50f, // 预警阈值比例（70%） + .Hsatu=0.25f, // 饱和阈值比例（40%） + .Hthres=0.05f, // 停射阈值比例（8%） }, .motor={ .fric = { @@ -480,7 +480,7 @@ Config_RobotParam_t robot_config = { .climb = { .forward_speed = 1.5f, /* 上台阶前进速度 (m/s) */ - .theta_retract_threshold = 0.9f, /* 腿后摆收腿阈值 (rad)，约30° */ + .theta_retract_threshold = 0.98f, /* 腿后摆收腿阈值 (rad)，约30° */ .tp_scale = 0.08f, /* 摆力矩缩放：削弱到10%，让腿自由后摆 */ .settle_time_ms = 500, /* 收腿后稳定时间 (ms) */ }, diff --git a/User/module/shoot.c b/User/module/shoot.c index e596451..f157778 100644 --- a/User/module/shoot.c +++ b/User/module/shoot.c @@ -353,8 +353,8 @@ static int8_t Shoot_FuseHeatData(Shoot_t *s) s->heatcontrol.Hnow_last = s->heatcontrol.Hnow; /* 记录本次值 */ } - /* 融合值就是裁判系统值（作为基准） */ - s->heatcontrol.Hnow_fused = s->heatcontrol.Hnow; + /* 连射场景下，裁判数据存在刷新延迟；取更保守的热量用于限热 */ + s->heatcontrol.Hnow_fused = fmaxf(s->heatcontrol.Hnow, s->heatcontrol.Hnow_estimated); } else { /* 裁判系统数据无效，仅使用自主估计值 */ s->heatcontrol.Hnow_fused = s->heatcontrol.Hnow_estimated; @@ -461,26 +461,8 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s) break; case SHOOT_HEAT_DETECT_CONFIRMED: - /* 确认发射状态：增加热量并返回准备状态 */ - /* 根据弹丸类型增加估计热量 */ - switch (s->param->basic.projectileType) { - case SHOOT_PROJECTILE_17MM: - s->heatcontrol.Hnow_estimated += 10.0f; - break; - case SHOOT_PROJECTILE_42MM: - s->heatcontrol.Hnow_estimated += 100.0f; - break; - default: - s->heatcontrol.Hnow_estimated += s->heatcontrol.Hgen; - break; - } - - /* 限制估计热量不超过最大值 */ - if (s->heatcontrol.Hnow_estimated > s->heatcontrol.Hmax) { - s->heatcontrol.Hnow_estimated = s->heatcontrol.Hmax; - } - - /* 增加发射计数 */ + /* 确认发射状态：仅记录检测计数。 + * 估计热量在实际下发拨弹时已预扣，避免重复累加。 */ s->heatcontrol.shots_detected++; /* 返回准备检测状态 */ @@ -539,6 +521,15 @@ static float Shoot_CaluFreqByHeat(Shoot_t *s) float nmax = s->param->heatControl.nmax; float ncd = s->heatcontrol.ncd; + if (nmax <= 0.0f) { + nmax = s->param->basic.shot_freq; + } + if (ncd < 0.0f) { + ncd = 0.0f; + } else if (ncd > nmax) { + ncd = nmax; + } + if (Hwarn <= 0.0f) { Hwarn = s->heatcontrol.Hmax * 0.7f; } @@ -609,6 +600,14 @@ int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd) CircleAdd(&s->target_variable.trig_angle, M_2PI/s->param->basic.num_trig_tooth, M_2PI); s->var_trig.num_toShoot--; s->var_trig.num_shooted++; + + /* 实际发射指令下发时，立刻预扣估计热量，抑制裁判数据延迟导致的超热。 */ + if (s->param->heatControl.enable && s->heatcontrol.ref_online && s->heatcontrol.Hgen > 0.0f) { + s->heatcontrol.Hnow_estimated += s->heatcontrol.Hgen; + if (s->heatcontrol.Hnow_estimated > s->heatcontrol.Hmax) { + s->heatcontrol.Hnow_estimated = s->heatcontrol.Hmax; + } + } } return SHOOT_OK; diff --git a/User/module/shoot.h b/User/module/shoot.h index abd9c9a..5a32e8b 100644 --- a/User/module/shoot.h +++ b/User/module/shoot.h @@ -13,8 +13,8 @@ extern "C" { #include "component/pid.h" #include "device/motor_rm.h" /* Exported constants ------------------------------------------------------- */ -#define MAX_FRIC_NUM 6 -#define MAX_NUM_MULTILEVEL 2 /* 多级发射级数 */ +#define MAX_FRIC_NUM 2 +#define MAX_NUM_MULTILEVEL 1 /* 多级发射级数 */ #define SHOOT_OK (0) /* 运行正常 */ #define SHOOT_ERR_NULL (-1) /* 运行时发现NULL指针 */ diff --git a/User/task/ctrl_shoot.c b/User/task/ctrl_shoot.c index a778670..7ded87e 100644 --- a/User/task/ctrl_shoot.c +++ b/User/task/ctrl_shoot.c @@ -38,13 +38,23 @@ static int print_shoot(const void *data, char *buf, size_t size) { " Fric1 : %.1f rpm (target: %.1f)\r\n" " Fric_Avg : %.1f rpm\r\n" " Trig : %.1f rpm (angle: %.2f deg)\r\n" - " Output : Fric0=%.1f Fric1=%.1f Trig=%.1f\r\n", + " Output : Fric0=%.1f Fric1=%.1f Trig=%.1f\r\n" + " Heat : online=%d Hnow=%.1f Hest=%.1f Hfused=%.1f Hres=%.1f Hmax=%.1f Hgen=%.1f ncd=%.2f avail=%u\r\n", shoot->running_state, shoot->feedback.fric[0].rotor_speed, shoot->target_variable.fric_rpm, shoot->feedback.fric[1].rotor_speed, shoot->target_variable.fric_rpm, shoot->var_fric.normalized_fil_avgrpm, shoot->feedback.trig.feedback.rotor_speed, shoot->feedback.trig.feedback.rotor_abs_angle, - shoot->output.out_fric[0], shoot->output.out_fric[1], shoot->output.outagl_trig); + shoot->output.out_fric[0], shoot->output.out_fric[1], shoot->output.outagl_trig, + shoot->heatcontrol.ref_online ? 1 : 0, + shoot->heatcontrol.Hnow, + shoot->heatcontrol.Hnow_estimated, + shoot->heatcontrol.Hnow_fused, + shoot->heatcontrol.Hres, + shoot->heatcontrol.Hmax, + shoot->heatcontrol.Hgen, + shoot->heatcontrol.ncd, + (unsigned int)shoot->heatcontrol.shots_available); return 0; } @@ -80,8 +90,20 @@ void Task_ctrl_shoot(void *argument) { shoot.heatcontrol.ref_online = true; shoot.heatcontrol.Hmax = (float)shoot_ref.robot_status.shooter_barrel_heat_limit; shoot.heatcontrol.Hcd = (float)shoot_ref.robot_status.shooter_barrel_cooling_value; - shoot.heatcontrol.Hnow = (float)shoot_ref.power_heat.shooter_42mm_barrel_heat; - shoot.heatcontrol.Hgen = 10.0f; /* 42mm弹丸每发产生热量 */ + switch (shoot.param->basic.projectileType) { + case SHOOT_PROJECTILE_17MM: + shoot.heatcontrol.Hnow = (float)shoot_ref.power_heat.shooter_17mm_barrel_heat; + shoot.heatcontrol.Hgen = 10.0f; + break; + case SHOOT_PROJECTILE_42MM: + shoot.heatcontrol.Hnow = (float)shoot_ref.power_heat.shooter_42mm_barrel_heat; + shoot.heatcontrol.Hgen = 100.0f; + break; + default: + shoot.heatcontrol.Hnow = (float)shoot_ref.power_heat.shooter_17mm_barrel_heat; + shoot.heatcontrol.Hgen = 10.0f; + break; + } } else { shoot.heatcontrol.ref_online = false; }