|
|
|
@@ -38,28 +38,28 @@ typedef enum { |
|
|
|
PH_DECEL |
|
|
|
} RunPhase_e; |
|
|
|
|
|
|
|
volatile uint8_t g_plsr_busy; |
|
|
|
volatile uint16_t g_plsr_cur_seg_idx; |
|
|
|
volatile int32_t g_plsr_accumulated_pulses; |
|
|
|
static volatile RcState_e s_state; |
|
|
|
static volatile uint8_t s_forward;/*当前端脉冲方向*/ |
|
|
|
static volatile int32_t s_seg_pulses_done; |
|
|
|
static volatile int32_t s_seg_pulse_target; |
|
|
|
static int32_t s_absolute_origin; |
|
|
|
static uint8_t s_absolute_origin_locked; |
|
|
|
static volatile uint8_t s_act_timer_armed; |
|
|
|
static volatile uint8_t s_act_expired; |
|
|
|
static volatile uint8_t s_act_waiting_for_pulse_boundary; |
|
|
|
static volatile uint8_t s_keep_freq_after_act; |
|
|
|
static volatile uint8_t s_wait_time_expired; |
|
|
|
static uint8_t s_waiting_for_input_signal; |
|
|
|
static volatile RunPhase_e s_phase; |
|
|
|
static uint8_t s_continue_without_stopping; |
|
|
|
static uint32_t s_next_seg_start_freq_hz; |
|
|
|
static uint8_t s_next_seg_start_freq_valid; |
|
|
|
static uint8_t s_stop_after_last_pulse; |
|
|
|
static volatile uint8_t s_waiting_for_last_period; |
|
|
|
static OS_EVENT *s_wake_sem; |
|
|
|
volatile uint8_t g_plsr_busy; /* 运行忙标志（上位机监控） */ |
|
|
|
volatile uint16_t g_plsr_cur_seg_idx; /* 当前段下标（0-based） */ |
|
|
|
volatile int32_t g_plsr_accumulated_pulses; /* 全局累计脉冲（有符号） */ |
|
|
|
static volatile RcState_e s_state; /* 主状态机：IDLE/DIR_WAIT/RUN/WAIT_COND */ |
|
|
|
static volatile uint8_t s_forward; /* 当前段脉冲方向：1=正转，0=反转 */ |
|
|
|
static volatile int32_t s_seg_pulses_done; /* 本段已发脉冲数 */ |
|
|
|
static volatile int32_t s_seg_pulse_target; /* 本段目标脉冲数（绝对值） */ |
|
|
|
static int32_t s_absolute_origin; /* 绝对定位坐标原点 */ |
|
|
|
static uint8_t s_absolute_origin_locked; /* 原点是否已锁定（CLR 后重锁） */ |
|
|
|
static volatile uint8_t s_act_timer_armed; /* ACT 定时器是否已启动 */ |
|
|
|
static volatile uint8_t s_act_expired; /* ACT 到期请求（TickMs 消费） */ |
|
|
|
static volatile uint8_t s_act_waiting_for_pulse_boundary; /* ACT 到期后等脉冲边界切段 */ |
|
|
|
static volatile uint8_t s_keep_freq_after_act; /* ACT 切段后保持当前频率衔接 */ |
|
|
|
static volatile uint8_t s_wait_time_expired; /* 段后 WAIT 时间到期请求 */ |
|
|
|
static uint8_t s_waiting_for_input_signal; /* 段后等 WAIT/EXT 输入沿 */ |
|
|
|
static volatile RunPhase_e s_phase; /* 段内相：加速/匀速/减速 */ |
|
|
|
static uint8_t s_continue_without_stopping; /* 段间不停表衔接（FOLLOW/keep） */ |
|
|
|
static uint32_t s_next_seg_start_freq_hz; /* 段间衔接起始频率 Hz */ |
|
|
|
static uint8_t s_next_seg_start_freq_valid; /* 衔接频率是否有效 */ |
|
|
|
static uint8_t s_stop_after_last_pulse; /* 末拍后完整停表（完成模式） */ |
|
|
|
static volatile uint8_t s_waiting_for_last_period; /* 等末拍 UPDATE 再收尾 */ |
|
|
|
static OS_EVENT *s_wake_sem; /* 唤醒 PlsrTask 的信号量 */ |
|
|
|
|
|
|
|
/* 策略层解析后交给 PlanSeg 的段端点 */ |
|
|
|
typedef struct { |
|
|
|
@@ -121,30 +121,30 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, |
|
|
|
/** @brief 运行控制初始化 */ |
|
|
|
void PlsrRunControlInit(void) |
|
|
|
{ |
|
|
|
s_state = RC_IDLE; |
|
|
|
g_plsr_busy = 0U; |
|
|
|
s_forward = 1U; |
|
|
|
g_plsr_cur_seg_idx = 0U; |
|
|
|
s_seg_pulses_done = 0; |
|
|
|
s_seg_pulse_target = 0; |
|
|
|
g_plsr_accumulated_pulses = 0; |
|
|
|
s_absolute_origin = 0; |
|
|
|
s_absolute_origin_locked = 0U; |
|
|
|
s_continue_without_stopping = 0U; |
|
|
|
s_next_seg_start_freq_hz = 0U; |
|
|
|
s_next_seg_start_freq_valid = 0U; |
|
|
|
g_plsr_accel_plan.accel_pulses = 0U; |
|
|
|
g_plsr_accel_plan.decel_pulses = 0U; |
|
|
|
g_plsr_accel_runtime.total_pulses = 0U; |
|
|
|
s_stop_after_last_pulse = 0U; |
|
|
|
s_waiting_for_last_period = 0U; |
|
|
|
s_phase = PH_CONST; |
|
|
|
s_act_timer_armed = 0U; |
|
|
|
s_act_expired = 0U; |
|
|
|
s_act_waiting_for_pulse_boundary = 0U; |
|
|
|
s_keep_freq_after_act = 0U; |
|
|
|
s_wait_time_expired = 0U; |
|
|
|
s_waiting_for_input_signal = 0U; |
|
|
|
s_state = RC_IDLE; /* 主状态机：空闲 */ |
|
|
|
g_plsr_busy = 0U; /* 运行忙标志（上位机监控） */ |
|
|
|
s_forward = 1U; /* 当前段脉冲方向：1=正转 */ |
|
|
|
g_plsr_cur_seg_idx = 0U; /* 当前段下标（0-based） */ |
|
|
|
s_seg_pulses_done = 0; /* 本段已发脉冲数 */ |
|
|
|
s_seg_pulse_target = 0; /* 本段目标脉冲数（绝对值） */ |
|
|
|
g_plsr_accumulated_pulses = 0; /* 全局累计脉冲（有符号） */ |
|
|
|
s_absolute_origin = 0; /* 绝对定位坐标原点 */ |
|
|
|
s_absolute_origin_locked = 0U; /* 原点是否已锁定（CLR 后重锁） */ |
|
|
|
s_continue_without_stopping = 0U; /* 段间不停表衔接（FOLLOW/keep） */ |
|
|
|
s_next_seg_start_freq_hz = 0U; /* 段间衔接起始频率 Hz */ |
|
|
|
s_next_seg_start_freq_valid = 0U; /* 衔接频率是否有效 */ |
|
|
|
g_plsr_accel_plan.accel_pulses = 0U; /* 规划：加速相脉冲预算 */ |
|
|
|
g_plsr_accel_plan.decel_pulses = 0U; /* 规划：减速相脉冲预算 */ |
|
|
|
g_plsr_accel_runtime.total_pulses = 0U; /* 当前相计划总脉冲 */ |
|
|
|
s_stop_after_last_pulse = 0U; /* 末拍后完整停表（完成模式） */ |
|
|
|
s_waiting_for_last_period = 0U; /* 等末拍 UPDATE 再收尾 */ |
|
|
|
s_phase = PH_CONST; /* 段内相：加速/匀速/减速 */ |
|
|
|
s_act_timer_armed = 0U; /* ACT 定时器是否已启动 */ |
|
|
|
s_act_expired = 0U; /* ACT 到期请求（TickMs 消费） */ |
|
|
|
s_act_waiting_for_pulse_boundary = 0U; /* ACT 到期后等脉冲边界切段 */ |
|
|
|
s_keep_freq_after_act = 0U; /* ACT 切段后保持当前频率衔接 */ |
|
|
|
s_wait_time_expired = 0U; /* 段后 WAIT 时间到期请求 */ |
|
|
|
s_waiting_for_input_signal = 0U; /* 段后等 WAIT/EXT 输入沿 */ |
|
|
|
/* s_wake_sem 在 OSInit 后由 WakeInit 创建 */ |
|
|
|
} |
|
|
|
|
|
|
|
@@ -200,21 +200,21 @@ uint8_t PlsrStart(uint16_t start_seg) |
|
|
|
{ |
|
|
|
uint16_t n; |
|
|
|
uint16_t first_seg; |
|
|
|
uint32_t f_chk; |
|
|
|
uint32_t f_chk;/* 解析后的首段目标频率；此处仅作合法性检查，结果不用 */ |
|
|
|
|
|
|
|
if (g_plsr_busy != 0U) |
|
|
|
{ |
|
|
|
return 0U; |
|
|
|
} |
|
|
|
|
|
|
|
n = PlsrParamGetSegCount(); |
|
|
|
n = PlsrParamGetSegCount();/*获取段的数量*/ |
|
|
|
if (n < 1U) |
|
|
|
{ |
|
|
|
return 0U; |
|
|
|
} |
|
|
|
if ((start_seg < 1U) || (start_seg > n)) |
|
|
|
{ |
|
|
|
start_seg = PlsrParamGetStartSeg(); |
|
|
|
start_seg = PlsrParamGetStartSeg();/*获取起始段*/ |
|
|
|
} |
|
|
|
|
|
|
|
first_seg = (uint16_t)(start_seg - 1U); |
|
|
|
@@ -226,20 +226,20 @@ uint8_t PlsrStart(uint16_t start_seg) |
|
|
|
PlsrCommandSetFault(PLSR_ERR_FREQ_ILLEGAL); |
|
|
|
return 0U; |
|
|
|
} |
|
|
|
s_continue_without_stopping = 0U; |
|
|
|
s_next_seg_start_freq_valid = 0U; |
|
|
|
s_next_seg_start_freq_hz = 0U; |
|
|
|
s_continue_without_stopping = 0U;/* 清段间不停表衔接标志 */ |
|
|
|
s_next_seg_start_freq_valid = 0U;/* 清段间衔接频率有效标志 */ |
|
|
|
s_next_seg_start_freq_hz = 0U;/*段间衔接频率置零*/ |
|
|
|
/* |
|
|
|
* 绝对原点：仅在首次 Start（或 ClearAccPulse 之后的第一次）锁定。 |
|
|
|
* 相对跑完再切绝对，仍相对「第一次启动时刻」坐标，不以当前位置重定原点。 |
|
|
|
*/ |
|
|
|
if (s_absolute_origin_locked == 0U) |
|
|
|
{ |
|
|
|
s_absolute_origin = g_plsr_accumulated_pulses; |
|
|
|
s_absolute_origin_locked = 1U; |
|
|
|
s_absolute_origin = g_plsr_accumulated_pulses;/*已经累计的脉冲*/ |
|
|
|
s_absolute_origin_locked = 1U;/*绝对原点锁定标志*/ |
|
|
|
} |
|
|
|
PlsrSignalIoClearEdges(); |
|
|
|
PlsrRunControlBeginSeg(first_seg); |
|
|
|
PlsrSignalIoClearEdges();/*清除io信号边沿*/ |
|
|
|
PlsrRunControlBeginSeg(first_seg);/*根据段号开一段*/ |
|
|
|
return 1U; |
|
|
|
} |
|
|
|
|
|
|
|
@@ -290,7 +290,7 @@ uint8_t PlsrChangeFreq(uint32_t new_tgt_hz) |
|
|
|
g_plsr_config.accel_ms, g_plsr_config.decel_ms); |
|
|
|
s_seg_pulse_target = (int32_t)remain; |
|
|
|
s_seg_pulses_done = 0; |
|
|
|
PlsrRunControlRefreshProfile(0U); |
|
|
|
PlsrRunControlRefreshProfile(0U);/*刷新下一拍频率*/ |
|
|
|
return 1U; |
|
|
|
} |
|
|
|
|
|
|
|
@@ -313,15 +313,17 @@ void PlsrRunControlRestoreAccPulse(int32_t acc_pulse) |
|
|
|
/** @brief 毫秒任务节拍 */ |
|
|
|
void PlsrRunControlTickMs(void) |
|
|
|
{ |
|
|
|
/* act时间到期或者act切段后需要保持频率*/ |
|
|
|
if (s_act_expired != 0U) |
|
|
|
{ |
|
|
|
s_act_expired = 0U; |
|
|
|
if ((s_state == RC_RUN) || (s_keep_freq_after_act != 0U)) |
|
|
|
{ |
|
|
|
PlsrRunControlActExpire(); |
|
|
|
PlsrRunControlActExpire();/*act时间到期处理函数*/ |
|
|
|
} |
|
|
|
return; |
|
|
|
} |
|
|
|
/*wait时间到期，清标志位，规划下一段*/ |
|
|
|
if (s_wait_time_expired != 0U) |
|
|
|
{ |
|
|
|
s_wait_time_expired = 0U; |
|
|
|
@@ -331,28 +333,29 @@ void PlsrRunControlTickMs(void) |
|
|
|
} |
|
|
|
return; |
|
|
|
} |
|
|
|
/*方向延时直接等待*/ |
|
|
|
if (s_state == RC_DIR_WAIT) |
|
|
|
{ |
|
|
|
/* 精确延时只靠 TIM5；此处占位，避免任务空转改状态 */ |
|
|
|
return; |
|
|
|
} |
|
|
|
|
|
|
|
/*运行态先处理改频请求，如果时EXT/EXT或脉冲发送完成，需要读取下降沿*/ |
|
|
|
if (s_state == RC_RUN) |
|
|
|
{ |
|
|
|
PlsrPulseDriverApplyPending(); |
|
|
|
PlsrPulseDriverApplyPending();//消费挂起改频 |
|
|
|
|
|
|
|
if ((g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_EXT) || |
|
|
|
(g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_EXT_OR_DONE)) |
|
|
|
{ |
|
|
|
if (PlsrSignalIoTakeExtFalling() != 0U) |
|
|
|
{ |
|
|
|
PlsrRunControlCutSegToNext(); |
|
|
|
PlsrRunControlCutSegToNext();/*EXT信号触发切段*/ |
|
|
|
return; |
|
|
|
} |
|
|
|
} |
|
|
|
return; |
|
|
|
} |
|
|
|
|
|
|
|
/*等待条件状态要读取下降沿，去下一段或者结束*/ |
|
|
|
if (s_state == RC_WAIT_COND) |
|
|
|
{ |
|
|
|
if (s_waiting_for_input_signal != 0U) |
|
|
|
@@ -385,9 +388,9 @@ void PlsrRunControlOnDelayTimer(void) |
|
|
|
if (s_state == RC_DIR_WAIT) |
|
|
|
{ |
|
|
|
s_state = RC_RUN; |
|
|
|
PlsrRunControlRefreshProfile(1U); |
|
|
|
PlsrRunControlArmActExtOnPulseStart(); |
|
|
|
PlsrRunControlWakePost(); |
|
|
|
PlsrRunControlRefreshProfile(1U);/*刷新频率*/ |
|
|
|
PlsrRunControlArmActExtOnPulseStart();/*开始输出时：EXT 类清旧沿；ACT 类启动段内定时 */ |
|
|
|
PlsrRunControlWakePost();/*释放信号量，唤醒plsr任务*/ |
|
|
|
return; |
|
|
|
} |
|
|
|
|
|
|
|
@@ -686,7 +689,7 @@ static void PlsrRunControlBeginSeg(uint16_t seg_idx) |
|
|
|
uint32_t f_tgt; |
|
|
|
uint32_t total; |
|
|
|
PlsrSegPlanEndpoints_t endpoints; |
|
|
|
|
|
|
|
/*如果段号大于总段数，直接结束输出*/ |
|
|
|
if (seg_idx >= PlsrParamGetSegCount()) |
|
|
|
{ |
|
|
|
PlsrRunControlFinishAll(); |
|
|
|
@@ -694,9 +697,10 @@ static void PlsrRunControlBeginSeg(uint16_t seg_idx) |
|
|
|
} |
|
|
|
|
|
|
|
g_plsr_cur_seg_idx = seg_idx; |
|
|
|
s_waiting_for_last_period = 0U; |
|
|
|
cnt = g_plsr_segs[seg_idx].pulse_cnt; |
|
|
|
s_waiting_for_last_period = 0U;/* 等末拍 UPDATE 再收尾标志 */ |
|
|
|
cnt = g_plsr_segs[seg_idx].pulse_cnt;/*读取总脉冲数*/ |
|
|
|
|
|
|
|
/*读取运行模式，绝对模式以启动位置为绝对原点*/ |
|
|
|
if (g_plsr_config.run_mode == PLSR_POS_ABSOLUTE) |
|
|
|
{ |
|
|
|
/* 以首次启动锁定原点：move = 段目标 − 相对原点位置 */ |
|
|
|
@@ -704,7 +708,7 @@ static void PlsrRunControlBeginSeg(uint16_t seg_idx) |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
move = cnt; |
|
|
|
move = cnt;/*行对模式下位移量=脉冲数*/ |
|
|
|
} |
|
|
|
|
|
|
|
if (move == 0) |
|
|
|
@@ -713,14 +717,14 @@ static void PlsrRunControlBeginSeg(uint16_t seg_idx) |
|
|
|
g_plsr_busy = 1U; |
|
|
|
s_seg_pulse_target = 0; |
|
|
|
s_seg_pulses_done = 0; |
|
|
|
PlsrRunControlAfterSegDone(); |
|
|
|
PlsrRunControlAfterSegDone();/*段正常结束*/ |
|
|
|
return; |
|
|
|
} |
|
|
|
|
|
|
|
if (move >= 0) |
|
|
|
{ |
|
|
|
s_forward = 1U; |
|
|
|
s_seg_pulse_target = move; |
|
|
|
s_forward = 1U;/*设置方向标志，正向1，反向0*/ |
|
|
|
s_seg_pulse_target = move;/*脉冲目标数=移动量*/ |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
@@ -732,11 +736,11 @@ static void PlsrRunControlBeginSeg(uint16_t seg_idx) |
|
|
|
if (move == (int32_t)(-2147483647L - 1L)) |
|
|
|
{ |
|
|
|
PlsrCommandSetFault(PLSR_ERR_PULSE_RANGE); |
|
|
|
PlsrRunControlFinishAll(); |
|
|
|
PlsrRunControlFinishAll();/*全段结束*/ |
|
|
|
return; |
|
|
|
} |
|
|
|
s_forward = 0U; |
|
|
|
s_seg_pulse_target = -move; |
|
|
|
s_forward = 0U;/* 反向移动 */ |
|
|
|
s_seg_pulse_target = -move;/* 移动量负脉冲个数 */ |
|
|
|
} |
|
|
|
|
|
|
|
/* 0x04：本段（或默认速度）频率非法 */ |
|
|
|
@@ -752,10 +756,13 @@ static void PlsrRunControlBeginSeg(uint16_t seg_idx) |
|
|
|
(void)PlsrRunControlResolveSegPlanEndpoints(seg_idx, f_tgt, &endpoints); |
|
|
|
|
|
|
|
total = (uint32_t)s_seg_pulse_target; |
|
|
|
|
|
|
|
/*规划本段并锁 PSC / 进初始相*/ |
|
|
|
PlsrRunControlPlanSeg(total, endpoints.freq_start_hz, f_tgt, endpoints.freq_end_hz, |
|
|
|
endpoints.accel_ms, endpoints.decel_ms); |
|
|
|
s_seg_pulses_done = 0; |
|
|
|
g_plsr_busy = 1U; |
|
|
|
|
|
|
|
s_seg_pulses_done = 0;/*已发脉冲数清零*/ |
|
|
|
g_plsr_busy = 1U;/*运行状态置1*/ |
|
|
|
{ |
|
|
|
/* 用本段结束后的坐标判 keep，与 AfterSegDone 一致；结果锁进 s_stop_after_last_pulse */ |
|
|
|
int32_t end_pos = PlsrRunControlAbsLocalPos(); |
|
|
|
@@ -774,43 +781,48 @@ static void PlsrRunControlBeginSeg(uint16_t seg_idx) |
|
|
|
|
|
|
|
if (total == 0U) |
|
|
|
{ |
|
|
|
PlsrRunControlAfterSegDone(); |
|
|
|
PlsrRunControlAfterSegDone();/*单段正常结束*/ |
|
|
|
return; |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
/*段间不停表衔接（后续模式/ACT切段）*/ |
|
|
|
if (s_continue_without_stopping != 0U) |
|
|
|
{ |
|
|
|
/* 分支 A：段间无缝（后续模式或 ACT切段） */ |
|
|
|
PlsrPulseDriverClearOnePulseStop(); |
|
|
|
PlsrPulseDriverClearOnePulseStop();/*清掉单脉冲停止*/ |
|
|
|
/* |
|
|
|
* 必须先 RC_RUN 再 RefreshProfile/Start：否则冷启动首拍 UPDATE 时 |
|
|
|
* OnPulseIsr 仍见非 RUN 直接 return，影子停在起跳频，第 1、2 拍同频。 |
|
|
|
*/ |
|
|
|
s_state = RC_RUN; |
|
|
|
s_continue_without_stopping = 0U; |
|
|
|
s_continue_without_stopping = 0U;/*段间不停表标志置0*/ |
|
|
|
/* |
|
|
|
* FOLLOW 真不停表：do_start=0 只改频。 |
|
|
|
* ACT 经 OPM 边界停表后 g_plsr_pwm_running 已为 0：须 Start 重开，否则无脉冲卡死。 |
|
|
|
*/ |
|
|
|
* 1需要开始pwm输出，0已经在运行。只刷新下一拍频率*/ |
|
|
|
PlsrRunControlRefreshProfile((g_plsr_pwm_running == 0U) ? 1U : 0U); |
|
|
|
/*开始输出时：EXT 类清旧沿；ACT 类启动段内定时 */ |
|
|
|
PlsrRunControlArmActExtOnPulseStart(); |
|
|
|
} |
|
|
|
else if ((g_plsr_direction_valid != 0U) && (s_forward == g_plsr_direction_forward)) |
|
|
|
{ |
|
|
|
/* 分支 B：真换向但本段与上一段同向，方向脚已正确，跳过延时 */ |
|
|
|
PlsrPulseDriverSetDir(s_forward); |
|
|
|
PlsrPulseDriverSetDir(s_forward);/*跳过方向延时*/ |
|
|
|
s_state = RC_RUN; |
|
|
|
PlsrRunControlRefreshProfile(1U); |
|
|
|
PlsrRunControlRefreshProfile(1U);/*传1，已运行，只刷新频率*/ |
|
|
|
/*开始输出时：EXT 类清旧沿；ACT 类启动段内定时 */ |
|
|
|
PlsrRunControlArmActExtOnPulseStart(); |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
/* 分支 C：需要换向或首次开跑 — 先打方向，再决定立刻开或 DIR_WAIT */ |
|
|
|
PlsrPulseDriverSetDir(s_forward); |
|
|
|
PlsrPulseDriverSetDir(s_forward);/*设置方向*/ |
|
|
|
if (g_plsr_config.dir_delay_ms == 0U) |
|
|
|
{ |
|
|
|
s_state = RC_RUN; |
|
|
|
/*传1，已运行，只刷新频率*/ |
|
|
|
PlsrRunControlRefreshProfile(1U); |
|
|
|
/*开始输出时：EXT 类清旧沿；ACT 类启动段内定时 */ |
|
|
|
PlsrRunControlArmActExtOnPulseStart(); |
|
|
|
} |
|
|
|
else |
|
|
|
@@ -820,9 +832,11 @@ static void PlsrRunControlBeginSeg(uint16_t seg_idx) |
|
|
|
*/ |
|
|
|
s_state = RC_DIR_WAIT; |
|
|
|
PlsrRunControlWakeDrain(); |
|
|
|
/*设置方向延时*/ |
|
|
|
PlsrSignalIoScheduleDelayMs((uint32_t)g_plsr_config.dir_delay_ms); |
|
|
|
if (g_plsr_accel_runtime.cur_freq_hz >= 1U) |
|
|
|
{ |
|
|
|
/*换向延时期间预装寄存器、CEN 保持关 */ |
|
|
|
PlsrPulseDriverPrepare(g_plsr_accel_runtime.cur_freq_hz); |
|
|
|
} |
|
|
|
} |
|
|
|
@@ -941,18 +955,21 @@ static void PlsrRunControlArmActExtOnPulseStart(void) |
|
|
|
{ |
|
|
|
uint16_t act_time_ms; |
|
|
|
|
|
|
|
/*EXT信号/EXT或脉冲发送完成，清掉X4，X5信号的边沿，防止误触*/ |
|
|
|
if ((g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_EXT) || |
|
|
|
(g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_EXT_OR_DONE)) |
|
|
|
{ |
|
|
|
PlsrSignalIoClearEdges(); |
|
|
|
} |
|
|
|
|
|
|
|
s_act_timer_armed = 0U; |
|
|
|
s_act_expired = 0U; |
|
|
|
s_act_waiting_for_pulse_boundary = 0U; |
|
|
|
s_keep_freq_after_act = 0U; |
|
|
|
s_wait_time_expired = 0U; |
|
|
|
/*清掉ACT定时的相关标志*/ |
|
|
|
s_act_timer_armed = 0U;/*启用定时器*/ |
|
|
|
s_act_expired = 0U;/*act定时到期*/ |
|
|
|
s_act_waiting_for_pulse_boundary = 0U;/*等待脉冲边界*/ |
|
|
|
s_keep_freq_after_act = 0U;/*act切断后保持频率*/ |
|
|
|
s_wait_time_expired = 0U;/*wait时间到期标志*/ |
|
|
|
|
|
|
|
/*act时间切段置标志位，开定时*/ |
|
|
|
if (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_ACT) |
|
|
|
{ |
|
|
|
act_time_ms = g_plsr_segs[g_plsr_cur_seg_idx].act_ms; |
|
|
|
@@ -994,9 +1011,9 @@ static void PlsrRunControlActExpire(void) |
|
|
|
s_act_expired = 0U; |
|
|
|
s_wait_time_expired = 0U; |
|
|
|
s_state = RC_IDLE; |
|
|
|
|
|
|
|
/*同向且有有效频率则不停表衔接，否则停表清链，传入1需要频率衔接*/ |
|
|
|
PlsrRunControlApplyKeepOrStop(1U); |
|
|
|
s_keep_freq_after_act = 0U; |
|
|
|
s_keep_freq_after_act = 0U;/*保持频率衔接标志位置0*/ |
|
|
|
|
|
|
|
PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); |
|
|
|
} |
|
|
|
@@ -1063,7 +1080,7 @@ static void PlsrRunControlEnterPostWaitOrNext(void) |
|
|
|
} |
|
|
|
|
|
|
|
/** |
|
|
|
* @brief 段正常结束：诊断 → 链式频率 → FOLLOW keep 或停表 → EnterPostWaitOrNext |
|
|
|
* @brief 单段正常结束：诊断 → 链式频率 → FOLLOW keep 或停表 → EnterPostWaitOrNext |
|
|
|
*/ |
|
|
|
static void PlsrRunControlAfterSegDone(void) |
|
|
|
{ |
|
|
|
@@ -1210,8 +1227,8 @@ static void PlsrRunControlOnApproachDone(void) |
|
|
|
|
|
|
|
/** |
|
|
|
* 解析段目标频率。 |
|
|
|
* freq_hz==0 用公共默认速度；结果须落在 [1, 100000]，否则返回 0（调用方写 0x04）。 |
|
|
|
*/ |
|
|
|
* freq_hz==0 用返回default_freq；结果须落在 [1, 100000]，否则返回 0（调用方写 0x04）。 |
|
|
|
* 返回1/0表示频率能不能用，能用返回1，不能用返回0*/ |
|
|
|
static uint8_t PlsrRunControlGetSegTargetFreq(int32_t freq_hz, |
|
|
|
uint32_t default_spd, |
|
|
|
uint32_t *out_hz) |
|
|
|
@@ -1220,22 +1237,22 @@ static uint8_t PlsrRunControlGetSegTargetFreq(int32_t freq_hz, |
|
|
|
|
|
|
|
if (freq_hz < 0) |
|
|
|
{ |
|
|
|
return 0U; |
|
|
|
return 0U;/*频率小于0，直接结束*/ |
|
|
|
} |
|
|
|
if (freq_hz == 0) |
|
|
|
{ |
|
|
|
f = default_spd; |
|
|
|
f = default_spd;/*频率为零，替换为默认速度*/ |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
f = (uint32_t)freq_hz; |
|
|
|
f = (uint32_t)freq_hz;/*频率不为零，替换为传入的频率*/ |
|
|
|
} |
|
|
|
/* 合法范围：1Hz ~ 100kHz（含） */ |
|
|
|
if ((f < 1U) || (f > 100000U)) |
|
|
|
{ |
|
|
|
return 0U; |
|
|
|
return 0U;/*频率范围不合法直接结束*/ |
|
|
|
} |
|
|
|
*out_hz = f; |
|
|
|
*out_hz = f;/*将最终的频率传回去*/ |
|
|
|
return 1U; |
|
|
|
} |
|
|
|
|
|
|
|
@@ -1409,7 +1426,7 @@ static void PlsrRunControlEnterConstHoldIsr(void) |
|
|
|
} |
|
|
|
|
|
|
|
/** |
|
|
|
* 任务侧：按当前段内相 s_phase，决定开表/改频用哪个频率。 |
|
|
|
* 任务侧：按当前段内相 s_phase，决定开表/改频用哪个频率。刷新频率 |
|
|
|
* @param do_start 1=需要 Start PWM（RefreshProfile(1)）；0=已在跑只改频 |
|
|
|
*/ |
|
|
|
static void PlsrRunControlRefreshProfile(uint8_t do_start) |
|
|
|
@@ -1507,14 +1524,14 @@ static uint8_t PlsrRunControlResolveSegPlanEndpoints(uint16_t seg_idx, uint32_t |
|
|
|
endpoints->accel_ms = g_plsr_config.accel_ms; |
|
|
|
endpoints->decel_ms = g_plsr_config.decel_ms; |
|
|
|
|
|
|
|
if ((s_continue_without_stopping != 0U) && (g_plsr_output_freq_hz >= 1U)) |
|
|
|
{ |
|
|
|
endpoints->freq_start_hz = g_plsr_output_freq_hz; |
|
|
|
} |
|
|
|
else if ((g_plsr_pwm_running != 0U) && (g_plsr_output_freq_hz >= 1U)) |
|
|
|
/*不停表衔接且输出频率>1或者下一段运行中,下一段的起始频率为当前输出频率*/ |
|
|
|
if (((s_continue_without_stopping != 0U) || (g_plsr_pwm_running != 0U)) |
|
|
|
&& (g_plsr_output_freq_hz >= 1U)) |
|
|
|
{ |
|
|
|
endpoints->freq_start_hz = g_plsr_output_freq_hz; |
|
|
|
} |
|
|
|
|
|
|
|
/*如果下一段的起始频率有效且>1,下一段的起始频率为起始频率 */ |
|
|
|
else if ((s_next_seg_start_freq_valid != 0U) && (s_next_seg_start_freq_hz >= 1U)) |
|
|
|
{ |
|
|
|
/* 完成模式段末停表后：用本段落地频作起速（如 EXT_OR_DONE 的匀速峰值） */ |
|
|
|
@@ -1596,6 +1613,7 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, |
|
|
|
uint32_t f_min; |
|
|
|
uint32_t f_max; |
|
|
|
|
|
|
|
/*传给曲线规划模块进行曲线规划*/ |
|
|
|
PlsrAccelCurvePlan(&g_plsr_accel_plan, |
|
|
|
total, |
|
|
|
f_from, |
|
|
|
@@ -1616,7 +1634,7 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, |
|
|
|
PlsrCommandSetFault(PLSR_ERR_FREQ_UNREACH); |
|
|
|
} |
|
|
|
|
|
|
|
g_plsr_accel_runtime.total_pulses = 0U; |
|
|
|
g_plsr_accel_runtime.total_pulses = 0U;/*开始运行前当前相计划步进的脉冲数清零*/ |
|
|
|
|
|
|
|
/* S/正弦：在任务上下文预建频率表，ISR 只查表 */ |
|
|
|
PlsrAccelPrebuildFreqTables(&g_plsr_accel_plan); |
|
|
|
@@ -1629,46 +1647,50 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, |
|
|
|
/* 本段可能出现的最低/最高频率，取 入口、目标、出口 三者的 min/max */ |
|
|
|
f_min = g_plsr_accel_plan.target_freq_hz; |
|
|
|
f_max = g_plsr_accel_plan.target_freq_hz; |
|
|
|
/*先找入口的最大最小频率*/ |
|
|
|
if (f_start > f_max) |
|
|
|
{ |
|
|
|
f_max = f_start; |
|
|
|
f_max = f_start;/*起始频率大于目标频率时，最大频率用起始频率*/ |
|
|
|
} |
|
|
|
if (f_start >= 1U) |
|
|
|
{ |
|
|
|
if (f_start < f_min) |
|
|
|
{ |
|
|
|
f_min = f_start; |
|
|
|
f_min = f_start;/*起始频率小于目标频率，最小频率使用起始频率*/ |
|
|
|
} |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
f_min = 1U; |
|
|
|
f_min = 1U;/* 否则最小频率为1 */ |
|
|
|
} |
|
|
|
|
|
|
|
/*找出口的最大最小频率*/ |
|
|
|
if (g_plsr_accel_plan.end_freq_hz > f_max) |
|
|
|
{ |
|
|
|
f_max = g_plsr_accel_plan.end_freq_hz; |
|
|
|
f_max = g_plsr_accel_plan.end_freq_hz;/*出口频率大于最大值，则成为最大值*/ |
|
|
|
} |
|
|
|
if (g_plsr_accel_plan.end_freq_hz >= 1U) |
|
|
|
{ |
|
|
|
if (g_plsr_accel_plan.end_freq_hz < f_min) |
|
|
|
{ |
|
|
|
f_min = g_plsr_accel_plan.end_freq_hz; |
|
|
|
f_min = g_plsr_accel_plan.end_freq_hz;/*出口频率小于小值，则成为最小值*/ |
|
|
|
} |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
if (f_min > 1U) |
|
|
|
{ |
|
|
|
f_min = 1U; |
|
|
|
f_min = 1U;/* 否则，最小值为1 */ |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
if (f_min < 1U) |
|
|
|
{ |
|
|
|
f_min = 1U; |
|
|
|
f_min = 1U;/*如果最小值小于1，则最小值改为1*/ |
|
|
|
} |
|
|
|
if (f_max < f_min) |
|
|
|
{ |
|
|
|
f_max = f_min; |
|
|
|
f_max = f_min;/*如果找出来的最大值小于最小值，让最大值等于最小值*/ |
|
|
|
} |
|
|
|
/* 后续同向不停表时 PWM 仍在跑：禁止重锁 PSC */ |
|
|
|
if (g_plsr_pwm_running == 0U) |
|
|
|
@@ -1687,12 +1709,12 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, |
|
|
|
} |
|
|
|
else if (f_start == g_plsr_accel_plan.target_freq_hz) |
|
|
|
{ |
|
|
|
s_phase = PH_CONST; |
|
|
|
s_phase = PH_CONST;/*匀速段*/ |
|
|
|
PlsrAccelBeginConstSpeed(&g_plsr_accel_runtime, &g_plsr_accel_plan); |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
s_phase = PH_APPROACH; |
|
|
|
s_phase = PH_APPROACH;/*加速段*/ |
|
|
|
PlsrAccelBeginAccel(&g_plsr_accel_runtime, &g_plsr_accel_plan); |
|
|
|
} |
|
|
|
} |
|
|
|
|
hanyongwei
2 тижднів тому