hanyongwei
/
TrainCamp_hanyongwei_StageTwo
1
0
Fork 0
Code Issues 0 Pull Requests 0 Releases 0 Wiki Activity
Browse Source

修复ACT时间切段异常；增加默认起始速度/默认终止速度为0时的起跳频率计算，使用起跳频率=根号下加速斜率，加速斜率采用Hz/s 

Signed-off-by: hanyongwei <2043702190@qq.com>
master
hanyongwei 1 month ago
parent
ffe0c8b2e6
commit
27c686fb81
3 changed files with 189 additions and 76 deletions
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +126
    -3
      plsr/accel_curve/plsr_accel_curve.c
  2. +8
    -0
      plsr/accel_curve/plsr_accel_curve.h
  3. +55
    -73
      plsr/run_control/plsr_run_control.c

+ 126
- 3
plsr/accel_curve/plsr_accel_curve.c View File

@@ -26,6 +26,78 @@ static uint32_t PlsrAccelCurveAbsDiff(uint32_t a, uint32_t b)
return (a >= b) ? (a - b) : (b - a);
}

/**
* 整数平方根（牛顿法）。
*/
static uint32_t PlsrAccelCurveIsqrt(uint32_t val)
{
uint32_t x;
uint32_t x2;
uint32_t i;

if (val <= 1U)
{
return val;
}
x = val;
if (x > 10000U)
{
x = 10000U;
}
for (i = 0U; i < 30U; i++)
{
x2 = (x + val / x) / 2U;
if (x2 >= x)
{
break;
}
x = x2;
}
return x;
}

/**
* 起跳频率：f_jump = sqrt(a_Hz_per_s)。
* 加速度 a = df * 1000 / t_ms（Hz/s），df = |f_to - f_from|。
* 当 t_ms=0 或 df=0 时返回 1。
*/
uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to,
uint32_t t_ms)
{
uint32_t df;
uint32_t a_hz_s;
uint32_t f;

if (t_ms == 0U)
{
return 1U;
}
df = PlsrAccelCurveAbsDiff(f_from, f_to);
if (df == 0U)
{
return 1U;
}
/* a = df / (t_ms / 1000) = df * 1000 / t_ms，可能很大 */
if (df <= 4294967U)
{
a_hz_s = (df * 1000U) / t_ms;
}
else
{
a_hz_s = df / t_ms * 1000U;
}
if (a_hz_s == 0U)
{
return 1U;
}
f = PlsrAccelCurveIsqrt(a_hz_s);
if (f < 1U)
{
f = 1U;
}
return f;
}

static uint32_t PlsrAccelCurveSlopeDen(uint32_t default_spd,
uint32_t primary_ref,
uint32_t fallback_ref)
@@ -656,9 +728,6 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
f_end = PlsrAccelCurveClampFreq(f_end);

m = (mode > PLSR_ACCEL_SINE) ? PLSR_ACCEL_LINEAR : mode;
plan->f_cur = f_cur;
plan->f_end = f_end;
plan->mode = m;

f_peak = f_tgt;
t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd,
@@ -667,6 +736,60 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
t_dec = PlsrAccelCurveRampTimeMs(f_peak, f_end, default_spd,
start_spd_ref, end_spd_ref,
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_end = f_end;
plan->mode = m;
acc_n = PlsrAccelCurveEstimatePulses(f_cur, f_peak, t_acc, m);
dec_n = PlsrAccelCurveEstimatePulses(f_peak, f_end, t_dec, m);



+ 8
- 0
plsr/accel_curve/plsr_accel_curve.h View File

@@ -33,6 +33,14 @@ typedef enum {

uint32_t PlsrAccelCurveClampFreq(uint32_t freq_hz);

/**
* 起跳频率：f_jump = sqrt(加速度_Hz/s)。
* 起速/止速为 0 时，第一个脉冲（或最后一个脉冲）应以此频率输出，
* 使脉冲周期恰好等于加速度产生该频率所需的时间。
*/
uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to,
uint32_t t_ms);

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


+ 55
- 73
plsr/run_control/plsr_run_control.c View File

@@ -60,6 +60,8 @@ static uint16_t s_act_time_ms; /* 本段 ACT 时长（出脉冲后计） */
static uint32_t s_act_arm_ms; /* 武装时刻的 TIM3 s_ms，与改频 elapsed 脱钩 */
static volatile uint8_t s_act_armed;
static volatile uint8_t s_act_expire_req; /* TIM3 置位，任务里切段（避免 ISR 里 PlanSeg） */
static volatile uint8_t s_act_cut_pending; /* ACT 到期，等待当前脉冲 UPDATE 边界 */
static volatile uint8_t s_act_handoff_keep; /* 边界停表后，下一段仍从当前频率衔接 */
static volatile uint8_t s_wait_expire_req; /* WAIT 时间到：ISR 只置位 */
static uint8_t s_wait_is_signal; /* RC_WAIT_COND：1=等 WAIT 沿，0=等时间 */

@@ -77,6 +79,7 @@ static uint8_t s_pwm_on;
/* 本段由策略层解析后的加减速时间（段内只用这两份，不再读 wait） */
static uint16_t s_run_accel_ms;
static uint16_t s_run_decel_ms;
static uint8_t s_orig_end_zero; /* 原始止速为 0（起跳频率已替代 plan.f_end） */

/** 策略层交给段处理的接口参数 */
typedef struct {
@@ -103,7 +106,6 @@ static uint16_t PlsrRunControlWaitTimeMs(const PlsrSeg_t *seg);
static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
uint32_t f_tgt, uint32_t f_end,
uint16_t accel_ms, uint16_t decel_ms);
static void PlsrRunControlApplyActTimeCap(uint16_t act_ms);
static uint32_t PlsrRunControlClampSpeed(uint32_t spd);
static uint8_t PlsrRunControlGetSegTargetFreq(int32_t freq_hz,
uint32_t default_spd,
@@ -124,6 +126,8 @@ static void PlsrRunControlArmActExtOnPulseStart(void)
}
s_act_armed = 0U;
s_act_expire_req = 0U;
s_act_cut_pending = 0U;
s_act_handoff_keep = 0U;
s_wait_expire_req = 0U;
if (seg->wait_type == PLSR_WAIT_ACT)
{
@@ -142,6 +146,8 @@ static void PlsrRunControlCutSegToNext(void)
s_state = RC_IDLE;
s_act_armed = 0U;
s_act_expire_req = 0U;
s_act_cut_pending = 0U;
s_act_handoff_keep = 0U;
s_wait_expire_req = 0U;
PlsrPulseDriverStop();
s_follow_cont = 0U;
@@ -176,7 +182,9 @@ static void PlsrRunControlActExpire(void)

next0 = PlsrPathPlanResolveAfterSeg(s_cur_seg);
keep = 0U;
if ((next0 >= 0) && (s_pwm_on != 0U) && (s_cur_freq >= 1U) &&
if ((next0 >= 0) &&
((s_pwm_on != 0U) || (s_act_handoff_keep != 0U)) &&
(s_cur_freq >= 1U) &&
(PlsrPathPlanIsForward((uint16_t)next0, s_acc_pulse) == s_forward))
{
keep = 1U;
@@ -200,6 +208,8 @@ static void PlsrRunControlActExpire(void)
s_chain_freq = 0U;
}

s_act_handoff_keep = 0U;

PlsrRunControlGotoNextOrFinish(s_cur_seg);
}

@@ -317,7 +327,7 @@ static void PlsrRunControlAfterSegDone(void)
if (cfg->send_mode == PLSR_SEND_COMPLETE)
{
/* 段正常结束：下一段起速 = 本段止速 f_end，不能用峰值 s_cur_freq */
s_chain_freq = s_accel_plan.f_end;
s_chain_freq = (s_orig_end_zero != 0U) ? 0U : s_accel_plan.f_end;
s_chain_valid = 1U;
}

@@ -393,7 +403,7 @@ static uint8_t PlsrRunControlResolveSegCallIo(uint16_t seg_0, uint32_t f_tgt,
* ---------- 终止频率：仅由发送模式 + 是否有下一段决定 ----------
* 完成方式 / 真末段：公共止速
* 后续方式非末段：下一段目标（无缝衔接）
* ACT 不在这里改 f_end：先按梯形算出加/匀/减时间，再 ApplyActTimeCap。
* ACT 只由运行控制层管理，不改变本段曲线的 f_end 或阶段规划。
*/
(void)f_tgt;
next0 = PlsrPathPlanNextSeg(seg_0);
@@ -609,7 +619,7 @@ static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses)
* （拉长会在尾部堆 1Hz 空耗）。
*/
if ((old_t_dec > 0U) && (old_dec_n > 0U) &&
((remain_pulses == old_dec_n) || (f_end == 0U)))
((remain_pulses == old_dec_n) || (s_orig_end_zero != 0U)))
{
s_accel_plan.t_dec_ms = old_t_dec;
return;
@@ -646,7 +656,7 @@ static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses)
}
}

if (f_end == 0U)
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);
@@ -696,10 +706,10 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start)
next = PlsrAccelCurveFreqAtSegTime(&s_accel_plan, s_seg_elapsed_ms);
if (next < 1U)
{
if ((s_accel_plan.f_end == 0U) && (s_seg_elapsed_ms > 0U))
if ((s_orig_end_zero != 0U) && (s_seg_elapsed_ms > 0U))
{
/*
* 时间轴已落到止速 0。剩余脉冲仍要发出并计数（ACT※4：
* 原始止速为 0，时间轴走完。剩余脉冲仍要发出并计数（ACT※4：
* 三阶段走完后等时间，不能把 s_target 截掉导致计数丢失）。
*/
if (s_done < s_target)
@@ -734,6 +744,7 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
/* 段内只吃传入端点与加减速时间，不感知 wait */
s_run_accel_ms = accel_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。
@@ -843,41 +854,6 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
s_seg_elapsed_ms = 0U;
}

/**
* 按 ACT 时间裁剪本段时间轴（规划已含加速/匀速/减速）：
* - T_acc > ACT → 加速未到目标频就被打断
* - T_acc+T_const > ACT → 匀速期内打断，不进入减速
* - T_acc+T_const < ACT < 三段总长 → 进入减速，可能被中途打断
* - ACT ≥ 加速+匀速+减速 → 三阶段走完（含减速到止速），脉冲发完后等 ACT（※4）
*/
static void PlsrRunControlApplyActTimeCap(uint16_t act_ms)
{
uint32_t t_act;
uint32_t t_acc;
uint32_t t_const_end;
uint32_t t_wave;

t_act = (act_ms == 0U) ? 1U : (uint32_t)act_ms;
t_acc = s_accel_plan.t_acc_ms;
t_const_end = s_accel_plan.t_decel_start_ms;
if (t_const_end < t_acc)
{
t_const_end = t_acc;
}
t_wave = t_const_end + s_accel_plan.t_dec_ms;

if ((t_act >= t_wave) || (t_act > t_const_end))
{
/* 进入减速，或 ACT 比三段总时间还长：保留减速规划 */
return;
}

/* 加速中途或匀速期内就会切段：时间轴上不再走减速 */
s_accel_plan.t_dec_ms = 0U;
s_accel_plan.dec_n = 0U;
s_accel_plan.f_end = s_accel_plan.f_tgt;
}

static void PlsrRunControlFinishAll(void)
{
PlsrPulseDriverStop();
@@ -892,6 +868,8 @@ static void PlsrRunControlFinishAll(void)
s_chain_freq = 0U;
s_act_armed = 0U;
s_act_expire_req = 0U;
s_act_cut_pending = 0U;
s_act_handoff_keep = 0U;
s_wait_expire_req = 0U;
s_act_time_ms = 0U;
s_act_arm_ms = 0U;
@@ -970,11 +948,6 @@ static void PlsrRunControlBeginSeg(uint16_t seg_0)
total = (uint32_t)s_target;
PlsrRunControlPlanSeg(total, io.f_from, f_tgt, io.f_end,
io.accel_ms, io.decel_ms);
if (seg->wait_type == PLSR_WAIT_ACT)
{
PlsrRunControlApplyActTimeCap(PlsrRunControlActTimeMs(seg));
}

s_done = 0;
s_busy = 1U;

@@ -1017,12 +990,15 @@ void PlsrRunControlInit(void)
s_chain_valid = 0U;
s_run_accel_ms = 0U;
s_run_decel_ms = 0U;
s_orig_end_zero = 0U;
s_phase = PH_CONST;
s_seg_elapsed_ms = 0U;
s_approach_from = 0U;
s_decel_from = 0U;
s_act_armed = 0U;
s_act_expire_req = 0U;
s_act_cut_pending = 0U;
s_act_handoff_keep = 0U;
s_wait_expire_req = 0U;
s_act_time_ms = 0U;
s_act_arm_ms = 0U;
@@ -1074,6 +1050,8 @@ void PlsrRunControlStop(void)
s_chain_freq = 0U;
s_act_armed = 0U;
s_act_expire_req = 0U;
s_act_cut_pending = 0U;
s_act_handoff_keep = 0U;
s_wait_expire_req = 0U;
s_act_time_ms = 0U;
s_act_arm_ms = 0U;
@@ -1101,13 +1079,6 @@ uint8_t PlsrRunControlChangeFreq(uint32_t new_tgt_hz)
f_end = s_accel_plan.f_end;
PlsrRunControlPlanSeg(remain, s_cur_freq, new_tgt_hz, f_end,
s_run_accel_ms, s_run_decel_ms);
if (PlsrParamGetSeg(s_cur_seg)->wait_type == PLSR_WAIT_ACT)
{
uint32_t used = PlsrSignalIoGetMs() - s_act_arm_ms;
uint16_t left = (used < (uint32_t)s_act_time_ms) ?
(uint16_t)((uint32_t)s_act_time_ms - used) : 1U;
PlsrRunControlApplyActTimeCap(left);
}
s_target = (int32_t)remain;
s_done = 0;
s_seg_elapsed_ms = 0U;
@@ -1123,7 +1094,7 @@ void PlsrRunControlTickMs(void)
if (s_act_expire_req != 0U)
{
s_act_expire_req = 0U;
if (s_state == RC_RUN)
if ((s_state == RC_RUN) || (s_act_handoff_keep != 0U))
{
PlsrRunControlActExpire();
}
@@ -1208,29 +1179,25 @@ void PlsrRunControlOn1ms(void)
s_seg_elapsed_ms++;
}

/*
* ACT 到点：若加/匀/减三阶段已经走完，不要按当前频率切段，
* 剩余脉冲继续发完，由 AfterSegDone ※4 停表等到点。
*/
if ((s_act_armed != 0U) &&
(PlsrSignalIoMsReached(s_act_arm_ms + (uint32_t)s_act_time_ms) != 0U))
{
uint32_t t_wave;
/*
* ACT 是运动控制条件，不参与曲线规划：加速、匀速、减速
* 任一阶段到期都在当前脉冲 UPDATE 边界切段。这样不会在
* 一个 PWM 周期中间改段，也不会把边界脉冲计到下一段。
*/
s_act_armed = 0U;
s_act_cut_pending = 1U;
}

t_wave = s_accel_plan.t_decel_start_ms + s_accel_plan.t_dec_ms;
if ((s_accel_plan.t_dec_ms >= 1U) && (s_seg_elapsed_ms >= t_wave))
{
/* 三段已结束，等脉冲 ISR 把 s_done 走到 s_target */
}
else
{
s_act_armed = 0U;
s_act_expire_req = 1U;
return;
}
/* 保持 ACT 到期瞬间的频率，不让 1ms 曲线继续推进到下一个边界。 */
if (s_act_cut_pending != 0U)
{
return;
}

if ((s_accel_plan.f_end == 0U) &&
if ((s_orig_end_zero != 0U) &&
(s_accel_plan.mode != PLSR_ACCEL_LINEAR) &&
(s_accel_plan.t_dec_ms >= 1U) &&
(s_seg_elapsed_ms >=
@@ -1266,6 +1233,21 @@ void PlsrRunControlOnPulseIsr(void)
s_acc_pulse--;
}

/*
* ACT 到期后的切段只在脉冲边界执行。当前 UPDATE 已完成本段
* 最后一个完整脉冲，立即停表，任务再用当前频率衔接下一段。
*/
if (s_act_cut_pending != 0U)
{
s_act_cut_pending = 0U;
PlsrPulseDriverStop();
s_pwm_on = 0U;
s_act_handoff_keep = 1U;
s_state = RC_IDLE;
s_act_expire_req = 1U;
return;
}

if (s_done >= s_target)
{
PlsrRunControlAfterSegDone();


Write Preview
Loading…
Cancel
Save