修热量控制
This commit is contained in:
parent
e96cd046ec
commit
1c5adab4a6
@ -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提供的云台控制数据 */
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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 = {
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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; /* 禁止发弹 */
|
||||
}
|
||||
|
||||
|
||||
@ -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];
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user