hanyongwei
/
TrainCamp_hanyongwei_StageTwo
1
0
派生 0
代码 工单 0 合并请求 0 版本发布 0 百科 动态
浏览代码

修正加速斜率和减速斜率的定义：加速斜率=（脉冲默认速度-0）/默认加速时间；减速斜率=（脉冲默认速度-0）/默认减速时间 

Signed-off-by: hanyongwei <2043702190@qq.com>
master
hanyongwei 1 个月前
父节点
27c686fb81
当前提交
6cc6c31564
共有 6 个文件被更改，包括 310 次插入 和 187 次删除
合并视图
Diff 选项
显示统计 下载 Patch 文件 下载 Diff 文件
  1. +122
    -65
      plsr/accel_curve/plsr_accel_curve.c
  2. +29
    -1
      plsr/accel_curve/plsr_accel_curve.h
  3. +3
    -3
      plsr/param/plsr_param.h
  4. +146
    -117
      plsr/run_control/plsr_run_control.c
  5. +6
    -0
      plsr/signal_io/plsr_signal_io.c
  6. +4
    -1
      plsr/signal_io/plsr_signal_io.h

+ 122
- 65
plsr/accel_curve/plsr_accel_curve.c 查看文件

@@ -98,20 +98,13 @@ uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to,
return f; return f;
} }


static uint32_t PlsrAccelCurveSlopeDen(uint32_t default_spd,
uint32_t primary_ref,
uint32_t fallback_ref)
/**
* 斜率分母：K = 默认速度 / 加减速时间，即 (默认速度 − 0)。
* 起/止速只决定端点，不参与斜率。
*/
static uint32_t PlsrAccelCurveSlopeDen(uint32_t default_spd)
{ {
uint32_t den = PlsrAccelCurveAbsDiff(default_spd, primary_ref);
if (den == 0U)
{
den = PlsrAccelCurveAbsDiff(default_spd, fallback_ref);
}
if (den == 0U)
{
den = default_spd;
}
return den;
return default_spd;
} }


static uint32_t PlsrAccelCurveRampTimeMs(uint32_t f_from, static uint32_t PlsrAccelCurveRampTimeMs(uint32_t f_from,
@@ -127,6 +120,9 @@ static uint32_t PlsrAccelCurveRampTimeMs(uint32_t f_from,
uint32_t ref_ms; uint32_t ref_ms;
uint32_t t; uint32_t t;


(void)start_spd_ref;
(void)end_spd_ref;

if (f_from == f_to) if (f_from == f_to)
{ {
return 0U; return 0U;
@@ -136,12 +132,12 @@ static uint32_t PlsrAccelCurveRampTimeMs(uint32_t f_from,
if (f_to > f_from) if (f_to > f_from)
{ {
ref_ms = accel_ms; ref_ms = accel_ms;
den = PlsrAccelCurveSlopeDen(default_spd, start_spd_ref, end_spd_ref);
den = PlsrAccelCurveSlopeDen(default_spd);
} }
else else
{ {
ref_ms = decel_ms; ref_ms = decel_ms;
den = PlsrAccelCurveSlopeDen(default_spd, end_spd_ref, start_spd_ref);
den = PlsrAccelCurveSlopeDen(default_spd);
} }


if (ref_ms == 0U) if (ref_ms == 0U)
@@ -167,6 +163,100 @@ static uint32_t PlsrAccelCurveRampTimeMs(uint32_t f_from,
return t; return t;
} }


/**
* 从 0 爬到 f_tgt（或从 f_tgt 落到 0）时的起跳/落地频率。
* start/end 参考速都为 0：a = f_tgt*1000/t_ms，f=sqrt(a)；
* 否则走 JumpFreq(0, f_tgt, t_ms)。
*/
static uint32_t PlsrAccelCurveJumpFromZero(uint32_t f_tgt,
uint32_t t_ms,
uint32_t start_spd_ref,
uint32_t end_spd_ref)
{
uint32_t f;

if ((f_tgt < 1U) || (t_ms < 1U))
{
return 1U;
}

if ((start_spd_ref == 0U) && (end_spd_ref == 0U))
{
uint64_t a_hz_s = ((uint64_t)f_tgt * 1000ULL) / (uint64_t)t_ms;
f = (a_hz_s == 0ULL) ? 1U : PlsrAccelCurveIsqrt((uint32_t)a_hz_s);
}
else
{
f = PlsrAccelCurveJumpFreq(0U, f_tgt, t_ms);
}

if (f < 1U)
{
f = 1U;
}
if (f > f_tgt)
{
f = f_tgt;
}
return f;
}

uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg,
uint32_t f_tgt,
uint32_t default_spd,
uint32_t start_spd_ref,
uint32_t end_spd_ref,
uint32_t accel_ms,
uint32_t decel_ms)
{
uint32_t t_acc;

f_cfg = PlsrAccelCurveClampFreq(f_cfg);
f_tgt = PlsrAccelCurveClampFreq(f_tgt);
if (f_cfg >= 1U)
{
return f_cfg;
}
if (f_tgt < 1U)
{
return 1U;
}

t_acc = PlsrAccelCurveRampTimeMs(0U, f_tgt, default_spd,
start_spd_ref, end_spd_ref,
accel_ms, decel_ms);
return PlsrAccelCurveJumpFromZero(f_tgt, t_acc,
start_spd_ref, end_spd_ref);
}

uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg,
uint32_t f_tgt,
uint32_t default_spd,
uint32_t start_spd_ref,
uint32_t end_spd_ref,
uint32_t accel_ms,
uint32_t decel_ms)
{
uint32_t t_dec;

f_cfg = PlsrAccelCurveClampFreq(f_cfg);
f_tgt = PlsrAccelCurveClampFreq(f_tgt);
if (f_cfg >= 1U)
{
return f_cfg;
}
if (f_tgt < 1U)
{
return 1U;
}

t_dec = PlsrAccelCurveRampTimeMs(f_tgt, 0U, default_spd,
start_spd_ref, end_spd_ref,
accel_ms, decel_ms);
return PlsrAccelCurveJumpFromZero(f_tgt, t_dec,
start_spd_ref, end_spd_ref);
}

/** 直线：进度千分比 → 形状千分比 */ /** 直线：进度千分比 → 形状千分比 */
static uint32_t PlsrAccelCurveShapeLinearPermille(uint32_t u) static uint32_t PlsrAccelCurveShapeLinearPermille(uint32_t u)
{ {
@@ -729,6 +819,23 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,


m = (mode > PLSR_ACCEL_SINE) ? PLSR_ACCEL_LINEAR : mode; m = (mode > PLSR_ACCEL_SINE) ? PLSR_ACCEL_LINEAR : mode;


/*
* 端点须由调用方解析完毕。若仍传入 0，用同一套 Resolve 兜底，
* 避免把 0 当真实频率规划出错误斜坡；正常路径不应走到这里。
*/
if (f_cur < 1U)
{
f_cur = PlsrAccelCurveResolveStartHz(0U, f_tgt, default_spd,
start_spd_ref, end_spd_ref,
accel_ms, decel_ms);
}
if (f_end < 1U)
{
f_end = PlsrAccelCurveResolveEndHz(0U, f_tgt, default_spd,
start_spd_ref, end_spd_ref,
accel_ms, decel_ms);
}

f_peak = f_tgt; f_peak = f_tgt;
t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd, t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd,
start_spd_ref, end_spd_ref, start_spd_ref, end_spd_ref,
@@ -737,56 +844,6 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
start_spd_ref, end_spd_ref, start_spd_ref, end_spd_ref,
accel_ms, decel_ms); accel_ms, decel_ms);


/*
* 起速=0：用起跳频率替代 0。
*
* 默认起始/终止速度都为 0 时，按你图里的离散模型：
* a_Hz/s = a_Hz_per_ms * 1000 = (slope_Hz/ms) * 1000
* 其中 slope_Hz/ms 由本段规划斜率参数决定。
*
* 本实现中：当 start_spd_ref==0 && end_spd_ref==0 时，
* 由斜率定义可得 a_Hz/s = default_spd * 1000 / accel_ms（加速段）。
*/
if ((f_cur == 0U) && (f_tgt >= 1U) && (t_acc >= 1U))
{
if ((start_spd_ref == 0U) && (end_spd_ref == 0U) && (accel_ms > 0U) &&
(default_spd > 0U))
{
/* a_Hz/s = default_spd / accel_ms(ms) * 1000 */
uint64_t a_hz_s = ((uint64_t)default_spd * 1000ULL) / (uint64_t)accel_ms;
f_cur = (a_hz_s == 0ULL) ? 1U : PlsrAccelCurveIsqrt((uint32_t)a_hz_s);
}
else
{
/* 兜底：用 df/t_ms 反推加速度再开方 */
f_cur = PlsrAccelCurveJumpFreq(0U, f_tgt, t_acc);
}
if (f_cur > f_tgt)
{
f_cur = f_tgt;
}
}
/* 止速=0：同理，减速末端用起跳频率，避免减速尾部 1Hz 拖尾 */
if ((f_end == 0U) && (f_tgt >= 1U) && (t_dec >= 1U))
{
if ((start_spd_ref == 0U) && (end_spd_ref == 0U) && (decel_ms > 0U) &&
(default_spd > 0U))
{
/* a_Hz/s = default_spd / decel_ms(ms) * 1000 */
uint64_t a_hz_s = ((uint64_t)default_spd * 1000ULL) / (uint64_t)decel_ms;
f_end = (a_hz_s == 0ULL) ? 1U : PlsrAccelCurveIsqrt((uint32_t)a_hz_s);
}
else
{
/* 兜底：用 df/t_ms 反推加速度再开方 */
f_end = PlsrAccelCurveJumpFreq(0U, f_tgt, t_dec);
}
if (f_end > f_tgt)
{
f_end = f_tgt;
}
}

plan->f_cur = f_cur; plan->f_cur = f_cur;
plan->f_end = f_end; plan->f_end = f_end;
plan->mode = m; plan->mode = m;


+ 29
- 1
plsr/accel_curve/plsr_accel_curve.h 查看文件

@@ -4,7 +4,9 @@
* *
* 1ms 改频：脉冲数按 t=0..T-1 实际取频累加；到不了目标频则降峰值走三角（无匀速）。 * 1ms 改频：脉冲数按 t=0..T-1 实际取频累加；到不了目标频则降峰值走三角（无匀速）。
* 匀速仅当加速+减速脉冲之和 < 总脉冲（真正到达目标频）时出现。 * 匀速仅当加速+减速脉冲之和 < 总脉冲（真正到达目标频）时出现。
* 斜率按公共参数：K≈(默认速度−起/止速)/加减速时间；小频差时 T 可能 <1ms，无可见斜坡属正常。
* 斜率按公共参数：K_acc=默认速度/加速时间，K_dec=默认速度/减速时间（减 0，不用起/止速）。
* 小频差时 T 可能 <1ms，无可见斜坡属正常。
* 开段端点由运行层 ResolveStart/EndHz 解析后再传入 Plan（Plan 不解释策略 0）。
*/ */
#ifndef PLSR_ACCEL_CURVE_H #ifndef PLSR_ACCEL_CURVE_H
#define PLSR_ACCEL_CURVE_H #define PLSR_ACCEL_CURVE_H
@@ -41,6 +43,32 @@ uint32_t PlsrAccelCurveClampFreq(uint32_t freq_hz);
uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to, uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to,
uint32_t t_ms); uint32_t t_ms);


/**
* 配置起速 → 规划用真实起点频率。
* f_cfg>=1：原样（钳位）；f_cfg==0：按斜率算起跳频率（从静止爬升）。
* 由运行层在开段前调用；Plan 只吃已经解析好的端点。
*/
uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg,
uint32_t f_tgt,
uint32_t default_spd,
uint32_t start_spd_ref,
uint32_t end_spd_ref,
uint32_t accel_ms,
uint32_t decel_ms);

/**
* 配置止速 → 规划用真实终点频率。
* f_cfg>=1：原样（钳位）；f_cfg==0：按斜率算落地频率（避免 1Hz 拖尾）。
* 段末是否停表由运行层决定，与本函数无关。
*/
uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg,
uint32_t f_tgt,
uint32_t default_spd,
uint32_t start_spd_ref,
uint32_t end_spd_ref,
uint32_t accel_ms,
uint32_t decel_ms);

/** /**
* 斜坡上按 mode 取频（µs 时间轴）。ISR 可用：O(1)。 * 斜坡上按 mode 取频（µs 时间轴）。ISR 可用：O(1)。
* f = f0+(f1-f0)*shape(t/T) * f = f0+(f1-f0)*shape(t/T)


+ 3
- 3
plsr/param/plsr_param.h 查看文件

@@ -101,9 +101,9 @@ typedef struct {
PlsrPosMode_e run_mode; PlsrPosMode_e run_mode;
uint16_t seg_count; uint16_t seg_count;
uint16_t start_seg; /* 1-based */ uint16_t start_seg; /* 1-based */
uint32_t default_speed; /* 脉冲默认速度 Hz：定加减速斜率 K；段频为 0 时也作段目标 */
uint32_t start_speed; /* 起速Hz；0=首拍跟曲线，非0=首拍即起速 */
uint32_t end_speed; /* 止速Hz；0=末拍跟曲线后停表，非0=末拍即止速 */
uint32_t default_speed; /* 脉冲默认速度 Hz：斜率 K=默认速度/加减速时间；段频为 0 时也作段目标 */
uint32_t start_speed; /* 起速Hz；0=从静止起跳（运行层 ResolveStartHz） */
uint32_t end_speed; /* 止速Hz；0=落地频率后停表（运行层 ResolveEndHz） */
uint16_t accel_ms; /* 默认速度加速时间 ms（定加速斜率） */ uint16_t accel_ms; /* 默认速度加速时间 ms（定加速斜率） */
uint16_t decel_ms; /* 默认速度减速时间 ms（定减速斜率） */ uint16_t decel_ms; /* 默认速度减速时间 ms（定减速斜率） */
} PlsrCfg_t; } PlsrCfg_t;


+ 146
- 117
plsr/run_control/plsr_run_control.c 查看文件

@@ -1,24 +1,16 @@
/** /**
* @file plsr_run_control.c * @file plsr_run_control.c
* @brief 完成方式 / 后续方式 运行控制（加减速形状由 accel_mode 决定）
* @brief 运行控制：等待策略 + 开段端点 + 执行驱动
* *
* 分层：
* 1) 调用/等待策略：只决定调用顺序，以及传给段处理的
* 起始频率 f_from、终止频率 f_end、加速/减速时间（斜率基准）
* 2) 段内处理：不感知 wait_type；只按 (f_from,f_tgt,f_end,accel,decel)
* 做加速→匀速→减速（脉冲不够则三角）
* 3) 曲线估加/减速脉冲；匀速=总数−加速−减速。
* 规划反向算 t_decel_start_ms；运行时 TIM3 1ms 按段内时间轴取频（直线为纯代数）。
* 起止≠0：首拍起速、末拍止速；起止=0：首末跟曲线，段末停表。
*
* 典型策略映射：
* - 完成方式：每段 f_end=止速；WAIT 只做段后等待与下一段 f_from（清链→起速）
* - 后续方式末段：f_end=止速；非末段无缝：f_end=下一段目标
* - ACT：先按完成/后续规划加速/匀速/减速时间，再和 act_ms 比较：
* 加速 > ACT → 加速中途打断；加速+匀速 > ACT → 不进减速；
* 加速+匀速 < ACT < 三段总时间 → 减速中途打断；
* ACT ≥ 加速+匀速+减速 → 三阶段（含减速）走完，脉冲照常计数，停表等到 ACT 再下一段（※4）。
* - 梯形/三角仅由 f_from/f_tgt/f_end/斜率/mode/脉冲数决定，不读 wait_type
* 分层（阶段1薄拆分）：
* 1) 曲线（accel_curve）：只吃 (f_from,f_tgt,f_end,斜率,N,mode) → 梯形/三角；
* 不读 wait_type。端点须为真实频率（>=1）；配置起/止=0 由运行层
* ResolveStart/EndHz 算起跳/落地后再传入。
* 2) 等待策略：只决定何时打断/段后等待；统一 handoff：
* - 仍在输出且 freq>=1 → 链式传给下一段
* - 已停表/输出为 0 → 下一段从配置起速（0 则起跳频率）开跑
* 3) 发送模式只影响本段 f_end（完成=止速；后续且不停表=下一段目标）
* 4) PlanSeg / ChangeFreq 共用规划入口，便于后期动态改频
* *
* 【运行节拍】TIM3 1ms：RefreshProfile 改频；脉冲 UPDATE ISR 只计 s_done。 * 【运行节拍】TIM3 1ms：RefreshProfile 改频；脉冲 UPDATE ISR 只计 s_done。
*/ */
@@ -79,7 +71,6 @@ static uint8_t s_pwm_on;
/* 本段由策略层解析后的加减速时间（段内只用这两份，不再读 wait） */ /* 本段由策略层解析后的加减速时间（段内只用这两份，不再读 wait） */
static uint16_t s_run_accel_ms; static uint16_t s_run_accel_ms;
static uint16_t s_run_decel_ms; static uint16_t s_run_decel_ms;
static uint8_t s_orig_end_zero; /* 原始止速为 0（起跳频率已替代 plan.f_end） */


/** 策略层交给段处理的接口参数 */ /** 策略层交给段处理的接口参数 */
typedef struct { typedef struct {
@@ -138,10 +129,15 @@ static void PlsrRunControlArmActExtOnPulseStart(void)
} }
} }


/** EXT 中途切段：停表后立即按 jump 跳转 */
/**
* EXT 触发切段（脉冲可能还在跑）。
* 同向且 PWM 在跑：不停表，下一段 f_from = 当前频率（衔接加速）。
* 反向或已停表：停表，下一段从起速爬。
*/
static void PlsrRunControlCutSegToNext(void) static void PlsrRunControlCutSegToNext(void)
{ {
PlsrCfg_t *cfg = PlsrParamGetCfg();
int16_t next0;
uint8_t keep;


s_state = RC_IDLE; s_state = RC_IDLE;
s_act_armed = 0U; s_act_armed = 0U;
@@ -149,26 +145,40 @@ static void PlsrRunControlCutSegToNext(void)
s_act_cut_pending = 0U; s_act_cut_pending = 0U;
s_act_handoff_keep = 0U; s_act_handoff_keep = 0U;
s_wait_expire_req = 0U; s_wait_expire_req = 0U;
PlsrPulseDriverStop();
s_follow_cont = 0U;
s_pwm_on = 0U;
if (cfg->send_mode == PLSR_SEND_COMPLETE)

next0 = PlsrPathPlanResolveAfterSeg(s_cur_seg);
keep = 0U;
if ((next0 >= 0) && (s_pwm_on != 0U) && (s_cur_freq >= 1U) &&
(PlsrPathPlanIsForward((uint16_t)next0, s_acc_pulse) == s_forward))
{ {
s_chain_freq = s_cur_freq;
keep = 1U;
}

if (keep != 0U)
{
PlsrPulseDriverClearOnePulseStop();
s_follow_cont = 1U;
s_chain_valid = 1U; s_chain_valid = 1U;
s_chain_freq = s_cur_freq;
} }
else else
{ {
PlsrPulseDriverStop();
PlsrPulseDriverClearOnePulseStop();
s_follow_cont = 0U;
s_pwm_on = 0U;
s_cur_freq = 0U; s_cur_freq = 0U;
s_chain_valid = 0U; s_chain_valid = 0U;
s_chain_freq = 0U;
} }

PlsrRunControlGotoNextOrFinish(s_cur_seg); PlsrRunControlGotoNextOrFinish(s_cur_seg);
} }


/** /**
* ACT 时间到（脉冲可能还没发完）：从当前频率切下一段。
* 同向：不停表，下一段 f_from=当前频率（※2 加速相 / ※3 减速相）。
* 反向或无 PWM：停表，下一段从起速爬。
* ACT 时间到（脉冲可能还没发完）：丢掉本段剩余脉冲/减速，从当前频率切下一段。
* 同向：下一段 f_from=当前频率（逐层跳，不进本段减速）。
* 反向或无频率：停表，下一段从起速爬。
*/ */
static void PlsrRunControlActExpire(void) static void PlsrRunControlActExpire(void)
{ {
@@ -220,7 +230,7 @@ static void PlsrRunControlEnterPostWaitOrNext(void)


/* /*
* 调用顺序策略（与段内波形无关）： * 调用顺序策略（与段内波形无关）：
* WAIT时间/信号 → 停表清链后等待，再 BeginSeg（下一段 f_from=起速）
* WAIT时间/信号 → 停表清链后等待，再 BeginSeg（下一段从起速/起跳开跑）
* 其它 → 立即跳转下一段 * 其它 → 立即跳转下一段
*/ */
if (seg->wait_type == PLSR_WAIT_TIME) if (seg->wait_type == PLSR_WAIT_TIME)
@@ -259,6 +269,31 @@ static void PlsrRunControlEnterPostWaitOrNext(void)
return; return;
} }


if ((seg->wait_type == PLSR_WAIT_EXT) ||
(seg->wait_type == PLSR_WAIT_EXT_OR_DONE))
{
/*
* EXT 脉冲发完后等待：停表，输出已经是 0。
* 下一段从配置起速（0 则起跳）重新规划；不是接着本段 f_end 往下跑。
*/
PlsrPulseDriverStop();
s_follow_cont = 0U;
s_pwm_on = 0U;
s_chain_valid = 0U;
s_chain_freq = 0U;
s_cur_freq = 0U;
/* 只清消抖中的假沿；已确认的 fell 保留，供下面 TakeExtFalling 消费 */
PlsrSignalIoClearPending();
if (PlsrSignalIoTakeExtFalling() != 0U)
{
PlsrRunControlGotoNextOrFinish(s_cur_seg);
return;
}
s_wait_is_signal = 1U;
s_state = RC_WAIT_COND;
return;
}

PlsrRunControlGotoNextOrFinish(s_cur_seg); PlsrRunControlGotoNextOrFinish(s_cur_seg);
} }


@@ -326,8 +361,8 @@ static void PlsrRunControlAfterSegDone(void)


if (cfg->send_mode == PLSR_SEND_COMPLETE) if (cfg->send_mode == PLSR_SEND_COMPLETE)
{ {
/* 段正常结束：下一段起速 = 本段止速 f_end，不能用峰值 s_cur_freq */
s_chain_freq = (s_orig_end_zero != 0U) ? 0U : s_accel_plan.f_end;
/* 段正常结束：下一段起速 = 本段实际止速 plan.f_end，不能用峰值 s_cur_freq */
s_chain_freq = s_accel_plan.f_end;
s_chain_valid = 1U; s_chain_valid = 1U;
} }


@@ -356,7 +391,13 @@ static uint8_t PlsrRunControlBlocksFollowKeep(uint16_t cur_seg)
{ {
PlsrSeg_t *seg = PlsrParamGetSeg(cur_seg); PlsrSeg_t *seg = PlsrParamGetSeg(cur_seg);


/* WAIT时间(含0→1ms)、WAIT信号、ACT/EXT：均禁止无缝衔接 */
/*
* WAIT时间/信号/ACT：禁止后续无缝衔接（发完后停表等条件）。
* EXT/EXT_OR_DONE：脉冲发完之前 EXT 可能尚未触发，发完后进等待态，
* 此时 PWM 必须停；所以也禁止无缝衔接。
* （EXT 触发时若脉冲还在跑，由 CutSegToNext 判断 keep，
* 那条路不走 BlocksFollowKeep。）
*/
if ((seg->wait_type == PLSR_WAIT_TIME) || if ((seg->wait_type == PLSR_WAIT_TIME) ||
(seg->wait_type == PLSR_WAIT_SIGNAL) || (seg->wait_type == PLSR_WAIT_SIGNAL) ||
(seg->wait_type == PLSR_WAIT_ACT) || (seg->wait_type == PLSR_WAIT_ACT) ||
@@ -385,38 +426,60 @@ static uint8_t PlsrRunControlResolveSegCallIo(uint16_t seg_0, uint32_t f_tgt,
io->accel_ms = cfg->accel_ms; io->accel_ms = cfg->accel_ms;
io->decel_ms = cfg->decel_ms; io->decel_ms = cfg->decel_ms;


/* ---------- 起始频率：由衔接状态决定（WAIT 会清链 → 起速） ---------- */
if (s_follow_cont != 0U)
/* ---------- 起始频率 ----------
* 中途切段（ACT/EXT）会置 follow_cont：用打断瞬间的频率衔接下一段。
* ACT 为了对齐脉冲边界可能先停表，此时 pwm_on=0，但仍要跟当前频率，
* 不能当成「段后停表」去走起跳。
* 真正段后停表：follow_cont/chain 已清，从配置起速（0 则起跳）开跑。
*/
if ((s_follow_cont != 0U) && (s_cur_freq >= 1U))
{ {
io->f_from = s_cur_freq; io->f_from = s_cur_freq;
} }
else if ((cfg->send_mode == PLSR_SEND_COMPLETE) && (s_chain_valid != 0U))
else if ((s_pwm_on != 0U) && (s_cur_freq >= 1U))
{
io->f_from = s_cur_freq;
}
else if ((s_chain_valid != 0U) && (s_chain_freq >= 1U) &&
((s_pwm_on != 0U) || (s_follow_cont != 0U)))
{ {
io->f_from = s_chain_freq; io->f_from = s_chain_freq;
} }
else else
{ {
io->f_from = PlsrRunControlClampSpeed(cfg->start_speed);
io->f_from = PlsrAccelCurveResolveStartHz(cfg->start_speed,
f_tgt,
cfg->default_speed,
cfg->start_speed,
cfg->end_speed,
io->accel_ms,
io->decel_ms);
} }


/* /*
* ---------- 终止频率：仅由发送模式 + 是否有下一段决定 ----------
* 完成方式 / 真末段：公共止速
* 后续方式非末段：下一段目标（无缝衔接）
* ACT 只由运行控制层管理，不改变本段曲线的 f_end 或阶段规划。
* ---------- 终止频率 ----------
* 完成方式 / 真末段 / 发完后要停表等（WAIT时间/信号）：本段规划到止速。
* ACT/EXT 中途打断时剩余减速本来就会丢弃，不影响「进不进减速」。
* 后续方式且无门禁：过渡到下一段目标。
*/ */
(void)f_tgt;
next0 = PlsrPathPlanNextSeg(seg_0); next0 = PlsrPathPlanNextSeg(seg_0);
end_at_stop = (next0 < 0) ? 1U : 0U; end_at_stop = (next0 < 0) ? 1U : 0U;


if (cfg->send_mode == PLSR_SEND_COMPLETE)
if ((cfg->send_mode == PLSR_SEND_COMPLETE) ||
(PlsrRunControlBlocksFollowKeep(seg_0) != 0U))
{ {
end_at_stop = 1U; end_at_stop = 1U;
} }


if (end_at_stop != 0U) if (end_at_stop != 0U)
{ {
io->f_end = PlsrRunControlClampSpeed(cfg->end_speed);
io->f_end = PlsrAccelCurveResolveEndHz(cfg->end_speed,
f_tgt,
cfg->default_speed,
cfg->start_speed,
cfg->end_speed,
io->accel_ms,
io->decel_ms);
return 1U; return 1U;
} }


@@ -619,30 +682,23 @@ static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses)
* （拉长会在尾部堆 1Hz 空耗）。 * （拉长会在尾部堆 1Hz 空耗）。
*/ */
if ((old_t_dec > 0U) && (old_dec_n > 0U) && if ((old_t_dec > 0U) && (old_dec_n > 0U) &&
((remain_pulses == old_dec_n) || (s_orig_end_zero != 0U)))
(remain_pulses == old_dec_n))
{ {
s_accel_plan.t_dec_ms = old_t_dec; s_accel_plan.t_dec_ms = old_t_dec;
return; return;
} }


/* 参数斜率决定的理想减速时间（本段 accel_ms/decel_ms） */
/* 参数斜率：K = 默认速度 / 加减速时间（减 0） */
df = (from_hz > f_end) ? (from_hz - f_end) : (f_end - from_hz); df = (from_hz > f_end) ? (from_hz - f_end) : (f_end - from_hz);
if (f_end > from_hz) if (f_end > from_hz)
{ {
ref_ms = s_run_accel_ms; ref_ms = s_run_accel_ms;
den = (cfg->default_speed > cfg->start_speed) ?
(cfg->default_speed - cfg->start_speed) :
((cfg->default_speed > cfg->end_speed) ?
(cfg->default_speed - cfg->end_speed) : cfg->default_speed);
} }
else else
{ {
ref_ms = s_run_decel_ms; ref_ms = s_run_decel_ms;
den = (cfg->default_speed > cfg->end_speed) ?
(cfg->default_speed - cfg->end_speed) :
((cfg->default_speed > cfg->start_speed) ?
(cfg->default_speed - cfg->start_speed) : cfg->default_speed);
} }
den = cfg->default_speed;
if (den == 0U) if (den == 0U)
{ {
den = df; den = df;
@@ -656,15 +712,8 @@ static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses)
} }
} }


if (s_orig_end_zero != 0U)
{
s_accel_plan.t_dec_ms = (t_slope > 0U) ? t_slope :
((old_t_dec > 0U) ? old_t_dec : 1U);
return;
}

/* /*
* 止速≠0：ISR 内禁止 FitRamp/EstimatePulses；用 O(1) 平均公式。
* ISR 内禁止 FitRamp/EstimatePulses；用 O(1) 平均公式。
* 脉冲被截断导致 remain 变化时才走这里。 * 脉冲被截断导致 remain 变化时才走这里。
*/ */
sum_f = from_hz + f_end; sum_f = from_hz + f_end;
@@ -706,25 +755,7 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start)
next = PlsrAccelCurveFreqAtSegTime(&s_accel_plan, s_seg_elapsed_ms); next = PlsrAccelCurveFreqAtSegTime(&s_accel_plan, s_seg_elapsed_ms);
if (next < 1U) if (next < 1U)
{ {
if ((s_orig_end_zero != 0U) && (s_seg_elapsed_ms > 0U))
{
/*
* 原始止速为 0，时间轴走完。剩余脉冲仍要发出并计数（ACT※4：
* 三阶段走完后等时间，不能把 s_target 截掉导致计数丢失）。
*/
if (s_done < s_target)
{
next = 1U;
}
else
{
return;
}
}
else
{
next = 1U;
}
next = 1U;
} }


if ((next != s_cur_freq) || (do_start != 0U) || (s_pwm_on == 0U)) if ((next != s_cur_freq) || (do_start != 0U) || (s_pwm_on == 0U))
@@ -738,17 +769,17 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
uint16_t accel_ms, uint16_t decel_ms) uint16_t accel_ms, uint16_t decel_ms)
{ {
PlsrCfg_t *cfg = PlsrParamGetCfg(); PlsrCfg_t *cfg = PlsrParamGetCfg();
uint32_t f_start;
uint32_t f_lo; uint32_t f_lo;
uint32_t f_hi; uint32_t f_hi;


/* 段内只吃传入端点与加减速时间，不感知 wait */ /* 段内只吃传入端点与加减速时间，不感知 wait */
s_run_accel_ms = accel_ms; s_run_accel_ms = accel_ms;
s_run_decel_ms = decel_ms; s_run_decel_ms = decel_ms;
s_orig_end_zero = (f_end == 0U) ? 1U : 0U;


/* /*
* 曲线估 acc_n / dec_n，const_n = total − acc − dec。 * 曲线估 acc_n / dec_n，const_n = total − acc − dec。
* 起止≠0：运行时首拍=起速、末拍=止速；起止=0：首末跟曲线，段末停表。
* f_from/f_end 已由策略层解析为真实频率；配置起止=0 时已换成起跳/落地。
*/ */
PlsrAccelCurvePlan(&s_accel_plan, PlsrAccelCurvePlan(&s_accel_plan,
total, total,
@@ -762,6 +793,9 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
decel_ms, decel_ms,
cfg->accel_mode); cfg->accel_mode);


/* 运行层使用规划后的实际起始频率。 */
f_start = s_accel_plan.f_cur;

/* /*
* 1ms 改频模式：运行时按 TIM3 毫秒时间轴取频。 * 1ms 改频模式：运行时按 TIM3 毫秒时间轴取频。
* 禁止在这里用脉冲递推模型二次拟合 T（FitRamp），否则规划与运行模型不一致， * 禁止在这里用脉冲递推模型二次拟合 T（FitRamp），否则规划与运行模型不一致，
@@ -771,15 +805,15 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
/* 本段频率范围锁 PSC，升降只改 ARR；跨度过大则 LockPscRange 自动不锁 */ /* 本段频率范围锁 PSC，升降只改 ARR；跨度过大则 LockPscRange 自动不锁 */
f_lo = s_accel_plan.f_tgt; f_lo = s_accel_plan.f_tgt;
f_hi = s_accel_plan.f_tgt; f_hi = s_accel_plan.f_tgt;
if (f_from > f_hi)
if (f_start > f_hi)
{ {
f_hi = f_from;
f_hi = f_start;
} }
if (f_from >= 1U)
if (f_start >= 1U)
{ {
if (f_from < f_lo)
if (f_start < f_lo)
{ {
f_lo = f_from;
f_lo = f_start;
} }
} }
else else
@@ -820,11 +854,11 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
PlsrPulseDriverLockPscRange(f_lo, f_hi); PlsrPulseDriverLockPscRange(f_lo, f_hi);
} }


s_approach_from = f_from;
s_approach_from = f_start;
s_decel_from = s_accel_plan.f_tgt; s_decel_from = s_accel_plan.f_tgt;
if (s_pwm_on == 0U) if (s_pwm_on == 0U)
{ {
s_cur_freq = f_from;
s_cur_freq = f_start;
} }


/* /*
@@ -839,17 +873,17 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
if ((s_accel_plan.dec_n >= total) && if ((s_accel_plan.dec_n >= total) &&
(total > 0U) && (total > 0U) &&
(s_accel_plan.f_end != s_accel_plan.f_tgt) && (s_accel_plan.f_end != s_accel_plan.f_tgt) &&
(f_from >= s_accel_plan.f_tgt))
(f_start >= s_accel_plan.f_tgt))
{ {
PlsrRunControlEnterDecel(f_from, total);
PlsrRunControlEnterDecel(f_start, total);
} }
else if (f_from == s_accel_plan.f_tgt)
else if (f_start == s_accel_plan.f_tgt)
{ {
PlsrRunControlEnterPhase(PH_CONST, f_from);
PlsrRunControlEnterPhase(PH_CONST, f_start);
} }
else else
{ {
PlsrRunControlEnterPhase(PH_APPROACH, f_from);
PlsrRunControlEnterPhase(PH_APPROACH, f_start);
} }
s_seg_elapsed_ms = 0U; s_seg_elapsed_ms = 0U;
} }
@@ -990,7 +1024,6 @@ void PlsrRunControlInit(void)
s_chain_valid = 0U; s_chain_valid = 0U;
s_run_accel_ms = 0U; s_run_accel_ms = 0U;
s_run_decel_ms = 0U; s_run_decel_ms = 0U;
s_orig_end_zero = 0U;
s_phase = PH_CONST; s_phase = PH_CONST;
s_seg_elapsed_ms = 0U; s_seg_elapsed_ms = 0U;
s_approach_from = 0U; s_approach_from = 0U;
@@ -1143,9 +1176,21 @@ void PlsrRunControlTickMs(void)
{ {
if (s_wait_is_signal != 0U) if (s_wait_is_signal != 0U)
{ {
if (PlsrSignalIoTakeWaitFalling() != 0U)
seg = PlsrParamGetSeg(s_cur_seg);
if ((seg->wait_type == PLSR_WAIT_EXT) ||
(seg->wait_type == PLSR_WAIT_EXT_OR_DONE))
{ {
PlsrRunControlGotoNextOrFinish(s_cur_seg);
if (PlsrSignalIoTakeExtFalling() != 0U)
{
PlsrRunControlGotoNextOrFinish(s_cur_seg);
}
}
else
{
if (PlsrSignalIoTakeWaitFalling() != 0U)
{
PlsrRunControlGotoNextOrFinish(s_cur_seg);
}
} }
} }
/* WAIT 时间 / ACT 剩余到期在 On1ms */ /* WAIT 时间 / ACT 剩余到期在 On1ms */
@@ -1197,21 +1242,6 @@ void PlsrRunControlOn1ms(void)
return; return;
} }


if ((s_orig_end_zero != 0U) &&
(s_accel_plan.mode != PLSR_ACCEL_LINEAR) &&
(s_accel_plan.t_dec_ms >= 1U) &&
(s_seg_elapsed_ms >=
(s_accel_plan.t_decel_start_ms + s_accel_plan.t_dec_ms)))
{
/* 非直线：时间轴走完减速后，剩余脉冲仍按最后频率发完并计数 */
if ((s_done >= s_target) &&
(PlsrRunControlWillFollowKeep(s_cur_seg) == 0U))
{
PlsrPulseDriverArmOnePulseStop();
}
return;
}

PlsrRunControlRefreshProfile(0U); PlsrRunControlRefreshProfile(0U);
} }


@@ -1234,14 +1264,13 @@ void PlsrRunControlOnPulseIsr(void)
} }


/* /*
* ACT 到期后的切段只在脉冲边界执行。当前 UPDATE 已完成本段
* 最后一个完整脉冲，立即停表，任务再用当前频率衔接下一段。
* ACT 到期切段只在脉冲边界执行，避免半个 PWM 周期改段。
* 同向衔接：不停表，任务里用当前频率规划下一段（丢掉本段剩余）。
* 停表会把 pwm_on 清掉，Resolve 若再要求 pwm_on 就会误走起跳。
*/ */
if (s_act_cut_pending != 0U) if (s_act_cut_pending != 0U)
{ {
s_act_cut_pending = 0U; s_act_cut_pending = 0U;
PlsrPulseDriverStop();
s_pwm_on = 0U;
s_act_handoff_keep = 1U; s_act_handoff_keep = 1U;
s_state = RC_IDLE; s_state = RC_IDLE;
s_act_expire_req = 1U; s_act_expire_req = 1U;


+ 6
- 0
plsr/signal_io/plsr_signal_io.c 查看文件

@@ -174,6 +174,12 @@ void PlsrSignalIoClearEdges(void)
s_db_x5.pending = 0U; s_db_x5.pending = 0U;
} }


void PlsrSignalIoClearPending(void)
{
s_db_x4.pending = 0U;
s_db_x5.pending = 0U;
}

static PlsrEdge_t *PlsrSignalIoDbBySel(uint16_t sel_01) static PlsrEdge_t *PlsrSignalIoDbBySel(uint16_t sel_01)
{ {
return (sel_01 != 0U) ? &s_db_x5 : &s_db_x4; return (sel_01 != 0U) ? &s_db_x5 : &s_db_x4;


+ 4
- 1
plsr/signal_io/plsr_signal_io.h 查看文件

@@ -21,9 +21,12 @@ uint32_t PlsrSignalIoDeadlineFromNow(uint32_t ms);
/** 1=已到或过期（含无符号环绕） */ /** 1=已到或过期（含无符号环绕） */
uint8_t PlsrSignalIoMsReached(uint32_t deadline_ms); uint8_t PlsrSignalIoMsReached(uint32_t deadline_ms);


/** 清 WAIT/EXT 下降沿锁存 */
/** 清 WAIT/EXT 下降沿锁存（包含已确认的 fell） */
void PlsrSignalIoClearEdges(void); void PlsrSignalIoClearEdges(void);


/** 只清消抖 pending，保留已确认的 fell（发完后过滤脉冲耦合假沿用） */
void PlsrSignalIoClearPending(void);

/** WAIT 口当前是否为低（已接地）。仅段末判断※2 时读一次，不作周期轮询触发 */ /** WAIT 口当前是否为低（已接地）。仅段末判断※2 时读一次，不作周期轮询触发 */
uint8_t PlsrSignalIoWaitIsOn(void); uint8_t PlsrSignalIoWaitIsOn(void);




撰写 预览
正在加载...
取消
保存