diff --git a/.gitignore b/.gitignore
index cd96f69..e6b0ab0 100644
--- a/.gitignore
+++ b/.gitignore
@@ -19,3 +19,4 @@ host/
iar/settings/modbus.reggroups
build-plsr_host-Desktop_Qt_5_6_3_MinGW_32bit-Debug/
plsr/test/
+tools/
diff --git a/drivers/Include/stm32f4xx_it.h b/drivers/Include/stm32f4xx_it.h
index 4bd1538..c484d85 100644
--- a/drivers/Include/stm32f4xx_it.h
+++ b/drivers/Include/stm32f4xx_it.h
@@ -57,6 +57,8 @@ void PendSV_Handler(void);
void SysTick_Handler(void);
void TIM2_IRQHandler(void);
void TIM1_UP_TIM10_IRQHandler(void);
+void TIM1_TRG_COM_TIM11_IRQHandler(void);
+void TIM8_UP_TIM13_IRQHandler(void);
void USART1_IRQHandler(void);
/* USER CODE BEGIN EFP */
diff --git a/drivers/Source/stm32f4xx_it.c b/drivers/Source/stm32f4xx_it.c
index 89d9a96..270bc84 100644
--- a/drivers/Source/stm32f4xx_it.c
+++ b/drivers/Source/stm32f4xx_it.c
@@ -60,6 +60,8 @@
extern UART_HandleTypeDef huart1;
extern TIM_HandleTypeDef htim2;
extern TIM_HandleTypeDef htim10;
+extern TIM_HandleTypeDef htim11;
+extern TIM_HandleTypeDef htim13;
/* USER CODE BEGIN EV */
@@ -222,7 +224,7 @@ void TIM2_IRQHandler(void)
}
/**
- * @brief TIM1 Update / TIM10 全局中断（PLSR 脉冲计数）
+ * @brief TIM1 Update / TIM10（PLSR Y0 脉冲计数）
*/
void TIM1_UP_TIM10_IRQHandler(void)
{
@@ -231,6 +233,26 @@ void TIM1_UP_TIM10_IRQHandler(void)
OSIntExit();
}
+/**
+ * @brief TIM1 Trigger/COM / TIM11（PLSR Y1 脉冲计数）
+ */
+void TIM1_TRG_COM_TIM11_IRQHandler(void)
+{
+ OSIntEnter();
+ HAL_TIM_IRQHandler(&htim11);
+ OSIntExit();
+}
+
+/**
+ * @brief TIM8 Update / TIM13（PLSR Y2 脉冲计数）
+ */
+void TIM8_UP_TIM13_IRQHandler(void)
+{
+ OSIntEnter();
+ HAL_TIM_IRQHandler(&htim13);
+ OSIntExit();
+}
+
/**
* @brief This function handles USART1 global interrupt.
*/
diff --git a/iar/plsr.dep b/iar/plsr.dep
index 08314c9..5fe708d 100644
--- a/iar/plsr.dep
+++ b/iar/plsr.dep
@@ -2804,7 +2804,7 @@
ICCARM
- 142 209 253 342 476 231 193 130 158 629 350 94 348 621 628 619 632 631 625 380 627 620 622 623 191 266 93 352 351 354 356 347 355 349 353 357 92 95 487 488 489 327 315 295 550 414 298 306 313 299 624 626 156 145
+ 142 209 253 342 476 231 193 130 158 629 350 94 348 621 628 619 632 631 625 380 627 620 622 623 191 266 93 352 351 354 356 347 355 349 353 357 92 95 487 488 489 327 315 295 550 414 298 306 313 299 624 626 156 145 128
@@ -2850,7 +2850,7 @@
ICCARM
- 137 209 253 342 476 231 193 134 136 145 128 629 350 94 348 621 628 619 632 631 625 380 627 620 622 623 191 266 93 352 351 354 356 347 355 349 353 357 92 95 487 488 489 327 315 295 550 414 298 306 313 299
+ 137 209 253 342 476 231 193 134 136 145 128 629 350 94 348 621 628 619 632 631 625 380 627 620 622 623 191 266 93 352 351 354 356 347 355 349 353 357 92 95 487 488 489 153 327 315 295 550 414 298 306 313 299
diff --git a/modbus/modbus_port.c b/modbus/modbus_port.c
index 137d55a..38366af 100644
--- a/modbus/modbus_port.c
+++ b/modbus/modbus_port.c
@@ -12,6 +12,7 @@
#include "modbus_common.h"
#include "gpio.h"
#include "plsr.h"
+#include "plsr_pulse_driver.h"
uint8_t g_modbus_rx_buf[MODBUS_RX_BUF_LEN] = {0};
volatile uint16_t g_modbus_rx_len = 0U;
@@ -129,9 +130,9 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
(void)OSSemPost(g_modbus_rx_sem);
}
}
- else if (htim->Instance == TIM10)
+ else if (PlsrPulseDriverIsPulseTim(htim->Instance) != 0U)
{
- /* 每个 PWM 周期溢出 = 1 个脉冲 */
+ /* 每个 PWM 周期溢出 = 1 个脉冲（TIM10/11/13） */
PlsrOnPulseIsr();
}
}
diff --git a/plsr/accel_curve/plsr_accel_curve.c b/plsr/accel_curve/plsr_accel_curve.c
index adf464c..5cfc8ee 100644
--- a/plsr/accel_curve/plsr_accel_curve.c
+++ b/plsr/accel_curve/plsr_accel_curve.c
@@ -1,15 +1,17 @@
/**
* @file plsr_accel_curve.c
- * @brief 加减速曲线实现：千分比整形 + 梯形脉冲分配 + 按已发脉冲求频
+ * @brief 预估相脉冲；相内时间域直线/S/正弦
+ *
+ * 脉冲预算不够到目标频时：降低峰值，保留加减速时间（勿把 T 压成 0，
+ * 否则示波器上变成方波，三种模式完全无法区分）。
+
+ * @note：目前脉冲输出在低频时和高频时均有毛刺，需要优化
*/
#include "plsr_accel_curve.h"
+/*频率大于100k时暂时固定到100k，后面增加报错*/
uint32_t PlsrAccelCurveClampFreq(uint32_t freq_hz)
{
- if (freq_hz < 1U)
- {
- freq_hz = 1U;
- }
if (freq_hz > 100000U)
{
freq_hz = 100000U;
@@ -17,150 +19,454 @@ uint32_t PlsrAccelCurveClampFreq(uint32_t freq_hz)
return freq_hz;
}
-/**
- * @brief 把进度 num/den 映射为 0~1000 的千分比，再按曲线整形
- * @return 0~1000，1000 表示该相结束
- */
-static uint32_t PlsrAccelCurveShape(uint32_t num, uint32_t den, PlsrAccelMode_e mode)
+static uint32_t PlsrAccelCurveAbsDiff(uint32_t a, uint32_t b)
{
- uint32_t x;
- uint32_t y;
- uint32_t d;
+ return (a >= b) ? (a - b) : (b - a);
+}
- if ((den == 0U) || (num >= den))
+static uint32_t PlsrAccelCurveSlopeDen(uint32_t default_spd,
+ uint32_t primary_ref,
+ uint32_t fallback_ref)
+{
+ 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;
+}
+
+static uint32_t PlsrAccelCurveRampTimeMs(uint32_t f_from,
+ uint32_t f_to,
+ uint32_t default_spd,
+ uint32_t start_spd_ref,
+ uint32_t end_spd_ref,
+ uint32_t accel_ms,
+ uint32_t decel_ms)
+{
+ uint32_t df;
+ uint32_t den;
+ uint32_t ref_ms;
+ uint32_t t;
+
+ if (f_from == f_to)
+ {
+ return 0U;
+ }
+
+ df = PlsrAccelCurveAbsDiff(f_from, f_to);
+ if (f_to > f_from)
+ {
+ ref_ms = accel_ms;
+ den = PlsrAccelCurveSlopeDen(default_spd, start_spd_ref, end_spd_ref);
+ }
+ else
+ {
+ ref_ms = decel_ms;
+ den = PlsrAccelCurveSlopeDen(default_spd, end_spd_ref, start_spd_ref);
+ }
+
+ if (ref_ms == 0U)
+ {
+ return 0U;
+ }
+ /* 默认速度未设时，用频差作分母：整段升降时间 = accel/decel_ms */
+ if (den == 0U)
+ {
+ den = df;
+ }
+ if (den == 0U)
+ {
+ return 0U;
+ }
+
+ /* 向上取整，避免 (df*ref)/den 截成 0 导致直接跳到目标频 */
+ t = (df * ref_ms + den - 1UL) / den;
+ if (t < 1U)
+ {
+ t = 1U;
+ }
+ return t;
+}
+
+static uint32_t PlsrAccelCurveEstimatePulses(uint32_t f_from,
+ uint32_t f_to,
+ uint32_t t_ms)
+{
+ uint32_t f_hi;
+
+ if ((t_ms == 0U) || (f_from == f_to))
+ {
+ return 0U;
+ }
+ /*
+ * 必须用相内最高频估脉冲上限，不能用平均频。
+ * 减速：起点就是 f_peak，若按 (f_peak+0)/2 估，实际前半段仍接近峰值，
+ * dec_n 会在 t_dec 走完前耗尽 → 段结束硬停，示波器上就是“速降”。
+ * 加速：从低到高，用最高频偏保守，只会多留一点加速脉冲（到频后持平），安全。
+ */
+ f_hi = (f_from > f_to) ? f_from : f_to;
+ return (f_hi * t_ms + 999UL) / 1000UL;
+}
+
+static uint32_t PlsrAccelCurveLerp(uint32_t a, uint32_t b, uint32_t ratio_permille)
+{
+ if (ratio_permille >= 1000U)
+ {
+ return b;
+ }
+ if (b >= a)
+ {
+ return a + ((b - a) * ratio_permille) / 1000UL;
+ }
+ return a - ((a - b) * ratio_permille) / 1000UL;
+}
+
+/** 直线：f 对时间线性 */
+static uint32_t PlsrAccelCurveShapeLinear(uint32_t t_ms, uint32_t T_ms)
+{
+ if ((T_ms == 0U) || (t_ms >= T_ms))
{
return 1000U;
}
- x = (num * 1000UL) / den;
+ return (t_ms * 1000UL) / T_ms;
+}
- if (mode == PLSR_ACCEL_S)
+///** 正弦缓入缓出 */
+//static uint32_t PlsrAccelCurveShapeSine(uint32_t t_ms, uint32_t T_ms)
+//{
+// uint32_t x;
+// uint32_t y;
+// uint32_t d;
+//
+// if ((T_ms == 0U) || (t_ms >= T_ms))
+// {
+// return 1000U;
+// }
+// x = (t_ms * 1000UL) / T_ms;
+// if (x <= 500UL)
+// {
+// y = (2UL * x * x) / 1000UL;
+// }
+// else
+// {
+// d = 1000UL - x;
+// y = 1000UL - (2UL * d * d) / 1000UL;
+// }
+// if (y > 1000UL)
+// {
+// y = 1000UL;
+// }
+// return y;
+//}
+//
+///**
+// * 七段一侧 1:2:1：相对直线，头尾更圆、中间更陡
+// */
+//static uint32_t PlsrAccelCurveShapeS7(uint32_t t_ms, uint32_t T_ms)
+//{
+// uint32_t u;
+// uint32_t d;
+// uint32_t p;
+//
+// if ((T_ms == 0U) || (t_ms >= T_ms))
+// {
+// return 1000U;
+// }
+// u = (t_ms * 1000UL) / T_ms;
+// if (u <= 250UL)
+// {
+// p = (8UL * u * u) / (3UL * 1000UL);
+// }
+// else if (u <= 750UL)
+// {
+// p = 167UL + (666UL * (u - 250UL)) / 500UL;
+// }
+// else
+// {
+// d = 1000UL - u;
+// p = 1000UL - (8UL * d * d) / (3UL * 1000UL);
+// }
+// if (p > 1000UL)
+// {
+// p = 1000UL;
+// }
+// return p;
+//}
+
+static uint32_t PlsrAccelCurveShape(uint32_t t_ms, uint32_t T_ms, PlsrAccelMode_e mode)
+{
+// if (mode == PLSR_ACCEL_S)
+// {
+// return PlsrAccelCurveShapeS7(t_ms, T_ms);
+// }
+// if (mode == PLSR_ACCEL_SINE)
+// {
+// return PlsrAccelCurveShapeSine(t_ms, T_ms);
+// }
+ return PlsrAccelCurveShapeLinear(t_ms, T_ms);
+}
+
+/** 在 f_cur→f_want 路径上按千分比取点 */
+static uint32_t PlsrAccelCurveOnPath(uint32_t f_cur, uint32_t f_want, uint32_t permille)
+{
+ if (permille >= 1000U)
{
- /* smoothstep：3x^2 - 2x^3（x 为千分比） */
- y = (3UL * x * x) / 1000UL;
- y -= (2UL * x * x * x) / (1000UL * 1000UL);
+ return f_want;
}
- else if (mode == PLSR_ACCEL_SINE)
+ if (f_want >= f_cur)
{
- /* (1-cos(πt))/2 的整数近似 */
- if (x <= 500UL)
+ return f_cur + ((f_want - f_cur) * permille) / 1000UL;
+ }
+ return f_cur - ((f_cur - f_want) * permille) / 1000UL;
+}
+
+/**
+ * 二分“路径进度”，使加+减估算脉冲 <= total，且尽量接近原目标频。
+ * 时间按峰值重算（斜率不变），避免旧逻辑把 T 按脉冲比例压扁。
+ */
+static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses,
+ uint32_t f_cur,
+ uint32_t f_want,
+ uint32_t f_end,
+ uint32_t default_spd,
+ uint32_t start_spd_ref,
+ uint32_t end_spd_ref,
+ uint32_t accel_ms,
+ uint32_t decel_ms)
+{
+ uint32_t lo = 0U;
+ uint32_t hi = 1000U;
+ uint32_t mid;
+ uint32_t best = 0U;
+ uint32_t peak;
+ uint32_t t_a;
+ uint32_t t_d;
+ uint32_t n_a;
+ uint32_t n_d;
+ uint32_t i;
+
+ for (i = 0U; i < 12U; i++)
+ {
+ mid = lo + ((hi - lo) / 2UL);
+ peak = PlsrAccelCurveOnPath(f_cur, f_want, mid);
+ t_a = PlsrAccelCurveRampTimeMs(f_cur, peak, default_spd,
+ start_spd_ref, end_spd_ref,
+ accel_ms, decel_ms);
+ t_d = PlsrAccelCurveRampTimeMs(peak, f_end, default_spd,
+ start_spd_ref, end_spd_ref,
+ accel_ms, decel_ms);
+ n_a = PlsrAccelCurveEstimatePulses(f_cur, peak, t_a);
+ n_d = PlsrAccelCurveEstimatePulses(peak, f_end, t_d);
+
+ if ((n_a + n_d) <= total_pulses)
{
- y = (2UL * x * x) / 1000UL;
+ best = mid;
+ lo = mid + 1UL;
+ if (lo > hi)
+ {
+ break;
+ }
}
else
{
- d = 1000UL - x;
- y = 1000UL - (2UL * d * d) / 1000UL;
+ if (mid == 0U)
+ {
+ break;
+ }
+ hi = mid - 1UL;
}
}
- else
- {
- y = x; /* 直线 */
- }
- if (y > 1000UL)
- {
- y = 1000UL;
- }
- return y;
+ return PlsrAccelCurveOnPath(f_cur, f_want, best);
}
void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
uint32_t total_pulses,
- uint32_t f0,
- uint32_t f1,
- uint32_t f2,
+ uint32_t f_cur,
+ uint32_t f_tgt,
+ uint32_t f_end,
+ uint32_t default_spd,
+ uint32_t start_spd_ref,
+ uint32_t end_spd_ref,
uint32_t accel_ms,
uint32_t decel_ms,
PlsrAccelMode_e mode)
{
+ uint32_t t_acc;
+ uint32_t t_dec;
uint32_t acc_n;
uint32_t dec_n;
+ uint32_t f_peak;
if (plan == (PlsrAccelPlan_t *)0)
{
return;
}
- plan->spd_start = f0;
- plan->spd_target = f1;
- plan->spd_end = f2;
+ f_cur = PlsrAccelCurveClampFreq(f_cur);
+ f_tgt = PlsrAccelCurveClampFreq(f_tgt);
+ f_end = PlsrAccelCurveClampFreq(f_end);
+
+ plan->f_cur = f_cur;
+ plan->f_end = f_end;
plan->mode = (mode > PLSR_ACCEL_SINE) ? PLSR_ACCEL_LINEAR : mode;
- if (accel_ms == 0U)
+ f_peak = f_tgt;
+ t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd,
+ start_spd_ref, end_spd_ref,
+ accel_ms, decel_ms);
+ t_dec = PlsrAccelCurveRampTimeMs(f_peak, f_end, default_spd,
+ start_spd_ref, end_spd_ref,
+ accel_ms, decel_ms);
+ acc_n = PlsrAccelCurveEstimatePulses(f_cur, f_peak, t_acc);
+ dec_n = PlsrAccelCurveEstimatePulses(f_peak, f_end, t_dec);
+
+ if (total_pulses == 0U)
{
acc_n = 0U;
- }
- else
- {
- acc_n = ((f0 + f1) / 2UL) * accel_ms / 1000UL;
- }
- if (decel_ms == 0U)
- {
dec_n = 0U;
- }
- else
- {
- dec_n = ((f1 + f2) / 2UL) * decel_ms / 1000UL;
+ plan->const_n = 0U;
+ plan->t_acc_ms = 0U;
+ plan->t_dec_ms = 0U;
+ plan->f_tgt = f_peak;
+ plan->acc_n = 0U;
+ plan->dec_n = 0U;
+ return;
}
- if ((acc_n + dec_n) >= total_pulses)
+ if ((acc_n + dec_n) > total_pulses)
{
- acc_n = total_pulses / 2UL;
- dec_n = total_pulses - acc_n;
- plan->const_n = 0U;
+ /*
+ * 旧逻辑：按脉冲比例压缩 t_acc/t_dec → 高速/少脉冲时 T→1ms，
+ * 示波器上看成方波，直线/S/正弦无差别。
+ * 新逻辑：降低峰值，按斜率重算时间，保留可辨认的相时间。
+ */
+ f_peak = PlsrAccelCurveFitPeak(total_pulses,
+ f_cur, f_tgt, f_end,
+ default_spd, start_spd_ref, end_spd_ref,
+ accel_ms, decel_ms);
+ t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd,
+ start_spd_ref, end_spd_ref,
+ accel_ms, decel_ms);
+ t_dec = PlsrAccelCurveRampTimeMs(f_peak, f_end, default_spd,
+ start_spd_ref, end_spd_ref,
+ accel_ms, decel_ms);
+ acc_n = PlsrAccelCurveEstimatePulses(f_cur, f_peak, t_acc);
+ dec_n = PlsrAccelCurveEstimatePulses(f_peak, f_end, t_dec);
+
+ if ((acc_n + dec_n) > total_pulses)
+ {
+ /* 仍略超：按比例切脉冲，但时间至少保留 1ms/相，且不按 raw 比例把 T 打没 */
+ uint32_t sum = acc_n + dec_n;
+ if (sum == 0U)
+ {
+ acc_n = total_pulses / 2UL;
+ dec_n = total_pulses - acc_n;
+ }
+ else
+ {
+ acc_n = (total_pulses * acc_n) / sum;
+ if (acc_n >= total_pulses)
+ {
+ acc_n = total_pulses;
+ dec_n = 0U;
+ }
+ else
+ {
+ dec_n = total_pulses - acc_n;
+ }
+ }
+ plan->const_n = 0U;
+ if ((t_acc == 0U) && (acc_n > 0U))
+ {
+ t_acc = 1U;
+ }
+ if ((t_dec == 0U) && (dec_n > 0U))
+ {
+ t_dec = 1U;
+ }
+ }
+ else
+ {
+ plan->const_n = total_pulses - acc_n - dec_n;
+ }
}
else
{
plan->const_n = total_pulses - acc_n - dec_n;
}
+
+ plan->f_tgt = f_peak;
+ plan->t_acc_ms = t_acc;
+ plan->t_dec_ms = t_dec;
plan->acc_n = acc_n;
plan->dec_n = dec_n;
}
-uint32_t PlsrAccelCurveFreqAt(const PlsrAccelPlan_t *plan, uint32_t done_pulses)
+PlsrCurvePhase_e PlsrAccelCurvePhaseAt(const PlsrAccelPlan_t *plan,
+ uint32_t done_pulses)
+{
+ if (plan == (PlsrAccelPlan_t *)0)
+ {
+ return PLSR_CURVE_PHASE_CONST;
+ }
+ if (done_pulses < plan->acc_n)
+ {
+ return PLSR_CURVE_PHASE_ACC;
+ }
+ if (done_pulses < (plan->acc_n + plan->const_n))
+ {
+ return PLSR_CURVE_PHASE_CONST;
+ }
+ return PLSR_CURVE_PHASE_DEC;
+}
+
+uint32_t PlsrAccelCurveFreqAt(const PlsrAccelPlan_t *plan,
+ uint32_t done_pulses,
+ uint32_t phase_elapsed_ms)
{
uint32_t done = done_pulses;
uint32_t ratio;
- uint32_t f;
+ uint32_t T;
if (plan == (PlsrAccelPlan_t *)0)
{
- return 1U;
+ return 0U;
}
if (done < plan->acc_n)
{
- ratio = PlsrAccelCurveShape(done, plan->acc_n, plan->mode);
- if (plan->spd_target >= plan->spd_start)
+ T = plan->t_acc_ms;
+ if (T == 0U)
{
- f = plan->spd_start + ((plan->spd_target - plan->spd_start) * ratio) / 1000UL;
+ return PlsrAccelCurveClampFreq(plan->f_tgt);
}
- else
- {
- f = plan->spd_start - ((plan->spd_start - plan->spd_target) * ratio) / 1000UL;
- }
- return PlsrAccelCurveClampFreq(f);
+ ratio = PlsrAccelCurveShape(phase_elapsed_ms, T, plan->mode);
+ return PlsrAccelCurveClampFreq(
+ PlsrAccelCurveLerp(plan->f_cur, plan->f_tgt, ratio));
}
done -= plan->acc_n;
if (done < plan->const_n)
{
- return PlsrAccelCurveClampFreq(plan->spd_target);
+ return PlsrAccelCurveClampFreq(plan->f_tgt);
}
- done -= plan->const_n;
if (plan->dec_n == 0U)
{
- return PlsrAccelCurveClampFreq(plan->spd_end);
+ return PlsrAccelCurveClampFreq(plan->f_end);
}
- ratio = PlsrAccelCurveShape(done, plan->dec_n, plan->mode);
- if (plan->spd_target >= plan->spd_end)
- {
- f = plan->spd_target - ((plan->spd_target - plan->spd_end) * ratio) / 1000UL;
- }
- else
+ T = plan->t_dec_ms;
+ if (T == 0U)
{
- f = plan->spd_target + ((plan->spd_end - plan->spd_target) * ratio) / 1000UL;
+ return PlsrAccelCurveClampFreq(plan->f_end);
}
- return PlsrAccelCurveClampFreq(f);
+ ratio = PlsrAccelCurveShape(phase_elapsed_ms, T, plan->mode);
+ return PlsrAccelCurveClampFreq(
+ PlsrAccelCurveLerp(plan->f_tgt, plan->f_end, ratio));
}
diff --git a/plsr/accel_curve/plsr_accel_curve.h b/plsr/accel_curve/plsr_accel_curve.h
index c4c3b7f..b9d12ce 100644
--- a/plsr/accel_curve/plsr_accel_curve.h
+++ b/plsr/accel_curve/plsr_accel_curve.h
@@ -1,9 +1,11 @@
/**
* @file plsr_accel_curve.h
- * @brief 加减速曲线：直线 / S / 正弦；按脉冲数规划梯形并求瞬时频率
+ * @brief 预估加/匀/减脉冲；相内按时间走直线/S/正弦
*
- * 输入：总脉冲、起速 f0、目标 f1、止速 f2、加减速时间、曲线类型
- * 输出：acc/const/dec 脉冲分配；按已发脉冲查当前 Hz
+ * 相界（何时匀速/减速）按预估脉冲划分。
+ * 相内频率形状按相内经过时间 t 与 T_acc/T_dec 计算。
+ * 脉冲不够时降低峰值、保留相时间。
+ * 加/减速脉冲按相内最高频估算，避免减速相高速时过早耗尽脉冲而硬停。
*/
#ifndef PLSR_ACCEL_CURVE_H
#define PLSR_ACCEL_CURVE_H
@@ -11,37 +13,46 @@
#include
#include "plsr_param.h"
-/** 一段运动的频率规划结果（由 Plan 填充，FreqAt 查询） */
typedef struct {
uint32_t acc_n;
uint32_t const_n;
uint32_t dec_n;
- uint32_t spd_start;
- uint32_t spd_target;
- uint32_t spd_end;
+ uint32_t f_cur;
+ uint32_t f_tgt;
+ uint32_t f_end;
+ uint32_t t_acc_ms;
+ uint32_t t_dec_ms;
PlsrAccelMode_e mode;
} PlsrAccelPlan_t;
-/** 限幅到 [1, 100000] Hz */
+typedef enum {
+ PLSR_CURVE_PHASE_ACC = 0,
+ PLSR_CURVE_PHASE_CONST,
+ PLSR_CURVE_PHASE_DEC
+} PlsrCurvePhase_e;
+
uint32_t PlsrAccelCurveClampFreq(uint32_t freq_hz);
-/**
- * @brief 规划梯形：用平均速度×时间估算加/减速脉冲数，剩余为匀速
- * @note 若 acc+dec 超过总脉冲，则对半分加速/减速、无匀速段
- */
void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
uint32_t total_pulses,
- uint32_t f0,
- uint32_t f1,
- uint32_t f2,
+ uint32_t f_cur,
+ uint32_t f_tgt,
+ uint32_t f_end,
+ uint32_t default_spd,
+ uint32_t start_spd_ref,
+ uint32_t end_spd_ref,
uint32_t accel_ms,
uint32_t decel_ms,
PlsrAccelMode_e mode);
+PlsrCurvePhase_e PlsrAccelCurvePhaseAt(const PlsrAccelPlan_t *plan,
+ uint32_t done_pulses);
+
/**
- * @brief 按本段已发出脉冲数插值当前频率
- * @param done_pulses 本段内已发个数（从 0 计）
+ * @param phase_elapsed_ms 当前相内已累计时间（脉冲周期累加）
*/
-uint32_t PlsrAccelCurveFreqAt(const PlsrAccelPlan_t *plan, uint32_t done_pulses);
+uint32_t PlsrAccelCurveFreqAt(const PlsrAccelPlan_t *plan,
+ uint32_t done_pulses,
+ uint32_t phase_elapsed_ms);
#endif
diff --git a/plsr/command/plsr_command.c b/plsr/command/plsr_command.c
index 9192f96..2e541af 100644
--- a/plsr/command/plsr_command.c
+++ b/plsr/command/plsr_command.c
@@ -1,6 +1,6 @@
/**
* @file plsr_command.c
- * @brief 指令接口：Modbus 命令/线圈 → 参数块 + 运行控制；状态回写
+ * @brief 指令接口：Modbus 命令/线圈 → 参数块 + 运行控制；状态与故障回写
*/
#include "plsr_command.h"
#include "plsr_param.h"
@@ -8,25 +8,41 @@
#include "plsr.h"
#include "modbus_rtu.h"
-/** 写 32 位值到相邻两个保持寄存器（低字在前） */
+static uint16_t s_fault = PLSR_ERR_NONE;
+
static void PlsrCommandWriteDWord(uint16_t addr, uint32_t value)
{
WriteHoldReg(addr, (uint16_t)(value & 0xFFFFU));
WriteHoldReg((uint16_t)(addr + 1U), (uint16_t)((value >> 16) & 0xFFFFU));
}
+void PlsrCommandSetFault(uint16_t code)
+{
+ s_fault = code;
+ WriteHoldReg(PLSR_CMD_REG_ERR, code);
+}
+
+void PlsrCommandClearFault(void)
+{
+ s_fault = PLSR_ERR_NONE;
+ WriteHoldReg(PLSR_CMD_REG_ERR, PLSR_ERR_NONE);
+}
+
+uint16_t PlsrCommandGetFault(void)
+{
+ return s_fault;
+}
+
void PlsrCommandInit(void)
{
WriteHoldReg(PLSR_CMD_REG_COMMAND, PLSR_CMD_IDLE);
- WriteHoldReg(PLSR_CMD_REG_ERR, 0U);
- /* TODO：重复启动防护、GOON、fault_buf */
+ PlsrCommandClearFault();
}
void PlsrCommandPoll(void)
{
uint16_t cmd = ReadHoldReg(PLSR_CMD_REG_COMMAND);
- /* --- 线圈边沿消费（写 1 触发后清 0） --- */
if (ReadCoil(PLSR_CMD_COIL_CLR_ACC) != 0U)
{
WriteCoil(PLSR_CMD_COIL_CLR_ACC, 0U);
@@ -38,7 +54,6 @@ void PlsrCommandPoll(void)
PlsrStop();
}
- /* --- 命令寄存器（写后清 IDLE） --- */
if (cmd == PLSR_CMD_STOP)
{
WriteHoldReg(PLSR_CMD_REG_COMMAND, PLSR_CMD_IDLE);
@@ -47,20 +62,18 @@ void PlsrCommandPoll(void)
else if ((cmd == PLSR_CMD_START) || (cmd == PLSR_CMD_IMPORT))
{
WriteHoldReg(PLSR_CMD_REG_COMMAND, PLSR_CMD_IDLE);
- /* 启动前刷新参数块；IMPORT 只刷新不启动 */
(void)PlsrParamBlockApplyFromHold();
if (cmd == PLSR_CMD_START)
{
- /* TODO：忙时再次 START 应写故障码，而非静默 */
+ PlsrCommandClearFault();
PlsrStop();
(void)PlsrStart(PlsrParamGetStartSeg());
}
}
- /* --- 状态镜像，供上位机读 --- */
WriteHoldReg(PLSR_CMD_REG_BUSY, PlsrIsBusy());
WriteHoldReg(PLSR_CMD_REG_CUR_SEG, PlsrRunControlGetCurSeg());
PlsrCommandWriteDWord(PLSR_CMD_REG_CUR_FREQ_L, PlsrRunControlGetCurFreq());
PlsrCommandWriteDWord(PLSR_CMD_REG_ACC_PULSE_L, (uint32_t)PlsrGetAccPulse());
- WriteHoldReg(PLSR_CMD_REG_ERR, 0U);
+ WriteHoldReg(PLSR_CMD_REG_ERR, s_fault);
}
diff --git a/plsr/command/plsr_command.h b/plsr/command/plsr_command.h
index db9994a..bfa5659 100644
--- a/plsr/command/plsr_command.h
+++ b/plsr/command/plsr_command.h
@@ -21,19 +21,24 @@
#define PLSR_CMD_REG_CUR_FREQ_H 0x10F4U
#define PLSR_CMD_REG_ACC_PULSE_L 0x10F5U
#define PLSR_CMD_REG_ACC_PULSE_H 0x10F6U
-#define PLSR_CMD_REG_ERR 0x10F7U /* 故障码（待完善） */
+#define PLSR_CMD_REG_ERR 0x10F7U /* 故障码 */
#define PLSR_CMD_IDLE 0U
#define PLSR_CMD_START 1U
#define PLSR_CMD_STOP 2U
-#define PLSR_CMD_IMPORT 3U /* 仅从寄存器刷新参数块，不启动 */
+#define PLSR_CMD_IMPORT 3U
-#define PLSR_CMD_COIL_STOP 0x2001U /* 急停线圈 */
-#define PLSR_CMD_COIL_CLR_ACC 0x2003U /* 清累计脉冲线圈 */
+#define PLSR_CMD_COIL_STOP 0x2001U
+#define PLSR_CMD_COIL_CLR_ACC 0x2003U
-void PlsrCommandInit(void);
+/* 故障码（写入 0x10F7，供 Modbus 读取） */
+#define PLSR_ERR_NONE 0U
+#define PLSR_ERR_SEG_FREQ 1U /* 段频率非法（非 0 且越界，或解析后不在 1~100k） */
-/* 由 PlsrTask 周期调用：收指令、分发、回写状态 */
+void PlsrCommandInit(void);
void PlsrCommandPoll(void);
+void PlsrCommandSetFault(uint16_t code);
+void PlsrCommandClearFault(void);
+uint16_t PlsrCommandGetFault(void);
#endif
diff --git a/plsr/param/plsr_param.c b/plsr/param/plsr_param.c
index 1c5f2e2..1197266 100644
--- a/plsr/param/plsr_param.c
+++ b/plsr/param/plsr_param.c
@@ -169,12 +169,8 @@ uint8_t PlsrParamBlockApplyFromHold(void)
{
s_cfg.pulse_y = 0U;
}
- s_cfg.dir_y = ReadHoldReg(PLSR_REG_DIR_Y);
- if (s_cfg.dir_y != 3U)
- {
- s_cfg.dir_y = 3U;
- WriteHoldReg(PLSR_REG_DIR_Y, 3U);
- }
+ s_cfg.dir_y = 3U; /* 方向固定 Y3，忽略上位机其它值 */
+ WriteHoldReg(PLSR_REG_DIR_Y, 3U);
s_cfg.wait_x_sel = ReadHoldReg(PLSR_REG_WAIT_X) ? 1U : 0U;
s_cfg.ext_x_sel = ReadHoldReg(PLSR_REG_EXT_X) ? 1U : 0U;
diff --git a/plsr/param/plsr_param.h b/plsr/param/plsr_param.h
index ef7a738..56c3f29 100644
--- a/plsr/param/plsr_param.h
+++ b/plsr/param/plsr_param.h
@@ -15,8 +15,8 @@
#define PLSR_SEG_MAX 10U
/* ---------- 公共参数保持寄存器 ---------- */
-#define PLSR_REG_PULSE_Y 0x1000U /* 脉冲端子 0=Y0..2=Y2 */
-#define PLSR_REG_DIR_Y 0x1001U /* 方向端子，当前仅 3=Y3 */
+#define PLSR_REG_PULSE_Y 0x1000U /* 0=Y0 1=Y1 2=Y2 */
+#define PLSR_REG_DIR_Y 0x1001U /* 固定写 3=Y3 */
#define PLSR_REG_WAIT_X 0x1002U /* WAIT 输入选择 */
#define PLSR_REG_EXT_X 0x1003U /* EXT 输入选择 */
#define PLSR_REG_SEND_MODE 0x1004U /* 0完成 1后续 */
@@ -90,8 +90,8 @@ typedef enum {
/** 公共运行参数（运动侧只读此映像，不直接啃寄存器） */
typedef struct {
- uint16_t pulse_y; /* 0=Y0,1=Y1,2=Y2 */
- uint16_t dir_y; /* 3=Y3 */
+ uint16_t pulse_y; /* 0=Y0/TIM10 1=Y1/TIM11 2=Y2/TIM13 */
+ uint16_t dir_y; /* 固定 3=Y3，不可选 */
uint16_t wait_x_sel;
uint16_t ext_x_sel;
PlsrSendMode_e send_mode;
@@ -101,17 +101,17 @@ typedef struct {
PlsrPosMode_e run_mode;
uint16_t seg_count;
uint16_t start_seg; /* 1-based */
- uint32_t default_speed;
- uint32_t start_speed;
- uint32_t end_speed;
- uint16_t accel_ms;
- uint16_t decel_ms;
+ uint32_t default_speed; /* 脉冲默认速度 Hz：定加减速斜率 K；段频为 0 时也作段目标 */
+ uint32_t start_speed; /* 脉冲起始速度：完成模式首段从该频起跳再升降 */
+ uint32_t end_speed; /* 脉冲终止速度：末段减速落到该频后再停到 0 */
+ uint16_t accel_ms; /* 默认速度加速时间 ms（定加速斜率） */
+ uint16_t decel_ms; /* 默认速度减速时间 ms（定减速斜率） */
} PlsrCfg_t;
/** 单段运行参数 */
typedef struct {
- int32_t freq_hz;
- int32_t pulse_cnt; /* 可负 */
+ int32_t freq_hz; /* 0=用默认速度；须解析后在 1~100k，否则故障 */
+ int32_t pulse_cnt; /* 可负=反向；0=本段不发 */
PlsrWaitType_e wait_type;
uint16_t wait_ms;
uint16_t act_ms;
diff --git a/plsr/plsr.c b/plsr/plsr.c
index ebbd694..30f0f15 100644
--- a/plsr/plsr.c
+++ b/plsr/plsr.c
@@ -32,6 +32,11 @@ void PlsrStop(void)
PlsrRunControlStop();
}
+uint8_t PlsrChangeFreq(uint32_t new_tgt_hz)
+{
+ return PlsrRunControlChangeFreq(new_tgt_hz);
+}
+
uint8_t PlsrIsBusy(void)
{
return PlsrRunControlIsBusy();
diff --git a/plsr/plsr.h b/plsr/plsr.h
index 6110d2d..6146444 100644
--- a/plsr/plsr.h
+++ b/plsr/plsr.h
@@ -31,6 +31,12 @@ uint8_t PlsrStart(uint16_t start_seg);
/** 急停：关 PWM、清方向、退出运行 */
void PlsrStop(void);
+/**
+ * @brief 运行中动态改目标频率（当前输出频率 → new_tgt，按 accel_mode 过渡）
+ * @return 1=已重规划，0=非运行或频率非法
+ */
+uint8_t PlsrChangeFreq(uint32_t new_tgt_hz);
+
uint8_t PlsrIsBusy(void); /**< 1=运动中 */
int32_t PlsrGetAccPulse(void); /**< 累计脉冲（绝对坐标用） */
void PlsrClearAccPulse(void);
diff --git a/plsr/pulse_driver/plsr_pulse_driver.c b/plsr/pulse_driver/plsr_pulse_driver.c
index a86fedb..a368168 100644
--- a/plsr/pulse_driver/plsr_pulse_driver.c
+++ b/plsr/pulse_driver/plsr_pulse_driver.c
@@ -1,25 +1,63 @@
/**
* @file plsr_pulse_driver.c
- * @brief 脉冲输出驱动模块实现（TIM10 PWM + 方向 GPIO）
- * @note TIMxCLK=168MHz；运行中改频不写 EGR.UG（避免 UPDATE 中断风暴）
- *
- * 板级：Y0=PF6 TIM10_CH1；Y3=PF9 方向（cfg->dir_y=3）
- * AB 正交模式待实现。
+ * @brief 脉冲输出：Y0/TIM10、Y1/TIM11、Y2/TIM13；方向固定 Y3
+ * @note TIMxCLK=168MHz；段内锁 PSC 只改 ARR；改频在周期边界生效
*/
#include "plsr_pulse_driver.h"
#include "plsr_param.h"
#include "main.h"
TIM_HandleTypeDef htim10;
+TIM_HandleTypeDef htim11;
+TIM_HandleTypeDef htim13;
#define PLSR_TIM_CLK_HZ 168000000UL
#define PLSR_Y0_PORT GPIOF
#define PLSR_Y0_PIN GPIO_PIN_6
+#define PLSR_Y1_PORT GPIOF
+#define PLSR_Y1_PIN GPIO_PIN_8
+#define PLSR_Y2_PORT GPIOF
+#define PLSR_Y2_PIN GPIO_PIN_7
#define PLSR_Y3_PORT GPIOF
#define PLSR_Y3_PIN GPIO_PIN_9
static uint32_t s_last_freq;
+static uint32_t s_last_psc = 0xFFFFFFFFUL;
+static uint32_t s_last_arr = 0xFFFFFFFFUL;
+static uint8_t s_active_y;
+static TIM_HandleTypeDef *s_active_htim;
+
+static uint8_t s_psc_locked;
+static uint32_t s_locked_psc;
+
+static volatile uint32_t s_req_freq;
+static volatile uint8_t s_req_pending;
+
+static TIM_HandleTypeDef *PlsrPulseDriverHtimByY(uint16_t pulse_y)
+{
+ if (pulse_y == 1U)
+ {
+ return &htim11;
+ }
+ if (pulse_y == 2U)
+ {
+ return &htim13;
+ }
+ return &htim10;
+}
+
+static uint8_t PlsrPulseDriverSelectY(void)
+{
+ PlsrCfg_t *cfg = PlsrParamGetCfg();
+ uint16_t y = cfg->pulse_y;
+
+ if (y > 2U)
+ {
+ y = 0U;
+ }
+ return (uint8_t)y;
+}
static void PlsrPulseDriverDirGpioInit(void)
{
@@ -39,36 +77,34 @@ void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef *htim)
{
GPIO_InitTypeDef gpio = {0};
- if (htim->Instance != TIM10)
- {
- return;
- }
-
- __HAL_RCC_TIM10_CLK_ENABLE();
__HAL_RCC_GPIOF_CLK_ENABLE();
- gpio.Pin = PLSR_Y0_PIN;
gpio.Mode = GPIO_MODE_AF_PP;
gpio.Pull = GPIO_NOPULL;
gpio.Speed = GPIO_SPEED_FREQ_HIGH;
- gpio.Alternate = GPIO_AF3_TIM10;
- HAL_GPIO_Init(PLSR_Y0_PORT, &gpio);
- HAL_NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 5, 0);
- HAL_NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn);
+ if (htim->Instance == TIM10)
+ {
+ __HAL_RCC_TIM10_CLK_ENABLE();
+ gpio.Pin = PLSR_Y0_PIN;
+ gpio.Alternate = GPIO_AF3_TIM10;
+ HAL_GPIO_Init(PLSR_Y0_PORT, &gpio);
+ HAL_NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 5, 0);
+ HAL_NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn);
+ }
}
-static void PlsrPulseDriverTim10Init(void)
+static void PlsrPulseDriverTimInitOne(TIM_HandleTypeDef *htim, TIM_TypeDef *inst)
{
TIM_OC_InitTypeDef oc = {0};
- htim10.Instance = TIM10;
- htim10.Init.Prescaler = 167U;
- htim10.Init.CounterMode = TIM_COUNTERMODE_UP;
- htim10.Init.Period = 999U;
- htim10.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
- htim10.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
- if (HAL_TIM_PWM_Init(&htim10) != HAL_OK)
+ htim->Instance = inst;
+ htim->Init.Prescaler = 167U;
+ htim->Init.CounterMode = TIM_COUNTERMODE_UP;
+ htim->Init.Period = 999U;
+ htim->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+ htim->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
+ if (HAL_TIM_PWM_Init(htim) != HAL_OK)
{
Error_Handler();
}
@@ -77,27 +113,32 @@ static void PlsrPulseDriverTim10Init(void)
oc.Pulse = 500U;
oc.OCPolarity = TIM_OCPOLARITY_HIGH;
oc.OCFastMode = TIM_OCFAST_DISABLE;
- if (HAL_TIM_PWM_ConfigChannel(&htim10, &oc, TIM_CHANNEL_1) != HAL_OK)
+ if (HAL_TIM_PWM_ConfigChannel(htim, &oc, TIM_CHANNEL_1) != HAL_OK)
{
Error_Handler();
}
- htim10.Instance->CCMR1 |= TIM_CCMR1_OC1PE;
+ htim->Instance->CCMR1 |= TIM_CCMR1_OC1PE;
}
void PlsrPulseDriverInit(void)
{
s_last_freq = 0U;
+ s_last_psc = 0xFFFFFFFFUL;
+ s_last_arr = 0xFFFFFFFFUL;
+ s_psc_locked = 0U;
+ s_locked_psc = 0U;
+ s_req_pending = 0U;
+ s_req_freq = 0U;
+ s_active_y = 0U;
+ s_active_htim = &htim10;
PlsrPulseDriverDirGpioInit();
- PlsrPulseDriverTim10Init();
- /* TODO：按全局参数切换脉冲+方向 / AB 正交；故障码上报自诊断 */
+ PlsrPulseDriverTimInitOne(&htim10, TIM10);
}
void PlsrPulseDriverSetDir(uint8_t forward)
{
PlsrCfg_t *cfg = PlsrParamGetCfg();
uint8_t level;
- GPIO_TypeDef *port;
- uint16_t pin;
if (cfg->dir_logic == PLSR_DIR_LOGIC_POS)
{
@@ -107,17 +148,8 @@ void PlsrPulseDriverSetDir(uint8_t forward)
{
level = (forward != 0U) ? 0U : 1U;
}
-
- switch (cfg->dir_y)
- {
- case 3U:
- default:
- port = PLSR_Y3_PORT;
- pin = PLSR_Y3_PIN;
- break;
- }
-
- HAL_GPIO_WritePin(port, pin, level ? GPIO_PIN_SET : GPIO_PIN_RESET);
+ HAL_GPIO_WritePin(PLSR_Y3_PORT, PLSR_Y3_PIN,
+ level ? GPIO_PIN_SET : GPIO_PIN_RESET);
}
void PlsrPulseDriverClearDir(void)
@@ -125,10 +157,15 @@ void PlsrPulseDriverClearDir(void)
HAL_GPIO_WritePin(PLSR_Y3_PORT, PLSR_Y3_PIN, GPIO_PIN_RESET);
}
-static void PlsrPulseDriverCalcPscArr(uint32_t freq_hz, uint32_t *psc, uint32_t *arr, uint32_t *ccr)
+uint8_t PlsrPulseDriverIsPulseTim(TIM_TypeDef *instance)
+{
+ return ((instance == TIM10) || (instance == TIM11) || (instance == TIM13)) ? 1U : 0U;
+}
+
+static void PlsrPulseDriverCalcArrWithPsc(uint32_t freq_hz, uint32_t psc,
+ uint32_t *arr, uint32_t *ccr)
{
uint32_t ticks;
- uint32_t p;
uint32_t a;
uint32_t c;
@@ -147,6 +184,47 @@ static void PlsrPulseDriverCalcPscArr(uint32_t freq_hz, uint32_t *psc, uint32_t
ticks = 2UL;
}
+ a = ticks / (psc + 1UL);
+ if (a < 2UL)
+ {
+ a = 2UL;
+ }
+ if (a > 65536UL)
+ {
+ a = 65536UL;
+ }
+ a -= 1UL;
+
+ c = (a + 1UL) / 2UL;
+ if (c == 0UL)
+ {
+ c = 1UL;
+ }
+
+ *arr = a;
+ *ccr = c;
+}
+
+static void PlsrPulseDriverCalcPscArr(uint32_t freq_hz, uint32_t *psc, uint32_t *arr, uint32_t *ccr)
+{
+ uint32_t ticks;
+ uint32_t p;
+
+ if (freq_hz < 1U)
+ {
+ freq_hz = 1U;
+ }
+ if (freq_hz > 100000U)
+ {
+ freq_hz = 100000U;
+ }
+
+ ticks = PLSR_TIM_CLK_HZ / freq_hz;
+ if (ticks < 2UL)
+ {
+ ticks = 2UL;
+ }
+
p = (ticks + 65535UL - 1UL) / 65535UL;
if (p == 0UL)
{
@@ -158,79 +236,242 @@ static void PlsrPulseDriverCalcPscArr(uint32_t freq_hz, uint32_t *psc, uint32_t
p = 65535UL;
}
- a = ticks / (p + 1UL);
- if (a < 2UL)
+ *psc = p;
+ PlsrPulseDriverCalcArrWithPsc(freq_hz, p, arr, ccr);
+}
+
+void PlsrPulseDriverLockPscRange(uint32_t f_min_hz, uint32_t f_max_hz)
+{
+ uint32_t ticks_lo;
+ uint32_t ticks_hi;
+ uint32_t psc_div_lo;
+ uint32_t psc_div_hi;
+ uint32_t div;
+
+ if (f_min_hz < 1U)
{
- a = 2UL;
+ f_min_hz = 1U;
}
- a -= 1UL;
- if (a > 65535UL)
+ if (f_max_hz < 1U)
{
- a = 65535UL;
+ f_max_hz = 1U;
+ }
+ if (f_max_hz < f_min_hz)
+ {
+ uint32_t t = f_min_hz;
+ f_min_hz = f_max_hz;
+ f_max_hz = t;
}
- c = (a + 1UL) / 2UL;
- if (c == 0UL)
+ ticks_lo = PLSR_TIM_CLK_HZ / f_min_hz;
+ ticks_hi = PLSR_TIM_CLK_HZ / f_max_hz;
+ if (ticks_lo < 2UL)
{
- c = 1UL;
+ ticks_lo = 2UL;
+ }
+ if (ticks_hi < 2UL)
+ {
+ ticks_hi = 2UL;
}
- *psc = p;
- *arr = a;
- *ccr = c;
+ /* psc_div = PSC+1：低频要求 div 足够大，高频要求 div 足够小 */
+ psc_div_lo = (ticks_lo + 65535UL) / 65536UL;
+ if (psc_div_lo < 1UL)
+ {
+ psc_div_lo = 1UL;
+ }
+ psc_div_hi = ticks_hi / 2UL;
+ if (psc_div_hi < 1UL)
+ {
+ psc_div_hi = 1UL;
+ }
+
+ if (psc_div_lo > psc_div_hi)
+ {
+ /* 跨度过大：优先保证低频可表示 */
+ div = psc_div_lo;
+ }
+ else
+ {
+ div = psc_div_lo;
+ }
+
+ s_locked_psc = div - 1UL;
+ s_psc_locked = 1U;
}
-void PlsrPulseDriverSetFreq(uint32_t freq_hz)
+void PlsrPulseDriverUnlockPsc(void)
+{
+ s_psc_locked = 0U;
+ s_locked_psc = 0U;
+}
+
+static void PlsrPulseDriverStopHtim(TIM_HandleTypeDef *htim)
+{
+ if (htim == (TIM_HandleTypeDef *)0)
+ {
+ return;
+ }
+ __HAL_TIM_DISABLE_IT(htim, TIM_IT_UPDATE);
+ (void)HAL_TIM_PWM_Stop(htim, TIM_CHANNEL_1);
+ __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_UPDATE);
+ __HAL_TIM_CLEAR_IT(htim, TIM_IT_UPDATE);
+}
+
+static void PlsrPulseDriverStartCommon(uint32_t freq_hz, uint8_t enable_update_it)
{
uint32_t psc;
uint32_t arr;
uint32_t ccr;
+ uint8_t y = PlsrPulseDriverSelectY();
+ TIM_HandleTypeDef *htim = PlsrPulseDriverHtimByY(y);
- if (freq_hz == s_last_freq)
+ s_req_pending = 0U;
+
+ if ((s_active_htim != (TIM_HandleTypeDef *)0) && (s_active_htim != htim))
+ {
+ PlsrPulseDriverStopHtim(s_active_htim);
+ }
+ s_active_y = y;
+ s_active_htim = htim;
+
+ if (freq_hz == 0U)
{
+ PlsrPulseDriverStopHtim(htim);
+ s_last_freq = 0U;
+ s_last_psc = 0xFFFFFFFFUL;
+ s_last_arr = 0xFFFFFFFFUL;
return;
}
- PlsrPulseDriverCalcPscArr(freq_hz, &psc, &arr, &ccr);
+ if (s_psc_locked != 0U)
+ {
+ psc = s_locked_psc;
+ PlsrPulseDriverCalcArrWithPsc(freq_hz, psc, &arr, &ccr);
+ }
+ else
+ {
+ PlsrPulseDriverCalcPscArr(freq_hz, &psc, &arr, &ccr);
+ }
+
s_last_freq = freq_hz;
+ s_last_psc = psc;
+ s_last_arr = arr;
+
+ __HAL_TIM_DISABLE(htim);
+ __HAL_TIM_DISABLE_IT(htim, TIM_IT_UPDATE);
+
+ __HAL_TIM_SET_PRESCALER(htim, psc);
+ __HAL_TIM_SET_AUTORELOAD(htim, arr);
+ __HAL_TIM_SET_COMPARE(htim, TIM_CHANNEL_1, ccr);
+ __HAL_TIM_SET_COUNTER(htim, 0U);
+
+ htim->Instance->EGR = TIM_EGR_UG;
+ __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_UPDATE);
+ __HAL_TIM_CLEAR_IT(htim, TIM_IT_UPDATE);
- __HAL_TIM_SET_PRESCALER(&htim10, psc);
- __HAL_TIM_SET_AUTORELOAD(&htim10, arr);
- __HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, ccr);
+ if (HAL_TIM_PWM_Start(htim, TIM_CHANNEL_1) != HAL_OK)
+ {
+ TIM_CHANNEL_STATE_SET(htim, TIM_CHANNEL_1, HAL_TIM_CHANNEL_STATE_READY);
+ (void)HAL_TIM_PWM_Start(htim, TIM_CHANNEL_1);
+ }
+
+ if (enable_update_it != 0U)
+ {
+ __HAL_TIM_ENABLE_IT(htim, TIM_IT_UPDATE);
+ }
}
-static void PlsrPulseDriverStartCommon(uint32_t freq_hz, uint8_t enable_update_it)
+void PlsrPulseDriverSetFreq(uint32_t freq_hz)
{
uint32_t psc;
uint32_t arr;
uint32_t ccr;
+ TIM_HandleTypeDef *htim;
- PlsrPulseDriverCalcPscArr(freq_hz, &psc, &arr, &ccr);
- s_last_freq = freq_hz;
+ if (freq_hz == s_last_freq)
+ {
+ return;
+ }
- /* 先停表再装载参数，并用 UG 同步影子寄存器；计脉冲时再开 UPDATE IT */
- __HAL_TIM_DISABLE(&htim10);
- __HAL_TIM_DISABLE_IT(&htim10, TIM_IT_UPDATE);
+ htim = (s_active_htim != (TIM_HandleTypeDef *)0) ?
+ s_active_htim : PlsrPulseDriverHtimByY(PlsrPulseDriverSelectY());
+ s_active_htim = htim;
- __HAL_TIM_SET_PRESCALER(&htim10, psc);
- __HAL_TIM_SET_AUTORELOAD(&htim10, arr);
- __HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, ccr);
- __HAL_TIM_SET_COUNTER(&htim10, 0U);
+ if (freq_hz == 0U)
+ {
+ PlsrPulseDriverStopHtim(htim);
+ s_last_freq = 0U;
+ s_last_psc = 0xFFFFFFFFUL;
+ s_last_arr = 0xFFFFFFFFUL;
+ return;
+ }
- htim10.Instance->EGR = TIM_EGR_UG;
- __HAL_TIM_CLEAR_FLAG(&htim10, TIM_FLAG_UPDATE);
- __HAL_TIM_CLEAR_IT(&htim10, TIM_IT_UPDATE);
+ if (s_psc_locked != 0U)
+ {
+ psc = s_locked_psc;
+ PlsrPulseDriverCalcArrWithPsc(freq_hz, psc, &arr, &ccr);
+ }
+ else
+ {
+ PlsrPulseDriverCalcPscArr(freq_hz, &psc, &arr, &ccr);
+ }
- if (HAL_TIM_PWM_Start(&htim10, TIM_CHANNEL_1) != HAL_OK)
+ if ((psc == s_last_psc) && (arr == s_last_arr))
{
- TIM_CHANNEL_STATE_SET(&htim10, TIM_CHANNEL_1, HAL_TIM_CHANNEL_STATE_READY);
- (void)HAL_TIM_PWM_Start(&htim10, TIM_CHANNEL_1);
+ s_last_freq = freq_hz;
+ return;
}
- if (enable_update_it != 0U)
+ if ((s_psc_locked != 0U) || (psc == s_last_psc))
+ {
+ /* 同 PSC：只改 ARR（预装载），下一周期自然切换，无停表毛刺 */
+ __HAL_TIM_SET_AUTORELOAD(htim, arr);
+ __HAL_TIM_SET_COMPARE(htim, TIM_CHANNEL_1, ccr);
+ __HAL_TIM_ENABLE_IT(htim, TIM_IT_UPDATE);
+ }
+ else
+ {
+ __HAL_TIM_DISABLE_IT(htim, TIM_IT_UPDATE);
+ __HAL_TIM_DISABLE(htim);
+ __HAL_TIM_SET_PRESCALER(htim, psc);
+ __HAL_TIM_SET_AUTORELOAD(htim, arr);
+ __HAL_TIM_SET_COMPARE(htim, TIM_CHANNEL_1, ccr);
+ __HAL_TIM_SET_COUNTER(htim, 0U);
+ htim->Instance->EGR = TIM_EGR_UG;
+ __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_UPDATE);
+ __HAL_TIM_CLEAR_IT(htim, TIM_IT_UPDATE);
+ __HAL_TIM_ENABLE(htim);
+ __HAL_TIM_ENABLE_IT(htim, TIM_IT_UPDATE);
+ }
+
+ s_last_freq = freq_hz;
+ s_last_psc = psc;
+ s_last_arr = arr;
+}
+
+void PlsrPulseDriverRequestFreq(uint32_t freq_hz)
+{
+ s_req_freq = freq_hz;
+ s_req_pending = 1U;
+}
+
+void PlsrPulseDriverApplyPending(void)
+{
+ uint32_t f;
+
+ if (s_req_pending == 0U)
{
- __HAL_TIM_ENABLE_IT(&htim10, TIM_IT_UPDATE);
+ return;
+ }
+ s_req_pending = 0U;
+ f = s_req_freq;
+ if (f == 0U)
+ {
+ PlsrPulseDriverStop();
+ return;
}
+ PlsrPulseDriverSetFreq(f);
}
void PlsrPulseDriverStart(uint32_t freq_hz)
@@ -245,9 +486,11 @@ void PlsrPulseDriverStartFreeRun(uint32_t freq_hz)
void PlsrPulseDriverStop(void)
{
- __HAL_TIM_DISABLE_IT(&htim10, TIM_IT_UPDATE);
- (void)HAL_TIM_PWM_Stop(&htim10, TIM_CHANNEL_1);
- __HAL_TIM_CLEAR_FLAG(&htim10, TIM_FLAG_UPDATE);
- __HAL_TIM_CLEAR_IT(&htim10, TIM_IT_UPDATE);
+ s_req_pending = 0U;
+ PlsrPulseDriverStopHtim(&htim10);
+ PlsrPulseDriverStopHtim(&htim11);
+ PlsrPulseDriverStopHtim(&htim13);
s_last_freq = 0U;
+ s_last_psc = 0xFFFFFFFFUL;
+ s_last_arr = 0xFFFFFFFFUL;
}
diff --git a/plsr/pulse_driver/plsr_pulse_driver.h b/plsr/pulse_driver/plsr_pulse_driver.h
index 8e6ae57..18973dc 100644
--- a/plsr/pulse_driver/plsr_pulse_driver.h
+++ b/plsr/pulse_driver/plsr_pulse_driver.h
@@ -2,10 +2,14 @@
* @file plsr_pulse_driver.h
* @brief 脉冲输出驱动（方案书 5.1）：TIM 脉冲 + 方向 GPIO
*
- * 已实现：脉冲+方向（TIM10→Y0，方向→Y3）
- * 待实现：AB 正交模式、硬件故障码→自诊断
+ * 脉冲端子（由参数 pulse_y 选择）：
+ * Y0 → PF6 / TIM10_CH1
+ * Y1 → PF8 / TIM11_CH1
+ * Y2 → PF7 / TIM13_CH1
+ * 方向固定 Y3 → PF9 GPIO（不可选）
*
- * 调用方：运行控制（启停/调频/方向）
+ * 频率 0：停止该路 PWM；>0 时按 Hz 输出。
+ * 段内锁定 PSC、只改 ARR；改频请求在脉冲周期边界生效，减少毛刺。
*/
#ifndef PLSR_PULSE_DRIVER_H
#define PLSR_PULSE_DRIVER_H
@@ -13,27 +17,28 @@
#include
#include "stm32f4xx_hal.h"
-/* TIM10 句柄，供中断服务里 HAL_TIM_IRQHandler 使用 */
extern TIM_HandleTypeDef htim10;
+extern TIM_HandleTypeDef htim11;
+extern TIM_HandleTypeDef htim13;
void PlsrPulseDriverInit(void);
-/* forward=1 为正转意图；实际电平还受 dir_logic、dir_y 影响 */
void PlsrPulseDriverSetDir(uint8_t forward);
-
-/* 方向脚拉低（停机/全部结束） */
void PlsrPulseDriverClearDir(void);
-/* 按频率启动 PWM，并打开 UPDATE 中断以便计脉冲 */
-void PlsrPulseDriverStart(uint32_t freq_hz);
+/** 按本段 [f_min,f_max] 锁定 PSC，整段只改 ARR */
+void PlsrPulseDriverLockPscRange(uint32_t f_min_hz, uint32_t f_max_hz);
+void PlsrPulseDriverUnlockPsc(void);
-/* 启动 PWM 但不计脉冲（预留） */
+void PlsrPulseDriverStart(uint32_t freq_hz);
void PlsrPulseDriverStartFreeRun(uint32_t freq_hz);
-
-/* 运行中改频：只改 PSC/ARR/CCR，禁止写 EGR.UG */
void PlsrPulseDriverSetFreq(uint32_t freq_hz);
-
-/* 停止 PWM 并关 UPDATE 中断 */
+/** 请求改频（ISR 周期边界再落到硬件） */
+void PlsrPulseDriverRequestFreq(uint32_t freq_hz);
+/** 在更新中断里调用：应用挂起频率 */
+void PlsrPulseDriverApplyPending(void);
void PlsrPulseDriverStop(void);
+uint8_t PlsrPulseDriverIsPulseTim(TIM_TypeDef *instance);
+
#endif
diff --git a/plsr/run_control/plsr_run_control.c b/plsr/run_control/plsr_run_control.c
index 2e782be..d3e12f5 100644
--- a/plsr/run_control/plsr_run_control.c
+++ b/plsr/run_control/plsr_run_control.c
@@ -1,37 +1,60 @@
/**
* @file plsr_run_control.c
- * @brief 运行控制：完成/后续模式状态机，调度路径规划与曲线，驱动脉冲输出
- * @note 延时一律用 OSTimeGet()（本工程 OS_APP_HOOKS_EN=0，HAL_GetTick 不递增）
+ * @brief 完成模式直线加减速（手册梯形/三角）
+ *
+ * 完成方式要点：
+ * - 段开始：阶跃到起速，再按时间直线升/降到本段目标（相时间按脉冲周期 µs 累加，避免 10ms 台阶）
+ * - 升降相按时间走完，不按脉冲数切相
+ * - 剩余脉冲 <= 预留减速脉冲时进入减速；非末段不预留减速
+ * - 末段减到终止速度后停机（落到 0）
*/
#include "plsr_run_control.h"
#include "plsr_path_plan.h"
#include "plsr_accel_curve.h"
#include "plsr_param.h"
#include "plsr_pulse_driver.h"
+#include "plsr_command.h"
#include "ucos_ii.h"
+#include
typedef enum {
- RC_IDLE = 0, /* 空闲 */
- RC_DIR_WAIT, /* 已置方向，等待 dir_delay 后再开 PWM */
- RC_RUN, /* 正在发脉冲 */
- RC_WAIT_COND /* 段间等待（时间类） */
+ RC_IDLE = 0,
+ RC_DIR_WAIT,
+ RC_RUN,
+ RC_WAIT_COND
} RcState_e;
+typedef enum {
+ PH_APPROACH = 0, /* 起速 → 本段目标/峰值，按时间 */
+ PH_CONST, /* 匀速 */
+ PH_DECEL /* → 终止速度，按时间 */
+} RunPhase_e;
+
static volatile RcState_e s_state;
static volatile uint8_t s_busy;
-static volatile uint8_t s_forward; /* 1=正转意图 */
-static volatile uint16_t s_cur_seg; /* 当前段 0-based */
+static volatile uint8_t s_forward;
+static volatile uint16_t s_cur_seg;
static volatile uint32_t s_cur_freq;
-static volatile int32_t s_done; /* 本段已发脉冲 */
-static volatile int32_t s_target; /* 本段目标脉冲数（绝对值） */
-static volatile int32_t s_acc_pulse; /* 全局累计脉冲 */
+static volatile int32_t s_done;
+static volatile int32_t s_target;
+static volatile int32_t s_acc_pulse;
static INT32U s_dir_deadline;
static INT32U s_wait_deadline;
static PlsrAccelPlan_t s_accel_plan;
-static uint32_t s_chain_freq; /* 后续模式：上一段结束频率，作下一段起速 */
-static uint8_t s_follow_cont; /* 1=同向衔接，不停 PWM */
+static RunPhase_e s_phase;
+static INT32U s_phase_t0;
+static uint32_t s_phase_elapsed_us; /* 相内时间（脉冲周期累加，µs） */
+static uint32_t s_approach_from;
+static uint32_t s_decel_from;
+static uint8_t s_follow_cont;
+
+static uint32_t s_chain_freq;
+static uint8_t s_chain_valid;
+static uint8_t s_pwm_on;
+
+static void PlsrRunControlAfterSegDone(void);
static INT32U PlsrRunControlMsToTicks(uint32_t ms)
{
@@ -54,21 +77,278 @@ static uint8_t PlsrRunControlOsTimeReached(INT32U deadline)
return ((INT32S)(OSTimeGet() - deadline) >= 0) ? 1U : 0U;
}
+static uint8_t PlsrRunControlGetSegTargetFreq(int32_t freq_hz,
+ uint32_t default_spd,
+ uint32_t *out_hz)
+{
+ uint32_t f;
+
+ if (freq_hz < 0)
+ {
+ return 0U;
+ }
+ if (freq_hz == 0)
+ {
+ f = default_spd;
+ }
+ else
+ {
+ f = (uint32_t)freq_hz;
+ }
+ if ((f < 1U) || (f > 100000U))
+ {
+ return 0U;
+ }
+ *out_hz = f;
+ return 1U;
+}
+
+static uint32_t PlsrRunControlClampSpeed(uint32_t spd)
+{
+ if (spd > 100000U)
+ {
+ spd = 100000U;
+ }
+ return spd;
+}
+
+static void PlsrRunControlApplyOutFreq(uint32_t profile_freq, uint8_t do_start)
+{
+ /*
+ * 完全按轮廓频率输出：0 就是停。
+ * 运行中改频只挂起，等脉冲周期边界再落到硬件，避免中途改 ARR 出毛刺。
+ */
+ s_cur_freq = profile_freq;
+
+ if (profile_freq == 0U)
+ {
+ PlsrPulseDriverStop();
+ s_pwm_on = 0U;
+ return;
+ }
+
+ if ((do_start != 0U) || (s_pwm_on == 0U))
+ {
+ PlsrPulseDriverStart(profile_freq);
+ s_pwm_on = 1U;
+ }
+ else
+ {
+ PlsrPulseDriverRequestFreq(profile_freq);
+ }
+}
+
+static uint32_t PlsrRunControlLerpUs(uint32_t a, uint32_t b, uint32_t t_us, uint32_t T_us)
+{
+ uint32_t r;
+
+ if ((T_us == 0U) || (t_us >= T_us))
+ {
+ return b;
+ }
+ /* r = t/T * 1000，用 64 位避免加速时间较长时溢出 */
+ r = (uint32_t)(((uint64_t)t_us * 1000ULL) / (uint64_t)T_us);
+ if (r > 1000U)
+ {
+ r = 1000U;
+ }
+ if (b >= a)
+ {
+ return a + ((b - a) * r) / 1000UL;
+ }
+ return a - ((a - b) * r) / 1000UL;
+}
+
+static void PlsrRunControlEnterPhase(RunPhase_e ph, uint32_t anchor_freq)
+{
+ s_phase = ph;
+ s_phase_t0 = OSTimeGet();
+ s_phase_elapsed_us = 0U;
+ if (ph == PH_APPROACH)
+ {
+ s_approach_from = anchor_freq;
+ }
+ else if (ph == PH_DECEL)
+ {
+ s_decel_from = anchor_freq;
+ }
+}
+
+static void PlsrRunControlRefreshProfile(uint8_t do_start)
+{
+ uint32_t next;
+ uint32_t remain;
+ uint32_t elapsed_us;
+ uint32_t T_us;
+ const PlsrAccelPlan_t *p = &s_accel_plan;
+
+ if (s_done < s_target)
+ {
+ remain = (uint32_t)(s_target - s_done);
+ }
+ else
+ {
+ remain = 0U;
+ }
+
+ elapsed_us = s_phase_elapsed_us;
+
+ /*
+ * 只有匀速相才按剩余脉冲进减速。
+ * 加速相未完成时禁止切减速，否则首段会跳过“起速→目标”的加速过程。
+ */
+ if ((s_phase == PH_CONST) && (p->dec_n > 0U) && (remain <= p->dec_n))
+ {
+ PlsrRunControlEnterPhase(PH_DECEL, s_cur_freq);
+ elapsed_us = 0U;
+ }
+
+ if (s_phase == PH_APPROACH)
+ {
+ T_us = p->t_acc_ms * 1000UL;
+ if (s_approach_from == p->f_tgt)
+ {
+ next = p->f_tgt;
+ PlsrRunControlEnterPhase(PH_CONST, next);
+ }
+ else if ((T_us == 0U) || (elapsed_us >= T_us))
+ {
+ next = p->f_tgt;
+ PlsrRunControlEnterPhase(PH_CONST, next);
+ if ((p->dec_n > 0U) && (remain <= p->dec_n))
+ {
+ PlsrRunControlEnterPhase(PH_DECEL, next);
+ elapsed_us = 0U;
+ T_us = p->t_dec_ms * 1000UL;
+ next = PlsrRunControlLerpUs(s_decel_from, p->f_end,
+ elapsed_us, T_us);
+ }
+ }
+ else
+ {
+ /* 时间线性：f = f0 + (f1-f0)*t/T，按 µs 连续变化，不是 10ms 台阶 */
+ next = PlsrRunControlLerpUs(s_approach_from, p->f_tgt,
+ elapsed_us, T_us);
+ }
+ }
+ else if (s_phase == PH_DECEL)
+ {
+ T_us = p->t_dec_ms * 1000UL;
+ if (T_us == 0U)
+ {
+ next = p->f_end;
+ }
+ else
+ {
+ next = PlsrRunControlLerpUs(s_decel_from, p->f_end,
+ elapsed_us, T_us);
+ }
+ }
+ else
+ {
+ next = p->f_tgt;
+ if ((p->dec_n > 0U) && (remain <= p->dec_n))
+ {
+ PlsrRunControlEnterPhase(PH_DECEL, next);
+ elapsed_us = 0U;
+ T_us = p->t_dec_ms * 1000UL;
+ next = PlsrRunControlLerpUs(s_decel_from, p->f_end,
+ elapsed_us, T_us);
+ }
+ }
+
+ if ((do_start != 0U) || (next != s_cur_freq))
+ {
+ PlsrRunControlApplyOutFreq(next, do_start);
+ }
+
+ if ((next == 0U) && (s_phase == PH_DECEL) && (s_state == RC_RUN) &&
+ (s_done < s_target))
+ {
+ s_done = s_target;
+ PlsrRunControlAfterSegDone();
+ }
+}
+
+static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
+ uint32_t f_tgt, uint32_t f_end)
+{
+ PlsrCfg_t *cfg = PlsrParamGetCfg();
+ uint32_t f_lo;
+ uint32_t f_hi;
+
+ PlsrAccelCurvePlan(&s_accel_plan,
+ total,
+ f_from,
+ f_tgt,
+ f_end,
+ cfg->default_speed,
+ cfg->start_speed,
+ cfg->end_speed,
+ cfg->accel_ms,
+ cfg->decel_ms,
+ PLSR_ACCEL_LINEAR);
+
+ /* 本段频率范围锁 PSC，升降过程只改 ARR */
+ f_lo = s_accel_plan.f_tgt;
+ f_hi = s_accel_plan.f_tgt;
+ if ((f_from > 0U) && (f_from < f_lo))
+ {
+ f_lo = f_from;
+ }
+ if (f_from > f_hi)
+ {
+ f_hi = f_from;
+ }
+ if ((s_accel_plan.f_end > 0U) && (s_accel_plan.f_end < f_lo))
+ {
+ f_lo = s_accel_plan.f_end;
+ }
+ if (s_accel_plan.f_end > f_hi)
+ {
+ f_hi = s_accel_plan.f_end;
+ }
+ if (f_lo < 1U)
+ {
+ f_lo = 1U;
+ }
+ PlsrPulseDriverLockPscRange(f_lo, f_hi);
+
+ s_cur_freq = f_from;
+ s_approach_from = f_from;
+ s_decel_from = s_accel_plan.f_tgt;
+
+ if (f_from == s_accel_plan.f_tgt)
+ {
+ PlsrRunControlEnterPhase(PH_CONST, f_from);
+ }
+ else
+ {
+ if (s_accel_plan.t_acc_ms < 1U)
+ {
+ s_accel_plan.t_acc_ms = 1U;
+ }
+ PlsrRunControlEnterPhase(PH_APPROACH, f_from);
+ }
+}
+
static void PlsrRunControlFinishAll(void)
{
PlsrPulseDriverStop();
+ PlsrPulseDriverUnlockPsc();
PlsrPulseDriverClearDir();
s_busy = 0U;
s_state = RC_IDLE;
s_cur_freq = 0U;
+ s_pwm_on = 0U;
s_follow_cont = 0U;
+ s_chain_valid = 0U;
s_chain_freq = 0U;
}
static void PlsrRunControlBeginSeg(uint16_t seg_0);
static void PlsrRunControlGotoNextOrFinish(uint16_t cur_seg);
-/** 按路径规划进入下一段，或全部结束 */
static void PlsrRunControlGotoNextOrFinish(uint16_t cur_seg)
{
int16_t next0 = PlsrPathPlanResolveAfterSeg(cur_seg);
@@ -83,7 +363,6 @@ static void PlsrRunControlGotoNextOrFinish(uint16_t cur_seg)
}
}
-/** 本段脉冲发完：处理完成/后续衔接、段间等待、跳转 */
static void PlsrRunControlAfterSegDone(void)
{
PlsrCfg_t *cfg = PlsrParamGetCfg();
@@ -92,18 +371,22 @@ static void PlsrRunControlAfterSegDone(void)
uint8_t follow;
uint8_t keep_pwm;
+ /* 先脱离 RUN，避免脉冲 ISR 重入 */
+ s_state = RC_IDLE;
+
next0 = PlsrPathPlanNextSeg(s_cur_seg);
follow = (cfg->send_mode == PLSR_SEND_FOLLOW) ? 1U : 0U;
keep_pwm = 0U;
- /*
- * 完成模式：整段发完后停表，下一段再加减速。
- * 后续模式：同向时不停表，衔接下一段频率。
- */
+ if (follow == 0U)
+ {
+ s_chain_freq = s_cur_freq;
+ s_chain_valid = 1U;
+ }
+
if ((follow != 0U) && (next0 >= 0) &&
(PlsrPathPlanIsForward((uint16_t)next0, s_acc_pulse) == s_forward))
{
- s_chain_freq = s_accel_plan.spd_end;
s_follow_cont = 1U;
keep_pwm = 1U;
}
@@ -111,7 +394,12 @@ static void PlsrRunControlAfterSegDone(void)
{
PlsrPulseDriverStop();
s_follow_cont = 0U;
- s_chain_freq = 0U;
+ s_pwm_on = 0U;
+ if (follow != 0U)
+ {
+ s_cur_freq = 0U;
+ s_chain_valid = 0U;
+ }
}
wait_ms = PlsrPathPlanGetWaitMs(s_cur_seg);
@@ -121,6 +409,7 @@ static void PlsrRunControlAfterSegDone(void)
{
PlsrPulseDriverStop();
s_follow_cont = 0U;
+ s_pwm_on = 0U;
}
s_wait_deadline = OSTimeGet() + PlsrRunControlMsToTicks(wait_ms);
s_state = RC_WAIT_COND;
@@ -130,20 +419,18 @@ static void PlsrRunControlAfterSegDone(void)
PlsrRunControlGotoNextOrFinish(s_cur_seg);
}
-/**
- * 装载并启动一段：算位移/方向、规划曲线、置方向延时或同向改频。
- */
static void PlsrRunControlBeginSeg(uint16_t seg_0)
{
PlsrSeg_t *seg;
PlsrCfg_t *cfg;
int32_t cnt;
int32_t move;
- uint32_t f0;
- uint32_t f1;
- uint32_t f2;
+ uint32_t f_tgt;
+ uint32_t f_end;
+ uint32_t f_from;
uint32_t total;
- PlsrAccelMode_e mode;
+ int16_t next0;
+ uint8_t is_last;
if (seg_0 >= PlsrParamGetSegCount())
{
@@ -154,16 +441,8 @@ static void PlsrRunControlBeginSeg(uint16_t seg_0)
seg = PlsrParamGetSeg(seg_0);
cfg = PlsrParamGetCfg();
s_cur_seg = seg_0;
-
- mode = cfg->accel_mode;
- if (mode > PLSR_ACCEL_SINE)
- {
- mode = PLSR_ACCEL_LINEAR;
- }
-
cnt = seg->pulse_cnt;
- /* 相对：位移=脉冲数；绝对：位移=目标-累计，为 0 则本段跳过 */
if (cfg->run_mode == PLSR_POS_ABSOLUTE)
{
move = cnt - s_acc_pulse;
@@ -192,73 +471,63 @@ static void PlsrRunControlBeginSeg(uint16_t seg_0)
s_target = -move;
}
- f1 = (uint32_t)((seg->freq_hz > 0) ? seg->freq_hz : (int32_t)cfg->default_speed);
- if (f1 == 0U)
+ if (PlsrRunControlGetSegTargetFreq(seg->freq_hz, cfg->default_speed, &f_tgt) == 0U)
{
- f1 = cfg->default_speed;
+ PlsrCommandSetFault(PLSR_ERR_SEG_FREQ);
+ s_busy = 1U;
+ s_target = 0;
+ s_done = 0;
+ PlsrRunControlAfterSegDone();
+ return;
}
- f1 = PlsrAccelCurveClampFreq(f1);
- /* 完成模式：起速→本段频→止速；后续模式：末速对准下一段，同向可链式衔接 */
- if (cfg->send_mode == PLSR_SEND_FOLLOW)
- {
- int16_t next0 = PlsrPathPlanNextSeg(seg_0);
+ next0 = PlsrPathPlanNextSeg(seg_0);
+ is_last = (next0 < 0) ? 1U : 0U;
- if (s_follow_cont != 0U)
- {
- f0 = PlsrAccelCurveClampFreq((s_chain_freq != 0U) ? s_chain_freq : f1);
- }
- else
- {
- f0 = cfg->start_speed;
- if (f0 == 0U)
- {
- f0 = 1U;
- }
- f0 = PlsrAccelCurveClampFreq(f0);
- }
+ if (s_follow_cont != 0U)
+ {
+ f_from = s_cur_freq;
+ }
+ else if ((cfg->send_mode == PLSR_SEND_COMPLETE) && (s_chain_valid != 0U))
+ {
+ f_from = s_chain_freq;
+ }
+ else
+ {
+ f_from = PlsrRunControlClampSpeed(cfg->start_speed);
+ }
- if (next0 >= 0)
+ if (cfg->send_mode == PLSR_SEND_FOLLOW)
+ {
+ if (is_last == 0U)
{
PlsrSeg_t *nseg = PlsrParamGetSeg((uint16_t)next0);
- f2 = (uint32_t)((nseg->freq_hz > 0) ? nseg->freq_hz
- : (int32_t)cfg->default_speed);
- if (f2 == 0U)
+ if (PlsrRunControlGetSegTargetFreq(nseg->freq_hz,
+ cfg->default_speed,
+ &f_end) == 0U)
{
- f2 = cfg->default_speed;
+ PlsrCommandSetFault(PLSR_ERR_SEG_FREQ);
+ s_busy = 1U;
+ s_target = 0;
+ s_done = 0;
+ PlsrRunControlAfterSegDone();
+ return;
}
- f2 = PlsrAccelCurveClampFreq(f2);
}
else
{
- f2 = cfg->end_speed;
- if (f2 == 0U)
- {
- f2 = 1U;
- }
- f2 = PlsrAccelCurveClampFreq(f2);
+ f_end = PlsrRunControlClampSpeed(cfg->end_speed);
}
}
else
{
- s_follow_cont = 0U;
- f0 = cfg->start_speed;
- f2 = cfg->end_speed;
- if (f0 == 0U)
- {
- f0 = 1U;
- }
- if (f2 == 0U)
- {
- f2 = 1U;
- }
- f0 = PlsrAccelCurveClampFreq(f0);
- f2 = PlsrAccelCurveClampFreq(f2);
+ /* 完成：非末段停在本段目标；末段收到终止速度 */
+ f_end = (is_last != 0U) ?
+ PlsrRunControlClampSpeed(cfg->end_speed) : f_tgt;
}
total = (uint32_t)s_target;
- PlsrAccelCurvePlan(&s_accel_plan, total, f0, f1, f2,
- cfg->accel_ms, cfg->decel_ms, mode);
+ PlsrRunControlPlanSeg(total, f_from, f_tgt, f_end);
s_done = 0;
s_busy = 1U;
@@ -271,15 +540,12 @@ static void PlsrRunControlBeginSeg(uint16_t seg_0)
if (s_follow_cont != 0U)
{
- /* 后续模式同向：PWM 仍在跑，只改频进入本段曲线 */
- s_cur_freq = PlsrAccelCurveFreqAt(&s_accel_plan, (uint32_t)s_done);
+ PlsrRunControlRefreshProfile(0U);
s_state = RC_RUN;
- PlsrPulseDriverSetFreq(s_cur_freq);
s_follow_cont = 0U;
}
else
{
- /* 先置方向，等 dir_delay 后再 Start（见 TickMs） */
PlsrPulseDriverSetDir(s_forward);
s_dir_deadline = OSTimeGet() + PlsrRunControlMsToTicks(cfg->dir_delay_ms);
s_state = RC_DIR_WAIT;
@@ -293,16 +559,24 @@ void PlsrRunControlInit(void)
s_forward = 1U;
s_cur_seg = 0U;
s_cur_freq = 0U;
+ s_pwm_on = 0U;
s_done = 0;
s_target = 0;
s_acc_pulse = 0;
- s_chain_freq = 0U;
s_follow_cont = 0U;
+ s_chain_freq = 0U;
+ s_chain_valid = 0U;
+ s_phase = PH_CONST;
+ s_phase_t0 = OSTimeGet();
+ s_phase_elapsed_us = 0U;
+ s_approach_from = 0U;
+ s_decel_from = 0U;
}
uint8_t PlsrRunControlStart(uint16_t start_seg_1based)
{
uint16_t n;
+ PlsrCfg_t *cfg;
if (s_busy != 0U)
{
@@ -319,8 +593,11 @@ uint8_t PlsrRunControlStart(uint16_t start_seg_1based)
start_seg_1based = PlsrParamGetStartSeg();
}
- s_chain_freq = 0U;
+ cfg = PlsrParamGetCfg();
s_follow_cont = 0U;
+ s_chain_valid = 0U;
+ s_chain_freq = 0U;
+ s_cur_freq = PlsrRunControlClampSpeed(cfg->start_speed);
PlsrRunControlBeginSeg((uint16_t)(start_seg_1based - 1U));
return 1U;
}
@@ -328,29 +605,93 @@ uint8_t PlsrRunControlStart(uint16_t start_seg_1based)
void PlsrRunControlStop(void)
{
PlsrPulseDriverStop();
+ PlsrPulseDriverUnlockPsc();
PlsrPulseDriverClearDir();
s_busy = 0U;
s_state = RC_IDLE;
s_cur_freq = 0U;
- s_chain_freq = 0U;
+ s_pwm_on = 0U;
s_follow_cont = 0U;
+ s_chain_valid = 0U;
+ s_chain_freq = 0U;
+}
+
+uint8_t PlsrRunControlChangeFreq(uint32_t new_tgt_hz)
+{
+ uint32_t remain;
+ uint32_t f_end;
+
+ if (s_state != RC_RUN)
+ {
+ return 0U;
+ }
+ if ((new_tgt_hz < 1U) || (new_tgt_hz > 100000U))
+ {
+ return 0U;
+ }
+ if (s_done >= s_target)
+ {
+ return 0U;
+ }
+
+ remain = (uint32_t)(s_target - s_done);
+ f_end = s_accel_plan.f_end;
+ PlsrRunControlPlanSeg(remain, s_cur_freq, new_tgt_hz, f_end);
+ s_target = (int32_t)remain;
+ s_done = 0;
+ PlsrRunControlRefreshProfile(0U);
+ return 1U;
}
void PlsrRunControlTickMs(void)
{
- /* 方向延时到：进入 RUN 并开脉冲 */
if (s_state == RC_DIR_WAIT)
{
if (PlsrRunControlOsTimeReached(s_dir_deadline) != 0U)
{
- s_cur_freq = PlsrAccelCurveFreqAt(&s_accel_plan, (uint32_t)s_done);
s_state = RC_RUN;
- PlsrPulseDriverStart(s_cur_freq);
+ /*
+ * 阶跃到起速后进入加速相；只有起速已等于目标才直接匀速。
+ * 禁止因 t_acc 曾算成 0 而一开跑就输出目标频。
+ */
+ if (s_approach_from != s_accel_plan.f_tgt)
+ {
+ if (s_accel_plan.t_acc_ms < 1U)
+ {
+ s_accel_plan.t_acc_ms = 1U;
+ }
+ PlsrRunControlEnterPhase(PH_APPROACH, s_approach_from);
+ }
+ else
+ {
+ PlsrRunControlEnterPhase(PH_CONST, s_approach_from);
+ }
+ /* 第一个脉冲 = 脉冲起始速度（不是目标频、不是 1Hz） */
+ PlsrRunControlApplyOutFreq(s_approach_from, 1U);
+ }
+ return;
+ }
+
+ if (s_state == RC_RUN)
+ {
+ if ((s_phase == PH_APPROACH) || (s_phase == PH_DECEL))
+ {
+ /* 尚未出脉冲（起速为 0）时用节拍推进相时间；有脉冲后由 ISR 按周期累加 */
+ if (s_pwm_on == 0U)
+ {
+ s_phase_elapsed_us += (1000000UL / (uint32_t)OS_TICKS_PER_SEC);
+ PlsrRunControlRefreshProfile(0U);
+ }
+ }
+ else if ((s_accel_plan.dec_n > 0U) &&
+ (s_done < s_target) &&
+ ((uint32_t)(s_target - s_done) <= s_accel_plan.dec_n))
+ {
+ PlsrRunControlRefreshProfile(0U);
}
return;
}
- /* 段间等待到：跳转或结束 */
if (s_state == RC_WAIT_COND)
{
if (PlsrRunControlOsTimeReached(s_wait_deadline) != 0U)
@@ -362,13 +703,34 @@ void PlsrRunControlTickMs(void)
void PlsrRunControlOnPulseIsr(void)
{
+ uint32_t remain;
+ uint32_t f_prev;
+
if (s_state != RC_RUN)
{
return;
}
+ /* 上一周期结束：先落到挂起频率 */
+ PlsrPulseDriverApplyPending();
+
+ f_prev = s_cur_freq;
+
+ /*
+ * 加/减速：按本脉冲周期推进相时间（µs），再算下一个频率。
+ * 这样 f-t 是连续直线，而不是 OS 10ms 台阶。
+ */
+ if ((s_phase == PH_APPROACH) || (s_phase == PH_DECEL))
+ {
+ if (f_prev >= 1U)
+ {
+ s_phase_elapsed_us += (1000000UL + (f_prev / 2UL)) / f_prev;
+ }
+ PlsrRunControlRefreshProfile(0U);
+ PlsrPulseDriverApplyPending();
+ }
+
s_done++;
- /* 累计脉冲随正反方向增减，供绝对定位 */
if (s_forward != 0U)
{
s_acc_pulse++;
@@ -384,12 +746,13 @@ void PlsrRunControlOnPulseIsr(void)
return;
}
+ if (s_phase == PH_CONST)
{
- uint32_t next = PlsrAccelCurveFreqAt(&s_accel_plan, (uint32_t)s_done);
- if (next != s_cur_freq)
+ remain = (uint32_t)(s_target - s_done);
+ if ((s_accel_plan.dec_n > 0U) && (remain <= s_accel_plan.dec_n))
{
- s_cur_freq = next;
- PlsrPulseDriverSetFreq(s_cur_freq);
+ PlsrRunControlRefreshProfile(0U);
+ PlsrPulseDriverApplyPending();
}
}
}
diff --git a/plsr/run_control/plsr_run_control.h b/plsr/run_control/plsr_run_control.h
index 6d0efdc..eda7666 100644
--- a/plsr/run_control/plsr_run_control.h
+++ b/plsr/run_control/plsr_run_control.h
@@ -1,11 +1,6 @@
/**
* @file plsr_run_control.h
- * @brief 运行控制模块
- *
- * 状态机：IDLE → DIR_WAIT（方向延时）→ RUN（发脉冲/调频）
- * → WAIT_COND（段间等待）→ 下一段 / IDLE
- *
- * 依赖：path_plan（段序/等待）、accel_curve（频率曲线）、pulse_driver（硬件）
+ * @brief 运行控制：完成/后续模式；当前先保证完成模式直线梯形
*/
#ifndef PLSR_RUN_CONTROL_H
#define PLSR_RUN_CONTROL_H
@@ -13,26 +8,15 @@
#include
void PlsrRunControlInit(void);
-
-/**
- * @brief 从指定段开始运行
- * @param start_seg_1based 起始段；非法则回退到参数块起始段
- * @return 1=已进入运行，0=忙或段数为 0
- */
uint8_t PlsrRunControlStart(uint16_t start_seg_1based);
-
void PlsrRunControlStop(void);
-
-/** 约 1ms 周期调用：处理方向延时到期、段间等待到期 */
+uint8_t PlsrRunControlChangeFreq(uint32_t new_tgt_hz);
void PlsrRunControlTickMs(void);
-
-/** 每个脉冲中断：累计计数、按曲线改频、段完成切换 */
void PlsrRunControlOnPulseIsr(void);
-
uint8_t PlsrRunControlIsBusy(void);
int32_t PlsrRunControlGetAccPulse(void);
void PlsrRunControlClearAccPulse(void);
-uint16_t PlsrRunControlGetCurSeg(void); /**< 忙时返回 1-based 当前段；闲=0 */
+uint16_t PlsrRunControlGetCurSeg(void);
uint32_t PlsrRunControlGetCurFreq(void);
#endif