fix-heatctrl

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 <stdint.h>
 #include <string.h>
@@ -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;
   }
@@ -654,16 +655,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;
 }
 

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 = {
@@ -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
         },
     },

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参数 */

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,
             &param->pid.fric_follow);
-	    PID_Init(&s->pid.fric_err[i], 
+	      PID_Init(&s->pid.fric_err[i], 
             KPID_MODE_CALC_D, 
             target_freq,
             &param->pid.fric_err);

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];