|
|
@@ -26,6 +26,7 @@
|
|
|
#include "dtu_uart2.h"
|
|
|
#include <fal.h>
|
|
|
#include "selfcheck.h"
|
|
|
+ #include <float.h>
|
|
|
#define DBG_ENABLE
|
|
|
#define DBG_COLOR
|
|
|
#define DBG_TAG "hmi"
|
|
|
@@ -1342,65 +1343,133 @@ int findMaxIndex(double arr[], int size) {
|
|
|
|
|
|
return maxIndex;
|
|
|
}
|
|
|
-//入口函数-电机控制及
|
|
|
-static void Ctr_Mortor_entry(void* parameter)
|
|
|
-{
|
|
|
|
|
|
- rt_uint8_t maxIndex = 0;
|
|
|
|
|
|
- /*孢子仪2.0*/
|
|
|
- const int maxIterations = 20; // 最大迭代次数,可根据实际情况调整
|
|
|
- const int stepS = 2; // 对焦步长
|
|
|
- double allSharpness[maxIterations]; // 当前图像清晰度评价值
|
|
|
- char tmp_s[10];
|
|
|
- char tmp[10];
|
|
|
- while(1)
|
|
|
- {
|
|
|
- if(device_status.autotakepic==2){
|
|
|
- memset(allSharpness,0,sizeof(allSharpness));
|
|
|
- for(int i = 0; i < 3; i++){
|
|
|
- slide_para.ydir = 1; //1位反向,0位正向;
|
|
|
- slide_para.ydis = stepS;
|
|
|
- slide_para.en_slide = 6;
|
|
|
- while(slide_para.en_slide!=0)
|
|
|
- {
|
|
|
- rt_thread_delay(3000);
|
|
|
- }
|
|
|
- allSharpness[i] = device_status.focus_value;
|
|
|
+void send_signal(void)
|
|
|
+{
|
|
|
+ rt_thread_kill(ctr_motor, SIGUSR1);
|
|
|
+}
|
|
|
+
|
|
|
+// 全局状态变量(需在合适位置初始化)
|
|
|
+static float g_best_sharpness = -FLT_MAX; // 全局最佳清晰度
|
|
|
+static int g_best_pos = 0; // 全局最佳位置
|
|
|
+static float g_last_sharpness = -FLT_MAX; // 上一次移动后的清晰度
|
|
|
+static bool g_initial_move = true; // 初始方向判断标志
|
|
|
+int maxIterations = 100;
|
|
|
+uint8_t dir = 1;
|
|
|
+uint8_t stepSize = 4; // 初始步长(单位:步进电机脉冲数)
|
|
|
+int current_pos = 0; // 根据实际类型修改(int/float等)
|
|
|
+static int g_peak_counter = 0; // 连续上升后下降的计数器
|
|
|
+static bool g_last_rising = false; // 记录上次是否上升
|
|
|
+static int counter_sharpness_reduce = 0;
|
|
|
+#define MAX(a, b) ((a) > (b) ? (a) : (b)) // 手动定义宏
|
|
|
+
|
|
|
+void thread1_signal_handler(int sig)
|
|
|
+{
|
|
|
+ LOG_I("[对焦] 清晰度值更新:%.2f", device_status.focus_value);
|
|
|
+ slide_para.pulse_period = mapSpeed(1); // 更新电机速度参数
|
|
|
+ maxIterations--;
|
|
|
+
|
|
|
+ if (device_status.autotakepic == 2 && maxIterations >= 0) {
|
|
|
+ float current_sharpness = device_status.focus_value;
|
|
|
+
|
|
|
+ /* 更新全局最佳状态 */
|
|
|
+ if (current_sharpness > g_best_sharpness) {
|
|
|
+ g_best_sharpness = current_sharpness;
|
|
|
+ g_best_pos = current_pos;
|
|
|
+ LOG_I("[对焦] 发现更佳位置:%d (清晰度:%.2f)", g_best_pos, g_best_sharpness);
|
|
|
+ }
|
|
|
+
|
|
|
+ /* 初始方向判断(首步移动后) */
|
|
|
+ if (g_initial_move) {
|
|
|
+ if (current_sharpness > g_last_sharpness) {
|
|
|
+ LOG_I("[对焦] 初始方向正确,保持方向:%d", dir);
|
|
|
+ } else {
|
|
|
+ dir = !dir; // 反转方向
|
|
|
+ LOG_I("[对焦] 初始方向错误,反转方向:%d", dir);
|
|
|
}
|
|
|
- //归位
|
|
|
- for(int i = 3; i < 9; i++){
|
|
|
- slide_para.ydir = 0; //1位反向,0位正向;
|
|
|
- slide_para.ydis = stepS;
|
|
|
- slide_para.en_slide = 6;
|
|
|
- while(slide_para.en_slide!=0)
|
|
|
- {
|
|
|
- rt_thread_delay(3000);
|
|
|
+ g_initial_move = false;
|
|
|
+ } else {
|
|
|
+ if(current_sharpness< g_last_sharpness){ //持续3次清晰度降低,调整行进方向
|
|
|
+ counter_sharpness_reduce++;
|
|
|
+ LOG_I("[对焦] 清晰度下降,次数:%d", counter_sharpness_reduce);
|
|
|
+ if(counter_sharpness_reduce>=3){
|
|
|
+ counter_sharpness_reduce = 0;
|
|
|
+ dir = !dir; // 清晰度下降,反转方向
|
|
|
+ LOG_I("[对焦] 连续清晰度下降,调转行进方向!");
|
|
|
}
|
|
|
- if(i>=6) //过了初始位置再进行记录
|
|
|
- allSharpness[i-3] = device_status.focus_value;
|
|
|
+ }else {
|
|
|
+ counter_sharpness_reduce = 0;
|
|
|
}
|
|
|
- //检索最大清晰度位置
|
|
|
- int max_i = findMaxIndex(allSharpness,6);
|
|
|
-
|
|
|
- for(int i = 0; i < 6-max_i; i++){
|
|
|
- slide_para.ydir = 1; //1位反向,0位正向;
|
|
|
- slide_para.ydis = stepS;
|
|
|
- slide_para.en_slide = 6;
|
|
|
- while(slide_para.en_slide!=0)
|
|
|
- {
|
|
|
- rt_thread_delay(3000);
|
|
|
+
|
|
|
+ /* 方向策略调整 */
|
|
|
+ if (current_sharpness > (g_last_sharpness - 3.5f)) { // 噪声容忍阈值3.0
|
|
|
+ // 保持方向,当接近峰值时自动减小步长
|
|
|
+ LOG_I("保持方向,当接近峰值时自动减小步长...");
|
|
|
+ g_last_rising = true;
|
|
|
+ } else {
|
|
|
+ if(g_last_rising){
|
|
|
+ g_peak_counter++;
|
|
|
+ LOG_I("[对焦] 检测到清晰度波动[%d/3]", g_peak_counter);
|
|
|
+ }else {
|
|
|
+ g_peak_counter = 0; // 连续下降则重置
|
|
|
}
|
|
|
+ g_last_rising = false;
|
|
|
+ dir = !dir; // 清晰度下降,反转方向
|
|
|
+ LOG_I("[对焦] 清晰度下降,反转方向:%d 新步长:%d", dir, stepSize);
|
|
|
}
|
|
|
- char *photo_f = makeJson_Warn("auto_focus","focus_done");
|
|
|
- DTU_Write("%s",photo_f);
|
|
|
- rt_free(photo_f);
|
|
|
- device_status.autotakepic = 0;
|
|
|
}
|
|
|
- /*对焦拍照*/
|
|
|
+
|
|
|
+ /* 记录本次清晰度值 */
|
|
|
+ g_last_sharpness = current_sharpness;
|
|
|
+
|
|
|
+ /* 计算并执行下一步移动 */
|
|
|
+ current_pos += dir ? stepSize : -stepSize;
|
|
|
+ LOG_I("[对焦] 方向:%d 移动步长:%d 新位置:%d", dir, stepSize, current_pos);
|
|
|
+
|
|
|
+ slide_para.ydir = dir;
|
|
|
+ slide_para.ydis = stepSize;
|
|
|
+ slide_para.en_slide = 6; // 触发移动
|
|
|
+ while (slide_para.en_slide != 0) {
|
|
|
+ rt_thread_delay(500); // 等待移动完成
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ /* 终止条件检测 */
|
|
|
+ if (stepSize < 1 || maxIterations <= 0 || g_peak_counter >= 3) {
|
|
|
+ LOG_I("[对焦] 完成搜索,最佳位置:%d (清晰度:%.2f)", g_best_pos, g_best_sharpness);
|
|
|
+ if (g_peak_counter >= 3) {
|
|
|
+ LOG_I("[对焦] 连续三次波动确认峰值");
|
|
|
+ }
|
|
|
+
|
|
|
+ LOG_I("[对焦] 已到达最佳对焦位置");
|
|
|
+ if(maxIterations==0)
|
|
|
+ {
|
|
|
+ maxIterations = 120;
|
|
|
+ dir = !dir;
|
|
|
+ }else{
|
|
|
+ char *c0 = makeJson_Warn("auto_focus","auto_focus");
|
|
|
+ DTU_Write("%s",c0);
|
|
|
+ rt_free(c0);
|
|
|
+ device_status.autotakepic = 0; // 退出自动对焦模式
|
|
|
+ }
|
|
|
+ counter_sharpness_reduce = 0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+//入口函数-电机控制及
|
|
|
+static void Ctr_Mortor_entry(void* parameter)
|
|
|
+{
|
|
|
+ /*孢子仪2.0*/
|
|
|
+ rt_signal_install(SIGUSR1, thread1_signal_handler);
|
|
|
+ rt_signal_unmask(SIGUSR1);
|
|
|
+ while(1)
|
|
|
+ {
|
|
|
+ /*对焦拍照,滑台初始化*/
|
|
|
if(device_status.autotakepic==1)
|
|
|
{
|
|
|
- memset(allSharpness,0,sizeof(allSharpness));
|
|
|
device_status.step_focus = 1;
|
|
|
slide_para.en_slide =3; //X轴复位
|
|
|
slide_para.pulse_period = mapSpeed(50);
|
|
|
@@ -1434,91 +1503,46 @@ static void Ctr_Mortor_entry(void* parameter)
|
|
|
rt_thread_delay(1000);
|
|
|
}
|
|
|
}
|
|
|
+ device_status.autotakepic = 3;
|
|
|
+ }
|
|
|
+ if(device_status.autotakepic==3)
|
|
|
+ {
|
|
|
+ slide_para.en_slide = 7; //Y轴复位
|
|
|
+ slide_para.pulse_period = mapSpeed(50);
|
|
|
+ while(slide_para.en_slide!=0){
|
|
|
+ LOG_D("等待Y轴电机完成复位!");
|
|
|
+ rt_thread_delay(1000);
|
|
|
+ }
|
|
|
//Y轴电机运行到指定距离
|
|
|
slide_para.ydir = 1;
|
|
|
- slide_para.ydis = 3400;
|
|
|
+ slide_para.ydis = 3350;
|
|
|
slide_para.pulse_period = mapSpeed(50);
|
|
|
slide_para.en_slide = 6;
|
|
|
while(slide_para.en_slide!=0){
|
|
|
LOG_D("Again:等待y轴电机到位!");
|
|
|
rt_thread_delay(1000);
|
|
|
}
|
|
|
-#if 0
|
|
|
- slide_para.pulse_period = 20; //修改滑台的脉宽为20ms
|
|
|
- //遍历所有位置,粗对焦
|
|
|
- for(int i = 0; i < maxIterations; i++){
|
|
|
- slide_para.ydir = 0; //1位反向,0位正向;
|
|
|
- slide_para.ydis = stepS;
|
|
|
- slide_para.en_slide = 6;
|
|
|
- while(slide_para.en_slide!=0)
|
|
|
- {
|
|
|
- rt_thread_delay(2500);
|
|
|
- }
|
|
|
- allSharpness[i] = device_status.focus_value;
|
|
|
- if(device_status.focus_value > 10.0) break;
|
|
|
- }
|
|
|
- for(int i = 0; i < maxIterations; i++){
|
|
|
- sprintf(tmp_s, "%f",allSharpness[i]);
|
|
|
- rt_kprintf("-%s",tmp_s);
|
|
|
- }
|
|
|
- maxIndex = findMaxIndex(allSharpness,maxIterations); //最大清晰度的位置
|
|
|
-
|
|
|
-
|
|
|
- for(int i = 0; i < (maxIterations-maxIndex); i++){ //运动到该位置
|
|
|
- slide_para.ydir = 1; //1位反向,0位正向;
|
|
|
- slide_para.ydis = stepS;
|
|
|
- slide_para.en_slide = 6;
|
|
|
- while(slide_para.en_slide!=0)
|
|
|
- {
|
|
|
- rt_thread_delay(2500);
|
|
|
- }
|
|
|
- }
|
|
|
- slide_para.pulse_period = 1; //修改滑台的脉宽为20ms
|
|
|
-#endif
|
|
|
-
|
|
|
-// //细对焦
|
|
|
-// memset(allSharpness,0,sizeof(allSharpness));
|
|
|
-// for(int i = 0; i < 3; i++){
|
|
|
-// slide_para.ydir = 1; //1位反向,0位正向;
|
|
|
-// slide_para.ydis = stepS;
|
|
|
-// slide_para.en_slide = 6;
|
|
|
-// while(slide_para.en_slide!=0)
|
|
|
-// {
|
|
|
-// rt_thread_delay(3000);
|
|
|
-// }
|
|
|
-// allSharpness[i] = device_status.focus_value;
|
|
|
-// }
|
|
|
-// //归位
|
|
|
-// for(int i = 3; i < 9; i++){
|
|
|
-// slide_para.ydir = 0; //1位反向,0位正向;
|
|
|
-// slide_para.ydis = stepS;
|
|
|
-// slide_para.en_slide = 6;
|
|
|
-// while(slide_para.en_slide!=0)
|
|
|
-// {
|
|
|
-// rt_thread_delay(3000);
|
|
|
-// }
|
|
|
-// if(i>=6) //过了初始位置再进行记录
|
|
|
-// allSharpness[i-3] = device_status.focus_value;
|
|
|
-// }
|
|
|
-// //检索最大清晰度位置
|
|
|
-// int max_i= findMaxIndex(allSharpness,6);
|
|
|
-//
|
|
|
-// for(int i = 0; i < 6-max_i; i++){
|
|
|
-// slide_para.ydir = 1; //1位反向,0位正向;
|
|
|
-// slide_para.ydis = stepS;
|
|
|
-// slide_para.en_slide = 6;
|
|
|
-// while(slide_para.en_slide!=0)
|
|
|
-// {
|
|
|
-// rt_thread_delay(3000);
|
|
|
-// }
|
|
|
-// }
|
|
|
-
|
|
|
//生成JSON指令
|
|
|
- //sprintf(tmp, "%f",device_status.focus_value);
|
|
|
char *photo = makeJson_Warn("mov_init_y","takephoto_done");
|
|
|
DTU_Write("%s",photo);
|
|
|
rt_free(photo);
|
|
|
- device_status.autotakepic = 0;
|
|
|
+ device_status.autotakepic = 4;
|
|
|
+
|
|
|
+ }
|
|
|
+ if(device_status.autotakepic == 4)
|
|
|
+ {
|
|
|
+ device_status.autotakepic = 2;
|
|
|
+ g_best_sharpness = -FLT_MAX; // 全局最佳清晰度
|
|
|
+ g_best_pos = 0; // 全局最佳位置
|
|
|
+ g_last_sharpness = -FLT_MAX; // 上一次移动后的清晰度
|
|
|
+ g_initial_move = true; // 初始方向判断标志
|
|
|
+ maxIterations = 100;
|
|
|
+ dir = 1;
|
|
|
+ stepSize = 4; // 初始步长(单位:步进电机脉冲数)
|
|
|
+ current_pos = 0; // 根据实际类型修改(int/float等)
|
|
|
+
|
|
|
+ g_peak_counter = 0;
|
|
|
+ g_last_rising = false;
|
|
|
}
|
|
|
if(device_status.fsl_sw==1)
|
|
|
{
|