From 4cc13d4a75131f3a7f95bf6d0170ff0e36b1c95c Mon Sep 17 00:00:00 2001 From: hanyongwei <2043702190@qq.com> Date: Tue, 25 Aug 2026 09:58:14 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0EXT=E6=88=96=E8=80=85?= =?UTF-8?q?=E8=84=89=E5=86=B2=E5=8F=91=E9=80=81=E5=AE=8C=E6=88=90=E4=B8=AD?= =?UTF-8?q?=E9=97=B4=E5=88=A4=E6=96=AD=EF=BC=8C=E8=B7=B3=E8=BF=87=E6=97=A0?= =?UTF-8?q?=E6=95=88=E6=AE=B5=E5=B9=B6=E6=8D=A2=E5=90=91=E8=A6=81=E5=87=8F?= =?UTF-8?q?=E9=80=9F=E5=88=B00?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: hanyongwei <2043702190@qq.com> --- plsr/param/plsr_param.c | 45 ++++++--- plsr/param/plsr_param.h | 28 +++++- plsr/run_control/plsr_run_control.c | 137 ++++++++++++++++++---------- 3 files changed, 144 insertions(+), 66 deletions(-) diff --git a/plsr/param/plsr_param.c b/plsr/param/plsr_param.c index 3ee12dc..84243a5 100644 --- a/plsr/param/plsr_param.c +++ b/plsr/param/plsr_param.c @@ -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); diff --git a/plsr/param/plsr_param.h b/plsr/param/plsr_param.h index e42f443..4804ce9 100644 --- a/plsr/param/plsr_param.h +++ b/plsr/param/plsr_param.h @@ -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 diff --git a/plsr/run_control/plsr_run_control.c b/plsr/run_control/plsr_run_control.c index 33e67f8..d97bde7 100644 --- a/plsr/run_control/plsr_run_control.c +++ b/plsr/run_control/plsr_run_control.c @@ -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;