From 1c5adab4a677ab17727f735c75f50a2fad2c86ce Mon Sep 17 00:00:00 2001 From: xxxxm <2389287465@qq.com> Date: Sat, 21 Mar 2026 02:35:23 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E7=83=AD=E9=87=8F=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/cmd/cmd.c | 11 +++++- User/module/cmd/cmd_behavior.c | 2 +- User/module/config.c | 10 +++--- User/module/gimbal.c | 6 +++- User/module/shoot.c | 65 +++++++++++++++++++++++++++++++--- User/module/shoot.h | 8 ++--- User/task/ctrl_shoot.c | 17 +++++++-- ozone/hero.jdebug.user | 25 +++++-------- 8 files changed, 109 insertions(+), 35 deletions(-) diff --git a/User/module/cmd/cmd.c b/User/module/cmd/cmd.c index a425aed..a5cc690 100644 --- a/User/module/cmd/cmd.c +++ b/User/module/cmd/cmd.c @@ -142,6 +142,8 @@ static void CMD_PC_BuildChassisCmd(CMD_t *ctx) { /* WASD控制移动 */ ctx->output.chassis.cmd.ctrl_vec.vx = 0.0f; ctx->output.chassis.cmd.ctrl_vec.vy = 0.0f; + ctx->output.chassis.cmd.ctrl_vec.wz = 0.0f; + CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_CHASSIS); } #endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_CHASSIS */ @@ -155,7 +157,7 @@ static void CMD_PC_BuildGimbalCmd(CMD_t *ctx) { ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX; return; } - ctx->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE; + ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELATIVE; /* 鼠标控制云台 */ ctx->output.gimbal.cmd.delta_yaw = (float)-ctx->input.pc.mouse.x * ctx->timer.dt * sens->mouse_sens; @@ -206,6 +208,13 @@ static void CMD_NUC_BuildGimbalCmd(CMD_t *ctx) { ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX; return; } + + if (ctx->input.nuc.mode == 0) { + ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELATIVE; + ctx->output.gimbal.cmd.delta_yaw = 0.0f; + ctx->output.gimbal.cmd.delta_pit = 0.0f; + return; + } /* 使用AI提供的云台控制数据 */ diff --git a/User/module/cmd/cmd_behavior.c b/User/module/cmd/cmd_behavior.c index 47855a9..841d207 100644 --- a/User/module/cmd/cmd_behavior.c +++ b/User/module/cmd/cmd_behavior.c @@ -78,7 +78,7 @@ int8_t CMD_Behavior_Handle_AUTOAIM(CMD_t *ctx) { /* 自瞄模式切换 */ #if CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_MODULE_SHOOT if (ctx->input.online[CMD_SRC_NUC]) { - if (ctx->active_source == CMD_SRC_PC){ + if (ctx->active_source == CMD_SRC_PC && ctx->input.nuc.mode != 0){ ctx->output.gimbal.source = CMD_SRC_NUC; ctx->output.shoot.source = CMD_SRC_NUC; #if CMD_ENABLE_MODULE_REFUI diff --git a/User/module/config.c b/User/module/config.c index cdae811..b8b1897 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -286,11 +286,11 @@ Config_RobotParam_t robot_config = { }, .heatControl={ .enable=true, - .safe_shots=1, // 安全出弹余量 - .nmax=2.0f, // 最大射频 Hz - .Hwarn=200.0f, // 热量预警值 - .Hsatu=100.0f, // 热量饱和值 - .Hthres=50.0f, // 热量阈值 + .safe_shots=0.00f, // 安全出弹余量比例（10%） + .nmax=3.0f, // 最大射频 Hz + .Hwarn=0.70f, // 预警阈值比例（70%） + .Hsatu=0.40f, // 饱和阈值比例（40%） + .Hthres=0.08f, // 停射阈值比例（8%） }, .motor={ .fric = { diff --git a/User/module/gimbal.c b/User/module/gimbal.c index cd1f200..0e10db3 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -61,6 +61,7 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) { g->setpoint.eulr.yaw = g->feedback.motor.yaw.rotor_abs_angle; } else if (mode == GIMBAL_MODE_RELATIVE) { g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw; + g->setpoint.eulr.pit = g->feedback.imu.eulr.pit; } else if (mode == GIMBAL_MODE_AI_CONTROL) { g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw; g->setpoint.eulr.pit =g->feedback.imu.eulr.pit; @@ -294,7 +295,10 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) { g->feedback.motor.pit.rotor_abs_angle, 0.0f, g->dt); g->out.pit = PID_Calc(&(g->pid.relative.pit_omega), pit_omega_set_point, g->feedback.imu.gyro.x, 0.f, g->dt); - + // pit_omega_set_point = PID_Calc(&(g->pid.relative.pit_angle), g->setpoint.eulr.pit, + // g->feedback.imu.eulr.pit, 0.0f, g->dt); + // g->out.pit = PID_Calc(&(g->pid.relative.pit_omega), pit_omega_set_point, + // g->feedback.imu.gyro.x, 0.f, g->dt); /* 输出滤波 */ g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw); g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit); diff --git a/User/module/shoot.c b/User/module/shoot.c index 18f0384..456873d 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; /* 禁止发弹 */ } diff --git a/User/module/shoot.h b/User/module/shoot.h index 03061ad..abd9c9a 100644 --- a/User/module/shoot.h +++ b/User/module/shoot.h @@ -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]; diff --git a/User/task/ctrl_shoot.c b/User/task/ctrl_shoot.c index 138185e..84d9f63 100644 --- a/User/task/ctrl_shoot.c +++ b/User/task/ctrl_shoot.c @@ -54,9 +54,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 = 100.0f; /* 42mm弹丸每发产生热量 */ - } else { + 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; shoot.heatcontrol.Hmax = 0.0f; shoot.heatcontrol.Hcd = 0.0f; diff --git a/ozone/hero.jdebug.user b/ozone/hero.jdebug.user index 1ddec96..a075654 100644 --- a/ozone/hero.jdebug.user +++ b/ozone/hero.jdebug.user @@ -2,15 +2,9 @@ Breakpoint=D:/CUBEMX/hero/god-yuan-hero/User/device/dr16.c:69:52, State=BP_STATE_DISABLED -GraphedExpression="((((gimbal).feedback).imu).eulr).pit", Color=#e56a6f, Show=0 -GraphedExpression="((((gimbal).feedback).imu).eulr).yaw", Color=#35792b, Show=0 -GraphedExpression="(((ai_cmd_from_can).gimbal_t).setpoint).yaw", Color=#769dda, Show=0 -GraphedExpression="(((ai_cmd_from_can).gimbal_t).setpoint).pit", Color=#b14f0d, Show=0 -GraphedExpression="((dr16).raw_data).ch_r_x", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#b3c38e, Show=0 -GraphedExpression="((dr16).data).ch_r_x", Color=#ab7b05, Show=0 -GraphedExpression="((((gimbal).feedback).motor).pit).rotor_abs_angle", Color=#7fd3b7, Show=0 -GraphedExpression="((shoot).heatcontrol).Hnow", Color=#50328f -GraphedExpression="((shoot).heatcontrol).shots_available", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#c587a5, Show=0 +GraphedExpression="((ref).robot_status).shooter_barrel_heat_limit", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#e56a6f, Show=0 +GraphedExpression="((ref).power_heat).shooter_42mm_barrel_heat", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#35792b, Show=0 +GraphedExpression="((shoot ).heatcontrol).shots_available", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#769dda OpenDocument="startup_stm32h723xx.s", FilePath="D:/CUBEMX/hero/god-yuan-hero/startup_stm32h723xx.s", Line=47 OpenDocument="port.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c", Line=716 OpenDocument="tasks.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=4104 @@ -52,19 +46,18 @@ OpenDocument="motor_rm.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/device/mo OpenDocument="stm32h7xx_hal_uart_ex.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart_ex.c", Line=964 OpenDocument="queue.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Middlewares/Third_Party/FreeRTOS/Source/queue.c", Line=1275 OpenDocument="chassis.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/module/chassis.c", Line=213 -OpenDocument="supercap.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/supercap.c", Line=15 OpenDocument="atti_esti.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/atti_esti.c", Line=121 OpenDocument="fdcan.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Core/Src/fdcan.c", Line=0 OpenDocument="time.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/bsp/time.c", Line=17 OpenToolbar="Debug", Floating=0, x=0, y=0 -OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=2, w=695, h=337, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1 +OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=2, w=695, h=336, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1 OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=312, h=983, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 -OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=1, w=695, h=322, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=1, w=695, h=323, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Memory 1", DockArea=BOTTOM, x=0, y=0, w=462, h=491, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0xFFFF5088 -OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=2, w=695, h=337, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=2, w=695, h=336, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Functions", DockArea=LEFT, x=0, y=0, w=312, h=983, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=752, h=472, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0 -OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1344, h=491, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="5 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="22;108", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1431;-69", CodeGraphLegendShown=0, CodeGraphLegendPosition="1447;0" +OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1344, h=491, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="5 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="22;108", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1148;-69", CodeGraphLegendShown=0, CodeGraphLegendPosition="1164;0" OpenWindow="Console", DockArea=BOTTOM, x=2, y=0, w=752, h=472, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="RTOS", DockArea=RIGHT, x=0, y=0, w=695, h=322, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, Showing="" SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1" @@ -74,8 +67,8 @@ TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[] TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[215;100;100;100;1022] TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[298;229;145;100] -TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ((((gimbal).feedback).imu).eulr).pit";" ((((gimbal).feedback).imu).eulr).yaw";" (((ai_cmd_from_can).gimbal_t).setpoint).yaw";" (((ai_cmd_from_can).gimbal_t).setpoint).pit";" ((dr16).raw_data).ch_r_x";" ((dr16).data).ch_r_x";" ((((gimbal).feedback).motor).pit).rotor_abs_angle";" ((shoot).heatcontrol).Hnow";" ((shoot).heatcontrol).shots_available"], ColWidths=[100;100;100;100;100;100;100;100;100;100;100] -TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;214;100;100;100;100;110;126;126] +TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ((ref).robot_status).shooter_barrel_heat_limit";" ((ref).power_heat).shooter_42mm_barrel_heat";" ((shoot ).heatcontrol).shots_available"], ColWidths=[100;100;100;100;100] +TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;154;100;100;100;114;110;126;126] TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;340] WatchedExpression="cmd", RefreshRate=5, Window=Watched Data 1 WatchedExpression="bmi088", RefreshRate=5, Window=Watched Data 1