|
@@ -213,23 +213,65 @@ void send_signal(int sig)
|
|
|
rt_thread_kill(ctr_motor, sig);
|
|
rt_thread_kill(ctr_motor, sig);
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
-/* ========== 自动对焦(Auto Focus)控制块 ========== */
|
|
|
|
|
|
|
+/* ========== 自动对焦(Auto Focus)控制块 ==========
|
|
|
|
|
+ * 两级对焦策略:
|
|
|
|
|
+ * 粗扫: ±AF_COARSE_STEP 探测选定方向后, 以 AF_COARSE_STEP 步长在
|
|
|
|
|
+ * ±AF_SEARCH_LIMIT 范围内有界扫描, 全程跟踪全局最大清晰度,
|
|
|
|
|
+ * 连续大幅跌落(明显变小)或连续未提升且最佳值显著高于本腿下限,
|
|
|
|
|
+ * 即确认越过峰值; 一侧未找到峰值则换向扫另一侧。
|
|
|
|
|
+ * 细扫: 回到粗扫最佳位置重新采样作基准, 爬山法沿单一方向以 AF_FINE_STEP
|
|
|
|
|
+ * 步长移动, 上升则继续, 显著下降则调转方向, 且当前值跌破细扫最佳值
|
|
|
|
|
+ * *AF_FINE_CONFIRM_RATIO(处于峰后下降段)计一次确认, 峰前后各确认
|
|
|
|
|
+ * AF_FINE_CONFIRM_CNT 次即完成, 回到细扫实测最佳位置。
|
|
|
|
|
+ * 注: 起点首帧清晰度可能异常(残留上一帧), 基准帧不计入最佳值,
|
|
|
|
|
+ * 探测/扫描帧均在移动后采样, 数值可靠。 */
|
|
|
#define AF_FOCUS_TIMEOUT_MS 5000 /* 等待 focus 回复超时时间(ms) */
|
|
#define AF_FOCUS_TIMEOUT_MS 5000 /* 等待 focus 回复超时时间(ms) */
|
|
|
#define AF_FOCUS_RETRY_MAX 3 /* 等待 focus 超时补发指令次数上限 */
|
|
#define AF_FOCUS_RETRY_MAX 3 /* 等待 focus 超时补发指令次数上限 */
|
|
|
|
|
|
|
|
|
|
+/* 对焦算法参数(集中定义, 便于现场调参) */
|
|
|
|
|
+#define AF_COARSE_STEP 20 /* 粗扫步长(脉冲) */
|
|
|
|
|
+#define AF_SEARCH_LIMIT 600 /* 粗扫单侧最大行程(脉冲), 兜底边界 */
|
|
|
|
|
+#define AF_PEAK_CONFIRM_CNT 3 /* 粗扫: 连续N次未超越最佳值/大幅跌落即确认越过峰值 */
|
|
|
|
|
+#define AF_PEAK_RISE_RATIO 2.0f /* 粗扫: 最佳值高于本腿清晰度下限该倍数才确认平台型峰 */
|
|
|
|
|
+#define AF_COARSE_FALL_RATIO 0.70f /* 粗扫: 清晰度跌破最佳值*该比例计为一次大幅跌落(明显变小立即收尾) */
|
|
|
|
|
+#define AF_FINE_STEP 2 /* 细扫步长(脉冲) */
|
|
|
|
|
+#define AF_FINE_WINDOW 10 /* 细扫窗口: 粗扫最佳位置 ±10 */
|
|
|
|
|
+#define AF_FINE_TREND_RATIO 0.95f /* 细扫: 清晰度不低于上次值*该比例视为上升/持平(噪声容忍), 低于则调头 */
|
|
|
|
|
+#define AF_FINE_CONFIRM_CNT 2 /* 细扫: 越峰确认次数(峰前后各一次下降), 达到即完成 */
|
|
|
|
|
+#define AF_FINE_CONFIRM_RATIO 0.90f /* 细扫: 当前值跌破细扫最佳值*该比例(已离开峰顶)才计为有效确认 */
|
|
|
|
|
+#define AF_FINE_VALID_RATIO 0.90f /* 细扫: 确认完成时细扫最佳须达到全局最佳值*该比例才允许结束, 否则以细扫实测重新初始化基准 */
|
|
|
|
|
+#define AF_FINE_STEPS_MAX 24 /* 细扫最大采样步数(防振荡兜底) */
|
|
|
|
|
+#define AF_MAX_ITERATIONS 120 /* 采样总数安全上限 */
|
|
|
|
|
+
|
|
|
|
|
+/* 对焦阶段 */
|
|
|
|
|
+enum {
|
|
|
|
|
+ AF_STAGE_BASE, /* 等待起点基准清晰度 */
|
|
|
|
|
+ AF_STAGE_PROBE_P, /* 等待 +AF_COARSE_STEP 探测点清晰度 */
|
|
|
|
|
+ AF_STAGE_PROBE_M, /* 等待 -AF_COARSE_STEP 探测点清晰度 */
|
|
|
|
|
+ AF_STAGE_COARSE, /* 粗扫 */
|
|
|
|
|
+ AF_STAGE_FINE, /* 细扫 */
|
|
|
|
|
+};
|
|
|
|
|
+
|
|
|
typedef struct {
|
|
typedef struct {
|
|
|
- float best_sharpness; /* 全局最佳清晰度 */
|
|
|
|
|
|
|
+ float best_sharpness; /* 全局最佳清晰度(粗扫+细扫全程维护) */
|
|
|
int best_pos; /* 全局最佳位置 */
|
|
int best_pos; /* 全局最佳位置 */
|
|
|
- float last_sharpness; /* 上一次移动后的清晰度 */
|
|
|
|
|
- int dir; /* 行进方向: 1=正向, 0=反向 */
|
|
|
|
|
- int step_size; /* 步进电机脉冲数 */
|
|
|
|
|
- int current_pos; /* 当前位置 */
|
|
|
|
|
- int peak_counter; /* 连续上升后下降的峰值计数 */
|
|
|
|
|
- int reduce_counter; /* 清晰度连续降低计数 */
|
|
|
|
|
- int diff_counter; /* 清晰度变化微小累计次数 */
|
|
|
|
|
- bool last_rising; /* 上次是否上升 */
|
|
|
|
|
- int max_iterations; /* 最大迭代次数 */
|
|
|
|
|
- bool initial_move; /* 初始方向判断标志 */
|
|
|
|
|
|
|
+ int current_pos; /* 当前位置(相对起点) */
|
|
|
|
|
+ int stage; /* 当前阶段 AF_STAGE_xxx */
|
|
|
|
|
+ float probe_plus; /* +AF_COARSE_STEP 探测点清晰度 */
|
|
|
|
|
+ int coarse_dir; /* 粗扫方向: 1=正向, 0=反向 */
|
|
|
|
|
+ int coarse_limit; /* 当前粗扫腿的边界位置 */
|
|
|
|
|
+ bool coarse_both_done; /* 双向均已扫描 */
|
|
|
|
|
+ int confirm_cnt; /* 连续未超越最佳值计数 */
|
|
|
|
|
+ int fall_cnt; /* 连续大幅跌落计数(清晰度明显变小, 越峰判断) */
|
|
|
|
|
+ float coarse_floor; /* 本腿清晰度下限(平台型峰显著性判断基准) */
|
|
|
|
|
+ int fine_dir; /* 细扫: 当前行进方向 1=正 0=反 */
|
|
|
|
|
+ int fine_center; /* 细扫: 中心位置(粗扫最佳位置) */
|
|
|
|
|
+ float fine_last; /* 细扫: 上一采样点清晰度, <0 表示等待中心基准帧 */
|
|
|
|
|
+ float fine_best_sharpness; /* 细扫: 阶段内最佳清晰度(只统计细扫期采样, 同一漂移基线互相可比) */
|
|
|
|
|
+ int fine_best_pos; /* 细扫: 阶段内最佳位置(最终回位依据) */
|
|
|
|
|
+ int fine_confirm; /* 细扫: 越峰确认次数(峰前后各一次显著下降) */
|
|
|
|
|
+ int fine_step_cnt; /* 细扫: 已采样步数 */
|
|
|
|
|
+ int max_iterations; /* 剩余采样数安全上限 */
|
|
|
volatile bool sharpness_updated; /* 信号处理程序置位: 有新清晰度值 */
|
|
volatile bool sharpness_updated; /* 信号处理程序置位: 有新清晰度值 */
|
|
|
float new_sharpness; /* 信号处理程序写入的新清晰度值 */
|
|
float new_sharpness; /* 信号处理程序写入的新清晰度值 */
|
|
|
bool active; /* 对焦进行中标志(用于超时重试判定) */
|
|
bool active; /* 对焦进行中标志(用于超时重试判定) */
|
|
@@ -251,160 +293,302 @@ static void auto_focus_init(void)
|
|
|
{
|
|
{
|
|
|
memset(&g_af, 0, sizeof(g_af));
|
|
memset(&g_af, 0, sizeof(g_af));
|
|
|
g_af.best_sharpness = -FLT_MAX;
|
|
g_af.best_sharpness = -FLT_MAX;
|
|
|
- g_af.last_sharpness = 0.0f;
|
|
|
|
|
- g_af.initial_move = true;
|
|
|
|
|
- g_af.last_rising = false;
|
|
|
|
|
- g_af.dir = 1;
|
|
|
|
|
- g_af.step_size = 20;
|
|
|
|
|
|
|
+ g_af.fine_best_sharpness = -FLT_MAX; /* 无细扫数据标志 */
|
|
|
g_af.current_pos = 0;
|
|
g_af.current_pos = 0;
|
|
|
- g_af.max_iterations = 100;
|
|
|
|
|
- g_af.peak_counter = 0;
|
|
|
|
|
- g_af.best_pos = 0;
|
|
|
|
|
- g_af.reduce_counter = 0;
|
|
|
|
|
|
|
+ g_af.stage = AF_STAGE_BASE;
|
|
|
|
|
+ g_af.max_iterations = AF_MAX_ITERATIONS;
|
|
|
/* 超时重试状态 */
|
|
/* 超时重试状态 */
|
|
|
g_af.active = true;
|
|
g_af.active = true;
|
|
|
g_af.wait_start_tick = rt_tick_get();
|
|
g_af.wait_start_tick = rt_tick_get();
|
|
|
g_af.retry_cnt = 0;
|
|
g_af.retry_cnt = 0;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
-/* 移动电机一步 */
|
|
|
|
|
-static void auto_focus_move(void)
|
|
|
|
|
|
|
+/* 相对移动电机到目标位置 */
|
|
|
|
|
+static void auto_focus_move_to(int target_pos)
|
|
|
{
|
|
{
|
|
|
- g_af.current_pos += g_af.dir ? g_af.step_size : -g_af.step_size;
|
|
|
|
|
- LOG_I("[对焦] 移动:方向=%d 步长=%d 位置=%d", g_af.dir, g_af.step_size, g_af.current_pos);
|
|
|
|
|
|
|
+ int delta = target_pos - g_af.current_pos;
|
|
|
|
|
+ if (delta == 0) {
|
|
|
|
|
+ /* 无需移动, 立即请求一次采样 */
|
|
|
|
|
+ g_af.wait_start_tick = rt_tick_get();
|
|
|
|
|
+ return;
|
|
|
|
|
+ }
|
|
|
|
|
+ g_af.current_pos = target_pos;
|
|
|
|
|
|
|
|
slide_para.pulse_period = mapSpeed(1);
|
|
slide_para.pulse_period = mapSpeed(1);
|
|
|
- slide_para.ystep = g_af.step_size;
|
|
|
|
|
- slide_para.ydir = g_af.dir;
|
|
|
|
|
- slide_para.ydis = g_af.step_size;
|
|
|
|
|
|
|
+ slide_para.ystep = abs(delta);
|
|
|
|
|
+ slide_para.ydir = (delta > 0) ? 1 : 0;
|
|
|
|
|
+ slide_para.ydis = abs(delta);
|
|
|
slide_para.en_slide = 6;
|
|
slide_para.en_slide = 6;
|
|
|
|
|
|
|
|
while (slide_para.en_slide != 0) {
|
|
while (slide_para.en_slide != 0) {
|
|
|
rt_thread_delay(100);
|
|
rt_thread_delay(100);
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
- LOG_I("[对焦] 电机到位");
|
|
|
|
|
|
|
+ LOG_I("[对焦] 电机到位 位置=%d", g_af.current_pos);
|
|
|
g_af.wait_start_tick = rt_tick_get(); /* 移动完成,开始等待 focus 回复 */
|
|
g_af.wait_start_tick = rt_tick_get(); /* 移动完成,开始等待 focus 回复 */
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
/* 完成对焦 */
|
|
/* 完成对焦 */
|
|
|
static void auto_focus_done(void)
|
|
static void auto_focus_done(void)
|
|
|
{
|
|
{
|
|
|
- LOG_I("[对焦] 完成! 最佳位置:%d 清晰度:%.2f", g_af.best_pos, g_af.best_sharpness);
|
|
|
|
|
|
|
+ /* 最终回位位置: 细扫有采样数据则以细扫实测最佳为准(同一漂移基线, 可比);
|
|
|
|
|
+ * 否则(粗扫阶段被采样上限中断等)回全局最佳位置 */
|
|
|
|
|
+ int final_pos = g_af.best_pos;
|
|
|
|
|
+ float final_sharpness = g_af.best_sharpness;
|
|
|
|
|
+ if (g_af.fine_best_sharpness > -FLT_MAX) {
|
|
|
|
|
+ final_pos = g_af.fine_best_pos;
|
|
|
|
|
+ final_sharpness = g_af.fine_best_sharpness;
|
|
|
|
|
+ }
|
|
|
|
|
+ LOG_I("[对焦] 完成! 最佳位置:%d 清晰度:%.2f", final_pos, final_sharpness);
|
|
|
|
|
+ /* 回到最佳位置 */
|
|
|
|
|
+ if (g_af.current_pos != final_pos) {
|
|
|
|
|
+ LOG_I("[对焦] 回位到最佳位置:%d", final_pos);
|
|
|
|
|
+ auto_focus_move_to(final_pos);
|
|
|
|
|
+ }
|
|
|
g_af.active = false; /* 对焦完成,停止超时重试检测 */
|
|
g_af.active = false; /* 对焦完成,停止超时重试检测 */
|
|
|
char *c0 = makeJson_Warn("auto_focus","auto_focus");
|
|
char *c0 = makeJson_Warn("auto_focus","auto_focus");
|
|
|
DTU_Write("%s",c0);
|
|
DTU_Write("%s",c0);
|
|
|
rt_free(c0);
|
|
rt_free(c0);
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
-/* 处理一次清晰度值,执行对焦算法 */
|
|
|
|
|
-static void auto_focus_process(void)
|
|
|
|
|
|
|
+/* 进入细扫阶段 */
|
|
|
|
|
+static void auto_focus_enter_fine(void)
|
|
|
{
|
|
{
|
|
|
- float current_sharpness = g_af.new_sharpness;
|
|
|
|
|
- g_af.sharpness_updated = false;
|
|
|
|
|
- g_af.max_iterations--;
|
|
|
|
|
- g_af.retry_cnt = 0; /* 收到 focus 回复,清零超时重试计数 */
|
|
|
|
|
- g_af.wait_start_tick = rt_tick_get(); /* 重置等待计时 */
|
|
|
|
|
|
|
+ LOG_I("[对焦] 进入细扫 最佳位置=%d 清晰度=%.2f", g_af.best_pos, g_af.best_sharpness);
|
|
|
|
|
+ g_af.stage = AF_STAGE_FINE;
|
|
|
|
|
+ g_af.fine_center = g_af.best_pos;
|
|
|
|
|
+ g_af.fine_dir = 1;
|
|
|
|
|
+ g_af.fine_last = -1.0f; /* 等待中心基准帧(回到中心后重新采样作趋势基准) */
|
|
|
|
|
+ g_af.fine_best_sharpness = -FLT_MAX;
|
|
|
|
|
+ g_af.fine_best_pos = g_af.best_pos;
|
|
|
|
|
+ g_af.fine_confirm = 0;
|
|
|
|
|
+ g_af.fine_step_cnt = 0;
|
|
|
|
|
+ auto_focus_move_to(g_af.fine_center);
|
|
|
|
|
+}
|
|
|
|
|
|
|
|
- LOG_I("[对焦] 清晰度:%.2f 位置:%d 剩余:%d",
|
|
|
|
|
- current_sharpness, g_af.current_pos, g_af.max_iterations);
|
|
|
|
|
|
|
+/* 粗扫阶段: 处理一次清晰度采样
|
|
|
|
|
+ * 越峰确认(满足其一即进入细扫):
|
|
|
|
|
+ * a) 连续 AF_PEAK_CONFIRM_CNT 次大幅跌落(当前值 < 最佳值*AF_COARSE_FALL_RATIO),
|
|
|
|
|
+ * 清晰度明显变小立即收尾, 不再扫到边界;
|
|
|
|
|
+ * b) 连续 AF_PEAK_CONFIRM_CNT 次未超越最佳值, 且最佳值显著高于本腿清晰度下限
|
|
|
|
|
+ * (平台型峰, 峰值恰在探测点时也能确认)。 */
|
|
|
|
|
+static void auto_focus_coarse_step(float current_sharpness)
|
|
|
|
|
+{
|
|
|
|
|
+ /* 维护本腿清晰度下限 */
|
|
|
|
|
+ if (current_sharpness < g_af.coarse_floor) {
|
|
|
|
|
+ g_af.coarse_floor = current_sharpness;
|
|
|
|
|
+ }
|
|
|
|
|
|
|
|
- /* 更新最佳位置 */
|
|
|
|
|
|
|
+ /* 更新全局最佳 */
|
|
|
if (current_sharpness > g_af.best_sharpness) {
|
|
if (current_sharpness > g_af.best_sharpness) {
|
|
|
g_af.best_sharpness = current_sharpness;
|
|
g_af.best_sharpness = current_sharpness;
|
|
|
g_af.best_pos = g_af.current_pos;
|
|
g_af.best_pos = g_af.current_pos;
|
|
|
- LOG_I("[对焦] 新最佳:位置%d 清晰度%.2f", g_af.best_pos, g_af.best_sharpness);
|
|
|
|
|
|
|
+ g_af.confirm_cnt = 0;
|
|
|
|
|
+ g_af.fall_cnt = 0;
|
|
|
|
|
+ LOG_I("[对焦] 粗扫新最佳:位置=%d 清晰度=%.2f", g_af.best_pos, g_af.best_sharpness);
|
|
|
|
|
+ } else {
|
|
|
|
|
+ g_af.confirm_cnt++;
|
|
|
|
|
+ if (current_sharpness < g_af.best_sharpness * AF_COARSE_FALL_RATIO) {
|
|
|
|
|
+ g_af.fall_cnt++;
|
|
|
|
|
+ } else {
|
|
|
|
|
+ g_af.fall_cnt = 0;
|
|
|
|
|
+ }
|
|
|
|
|
+ LOG_I("[对焦] 粗扫未提升[%d/%d] 跌落[%d/%d] 清晰度=%.2f",
|
|
|
|
|
+ g_af.confirm_cnt, AF_PEAK_CONFIRM_CNT,
|
|
|
|
|
+ g_af.fall_cnt, AF_PEAK_CONFIRM_CNT, current_sharpness);
|
|
|
|
|
+ if (g_af.fall_cnt >= AF_PEAK_CONFIRM_CNT ||
|
|
|
|
|
+ (g_af.confirm_cnt >= AF_PEAK_CONFIRM_CNT &&
|
|
|
|
|
+ g_af.best_sharpness > g_af.coarse_floor * AF_PEAK_RISE_RATIO)) {
|
|
|
|
|
+ LOG_I("[对焦] 粗扫确认越过峰值");
|
|
|
|
|
+ auto_focus_enter_fine();
|
|
|
|
|
+ return;
|
|
|
|
|
+ }
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
- /* 初始方向判断:比较首次移动前后的清晰度 */
|
|
|
|
|
- if (g_af.initial_move) {
|
|
|
|
|
- g_af.initial_move = false;
|
|
|
|
|
- if (current_sharpness > g_af.last_sharpness) {
|
|
|
|
|
- LOG_I("[对焦] 初始方向正确");
|
|
|
|
|
|
|
+ /* 到达本腿边界: 换向扫另一侧或结束粗扫 */
|
|
|
|
|
+ if ((g_af.coarse_dir && g_af.current_pos >= g_af.coarse_limit) ||
|
|
|
|
|
+ (!g_af.coarse_dir && g_af.current_pos <= g_af.coarse_limit)) {
|
|
|
|
|
+ if (g_af.coarse_both_done) {
|
|
|
|
|
+ LOG_I("[对焦] 粗扫双向完成");
|
|
|
|
|
+ auto_focus_enter_fine();
|
|
|
} else {
|
|
} else {
|
|
|
- g_af.dir = !g_af.dir;
|
|
|
|
|
- LOG_I("[对焦] 初始方向错误,反转");
|
|
|
|
|
|
|
+ LOG_I("[对焦] 粗扫本腿未找到峰值, 换向扫描");
|
|
|
|
|
+ g_af.coarse_both_done = true;
|
|
|
|
|
+ g_af.coarse_dir = !g_af.coarse_dir;
|
|
|
|
|
+ g_af.coarse_limit = g_af.coarse_dir ? AF_SEARCH_LIMIT : -AF_SEARCH_LIMIT;
|
|
|
|
|
+ g_af.confirm_cnt = 0;
|
|
|
|
|
+ g_af.fall_cnt = 0;
|
|
|
|
|
+ g_af.coarse_floor = FLT_MAX; /* 新一腿重新统计清晰度下限 */
|
|
|
|
|
+ /* 已扫过的区域直接跳回起点0(基准帧不可靠, 0点重新采样), 避免重复采样 */
|
|
|
|
|
+ auto_focus_move_to(0);
|
|
|
}
|
|
}
|
|
|
- g_af.last_sharpness = current_sharpness;
|
|
|
|
|
- auto_focus_move();
|
|
|
|
|
return;
|
|
return;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
- /* 计算清晰度变化 */
|
|
|
|
|
- float diff = current_sharpness - g_af.last_sharpness;
|
|
|
|
|
-
|
|
|
|
|
- /* 动态调整步长 */
|
|
|
|
|
- if (fabs(diff) > 3.0f) {
|
|
|
|
|
- /* 清晰度大幅变化 -> 缩小步长精细搜索 */
|
|
|
|
|
- g_af.step_size = 10;
|
|
|
|
|
- g_af.diff_counter = 0;
|
|
|
|
|
- LOG_I("[对焦] 大幅变化,步长=%d", g_af.step_size);
|
|
|
|
|
- } else if (fabs(diff) < 1.0f) {
|
|
|
|
|
- /* 清晰度变化微小 -> 累计平坦区域 */
|
|
|
|
|
- g_af.diff_counter++;
|
|
|
|
|
- if (g_af.diff_counter >= 10) {
|
|
|
|
|
- g_af.step_size = 20;
|
|
|
|
|
- }
|
|
|
|
|
- LOG_I("[对焦] 平坦[%d/10] 步长=%d", g_af.diff_counter, g_af.step_size);
|
|
|
|
|
- } else {
|
|
|
|
|
- g_af.diff_counter = 0;
|
|
|
|
|
|
|
+ /* 继续粗扫 */
|
|
|
|
|
+ auto_focus_move_to(g_af.coarse_dir ?
|
|
|
|
|
+ g_af.current_pos + AF_COARSE_STEP :
|
|
|
|
|
+ g_af.current_pos - AF_COARSE_STEP);
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+/* 细扫阶段: 爬山法 + 峰前后双次下降确认
|
|
|
|
|
+ * 回到中心重新采样作趋势基准, 然后沿当前方向以 AF_FINE_STEP 步长移动:
|
|
|
|
|
+ * 上升/持平(不低于上次值*AF_FINE_TREND_RATIO) → 继续同方向找更清晰位置;
|
|
|
|
|
+ * 显著下降 → 调转方向, 且当前值已跌破细扫最佳值*AF_FINE_CONFIRM_RATIO
|
|
|
|
|
+ * (确认已越过峰顶, 处于峰后下降段) → 计一次越峰确认;
|
|
|
|
|
+ * 峰的前后各确认一次(AF_FINE_CONFIRM_CNT 次) → 峰值确认完成, 回位最佳点;
|
|
|
|
|
+ * 到达 ±AF_FINE_WINDOW 窗口边界强制调头; 步数上限兜底。 */
|
|
|
|
|
+static void auto_focus_fine_step(float current_sharpness)
|
|
|
|
|
+{
|
|
|
|
|
+ /* 更新细扫阶段最佳(只统计细扫期采样, 与粗扫历史值不混用,
|
|
|
|
|
+ * 避免相机漂移导致早期虚高值掩盖当前真实峰位) */
|
|
|
|
|
+ if (current_sharpness > g_af.fine_best_sharpness) {
|
|
|
|
|
+ g_af.fine_best_sharpness = current_sharpness;
|
|
|
|
|
+ g_af.fine_best_pos = g_af.current_pos;
|
|
|
|
|
+ LOG_I("[对焦] 细扫新最佳:位置=%d 清晰度=%.2f",
|
|
|
|
|
+ g_af.fine_best_pos, g_af.fine_best_sharpness);
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
- if (current_sharpness > (g_af.last_sharpness - 3.5f)) { // 噪声容忍阈值3.0
|
|
|
|
|
- // 保持方向,当接近峰值时自动减小步长
|
|
|
|
|
- LOG_I("保持方向,当接近峰值时自动减小步长...");
|
|
|
|
|
- g_af.last_rising = true;
|
|
|
|
|
- g_af.step_size += 2; //动态调整步幅
|
|
|
|
|
- } else {
|
|
|
|
|
- if(g_af.last_rising){
|
|
|
|
|
- g_af.peak_counter++;
|
|
|
|
|
- LOG_I("[对焦] 检测到清晰度波动[%d/2]", g_af.peak_counter);
|
|
|
|
|
- g_af.step_size = 2;
|
|
|
|
|
- }else {
|
|
|
|
|
- g_af.peak_counter = 0; // 连续下降则重置
|
|
|
|
|
- }
|
|
|
|
|
- g_af.last_rising = false;
|
|
|
|
|
- g_af.dir = !g_af.dir; // 清晰度下降,反转方向
|
|
|
|
|
- LOG_I("[对焦] 清晰度下降,反转方向:%d 步长:%d", g_af.dir, g_af.step_size);
|
|
|
|
|
|
|
+ g_af.fine_step_cnt++;
|
|
|
|
|
+ if (g_af.fine_step_cnt > AF_FINE_STEPS_MAX) {
|
|
|
|
|
+ LOG_W("[对焦] 细扫步数达上限, 结束");
|
|
|
|
|
+ auto_focus_done();
|
|
|
|
|
+ return;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
- g_af.last_sharpness = current_sharpness;
|
|
|
|
|
|
|
+ /* 中心基准帧: 回到中心后的第一次采样仅作趋势基准 */
|
|
|
|
|
+ if (g_af.fine_last < 0.0f) {
|
|
|
|
|
+ g_af.fine_last = current_sharpness;
|
|
|
|
|
+ auto_focus_move_to(g_af.fine_center + AF_FINE_STEP);
|
|
|
|
|
+ return;
|
|
|
|
|
+ }
|
|
|
|
|
|
|
|
|
|
+ int dir_sign = g_af.fine_dir ? 1 : -1;
|
|
|
|
|
|
|
|
- /* 平坦区域超时:强制反转并增大步长 */
|
|
|
|
|
- if (g_af.diff_counter >= 30) {
|
|
|
|
|
- g_af.dir = !g_af.dir;
|
|
|
|
|
- g_af.step_size += 10;
|
|
|
|
|
- g_af.diff_counter = 0;
|
|
|
|
|
- LOG_I("[对焦] 平坦超时,增大步长=%d", g_af.step_size);
|
|
|
|
|
|
|
+ if (current_sharpness >= g_af.fine_last * AF_FINE_TREND_RATIO) {
|
|
|
|
|
+ /* 上升/持平: 继续同方向 */
|
|
|
|
|
+ } else {
|
|
|
|
|
+ /* 显著下降: 调转方向 */
|
|
|
|
|
+ g_af.fine_dir = !g_af.fine_dir;
|
|
|
|
|
+ dir_sign = -dir_sign;
|
|
|
|
|
+ /* 当前值已跌破峰顶(fine_best*AF_FINE_CONFIRM_RATIO), 确认处于峰后下降段,
|
|
|
|
|
+ * 计一次越峰确认; 峰的前后各确认一次即完成 */
|
|
|
|
|
+ if (current_sharpness < g_af.fine_best_sharpness * AF_FINE_CONFIRM_RATIO) {
|
|
|
|
|
+ g_af.fine_confirm++;
|
|
|
|
|
+ LOG_I("[对焦] 细扫峰后下降确认[%d/%d] 清晰度=%.2f 峰顶=%.2f",
|
|
|
|
|
+ g_af.fine_confirm, AF_FINE_CONFIRM_CNT,
|
|
|
|
|
+ current_sharpness, g_af.fine_best_sharpness);
|
|
|
|
|
+ if (g_af.fine_confirm >= AF_FINE_CONFIRM_CNT) {
|
|
|
|
|
+ /* 细扫最佳须达到全局最佳值*AF_FINE_VALID_RATIO才允许结束,
|
|
|
|
|
+ * 验证粗扫峰不是漂移虚高; 未达到则以细扫实测最佳重新初始化
|
|
|
|
|
+ * 全局基准, 清零确认计数继续扫描验证 */
|
|
|
|
|
+ if (g_af.fine_best_sharpness >= g_af.best_sharpness * AF_FINE_VALID_RATIO) {
|
|
|
|
|
+ LOG_I("[对焦] 细扫峰值确认完成");
|
|
|
|
|
+ auto_focus_done();
|
|
|
|
|
+ return;
|
|
|
|
|
+ }
|
|
|
|
|
+ LOG_I("[对焦] 细扫最佳%.2f低于全局最佳%.2f的0.9倍, 以细扫实测重新初始化基准",
|
|
|
|
|
+ g_af.fine_best_sharpness, g_af.best_sharpness);
|
|
|
|
|
+ g_af.best_sharpness = g_af.fine_best_sharpness;
|
|
|
|
|
+ g_af.best_pos = g_af.fine_best_pos;
|
|
|
|
|
+ g_af.fine_confirm = 0;
|
|
|
|
|
+ }
|
|
|
|
|
+ }
|
|
|
|
|
+ LOG_I("[对焦] 细扫下降 %.2f->%.2f, 调转方向:%d",
|
|
|
|
|
+ g_af.fine_last, current_sharpness, g_af.fine_dir);
|
|
|
}
|
|
}
|
|
|
|
|
+ g_af.fine_last = current_sharpness;
|
|
|
|
|
+
|
|
|
|
|
+ /* 计算下一步位置, 窗口边界强制调头 */
|
|
|
|
|
+ int next = g_af.current_pos + dir_sign * AF_FINE_STEP;
|
|
|
|
|
+ if (next > g_af.fine_center + AF_FINE_WINDOW ||
|
|
|
|
|
+ next < g_af.fine_center - AF_FINE_WINDOW) {
|
|
|
|
|
+ g_af.fine_dir = !g_af.fine_dir;
|
|
|
|
|
+ dir_sign = -dir_sign;
|
|
|
|
|
+ next = g_af.current_pos + dir_sign * AF_FINE_STEP;
|
|
|
|
|
+ LOG_I("[对焦] 细扫到达窗口边界, 调转方向:%d", g_af.fine_dir);
|
|
|
|
|
+ }
|
|
|
|
|
+ auto_focus_move_to(next);
|
|
|
|
|
+}
|
|
|
|
|
|
|
|
- LOG_I("[对焦] 最佳:位置%d 清晰度%.2f", g_af.best_pos, g_af.best_sharpness);
|
|
|
|
|
|
|
+/* 处理一次清晰度值,执行对焦算法 */
|
|
|
|
|
+static void auto_focus_process(void)
|
|
|
|
|
+{
|
|
|
|
|
+ float current_sharpness = g_af.new_sharpness;
|
|
|
|
|
+ g_af.sharpness_updated = false;
|
|
|
|
|
|
|
|
- /* 终止条件 */
|
|
|
|
|
- bool done = false;
|
|
|
|
|
- if (g_af.peak_counter >= 2 && current_sharpness > g_af.best_sharpness * 0.90f) {
|
|
|
|
|
- done = true;
|
|
|
|
|
- LOG_I("[对焦] 峰值确认完成");
|
|
|
|
|
- g_af.max_iterations = 100;
|
|
|
|
|
- g_af.reduce_counter = 0;
|
|
|
|
|
- g_af.diff_counter = 0;
|
|
|
|
|
- }
|
|
|
|
|
- if (g_af.max_iterations <= 0) {
|
|
|
|
|
- done = true;
|
|
|
|
|
- g_af.step_size = 20;
|
|
|
|
|
- g_af.dir = !g_af.dir;
|
|
|
|
|
- g_af.current_pos = 0;
|
|
|
|
|
- LOG_I("[对焦] 迭代次数耗尽");
|
|
|
|
|
|
|
+ /* 对焦已结束: 丢弃迟到帧(如回位移动触发的相机回复), 防止重复处理 */
|
|
|
|
|
+ if (!g_af.active) {
|
|
|
|
|
+ return;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
- if (done) {
|
|
|
|
|
|
|
+ g_af.retry_cnt = 0; /* 收到 focus 回复,清零超时重试计数 */
|
|
|
|
|
+ g_af.wait_start_tick = rt_tick_get(); /* 重置等待计时 */
|
|
|
|
|
+
|
|
|
|
|
+ LOG_I("[对焦] 清晰度:%.2f 位置:%d 阶段:%d 剩余:%d",
|
|
|
|
|
+ current_sharpness, g_af.current_pos, g_af.stage, g_af.max_iterations);
|
|
|
|
|
+
|
|
|
|
|
+ /* 采样次数安全上限 */
|
|
|
|
|
+ g_af.max_iterations--;
|
|
|
|
|
+ if (g_af.max_iterations <= 0) {
|
|
|
|
|
+ LOG_W("[对焦] 采样次数耗尽, 结束对焦");
|
|
|
auto_focus_done();
|
|
auto_focus_done();
|
|
|
return;
|
|
return;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
- /* 移动电机 */
|
|
|
|
|
- auto_focus_move();
|
|
|
|
|
|
|
+ switch (g_af.stage) {
|
|
|
|
|
+ case AF_STAGE_BASE:
|
|
|
|
|
+ /* 起点基准帧: 首帧清晰度可能异常(残留上一帧), 仅记录不纳入最佳值 */
|
|
|
|
|
+ LOG_I("[对焦] 基准清晰度:%.2f", current_sharpness);
|
|
|
|
|
+ g_af.stage = AF_STAGE_PROBE_P;
|
|
|
|
|
+ auto_focus_move_to(AF_COARSE_STEP);
|
|
|
|
|
+ break;
|
|
|
|
|
+
|
|
|
|
|
+ case AF_STAGE_PROBE_P:
|
|
|
|
|
+ g_af.probe_plus = current_sharpness;
|
|
|
|
|
+ /* 探测点在移动后采样, 数值可靠, 计入全局最佳 */
|
|
|
|
|
+ if (current_sharpness > g_af.best_sharpness) {
|
|
|
|
|
+ g_af.best_sharpness = current_sharpness;
|
|
|
|
|
+ g_af.best_pos = g_af.current_pos;
|
|
|
|
|
+ }
|
|
|
|
|
+ g_af.stage = AF_STAGE_PROBE_M;
|
|
|
|
|
+ auto_focus_move_to(-AF_COARSE_STEP);
|
|
|
|
|
+ break;
|
|
|
|
|
+
|
|
|
|
|
+ case AF_STAGE_PROBE_M:
|
|
|
|
|
+ if (current_sharpness > g_af.best_sharpness) {
|
|
|
|
|
+ g_af.best_sharpness = current_sharpness;
|
|
|
|
|
+ g_af.best_pos = g_af.current_pos;
|
|
|
|
|
+ }
|
|
|
|
|
+ /* 比较 ±AF_COARSE_STEP 探测点, 选择清晰度更高的一侧开始粗扫 */
|
|
|
|
|
+ if (g_af.probe_plus >= current_sharpness) {
|
|
|
|
|
+ g_af.coarse_dir = 1;
|
|
|
|
|
+ g_af.coarse_limit = AF_SEARCH_LIMIT;
|
|
|
|
|
+ } else {
|
|
|
|
|
+ g_af.coarse_dir = 0;
|
|
|
|
|
+ g_af.coarse_limit = -AF_SEARCH_LIMIT;
|
|
|
|
|
+ }
|
|
|
|
|
+ g_af.coarse_both_done = false;
|
|
|
|
|
+ g_af.confirm_cnt = 0;
|
|
|
|
|
+ g_af.fall_cnt = 0;
|
|
|
|
|
+ g_af.coarse_floor = FLT_MAX; /* 本腿清晰度下限, 进入粗扫后统计 */
|
|
|
|
|
+ g_af.stage = AF_STAGE_COARSE;
|
|
|
|
|
+ LOG_I("[对焦] 探测:+%d=%.2f -%d=%.2f 粗扫方向=%d",
|
|
|
|
|
+ AF_COARSE_STEP, g_af.probe_plus, AF_COARSE_STEP, current_sharpness,
|
|
|
|
|
+ g_af.coarse_dir);
|
|
|
|
|
+ auto_focus_move_to(g_af.coarse_dir ?
|
|
|
|
|
+ g_af.current_pos + AF_COARSE_STEP :
|
|
|
|
|
+ g_af.current_pos - AF_COARSE_STEP);
|
|
|
|
|
+ break;
|
|
|
|
|
+
|
|
|
|
|
+ case AF_STAGE_COARSE:
|
|
|
|
|
+ auto_focus_coarse_step(current_sharpness);
|
|
|
|
|
+ break;
|
|
|
|
|
+
|
|
|
|
|
+ case AF_STAGE_FINE:
|
|
|
|
|
+ auto_focus_fine_step(current_sharpness);
|
|
|
|
|
+ break;
|
|
|
|
|
+ }
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
/* ========== Y轴校准(Calibration)控制块 ========== */
|
|
/* ========== Y轴校准(Calibration)控制块 ========== */
|
|
@@ -563,7 +747,6 @@ static void Ctr_Mortor_entry(void* parameter)
|
|
|
char *p1 = makeJson_Warn("movy", "4");
|
|
char *p1 = makeJson_Warn("movy", "4");
|
|
|
DTU_Write("%s", p1);
|
|
DTU_Write("%s", p1);
|
|
|
rt_free(p1);
|
|
rt_free(p1);
|
|
|
- DTU_Write("{\"cmd\": \"takephoto\" }");
|
|
|
|
|
} else {
|
|
} else {
|
|
|
LOG_E("[对焦] focus超时,重试%d次无回复,中止对焦", AF_FOCUS_RETRY_MAX);
|
|
LOG_E("[对焦] focus超时,重试%d次无回复,中止对焦", AF_FOCUS_RETRY_MAX);
|
|
|
g_af.active = false;
|
|
g_af.active = false;
|