|
@@ -1193,9 +1193,14 @@ static void Work_Loop_thread_entry(void *parameter)
|
|
|
* 而且每个点位都重新锚定一次焦面, 漂移不再跨点位累积。
|
|
* 而且每个点位都重新锚定一次焦面, 漂移不再跨点位累积。
|
|
|
* 注意这里只找"上升沿", 不找峰值本身 —— 采样量比完整自动对焦少一个数量级。
|
|
* 注意这里只找"上升沿", 不找峰值本身 —— 采样量比完整自动对焦少一个数量级。
|
|
|
*
|
|
*
|
|
|
- * 失败处理: 阶段1(向下搜索 PF_SEARCH_LIMIT)没找到, 多半是回程丢步把整条轨迹往下带,
|
|
|
|
|
- * 起点已经越过峰值陷入下降段。此时向上退回 PF_PHASE2_UP 再往下重扫一趟更长的行程。
|
|
|
|
|
- * 仍然失败则按 slide_set_record_travel(0) 的加长兜底行程录制(等价于加大覆盖范围)。
|
|
|
|
|
|
|
+ * 失败处理(阶段2, 双向搜索):
|
|
|
|
|
+ * 阶段1(向下搜索 PF_SEARCH_LIMIT)没找到, 有两种原因 ——
|
|
|
|
|
+ * a) 下行一开始"从未上升且连续下降": 方向反了, 焦区其实在起始位置上方;
|
|
|
|
|
+ * b) 走完整段行程才停: 回程丢步把整条轨迹往下带, 起点已越过峰值。
|
|
|
|
|
+ * 两种都先回退到最开始的位置(进入预对焦时的高度), 再反向(向上)搜索峰值:
|
|
|
|
|
+ * 上行时清晰度先升后降, 越过峰值后连续回落即确认, 随后上移 PF_HEADROOM 作为录制起点
|
|
|
|
|
+ * (录制方向是下行, 起点必须在焦区上方)。
|
|
|
|
|
+ * 反向也失败则再退回到最初位置, 按 slide_set_record_travel(0) 的加长兜底行程录制。
|
|
|
*
|
|
*
|
|
|
* 单位口径(改动前必读, 与 ctr_move 的实现强耦合):
|
|
* 单位口径(改动前必读, 与 ctr_move 的实现强耦合):
|
|
|
* ctr_move(Y_,dir,dis_,period) 是"定时开PWM": for(j=0;j<dis_;j++){ delay },
|
|
* ctr_move(Y_,dir,dis_,period) 是"定时开PWM": for(j=0;j<dis_;j++){ delay },
|
|
@@ -1210,13 +1215,15 @@ static void Work_Loop_thread_entry(void *parameter)
|
|
|
#define PF_PWM_PERIOD_NS 1000000UL /* 预对焦PWM周期(ns): 1kHz -> 1 格 = 1 脉冲 */
|
|
#define PF_PWM_PERIOD_NS 1000000UL /* 预对焦PWM周期(ns): 1kHz -> 1 格 = 1 脉冲 */
|
|
|
#define PF_STEP 60 /* 扫描步长(脉冲): 必须 <= 焦区上升沿宽度的 1/2, 否则整步跨过上升沿漏检 */
|
|
#define PF_STEP 60 /* 扫描步长(脉冲): 必须 <= 焦区上升沿宽度的 1/2, 否则整步跨过上升沿漏检 */
|
|
|
#define PF_SEARCH_LIMIT 600 /* 阶段1: 向下搜索行程(脉冲) */
|
|
#define PF_SEARCH_LIMIT 600 /* 阶段1: 向下搜索行程(脉冲) */
|
|
|
-#define PF_PHASE2_UP 600 /* 阶段1失败后向上退回的行程(脉冲): 用于"起点已在焦后"的情形 */
|
|
|
|
|
-#define PF_PHASE2_LIMIT (PF_SEARCH_LIMIT + PF_PHASE2_UP) /* 阶段2: 向下搜索行程(脉冲) */
|
|
|
|
|
|
|
+#define PF_PHASE2_UP 600 /* 阶段2 反向搜索的基准行程(脉冲), 用于换算 PF_UP_SEARCH_LIMIT */
|
|
|
#define PF_HEADROOM 60 /* 录制起点相对上升沿再上移的量(脉冲): 保证从上升沿之前起录, 同时吃掉反向间隙 */
|
|
#define PF_HEADROOM 60 /* 录制起点相对上升沿再上移的量(脉冲): 保证从上升沿之前起录, 同时吃掉反向间隙 */
|
|
|
#define PF_RISE_STEP_RATIO 1.05f /* 单步涨幅超过该比例才计一次上升(本机清晰度噪声实测 <1%) */
|
|
#define PF_RISE_STEP_RATIO 1.05f /* 单步涨幅超过该比例才计一次上升(本机清晰度噪声实测 <1%) */
|
|
|
#define PF_RISE_CONFIRM 2 /* 连续上升该次数 -> 确认处于上升沿 */
|
|
#define PF_RISE_CONFIRM 2 /* 连续上升该次数 -> 确认处于上升沿 */
|
|
|
#define PF_RISE_RATIO 1.30f /* 且当前清晰度须高于本轮底噪该倍数(排除噪声抖动; 用比值故与清晰度绝对量纲无关) */
|
|
#define PF_RISE_RATIO 1.30f /* 且当前清晰度须高于本轮底噪该倍数(排除噪声抖动; 用比值故与清晰度绝对量纲无关) */
|
|
|
#define PF_FALL_GIVEUP_CNT 3 /* 阶段1: 从未上升过且连续下降该次数 -> 判为已身处焦后下降段, 提前换向省时间 */
|
|
#define PF_FALL_GIVEUP_CNT 3 /* 阶段1: 从未上升过且连续下降该次数 -> 判为已身处焦后下降段, 提前换向省时间 */
|
|
|
|
|
+#define PF_UP_SEARCH_LIMIT (PF_SEARCH_LIMIT + PF_PHASE2_UP) /* 阶段2 反向(向上)搜索行程(脉冲) */
|
|
|
|
|
+#define PF_PEAK_FALL_CNT 2 /* 上行搜索: 越过峰值后连续下降该次数 -> 认为已离开峰值区 */
|
|
|
|
|
+#define PF_PEAK_DROP_RATIO 0.90f /* 且当前值较记录到的峰值回落该比例以下才确认(抗噪声) */
|
|
|
#define PF_FOCUS_TIMEOUT_MS 5000 /* 单次等待 focus 回传的超时(ms) */
|
|
#define PF_FOCUS_TIMEOUT_MS 5000 /* 单次等待 focus 回传的超时(ms) */
|
|
|
#define PF_FOCUS_RETRY_MAX 3 /* 超时后补发 movy 的次数上限 */
|
|
#define PF_FOCUS_RETRY_MAX 3 /* 超时后补发 movy 的次数上限 */
|
|
|
#define PF_TOTAL_TIMEOUT_MS 120000 /* 预对焦总时间预算(ms): 超时整体判失败走兜底, 防止通信异常时长时间卡死 */
|
|
#define PF_TOTAL_TIMEOUT_MS 120000 /* 预对焦总时间预算(ms): 超时整体判失败走兜底, 防止通信异常时长时间卡死 */
|
|
@@ -1357,12 +1364,15 @@ static uint8_t pf_move_sample(uint8_t dir, uint32_t pulse, float *out)
|
|
|
* comm_ok: 出参, 0 表示 focus 通信链路断了(调用方不应再重试)。
|
|
* comm_ok: 出参, 0 表示 focus 通信链路断了(调用方不应再重试)。
|
|
|
* 返回 1 = 找到上升沿(Y 停在发现点); 0 = 未找到(Y 停在起点+traveled)。 */
|
|
* 返回 1 = 找到上升沿(Y 停在发现点); 0 = 未找到(Y 停在起点+traveled)。 */
|
|
|
static uint8_t pf_scan_down(uint32_t limit_pulse, rt_tick_t deadline,
|
|
static uint8_t pf_scan_down(uint32_t limit_pulse, rt_tick_t deadline,
|
|
|
- int *traveled, int *pre_rise, uint8_t *comm_ok)
|
|
|
|
|
|
|
+ int *traveled, int *pre_rise, uint8_t *comm_ok,
|
|
|
|
|
+ uint8_t *fall_giveup)
|
|
|
{
|
|
{
|
|
|
float prev = 0.0f, cur = 0.0f, base = 0.0f;
|
|
float prev = 0.0f, cur = 0.0f, base = 0.0f;
|
|
|
int pos = 0, rise_cnt = 0, fall_cnt = 0;
|
|
int pos = 0, rise_cnt = 0, fall_cnt = 0;
|
|
|
int ever_risen = 0, rise_from = 0;
|
|
int ever_risen = 0, rise_from = 0;
|
|
|
|
|
|
|
|
|
|
+ *fall_giveup = 0;
|
|
|
|
|
+
|
|
|
/* 起点基准值: 未移动直接请求一次 */
|
|
/* 起点基准值: 未移动直接请求一次 */
|
|
|
if (!pf_request_focus()) {
|
|
if (!pf_request_focus()) {
|
|
|
*comm_ok = 0;
|
|
*comm_ok = 0;
|
|
@@ -1408,6 +1418,7 @@ static uint8_t pf_scan_down(uint32_t limit_pulse, rt_tick_t deadline,
|
|
|
if (!ever_risen && fall_cnt >= PF_FALL_GIVEUP_CNT) {
|
|
if (!ever_risen && fall_cnt >= PF_FALL_GIVEUP_CNT) {
|
|
|
LOG_W("[预对焦] 起点已在焦后下降段(从未上升且连续下滑%d次), 提前结束本趟下行",
|
|
LOG_W("[预对焦] 起点已在焦后下降段(从未上升且连续下滑%d次), 提前结束本趟下行",
|
|
|
fall_cnt);
|
|
fall_cnt);
|
|
|
|
|
+ *fall_giveup = 1; /* 告诉调用方: 是"走错方向", 需要反向上搜 */
|
|
|
break;
|
|
break;
|
|
|
}
|
|
}
|
|
|
} else {
|
|
} else {
|
|
@@ -1421,6 +1432,65 @@ static uint8_t pf_scan_down(uint32_t limit_pulse, rt_tick_t deadline,
|
|
|
return 0;
|
|
return 0;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
+/* 阶段2 反向(向上)搜索峰值: 判定"方向反了"时使用 —— 焦区在起始位置上方。
|
|
|
|
|
+ * 上行过程中清晰度先升后降, 越过峰值后连续回落 PF_PEAK_FALL_CNT 次且跌破峰值的
|
|
|
|
|
+ * PF_PEAK_DROP_RATIO, 即认为峰值已过; 此时 Y 在峰值上方一点点, 再由调用方上移
|
|
|
|
|
+ * PF_HEADROOM 作为录制起点(录制方向是下行, 起点必须在焦区上方)。
|
|
|
|
|
+ * 返回 1 = 找到峰值; 0 = 未找到(通信中断/急停/超时/一路升到行程上限仍不见回落)。
|
|
|
|
|
+ * *climb: 出参, 本趟实际上行的脉冲数(供失败时回原位);
|
|
|
|
|
+ * *peak_pos: 出参, 峰值位置(相对本趟起点的上行脉冲)。 */
|
|
|
|
|
+static uint8_t pf_scan_up(uint32_t limit_pulse, rt_tick_t deadline,
|
|
|
|
|
+ int *climb, int *peak_pos, uint8_t *comm_ok)
|
|
|
|
|
+{
|
|
|
|
|
+ float prev = 0.0f, cur = 0.0f, peak = 0.0f;
|
|
|
|
|
+ int pos = 0, peak_at = 0, fall_cnt = 0;
|
|
|
|
|
+
|
|
|
|
|
+ if (!pf_request_focus()) {
|
|
|
|
|
+ *comm_ok = 0;
|
|
|
|
|
+ *climb = 0;
|
|
|
|
|
+ *peak_pos = 0;
|
|
|
|
|
+ return 0;
|
|
|
|
|
+ }
|
|
|
|
|
+ peak = prev = (float)device_status.focus_value;
|
|
|
|
|
+
|
|
|
|
|
+ while ((pos + (int)PF_STEP) <= (int)limit_pulse) {
|
|
|
|
|
+ if (slide_para.stop_flag != 0) {
|
|
|
|
|
+ LOG_W("[预对焦] 上行搜索收到急停, 中断(已上行%d脉冲)", pos);
|
|
|
|
|
+ break;
|
|
|
|
|
+ }
|
|
|
|
|
+ if ((rt_int32_t)(rt_tick_get() - deadline) > 0) {
|
|
|
|
|
+ LOG_W("[预对焦] 上行搜索超出时间预算, 中断(已上行%d脉冲)", pos);
|
|
|
|
|
+ break;
|
|
|
|
|
+ }
|
|
|
|
|
+ if (!pf_move_sample(0, PF_STEP, &cur)) { /* dir=0 = 上行 */
|
|
|
|
|
+ *comm_ok = 0;
|
|
|
|
|
+ break;
|
|
|
|
|
+ }
|
|
|
|
|
+ pos += (int)PF_STEP;
|
|
|
|
|
+
|
|
|
|
|
+ if (cur > peak) {
|
|
|
|
|
+ peak = cur;
|
|
|
|
|
+ peak_at = pos;
|
|
|
|
|
+ fall_cnt = 0;
|
|
|
|
|
+ } else if (cur < prev && peak > 0.0f && cur < peak * PF_PEAK_DROP_RATIO) {
|
|
|
|
|
+ if (++fall_cnt >= PF_PEAK_FALL_CNT) {
|
|
|
|
|
+ *climb = pos;
|
|
|
|
|
+ *peak_pos = peak_at;
|
|
|
|
|
+ LOG_I("[预对焦] 上行搜索发现峰值: 峰值位置=+%d脉冲(清晰度%.2f), 当前已上行%d脉冲",
|
|
|
|
|
+ peak_at, peak, pos);
|
|
|
|
|
+ return 1;
|
|
|
|
|
+ }
|
|
|
|
|
+ } else {
|
|
|
|
|
+ fall_cnt = 0;
|
|
|
|
|
+ }
|
|
|
|
|
+ prev = cur;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ *climb = pos;
|
|
|
|
|
+ *peak_pos = peak_at;
|
|
|
|
|
+ return 0;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
/* 预对焦主流程: 找到"清晰度上升前"的位置, 并把 Y 轴回退到该位置上方 PF_HEADROOM 处待录。
|
|
/* 预对焦主流程: 找到"清晰度上升前"的位置, 并把 Y 轴回退到该位置上方 PF_HEADROOM 处待录。
|
|
|
* 返回 1 = 成功(Y 已在录制起点); 0 = 失败, 调用方应按 slide_set_record_travel(0) 的兜底行程录制 */
|
|
* 返回 1 = 成功(Y 已在录制起点); 0 = 失败, 调用方应按 slide_set_record_travel(0) 的兜底行程录制 */
|
|
|
int slide_pre_focus(void)
|
|
int slide_pre_focus(void)
|
|
@@ -1429,6 +1499,7 @@ int slide_pre_focus(void)
|
|
|
int traveled = 0, pre_rise = 0;
|
|
int traveled = 0, pre_rise = 0;
|
|
|
int net = 0; /* 相对进入时位置的净下行量(脉冲) */
|
|
int net = 0; /* 相对进入时位置的净下行量(脉冲) */
|
|
|
uint8_t comm_ok = 1, found = 0;
|
|
uint8_t comm_ok = 1, found = 0;
|
|
|
|
|
+ uint8_t fall_giveup = 0; /* 阶段1: 是否因"连续下降=方向反了"提前结束 */
|
|
|
uint32_t saved_period = slide_para.pulse_period; /* 预对焦用小步PWM, 退出时恢复,
|
|
uint32_t saved_period = slide_para.pulse_period; /* 预对焦用小步PWM, 退出时恢复,
|
|
|
* 否则会污染后续 X/Z 复位的速度档 */
|
|
* 否则会污染后续 X/Z 复位的速度档 */
|
|
|
|
|
|
|
@@ -1440,28 +1511,59 @@ int slide_pre_focus(void)
|
|
|
deadline = rt_tick_get() + rt_tick_from_millisecond(PF_TOTAL_TIMEOUT_MS);
|
|
deadline = rt_tick_get() + rt_tick_from_millisecond(PF_TOTAL_TIMEOUT_MS);
|
|
|
LOG_I("[预对焦] 开始: 步长=%d脉冲 阶段1搜索%d脉冲", PF_STEP, PF_SEARCH_LIMIT);
|
|
LOG_I("[预对焦] 开始: 步长=%d脉冲 阶段1搜索%d脉冲", PF_STEP, PF_SEARCH_LIMIT);
|
|
|
|
|
|
|
|
- found = pf_scan_down(PF_SEARCH_LIMIT, deadline, &traveled, &pre_rise, &comm_ok);
|
|
|
|
|
|
|
+ found = pf_scan_down(PF_SEARCH_LIMIT, deadline, &traveled, &pre_rise, &comm_ok,
|
|
|
|
|
+ &fall_giveup);
|
|
|
net += traveled;
|
|
net += traveled;
|
|
|
|
|
|
|
|
- /* 阶段2: 阶段1没找到, 通常是回程丢步把轨迹整体往下带、起点已经越过峰值。
|
|
|
|
|
- * 向上退回 PF_PHASE2_UP 之后再往下重扫一趟更长的行程。 */
|
|
|
|
|
|
|
+ /* 阶段2: 向下没找到 —— 回到最开始的位置, 再反向(向上)搜一次。
|
|
|
|
|
+ * 触发依据: ① 下行一开始就"从未上升且连续下降"(fall_giveup), 说明方向反了、焦区在起始位置上方;
|
|
|
|
|
+ * ② 走完整段行程才停(回程丢步把轨迹整体往下带), 也按同样方式回到原点重搜,
|
|
|
|
|
+ * 避免继续在已经跑偏的方向上白走。 */
|
|
|
if (!found && comm_ok && slide_para.stop_flag == 0 &&
|
|
if (!found && comm_ok && slide_para.stop_flag == 0 &&
|
|
|
(rt_int32_t)(rt_tick_get() - deadline) <= 0) {
|
|
(rt_int32_t)(rt_tick_get() - deadline) <= 0) {
|
|
|
- LOG_W("[预对焦] 阶段1未发现上升沿(已下行%d脉冲), 上退%d脉冲后重扫%d脉冲",
|
|
|
|
|
- traveled, PF_PHASE2_UP, PF_PHASE2_LIMIT);
|
|
|
|
|
- pf_move(0, PF_PHASE2_UP);
|
|
|
|
|
- net -= (int)PF_PHASE2_UP;
|
|
|
|
|
- found = pf_scan_down(PF_PHASE2_LIMIT, deadline, &traveled, &pre_rise, &comm_ok);
|
|
|
|
|
- net += traveled;
|
|
|
|
|
|
|
+ int up_climb = 0, peak_pos = 0;
|
|
|
|
|
+
|
|
|
|
|
+ /* ① 回到最开始的位置(进入预对焦时的高度) */
|
|
|
|
|
+ if (net > 0) {
|
|
|
|
|
+ pf_move(0, (uint32_t)net);
|
|
|
|
|
+ LOG_W("[预对焦] 阶段1未找到上升沿(%s, 已下行%d脉冲), 回退到起始位置后反向搜索",
|
|
|
|
|
+ fall_giveup ? "方向反了" : "行程内未见上升", net);
|
|
|
|
|
+ net = 0;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ /* ② 反向(向上)搜索峰值 */
|
|
|
|
|
+ LOG_I("[预对焦] 阶段2: 自起始位置向上搜索%d脉冲", PF_UP_SEARCH_LIMIT);
|
|
|
|
|
+ found = pf_scan_up(PF_UP_SEARCH_LIMIT, deadline, &up_climb, &peak_pos, &comm_ok);
|
|
|
|
|
+ net -= up_climb; /* 上行 = 净下行量减少 */
|
|
|
|
|
+
|
|
|
|
|
+ if (found) {
|
|
|
|
|
+ /* 录制方向是下行, 起点必须再往焦区上方让出 PF_HEADROOM;
|
|
|
|
|
+ 但不要超出本次上行搜索的行程上限, 免得顶到机械限位 */
|
|
|
|
|
+ if (PF_HEADROOM > 0 && slide_para.stop_flag == 0 &&
|
|
|
|
|
+ (up_climb + (int)PF_HEADROOM) <= (int)PF_UP_SEARCH_LIMIT) {
|
|
|
|
|
+ pf_move(0, PF_HEADROOM);
|
|
|
|
|
+ net -= (int)PF_HEADROOM;
|
|
|
|
|
+ }
|
|
|
|
|
+ LOG_I("[预对焦] 反向搜索成功: 峰值+%d脉冲, 停在峰值上方%d脉冲处等待录制",
|
|
|
|
|
+ peak_pos, -net);
|
|
|
|
|
+ slide_para.pulse_period = saved_period;
|
|
|
|
|
+ g_pf_busy = 0;
|
|
|
|
|
+ return 1;
|
|
|
|
|
+ }
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
if (!found) {
|
|
if (!found) {
|
|
|
LOG_W("[预对焦] 失败(净下行%d脉冲, %s), 回原位后按加长行程兜底录制",
|
|
LOG_W("[预对焦] 失败(净下行%d脉冲, %s), 回原位后按加长行程兜底录制",
|
|
|
- net, comm_ok ? "未搜到上升沿" : "focus回传中断");
|
|
|
|
|
|
|
+ net, comm_ok ? "未搜到焦区(已双向搜索)" : "focus回传中断");
|
|
|
/* 关键: 必须退回进入预对焦时的高度再开始录。兜底行程是照着"到位就录"的老行为
|
|
/* 关键: 必须退回进入预对焦时的高度再开始录。兜底行程是照着"到位就录"的老行为
|
|
|
- * 标定的, 若不退回, 净下行的这部分会叠加在兜底行程上, 有可能超出机械下限。 */
|
|
|
|
|
- if (net > 0 && comm_ok) {
|
|
|
|
|
- pf_move(0, (uint32_t)net);
|
|
|
|
|
|
|
+ * 标定的, 若不退回, 净下行(反向搜索后也可能是净上行)的部分会叠加在兜底行程上,
|
|
|
|
|
+ * 有可能超出机械下限。 */
|
|
|
|
|
+ if (comm_ok) {
|
|
|
|
|
+ if (net > 0) {
|
|
|
|
|
+ pf_move(0, (uint32_t)net); /* 曾在起始位置下方: 上行回原位 */
|
|
|
|
|
+ } else if (net < 0) {
|
|
|
|
|
+ pf_move(1, (uint32_t)(-net)); /* 曾在起始位置上方: 下行回原位 */
|
|
|
|
|
+ }
|
|
|
}
|
|
}
|
|
|
slide_para.pulse_period = saved_period;
|
|
slide_para.pulse_period = saved_period;
|
|
|
g_pf_busy = 0;
|
|
g_pf_busy = 0;
|