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

增加EXT或者脉冲发送完成中间判断，跳过无效段并换向要减速到0 

Signed-off-by: hanyongwei <2043702190@qq.com>
dev1
hanyongwei 3 weeks ago
parent
96fe4e6461
commit
4cc13d4a75
3 changed files with 144 additions and 66 deletions
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +33
    -12
      plsr/param/plsr_param.c
  2. +23
    -5
      plsr/param/plsr_param.h
  3. +88
    -49
      plsr/run_control/plsr_run_control.c

+ 33
- 12
plsr/param/plsr_param.c View File

@@ -4,13 +4,13 @@
*
* @details 模块职责
* 维护静态映像 s_cfg / s_seg[]，实现寄存器 ↔ 结构体双向转换，以及
* 「1+N 分帧」收齐后的异步 Apply（信号量唤醒本任务）。
* 「5+N 分帧」收齐后的异步 Apply（信号量唤醒本任务）。
*
* 静态状态说明
* s_cfg / s_seg[] — 运行唯一参数源
* s_param_sem — 收齐分帧后 Post，Task Pend 后 Apply
* s_expect_frames — 期望总帧 = 1 + 公共帧中的段数
* s_recv_frames — 已收帧计数（公共算第 1 帧）
* s_expect_frames — 期望总帧 = 5 + 公共帧1 中的段数
* s_recv_frames — 已收帧计数（公共帧1 起算）
* s_xfer_active — 正在接收一批分帧传输
*
* 与其它模块关系
@@ -29,7 +29,7 @@ static PlsrCfg_t s_cfg; /* 公共运行参数映像 */
static PlsrSeg_t s_seg[PLSR_SEG_MAX]; /* 段表映像（最多 PLSR_SEG_MAX 段） */

static OS_EVENT *s_param_sem; /* 分帧收齐后唤醒 ParamBlockTask */
static volatile uint16_t s_expect_frames; /* 本批期望总帧数 = 1 + 段数 */
static volatile uint16_t s_expect_frames; /* 本批期望总帧数 = 5 + 段数 */
static volatile uint16_t s_recv_frames; /* 已收帧计数 */
static volatile uint8_t s_xfer_active; /* 1=正在接收一批分帧 */

@@ -159,6 +159,26 @@ static int32_t PlsrParamReadSignedDWord(uint16_t addr)
return (int32_t)PlsrParamReadDWord(addr);
}

/**
* @brief 判断本次 FC10 是否为公共帧 2~5（双字速度/加减速）
* @return 1=是；0=否
*/
static uint8_t PlsrParamIsCommonTailFrameWrite(uint16_t start_addr, uint16_t quantity)
{
if (quantity < PLSR_PARAM_COMMON_DWORD_WORDS)
{
return 0U;
}
if ((start_addr == PLSR_REG_COMMON_F2_BASE) ||
(start_addr == PLSR_REG_COMMON_F3_BASE) ||
(start_addr == PLSR_REG_COMMON_F4_BASE) ||
(start_addr == PLSR_REG_COMMON_F5_BASE))
{
return 1U;
}
return 0U;
}

/**
* @brief 判断本次 FC10 是否为「整段 8 字写到某段基址」
* @return 1=是段帧；0=否
@@ -422,8 +442,8 @@ void PlsrParamBlockExportToHold(void)
* @brief FC10 写后分帧状态机（见 plsr_param.h）
*
* 状态转移：
* 写满公共 20 字 → BUSY，expect=1+N，recv=1
* 传输中写段基址 8 字 → recv++
* 公共帧1@0x1000~0x100A → BUSY，expect=5+N，recv=1
* 传输中公共帧2~5 / 段帧 → recv++
* recv>=expect → 清 active，Post 信号量（或同步 Apply）
* 其它写地址忽略（例如运行中只写频率双字不走本路径的分帧逻辑）
*/
@@ -435,25 +455,26 @@ void PlsrParamBlockOnHoldWrite(uint16_t start_addr, uint16_t quantity)
}

/*
* 公共帧：起始 0x1000 且覆盖到 0x1013 → 开启传输，期望帧=1+段数。
* 段帧：传输中且写到某段基址 → 帧计数+1。
* 公共帧1：0x1000~0x100A（含段数）→ 开启传输，期望帧=5+段数。
* 公共帧2~5 / 段帧：传输中且地址匹配 → 帧计数+1。
* 收齐后 post 信号量，由 ParamBlockTask 执行 Apply。
*/
if ((start_addr == PLSR_REG_PULSE_Y) &&
(quantity >= (uint16_t)(PLSR_REG_DECEL_MS - PLSR_REG_PULSE_Y + 1U)))
if ((start_addr == PLSR_REG_COMMON_F1_BASE) &&
(quantity >= PLSR_PARAM_COMMON_FRAME1_WORDS))
{
uint16_t seg_n = ReadHoldReg(PLSR_REG_SEG_COUNT);
if (seg_n > PLSR_SEG_MAX)
{
seg_n = (uint16_t)PLSR_SEG_MAX;
}
s_expect_frames = (uint16_t)(1U + seg_n);
s_expect_frames = (uint16_t)(PLSR_PARAM_COMMON_FRAMES + seg_n);
s_recv_frames = 1U;
s_xfer_active = 1U;
PlsrParamSetXferStatus(PLSR_PARAM_XFER_BUSY, 0U);
}
else if ((s_xfer_active != 0U) &&
(PlsrParamIsSegFrameWrite(start_addr, quantity) != 0U))
((PlsrParamIsCommonTailFrameWrite(start_addr, quantity) != 0U) ||
(PlsrParamIsSegFrameWrite(start_addr, quantity) != 0U)))
{
s_recv_frames++;
WriteHoldReg(PLSR_PARAM_XFER_RECV_REG, s_recv_frames);


+ 23
- 5
plsr/param/plsr_param.h View File

@@ -5,7 +5,7 @@
* @details 模块职责
* 1. 定义公共参数区 0x1000~0x1013、段表区 0x1100+i*0x10 的地址与字段语义；
* 2. 维护运行侧唯一映像 s_cfg / s_seg[]（运动规划只读此映像，不直接啃寄存器）；
* 3. 支持上位机「1 公共帧 + N 段帧」分帧 FC10 写入：收齐后 ApplyFromHold；
* 3. 支持上位机「5 公共帧 + N 段帧」分帧 FC10 写入：收齐后 ApplyFromHold；
* 4. 上电由 PersistLoad 填 hold；ExportToHold 仅保留作调试接口。
*
* 与其它模块关系
@@ -16,14 +16,20 @@
* - PlsrParamBlockTask ：独立 uC/OS 任务，等信号量后 Apply + 整包落 BKP
*
* 关键数据流
* 上位机写公共帧@0x1000 → OnHoldWrite 开传输 expect=1+N
* 上位机写段帧@0x1100+ → recv++；收齐 → OSSemPost
* 上位机写公共帧1@0x1000 → OnHoldWrite 开传输 expect=5+N，recv=1
* 再写公共帧2~5 → recv++；写段帧@0x1100+ → recv++；收齐 → OSSemPost
* ParamBlockTask → ApplyFromHold（校验）→ 通过则 SaveParamsFromHold → 状态 OK/FAIL
* 上电：PersistLoad(BKP→hold) → ApplyFromHold(hold→结构体)
*
* 协议概要
* - 公共帧：0x1000~0x1013（20 字），0x1009=段数 N，预期总帧=1+N
* - 公共 5 帧：
* 1: 0x1000~0x100A（11 字，含段数 N@0x1009）
* 2: 0x100B~0x100C（默认速度）
* 3: 0x100D~0x100E（起速）
* 4: 0x1010~0x1011（止速；跳过 0x100F）
* 5: 0x1012~0x1013（加减速时间）
* - 段帧：0x1100+(i)*0x10，每段 8 字
* - 预期总帧 = PLSR_PARAM_COMMON_FRAMES + N
* - 传输状态：0x0FFC 已收帧 / 0x0FFD 失败帧 / 0x0FFE 状态
*/
#ifndef PLSR_PARAM_H
@@ -44,7 +50,7 @@
#define PLSR_REG_DIR_LOGIC 0x1006U /* 方向逻辑：0正 1负 */
#define PLSR_REG_ACCEL_MODE 0x1007U /* 曲线：0直线 1=S 2正弦 */
#define PLSR_REG_RUN_MODE 0x1008U /* 定位：0相对 1绝对 */
#define PLSR_REG_SEG_COUNT 0x1009U /* 脉冲总段数 N（决定 expect 帧数） */
#define PLSR_REG_SEG_COUNT 0x1009U /* 脉冲总段数 N（决定 expect=5+N） */
#define PLSR_REG_START_SEG 0x100AU /* 启动段号 1-based */
#define PLSR_REG_DEFAULT_SPD_L 0x100BU /* 默认速度 Hz，双字低 */
#define PLSR_REG_DEFAULT_SPD_H 0x100CU
@@ -56,6 +62,18 @@
#define PLSR_REG_ACCEL_MS 0x1012U /* 默认速度加速时间 ms（定斜率） */
#define PLSR_REG_DECEL_MS 0x1013U /* 默认速度减速时间 ms */

/* ---------- 公共参数分帧（5 帧 + N 段帧） ---------- */
#define PLSR_PARAM_COMMON_FRAMES 5U
#define PLSR_PARAM_COMMON_FRAME1_WORDS 11U /* 0x1000~0x100A */
#define PLSR_PARAM_COMMON_DWORD_WORDS 2U /* 双字帧字数 */

#define PLSR_REG_COMMON_F1_BASE PLSR_REG_PULSE_Y /* 0x1000 */
#define PLSR_REG_COMMON_F1_END PLSR_REG_START_SEG /* 0x100A */
#define PLSR_REG_COMMON_F2_BASE PLSR_REG_DEFAULT_SPD_L /* 0x100B */
#define PLSR_REG_COMMON_F3_BASE PLSR_REG_START_SPD_L /* 0x100D */
#define PLSR_REG_COMMON_F4_BASE PLSR_REG_END_SPD_L /* 0x1010 */
#define PLSR_REG_COMMON_F5_BASE PLSR_REG_ACCEL_MS /* 0x1012 */

/* ---------- 段参数：第 i 段基址 = 0x1100 + i*0x10，每段 8 字 ---------- */
#define PLSR_REG_SEG1_BASE 0x1100U
#define PLSR_SEG_STRIDE 0x0010U


+ 88
- 49
plsr/run_control/plsr_run_control.c View File

@@ -9,13 +9,13 @@
* 2) 等待策略：只决定何时打断/段后等待；统一 handoff：
* - 仍在输出且 freq>=1 → 链式传给下一段
* - 已停表/输出为 0 → 下一段从配置起速（0 则起跳频率）开跑
* 3) 本段 f_end 由等待类型 + 发送模式 + 与下一段方向关系决定：
* - EXT_OR_DONE + 有下一段 + 同向 + 后续 → 过渡到下一段目标
* - EXT_OR_DONE + 有下一段 + 同向 + 完成 → f_end=峰值，匀速跑完
* - EXT_OR_DONE + 有下一段 + 反向 → 与其它等待相同，规划到公共止速
* 3) 本段 f_end 由等待类型 + 发送模式 + 与「下一有效段」方向关系决定：
* - EXT_OR_DONE + 下一有效段同向 + 后续 → 过渡到该段目标
* - EXT_OR_DONE + 下一有效段同向 + 完成 → f_end=峰值，匀速跑完
* - EXT_OR_DONE + 下一有效段反向（可跳过中间频率0且脉冲0占位）→ 规划到公共止速
* - 真末段 → 一律规划到公共止速
* - TIME/SIGNAL/ACT/EXT → 完成/后续相同，规划到公共止速
* FOLLOW 另允许 EXT_OR_DONE 同向不停表衔接。
* FOLLOW 另允许 EXT_OR_DONE 同向不停表衔接（直接下一段不得为无效占位）。
* 4) PlanSeg / ChangeFreq 共用规划入口，便于后期动态改频
*
* 【运行节拍】纯脉冲计数：
@@ -204,6 +204,37 @@ static int16_t PlsrRunControlNextSeg(uint16_t cur_seg_0based)
return -1;
}

/**
* @brief 无效占位段：频率 0 且脉冲 0（不参与 EXT_OR_DONE 同向衔接/换向判断）
*/
static uint8_t PlsrRunControlIsEmptyPlaceholderSeg(uint16_t seg_0based)
{
PlsrSeg_t *seg = PlsrParamGetSeg(seg_0based);

return ((seg->freq_hz == 0) && (seg->pulse_cnt == 0)) ? 1U : 0U;
}

/**
* @brief 从 cur 的下一段起，沿 jump 跳过「频率0且脉冲0」无效段，找下一有效段
* @return 有效段 0-based；无则 -1
*/
static int16_t PlsrRunControlNextEffectiveSeg(uint16_t cur_seg_0based)
{
int16_t next = PlsrRunControlNextSeg(cur_seg_0based);
uint16_t guard = 0U;

while ((next >= 0) && (guard < PLSR_SEG_MAX))
{
guard++;
if (PlsrRunControlIsEmptyPlaceholderSeg((uint16_t)next) == 0U)
{
return next;
}
next = PlsrRunControlNextSeg((uint16_t)next);
}
return -1;
}

/**
* @brief 判断段方向：相对看 pulse_cnt 符号；绝对看目标−局部位置
*/
@@ -609,7 +640,6 @@ static uint8_t PlsrRunControlResolveSegPlanEndpoints(uint16_t seg_idx0, uint32_t
{
PlsrCfg_t *cfg = PlsrParamGetCfg();
PlsrSeg_t *seg = PlsrParamGetSeg(seg_idx0);
int16_t next_seg_idx;

endpoints->accel_ms = cfg->accel_ms;
endpoints->decel_ms = cfg->decel_ms;
@@ -646,50 +676,49 @@ static uint8_t PlsrRunControlResolveSegPlanEndpoints(uint16_t seg_idx0, uint32_t

/*
* ---------- 终止频率 ----------
* EXT_OR_DONE + 有下一段 + 同向 + 后续 → 过渡到下一段目标
* EXT_OR_DONE + 有下一段 + 同向 + 完成 → freq_end_hz=峰值，匀速跑完
* EXT_OR_DONE + 有下一段 + 反向 → 与 TIME/SIGNAL/ACT/EXT 相同，规划到止速
* 真末段（无下一段）→ 一律规划到公共止速（含 EXT_OR_DONE）
* TIME / SIGNAL / ACT / EXT → 完成与后续相同，一律规划到公共止速
* EXT_OR_DONE：同向/反向只看「下一有效段」（跳过频率0且脉冲0 的占位段）。
* 是否向止速规划 = 与下一有效段是否换向；夹无效段本身不单独强制减速。
* 同向 + 后续 → 过渡到下一有效段目标；同向 + 完成 → 匀速峰值收尾。
* 真末段 / TIME/SIGNAL/ACT/EXT → 一律规划到公共止速
*/
next_seg_idx = PlsrRunControlNextSeg(seg_idx0);

if ((seg->wait_type == PLSR_WAIT_EXT_OR_DONE) && (next_seg_idx >= 0))
{
int32_t end_pos = PlsrRunControlAbsLocalPos();
uint8_t next_fwd;
int16_t next_eff = PlsrRunControlNextEffectiveSeg(seg_idx0);

/* 用本段结束后的坐标判下一段方向（与 WillFollowKeepAt 一致） */
if (s_forward != 0U)
{
end_pos += s_target;
}
else
if ((seg->wait_type == PLSR_WAIT_EXT_OR_DONE) && (next_eff >= 0))
{
end_pos -= s_target;
}
next_fwd = PlsrRunControlIsForward((uint16_t)next_seg_idx, end_pos);
int32_t end_pos = PlsrRunControlAbsLocalPos();
uint8_t next_fwd;

if (next_fwd == s_forward)
{
if (cfg->send_mode == PLSR_SEND_FOLLOW)
if (s_forward != 0U)
{
PlsrSeg_t *nseg = PlsrParamGetSeg((uint16_t)next_seg_idx);
if (PlsrRunControlGetSegTargetFreq(nseg->freq_hz,
cfg->default_speed,
&endpoints->freq_end_hz) != 0U)
end_pos += s_target;
}
else
{
end_pos -= s_target;
}
next_fwd = PlsrRunControlIsForward((uint16_t)next_eff, end_pos);

/* 与下一有效段同向 → FOLLOW/COMPLETE 衔接（换向才落到下方止速） */
if (next_fwd == s_forward)
{
if (cfg->send_mode == PLSR_SEND_FOLLOW)
{
PlsrSeg_t *nseg = PlsrParamGetSeg((uint16_t)next_eff);
if (PlsrRunControlGetSegTargetFreq(nseg->freq_hz,
cfg->default_speed,
&endpoints->freq_end_hz) != 0U)
{
return 1U;
}
endpoints->freq_end_hz = f_tgt;
return 1U;
}
/* 下一段频率非法：本段匀速收尾，下一段 Begin 再报 0x04 */
endpoints->freq_end_hz = f_tgt;
return 1U;
}
/* 完成模式非末段同向：匀速跑完，不减速到 0 */
endpoints->freq_end_hz = f_tgt;
return 1U;
/* 与下一有效段反向：落入下方规划到止速 */
}
/* 反向：落入下方，规划到止速 */
}

endpoints->freq_end_hz = PlsrAccelCurveResolveEndHz(cfg->end_speed,
@@ -733,18 +762,26 @@ static uint16_t PlsrRunControlActTimeMs(const PlsrSeg_t *seg)
static uint8_t PlsrRunControlWillFollowKeepAt(uint16_t cur_seg, int32_t local_pos)
{
PlsrCfg_t *cfg = PlsrParamGetCfg();
int16_t next_seg_idx;
int16_t next_eff;

if (cfg->send_mode != PLSR_SEND_FOLLOW)
{
return 0U;
}
next_seg_idx = PlsrRunControlNextSeg(cur_seg);
if (next_seg_idx < 0)
if (PlsrRunControlNextSeg(cur_seg) < 0)
{
return 0U;
}
if (PlsrRunControlIsForward((uint16_t)next_seg_idx, local_pos) != s_forward)
next_eff = PlsrRunControlNextEffectiveSeg(cur_seg);
if (next_eff < 0)
{
return 0U;
}
/*
* 直接下一段为无效占位时不能「跳过」该段，但仍可与下一有效段同向 keep
* （EXT_OR_DONE 同向不应因中间占位而强制停到 0 再爬）。
*/
if (PlsrRunControlIsForward((uint16_t)next_eff, local_pos) != s_forward)
{
return 0U;
}
@@ -1256,20 +1293,22 @@ static void PlsrRunControlBeginSeg(uint16_t seg_idx0)
{
/* 以首次启动锁定原点：move = 段目标 − 相对原点位置 */
move = cnt - PlsrRunControlAbsLocalPos();
if (move == 0)
{
s_busy = 1U;
s_target = 0;
s_done = 0;
PlsrRunControlAfterSegDone();
return;
}
}
else
{
move = cnt;
}

if (move == 0)
{
/* 相对 0 脉冲 / 无效占位(频率0且脉冲0) / 绝对已在目标：立刻段后逻辑 */
s_busy = 1U;
s_target = 0;
s_done = 0;
PlsrRunControlAfterSegDone();
return;
}

if (move >= 0)
{
s_forward = 1U;


Write Preview
Loading…
Cancel
Save