generated from hulk/gd32f4xx_template_cmake_vscode
Heater: PID v0.1.14 - banded controller + sensor glitch guards
Rebuild PID as three-zone banded controller: far field P/D full power,
This commit is contained in:
+175
-68
@@ -1,16 +1,26 @@
|
||||
/**
|
||||
* @file heat.c
|
||||
* @brief 加热控制：慢 PWM 驱动（PB12）+ 分段 PID 控温闭环 + 安全保护。
|
||||
* @brief 加热控制：慢 PWM 驱动（PB12）+ 斜坡设定值 + 分段 PID 控温闭环 + 安全保护。
|
||||
* @details 硬件：PB12=HEAT_EN，高 = 加热开（2N7002K 电平转换 → VBE2412 PMOS → 24V）。
|
||||
* 开关频率约束（<=10Hz）由"2s 载波周期 + 5% 占空步进"保证——每档脉宽
|
||||
* 恒为 100ms 的整数倍（0.5Hz 载波、20 档占空分辨率）。
|
||||
* 控制为分段式：
|
||||
* err >= 温和带 全量 PID（满功率爬升）；
|
||||
* 0 <= err < 温和带 正常 PID 但占空上限压到温和档（小功率缓升，
|
||||
* 减少进带蓄热，降低越过目标后的惯性过冲）；
|
||||
* err < 0（超温） 强制关断 + 积分按负误差回退（执行器只能加热不能制冷）。
|
||||
* @note 安全保护：上电默认关闭；NTC 传感器故障（OPEN/SHRT）强制关断；
|
||||
* 过温（>= HEATER_OVERTEMP_C）滞回保护；未使能强制关断。
|
||||
* 控制结构（平滑逼近目标，抑制过冲/震荡）：
|
||||
* 1. 目标斜坡：内部设定值以 HEATER_RAMP_CPS 限速滑向 M4 目标，
|
||||
* 设定值阶跃/跳变不直接进入误差；
|
||||
* 2. 全误差区间连续输出：err >= 温和带 P/D 全量；0 <= err < 温和带
|
||||
* P/D 钳到 HEATER_SLOWBAND_DUTY（积分不参与钳）；err < 0（超过
|
||||
* 内部目标）P 负项直接抵扣积分后钳 0——P 参与使输出随超温幅度
|
||||
* 立即收敛，积分只按剩余需要维持（若 P 铰 0 积分独撑，过线后
|
||||
* 输出不随 err 变化，形成 sag->重建->过冲->排空自持极限环）；
|
||||
* 3. 微分基于温度变化率（D on measurement，-Kd*ΔT/拍）：设定值
|
||||
* 变化不产生微分踢；升温时提前退功率，回落越过目标时提前止跌；
|
||||
* 4. 积分钳位 [0, HEATER_PID_I_MAX]，吸收仅限目标近旁窄带
|
||||
* （HEATER_PID_INTEG_ZONE_C）且 P/D 未被钳时（防爬升段
|
||||
* windup）；err<0 且 u>0 时小量回退，积分向稳态收敛；
|
||||
* 稳态维持功率由积分承载。
|
||||
* @note 安全保护：上电默认关闭；NTC 传感器故障（OPEN/SHRT，连续 3 拍去抖确认，
|
||||
* 单拍毛刺不触发；短故障恢复无扰还原积分）强制关断；过温
|
||||
* （>= HEATER_OVERTEMP_C）滞回保护；未使能强制关断。
|
||||
*/
|
||||
|
||||
#include "heat.h"
|
||||
@@ -23,17 +33,27 @@
|
||||
/* 内部状态 */
|
||||
|
||||
static bool s_enable = false; /* 加热使能（M3） */
|
||||
static float s_target = 0.0f; /* 目标温度（M4） */
|
||||
static float s_target = 0.0f; /* 目标温度（M4，外部设定值） */
|
||||
static float s_ramp_target = 0.0f; /* 内部斜坡目标（以 HEATER_RAMP_CPS 滑向 s_target） */
|
||||
static uint8_t s_duty = 0; /* 量化占空档位 0~HEATER_PWM_STEPS（x STEP%） */
|
||||
static bool s_overtemp = false; /* 过温锁存（滞回恢复） */
|
||||
static float s_integral = 0.0f; /* PID 积分项 */
|
||||
static float s_prev_err = 0.0f; /* PID 上次误差 */
|
||||
static float s_integral = 0.0f; /* PID 积分累加（钳 [0, HEATER_PID_I_MAX]） */
|
||||
static float s_prev_temp = 0.0f; /* 上拍温度（微分 on measurement 用） */
|
||||
static float s_duty_acc = 0.0f; /* 占空抖动累加器（档位单位，<1） */
|
||||
static uint8_t s_fault_debounce = 0; /* NTC 故障去抖（连续故障拍数，255 封顶记时长） */
|
||||
static float s_fault_stash_I = 0.0f; /* 故障确认时刻积分暂存（短故障无扰还原用） */
|
||||
static float s_fault_stash_T = 0.0f; /* 故障前末次正常温度（还原条件校验用） */
|
||||
static float s_t_filt = 25.0f; /* 斜率限幅后的温度（读数毛刺防护） */
|
||||
static uint8_t s_slew_cnt = 0; /* 连续超斜率拍数（真实快速变化放行计数） */
|
||||
static uint16_t s_pwm_phase_ms = 0; /* PWM 载波相位 0..PERIOD-1 */
|
||||
static uint16_t s_pid_divider = 0; /* PID 周期分频（10ms 圈） */
|
||||
|
||||
/* 档位总数 = 100% / 步进% */
|
||||
#define HEATER_PWM_STEPS (100U / HEATER_PWM_STEP_PCT)
|
||||
|
||||
/* NTC 故障确认拍数（每拍 = 1 个 PID 周期 500ms；连续 3 拍 = 1.5s） */
|
||||
#define NTC_FAULT_CONFIRM_PID_STEPS 3U
|
||||
|
||||
/******************************************************************************/
|
||||
/* 底层输出 */
|
||||
|
||||
@@ -56,38 +76,74 @@ void heat_init(void) {
|
||||
/******************************************************************************/
|
||||
/* PID */
|
||||
|
||||
/**
|
||||
* @brief 复位 PID 积分与历史误差（使能切换/传感器故障时调用，防积分残留冲击）。
|
||||
*/
|
||||
static void pid_reset(void) {
|
||||
s_integral = 0.0f;
|
||||
s_prev_err = 0.0f;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 积分试算值（err 一步积分；供抗饱和逻辑条件采纳）。
|
||||
*/
|
||||
static float integ_step(float err) {
|
||||
return s_integral + err;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 分段 PID 计算（500ms 周期）：误差 -> 占空档位 s_duty。
|
||||
*/
|
||||
static void pid_compute(void) {
|
||||
float t = ntc_get_temperature();
|
||||
float t_raw = ntc_get_temperature();
|
||||
|
||||
/* 传感器故障（开路/短路）：安全第一，强制关断并复位 PID */
|
||||
if (t <= -273.0f) {
|
||||
s_duty = 0;
|
||||
pid_reset();
|
||||
return;
|
||||
/* NTC 读数斜率限幅（V0.1.9）+ 大偏差冻结（V0.1.13）：真实热速率
|
||||
<=0.2C/拍，单拍 |Δ|>0.5C 判为读数毛刺，沿限幅值过渡，连续 2 拍超限
|
||||
后——若累计偏差 > HEATER_NTC_JUMP_C（秒级塌陷/卡死事件，真机实测
|
||||
-2.5C/4.5s 塌陷与 +2.3C/10s 顶格，焊接点接触不良）则冻结滤波值
|
||||
不再跟随，直到读数回弹到偏差带内自动恢复。真实温度变化永远不会
|
||||
触发冻结（最大冷却 ~0.05C/拍）。过温方向冻结偏差最大 1.5C，安全。 */
|
||||
float t = t_raw;
|
||||
if (t_raw > -273.0f) {
|
||||
float d_raw = t_raw - s_t_filt;
|
||||
if (d_raw > HEATER_NTC_SLEW_C || d_raw < -HEATER_NTC_SLEW_C) {
|
||||
if (s_slew_cnt < 2U) {
|
||||
s_slew_cnt++;
|
||||
t = s_t_filt + ((d_raw > 0.0f) ? HEATER_NTC_SLEW_C : -HEATER_NTC_SLEW_C);
|
||||
} else if (d_raw > HEATER_NTC_JUMP_C || d_raw < -HEATER_NTC_JUMP_C) {
|
||||
t = s_t_filt; /* 秒级坏值：冻结 */
|
||||
} else {
|
||||
s_slew_cnt = 2U; /* 偏差带内：放行 */
|
||||
}
|
||||
} else {
|
||||
s_slew_cnt = 0U;
|
||||
}
|
||||
s_t_filt = t;
|
||||
} else {
|
||||
s_slew_cnt = 0U;
|
||||
}
|
||||
|
||||
/* 未使能：强制关断 */
|
||||
/* NTC 故障去抖：单拍毛刺（采样/EMI）保持输出不动；连续确认才安全关断。
|
||||
毛刺拍不更新任何 PID 状态（含 s_prev_temp），下拍用真实温差继续。
|
||||
确认沿暂存积分与故障前温度——短故障恢复时无扰还原（见恢复分支）。 */
|
||||
if (t <= -273.0f) {
|
||||
if (s_fault_debounce < 255U) {
|
||||
s_fault_debounce++;
|
||||
}
|
||||
if (s_fault_debounce >= NTC_FAULT_CONFIRM_PID_STEPS) {
|
||||
if (s_fault_debounce == NTC_FAULT_CONFIRM_PID_STEPS) {
|
||||
s_fault_stash_I = s_integral;
|
||||
s_fault_stash_T = s_prev_temp;
|
||||
}
|
||||
s_duty = 0;
|
||||
s_integral = 0.0f;
|
||||
s_duty_acc = 0.0f;
|
||||
s_prev_temp = t;
|
||||
}
|
||||
return;
|
||||
}
|
||||
/* 故障恢复：短故障（<=HEATER_FAULT_RESTORE_MAX_S）且温度未显著漂移
|
||||
（|ΔT|<2℃）时还原暂存积分——避免毛刺清零后稳态功率丢失引发
|
||||
sag->重建->过冲循环；长故障/温度已漂移则从零重建（安全优先） */
|
||||
if (s_fault_debounce >= NTC_FAULT_CONFIRM_PID_STEPS) {
|
||||
float fault_dur_s = (float)s_fault_debounce * (HEATER_PID_PERIOD_MS / 1000.0f);
|
||||
float drift = t - s_fault_stash_T;
|
||||
if (fault_dur_s <= HEATER_FAULT_RESTORE_MAX_S && drift < 2.0f && drift > -2.0f) {
|
||||
s_integral = s_fault_stash_I;
|
||||
}
|
||||
}
|
||||
s_fault_debounce = 0U;
|
||||
|
||||
/* 未使能：强制关断（每拍刷新微分基线，恢复时无假温差） */
|
||||
if (!s_enable) {
|
||||
s_duty = 0;
|
||||
pid_reset();
|
||||
s_integral = 0.0f;
|
||||
s_prev_temp = t;
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -99,52 +155,88 @@ static void pid_compute(void) {
|
||||
}
|
||||
if (s_overtemp) {
|
||||
s_duty = 0;
|
||||
pid_reset();
|
||||
s_integral = 0.0f;
|
||||
s_duty_acc = 0.0f;
|
||||
s_prev_temp = t;
|
||||
return;
|
||||
}
|
||||
|
||||
float err = s_target - t;
|
||||
|
||||
/* 超温区间：执行器只能加热不能制冷，输出必须为 0；
|
||||
积分按负误差回退，为温度回落后的重新加热保留正确初始态 */
|
||||
if (err < 0.0f) {
|
||||
s_integral = integ_step(err);
|
||||
s_prev_err = err;
|
||||
s_duty = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
/* 位置式 PID（带抗积分饱和） */
|
||||
float u = HEATER_PID_KP * err
|
||||
+ HEATER_PID_KD * (err - s_prev_err)
|
||||
+ HEATER_PID_KI * integ_step(err);
|
||||
|
||||
if (u > 100.0f) {
|
||||
u = 100.0f;
|
||||
/* 饱和：err>=0 分支积分不吸收（防 windup） */
|
||||
/* 目标斜坡：内部设定值限速滑向 s_target（升/降同限速）。
|
||||
小步进（斜坡快于温升）全程落在温和带内缓升；大步进爬升段 ramp 超前、
|
||||
到站后误差随实际温度平滑收缩，抑制热滞后过冲。 */
|
||||
float diff = s_target - s_ramp_target;
|
||||
float ramp_step = HEATER_RAMP_CPS * (HEATER_PID_PERIOD_MS / 1000.0f);
|
||||
if (diff > ramp_step) {
|
||||
s_ramp_target += ramp_step;
|
||||
} else if (diff < -ramp_step) {
|
||||
s_ramp_target -= ramp_step;
|
||||
} else {
|
||||
s_integral = integ_step(err);
|
||||
s_ramp_target = s_target;
|
||||
}
|
||||
|
||||
/* 温和带：距目标不足 HEATER_SLOWBAND_C 时，比例/微分即时响应受限
|
||||
（小功率缓升，减少进带蓄热、降低惯性过冲）；积分项不受限——
|
||||
稳态维持功率由积分缓慢顶到实际需要值 */
|
||||
if (err < HEATER_SLOWBAND_C) {
|
||||
float u_pd = HEATER_PID_KP * err + HEATER_PID_KD * (err - s_prev_err);
|
||||
if (u_pd > (float)HEATER_SLOWBAND_DUTY) {
|
||||
u_pd = (float)HEATER_SLOWBAND_DUTY;
|
||||
float err = s_ramp_target - t;
|
||||
float d_t = t - s_prev_temp; /* 本拍温度变化（D on measurement） */
|
||||
|
||||
/* 分区作用（V0.1.12）：近场扩展区 [-BAND, SLOWBAND_C) 合并"带内退闸"
|
||||
与"带外下方"为一条连续减速曲线——u = Ki*I（维持功率，不衰减）+
|
||||
ASSIST 退闸曲线（err=+SLOWBAND_C 满档快爬、err=+BAND 处归零）+ Kd 减速。
|
||||
原结构近场推力真空（P 随 err 缩小消失、I 未成熟，最后 2C 爬 141s），
|
||||
ASSIST 提供与积分成熟度无关的确定推力。远场 err >= SLOWBAND_C：P/D
|
||||
全量 + I。过温缘 err < -BAND：关断 + I 限速回退。 */
|
||||
float u;
|
||||
if (err >= HEATER_SLOWBAND_C) {
|
||||
float u_pd = HEATER_PID_KP * err - HEATER_PID_KD * d_t;
|
||||
if (u_pd > 100.0f) {
|
||||
u_pd = 100.0f;
|
||||
}
|
||||
if (u_pd < 0.0f) {
|
||||
u_pd = 0.0f;
|
||||
}
|
||||
u = u_pd + HEATER_PID_KI * s_integral;
|
||||
if (u < 100.0f) {
|
||||
s_integral += err * HEATER_PID_INTEG_GAIN_IN;
|
||||
}
|
||||
if (u > 100.0f) {
|
||||
u = 100.0f;
|
||||
}
|
||||
} else if (err >= -HEATER_PID_BAND_C) {
|
||||
float assist = (float)HEATER_SLOWBAND_DUTY * (err + HEATER_PID_BAND_C)
|
||||
/ (HEATER_SLOWBAND_C + HEATER_PID_BAND_C);
|
||||
u = HEATER_PID_KI * s_integral + assist - HEATER_PID_KD * d_t;
|
||||
s_integral += err * HEATER_PID_INTEG_GAIN_IN;
|
||||
if (u > (float)HEATER_NEAR_MAX_DUTY) {
|
||||
u = (float)HEATER_NEAR_MAX_DUTY; /* 近场推力上限：快而不过冲 */
|
||||
}
|
||||
if (u < 0.0f) {
|
||||
u = 0.0f;
|
||||
}
|
||||
} else {
|
||||
s_integral -= HEATER_PID_INTEG_BLEED;
|
||||
u = 0.0f;
|
||||
}
|
||||
if (s_integral > HEATER_PID_I_MAX) {
|
||||
s_integral = HEATER_PID_I_MAX;
|
||||
}
|
||||
if (s_integral < 0.0f) {
|
||||
s_integral = 0.0f;
|
||||
}
|
||||
|
||||
/* 输出量化到档位（四舍五入） */
|
||||
s_duty = (uint8_t)((u + (float)HEATER_PWM_STEP_PCT / 2.0f) / (float)HEATER_PWM_STEP_PCT);
|
||||
s_prev_temp = t;
|
||||
|
||||
/* 输出 Bresenham 抖动量化：载波 2s = 4 个 PID 拍，档位余数按拍分摊，
|
||||
平均分辨率 5%/4 = 1.25%。稳态维持功率（实测 ~10-12%）恰落在整数档
|
||||
之间时，固定取整会迫使输出在相邻档间满幅切换（±1℃ 极限环残因）；
|
||||
抖动后 PI 可停在真实需要的平均功率上。脉宽仍为 100ms 整数倍，
|
||||
开关频率仍 0.5Hz，硬件约束不变 */
|
||||
s_duty_acc += u / (float)HEATER_PWM_STEP_PCT;
|
||||
if (s_duty_acc >= (float)(HEATER_PWM_STEPS + 1)) {
|
||||
s_duty_acc = (float)(HEATER_PWM_STEPS + 1);
|
||||
}
|
||||
{
|
||||
uint8_t level = (uint8_t)s_duty_acc; /* u >= 0，截断即 floor */
|
||||
s_duty_acc -= (float)level;
|
||||
s_duty = level;
|
||||
}
|
||||
}
|
||||
|
||||
/******************************************************************************/
|
||||
@@ -164,8 +256,10 @@ void heat_task(void) {
|
||||
if (s_pid_divider >= HEATER_PID_PERIOD_MS) {
|
||||
s_pid_divider = 0U;
|
||||
pid_compute();
|
||||
if (s_duty != prev_duty) {
|
||||
s_pwm_phase_ms = 0U; /* 占空变更：从新周期起点开始，响应即时 */
|
||||
if (s_duty > prev_duty) {
|
||||
s_pwm_phase_ms = 0U; /* 仅升档复位相位：立即起算新 ON 窗口；
|
||||
降档不复位——相位已过新窗口即刻关断，
|
||||
不再额外补发一段 ON 脉冲 */
|
||||
}
|
||||
}
|
||||
|
||||
@@ -183,9 +277,22 @@ void heat_task(void) {
|
||||
void heat_set_enable(bool on) {
|
||||
if (s_enable == on) return; /* 幂等 */
|
||||
s_enable = on;
|
||||
if (!on) {
|
||||
if (on) {
|
||||
/* 从当前实际温度起坡：避免内部目标从 0 跳变造成功率冲击；
|
||||
微分基线/读数滤波基线同步取当前温度，首拍无假温差 */
|
||||
s_ramp_target = ntc_get_temperature();
|
||||
if (s_ramp_target < 0.0f) {
|
||||
s_ramp_target = 0.0f; /* 传感器故障标记值非法，退化为 0 */
|
||||
}
|
||||
s_prev_temp = s_ramp_target;
|
||||
s_t_filt = (s_ramp_target > 0.0f) ? s_ramp_target : 25.0f;
|
||||
s_slew_cnt = 0U;
|
||||
s_fault_debounce = 0U;
|
||||
} else {
|
||||
s_ramp_target = 0.0f;
|
||||
s_duty = 0;
|
||||
pid_reset();
|
||||
s_integral = 0.0f;
|
||||
s_duty_acc = 0.0f;
|
||||
heat_out(false);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -97,11 +97,43 @@ float ntc_read_temperature(void) {
|
||||
static float s_ntc_last_temp = -273.15f; /* 最新温度（初值 = 开路标记） */
|
||||
static uint16_t s_ntc_sample_divider = 0; /* 主循环 10ms 圈分频计数 */
|
||||
|
||||
/* 滑动中值滤波（V0.1.11）：真机读数毛刺可持续 1.4s+（曾抓到 14 帧连续
|
||||
OPEN），会穿透控制环的斜率限幅并触发猛拉。最近 NTC_MEDIAN_N 个样本取
|
||||
中值——单点/短串毛刺被完全剔除，滞后仅 ~0.2s（远小于低通滤波）。
|
||||
故障标记值（-273/-274）参与排序：偶发 OPEN 被中值滤掉，连续 >=3 帧才
|
||||
成为中值（等效又一层去抖），故障判定语义不变。 */
|
||||
#define NTC_MEDIAN_N 5U
|
||||
static float s_ntc_win[NTC_MEDIAN_N];
|
||||
static uint8_t s_ntc_win_idx = 0;
|
||||
static uint8_t s_ntc_win_fill = 0;
|
||||
|
||||
void ntc_sample_task(void) {
|
||||
/* 主循环周期 10ms：按 NTC_SAMPLE_PERIOD_MS 分频触发一次采样 */
|
||||
if (++s_ntc_sample_divider >= (NTC_SAMPLE_PERIOD_MS / 10U)) {
|
||||
s_ntc_sample_divider = 0;
|
||||
s_ntc_last_temp = ntc_read_temperature();
|
||||
|
||||
float v = ntc_read_temperature();
|
||||
s_ntc_win[s_ntc_win_idx] = v;
|
||||
s_ntc_win_idx = (uint8_t)((s_ntc_win_idx + 1U) % NTC_MEDIAN_N);
|
||||
if (s_ntc_win_fill < NTC_MEDIAN_N) {
|
||||
s_ntc_win_fill++;
|
||||
}
|
||||
|
||||
/* 插入排序后取中值（5 点，开销可忽略） */
|
||||
float tmp[NTC_MEDIAN_N];
|
||||
for (uint8_t i = 0; i < s_ntc_win_fill; i++) {
|
||||
tmp[i] = s_ntc_win[i];
|
||||
}
|
||||
for (uint8_t i = 1; i < s_ntc_win_fill; i++) {
|
||||
float key = tmp[i];
|
||||
int8_t j = (int8_t)(i - 1);
|
||||
while (j >= 0 && tmp[j] > key) {
|
||||
tmp[j + 1] = tmp[j];
|
||||
j--;
|
||||
}
|
||||
tmp[j + 1] = key;
|
||||
}
|
||||
s_ntc_last_temp = tmp[s_ntc_win_fill / 2U];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user