update laser cabration

laser_manager.py

@@ -175,23 +175,189 @@ class LaserManager:
             if logger:
                 logger.error(f"闪激光失败: {e}")
     
-    def find_red_laser(self, frame, threshold=150):
+    # def find_red_laser(self, frame, threshold=150):
-        """在图像中查找最亮的红色激光点（基于 RGB 阈值）"""
+    #     """在图像中查找最亮的红色激光点（基于 RGB 阈值）"""
+    #     w, h = frame.width(), frame.height()
+    #     img_bytes = frame.to_bytes()
+    #     max_sum = 0
+    #     best_pos = None
+    #     for y in range(0, h, 2):
+    #         for x in range(0, w, 2):
+    #             idx = (y * w + x) * 3
+    #             r, g, b = img_bytes[idx], img_bytes[idx+1], img_bytes[idx+2]
+    #             if r > threshold and r > g * 2 and r > b * 2:
+    #                 rgb_sum = r + g + b
+    #                 if rgb_sum > max_sum:
+    #                     max_sum = rgb_sum
+    #                     best_pos = (x, y)
+    #     return best_pos
+
+    # def find_red_laser(self, frame, threshold=150, search_radius=50):
+    #     """
+    #     在图像中心附近查找最亮的红色激光点（基于 RGB 阈值）
+        
+    #     Args:
+    #         frame: 图像帧
+    #         threshold: 红色通道阈值（默认150）
+    #         search_radius: 搜索半径（像素），从图像中心开始搜索（默认150）
+        
+    #     Returns:
+    #         (x, y) 坐标，如果未找到则返回 None
+    #     """
+    #     w, h = frame.width(), frame.height()
+    #     center_x, center_y = w // 2, h // 2
+        
+    #     # 只在中心区域搜索
+    #     x_min = max(0, center_x - search_radius)
+    #     x_max = min(w, center_x + search_radius)
+    #     y_min = max(0, center_y - search_radius)
+    #     y_max = min(h, center_y + search_radius)
+        
+    #     img_bytes = frame.to_bytes()
+    #     max_score = 0
+    #     best_pos = None
+        
+    #     for y in range(y_min, y_max, 2):
+    #         for x in range(x_min, x_max, 2):
+    #             idx = (y * w + x) * 3
+    #             r, g, b = img_bytes[idx], img_bytes[idx+1], img_bytes[idx+2]
+                
+    #             # 判断是否为红色或过曝的红色（发白）
+    #             is_red = False
+    #             is_overexposed_red = False
+                
+    #             # 情况1：正常红色（r 明显大于 g 和 b）
+    #             if r > threshold and r > g * 2 and r > b * 2:
+    #                 is_red = True
+                
+    #             # 情况2：过曝的红色（发白，r, g, b 都接近255，但 r 仍然最大）
+    #             # 过曝时，r, g, b 都接近 255，但 r 应该仍然是最高的
+    #             elif r > 200 and g > 200 and b > 200:  # 接近白色
+    #                 if r >= g and r >= b and (r - g) > 10 and (r - b) > 10:  # r 仍然明显最大
+    #                     is_overexposed_red = True
+                
+    #             if is_red or is_overexposed_red:
+    #                 # 计算得分：RGB 总和 + 距离中心权重（越靠近中心得分越高）
+    #                 rgb_sum = r + g + b
+    #                 # 计算到中心的距离
+    #                 dx = x - center_x
+    #                 dy = y - center_y
+    #                 distance_from_center = (dx * dx + dy * dy) ** 0.5
+    #                 # 距离权重：距离越近，权重越高（最大权重为 1.0，距离为 0 时）
+    #                 # 当距离为 search_radius 时，权重为 0.5
+    #                 distance_weight = 1.0 - (distance_from_center / search_radius) * 0.5
+    #                 distance_weight = max(0.5, distance_weight)  # 最小权重 0.5
+                    
+    #                 # 综合得分：RGB 总和 * 距离权重
+    #                 score = rgb_sum * distance_weight
+                    
+    #                 if score > max_score:
+    #                     max_score = score
+    #                     best_pos = (x, y)
+    #     print("best_pos:", best_pos)
+    #     return best_pos
+    def find_red_laser(self, frame, threshold=150, search_radius=150):
+        """
+        在图像中心附近查找最亮的红色激光点（基于 RGB 阈值）
+        使用两阶段搜索：先粗搜索找到候选区域，再精细搜索找到最亮点
+        
+        Args:
+            frame: 图像帧
+            threshold: 红色通道阈值（默认150）
+            search_radius: 搜索半径（像素），从图像中心开始搜索（默认150）
+        
+        Returns:
+            (x, y) 坐标，如果未找到则返回 None
+        """
         w, h = frame.width(), frame.height()
+        center_x, center_y = w // 2, h // 2
+        
+        # 只在中心区域搜索
+        x_min = max(0, center_x - search_radius)
+        x_max = min(w, center_x + search_radius)
+        y_min = max(0, center_y - search_radius)
+        y_max = min(h, center_y + search_radius)
+        
         img_bytes = frame.to_bytes()
-        max_sum = 0
+        max_score = 0
-        best_pos = None
+        candidate_pos = None
-        for y in range(0, h, 2):
+        
-            for x in range(0, w, 2):
+        # 第一阶段：粗搜索（每2像素采样），找到候选点
+        for y in range(y_min, y_max, 2):
+            for x in range(x_min, x_max, 2):
                 idx = (y * w + x) * 3
                 r, g, b = img_bytes[idx], img_bytes[idx+1], img_bytes[idx+2]
+                
+                # 判断是否为红色或过曝的红色（发白）
+                is_red = False
+                is_overexposed_red = False
+                
+                # 情况1：正常红色（r 明显大于 g 和 b）
                 if r > threshold and r > g * 2 and r > b * 2:
+                    is_red = True
+                
+                # 情况2：过曝的红色（发白，r, g, b 都接近255，但 r 仍然最大）
+                elif r > 200 and g > 200 and b > 200:  # 接近白色
+                    if r >= g and r >= b and (r - g) > 10 and (r - b) > 10:  # r 仍然明显最大
+                        is_overexposed_red = True
+                
+                if is_red or is_overexposed_red:
+                    # 计算得分：RGB 总和 + 距离中心权重
                     rgb_sum = r + g + b
-                    if rgb_sum > max_sum:
+                    dx = x - center_x
-                        max_sum = rgb_sum
+                    dy = y - center_y
+                    distance_from_center = (dx * dx + dy * dy) ** 0.5
+                    distance_weight = 1.0 - (distance_from_center / search_radius) * 0.5
+                    distance_weight = max(0.5, distance_weight)
+                    
+                    score = rgb_sum * distance_weight
+                    
+                    if score > max_score:
+                        max_score = score
+                        candidate_pos = (x, y)
+        
+        # 如果没有找到候选点，直接返回
+        if candidate_pos is None:
+            return None
+        
+        # 第二阶段：在候选点周围进行精细搜索（1像素间隔）
+        # 在候选点周围 5x5 或 7x7 区域内找最亮的点
+        refine_radius = 3  # 精细搜索半径（像素）
+        cx, cy = candidate_pos
+        
+        x_min_fine = max(0, cx - refine_radius)
+        x_max_fine = min(w, cx + refine_radius + 1)
+        y_min_fine = max(0, cy - refine_radius)
+        y_max_fine = min(h, cy + refine_radius + 1)
+        
+        max_brightness = 0
+        best_pos = candidate_pos
+        
+        # 精细搜索：1像素间隔，只考虑亮度（RGB总和）
+        for y in range(y_min_fine, y_max_fine, 1):
+            for x in range(x_min_fine, x_max_fine, 1):
+                idx = (y * w + x) * 3
+                r, g, b = img_bytes[idx], img_bytes[idx+1], img_bytes[idx+2]
+                
+                # 判断是否为红色或过曝的红色
+                is_red = False
+                is_overexposed_red = False
+                
+                if r > threshold and r > g * 2 and r > b * 2:
+                    is_red = True
+                elif r > 200 and g > 200 and b > 200:
+                    if r >= g and r >= b and (r - g) > 10 and (r - b) > 10:
+                        is_overexposed_red = True
+                
+                if is_red or is_overexposed_red:
+                    rgb_sum = r + g + b
+                    # 精细搜索阶段只考虑亮度，不考虑距离权重
+                    if rgb_sum > max_brightness:
+                        max_brightness = rgb_sum
                         best_pos = (x, y)
+        
         return best_pos
-    
+
     def calibrate_laser_position(self):
         """执行一次激光校准：拍照 → 找红点 → 保存坐标"""
         time.sleep_ms(80)

main.py

@@ -23,17 +23,91 @@ from logger_manager import logger_manager
 from time_sync import sync_system_time_from_4g
 from power import init_ina226, get_bus_voltage, voltage_to_percent
 from laser_manager import laser_manager
-from vision import detect_circle_v3, estimate_distance, compute_laser_position, save_shot_image
+from vision import detect_circle_v3, estimate_distance, compute_laser_position, save_shot_image, save_calibration_image
 from network import network_manager
 from ota_manager import ota_manager
 from hardware import hardware_manager
 
 
+# def laser_calibration_worker():
+#     """后台线程：持续检测是否需要执行激光校准"""
+#     from maix import camera
+#     from laser_manager import laser_manager
+#     from ota_manager import ota_manager
+    
+#     logger = logger_manager.logger
+#     if logger:
+#         logger.info("[LASER] 激光校准线程启动")
+    
+#     while True:
+#         try:
+#             try:
+#                 if ota_manager.ota_in_progress:
+#                     time.sleep_ms(200)
+#                     continue
+#             except Exception as e:
+#                 logger = logger_manager.logger
+#                 if logger:
+#                     logger.error(f"[LASER] OTA检查异常: {e}")
+#                 time.sleep_ms(200)
+#                 continue
+
+#             if laser_manager.calibration_active:
+#                 cam = None
+#                 try:
+#                     cam = camera.Camera(640, 480)
+#                     start = time.ticks_ms()
+#                     timeout_ms = 8000
+#                     while laser_manager.calibration_active and time.ticks_diff(time.ticks_ms(), start) < timeout_ms:
+#                         frame = cam.read()
+#                         pos = laser_manager.find_red_laser(frame)
+#                         if pos:
+#                             laser_manager.set_calibration_result(pos)
+#                             laser_manager.stop_calibration()
+#                             laser_manager.save_laser_point(pos)
+#                             logger = logger_manager.logger
+#                             if logger:
+#                                 logger.info(f"✅ 后台校准成功: {pos}")
+#                             break
+#                         time.sleep_ms(60)
+#                 except Exception as e:
+#                     logger = logger_manager.logger
+#                     if logger:
+#                         logger.error(f"[LASER] calibration error: {e}")
+#                         import traceback
+#                         logger.error(traceback.format_exc())
+#                     time.sleep_ms(200)
+#                 finally:
+#                     try:
+#                         if cam is not None:
+#                             del cam
+#                     except Exception as e:
+#                         logger = logger_manager.logger
+#                         if logger:
+#                             logger.error(f"[LASER] 释放相机资源异常: {e}")
+
+#                 if laser_manager.calibration_active:
+#                     time.sleep_ms(300)
+#             else:
+#                 time.sleep_ms(50)
+#         except Exception as e:
+#             # 线程顶层异常捕获，防止线程静默退出
+#             logger = logger_manager.logger
+#             if logger:
+#                 logger.error(f"[LASER] 校准线程异常: {e}")
+#                 import traceback
+#                 logger.error(traceback.format_exc())
+#             else:
+#                 print(f"[LASER] 校准线程异常: {e}")
+#                 import traceback
+#                 traceback.print_exc()
+#             time.sleep_ms(1000)  # 等待1秒后继续
 def laser_calibration_worker():
     """后台线程：持续检测是否需要执行激光校准"""
     from maix import camera
     from laser_manager import laser_manager
     from ota_manager import ota_manager
+    from vision import save_calibration_image  # 添加导入
     
     logger = logger_manager.logger
     if logger:
@@ -62,6 +136,14 @@ def laser_calibration_worker():
                         frame = cam.read()
                         pos = laser_manager.find_red_laser(frame)
                         if pos:
+                            # 保存校准图像（带标注）
+                            try:
+                                save_calibration_image(frame, pos)
+                            except Exception as e:
+                                logger = logger_manager.logger
+                                if logger:
+                                    logger.error(f"[LASER] 保存校准图像失败: {e}")
+                            
                             laser_manager.set_calibration_result(pos)
                             laser_manager.stop_calibration()
                             laser_manager.save_laser_point(pos)
@@ -103,7 +185,6 @@ def laser_calibration_worker():
                 traceback.print_exc()
             time.sleep_ms(1000)  # 等待1秒后继续
 
-
 def cmd_str():
     """主程序入口"""
     # ==================== 第一阶段：硬件初始化 ====================

vision.py

@@ -12,6 +12,81 @@ from maix import image
 import config
 from logger_manager import logger_manager
 
+def save_calibration_image(frame, laser_pos, photo_dir=None):
+    """
+    保存激光校准图像（带标注）
+    在找到的激光点位置绘制圆圈，便于检查算法是否正确
+    
+    Args:
+        frame: 原始图像帧
+        laser_pos: 找到的激光点坐标 (x, y)
+        photo_dir: 照片存储目录，如果为None则使用 config.PHOTO_DIR
+    
+    Returns:
+        str: 保存的文件路径，如果保存失败则返回 None
+    """
+    # 检查是否启用图像保存
+    if not config.SAVE_IMAGE_ENABLED:
+        return None
+    
+    if photo_dir is None:
+        photo_dir = config.PHOTO_DIR
+    
+    try:
+        # 确保照片目录存在
+        try:
+            if photo_dir not in os.listdir("/root"):
+                os.mkdir(photo_dir)
+        except:
+            pass
+        
+        # 生成文件名
+        try:
+            all_images = [f for f in os.listdir(photo_dir) if f.endswith(('.bmp', '.jpg', '.jpeg'))]
+            img_count = len(all_images)
+        except:
+            img_count = 0
+        
+        x, y = laser_pos
+        filename = f"{photo_dir}/calibration_{int(x)}_{int(y)}_{img_count:04d}.bmp"
+        
+        logger = logger_manager.logger
+        if logger:
+            logger.info(f"保存校准图像: {filename}, 激光点: ({x}, {y})")
+        
+        # 转换图像为 OpenCV 格式以便绘制
+        img_cv = image.image2cv(frame, False, False)
+        
+        # 绘制激光点圆圈（用绿色圆圈标出找到的激光点）
+        cv2.circle(img_cv, (int(x), int(y)), 10, (0, 255, 0), 2)  # 外圈：绿色，半径10
+        cv2.circle(img_cv, (int(x), int(y)), 5, (0, 255, 0), 2)  # 中圈：绿色，半径5
+        cv2.circle(img_cv, (int(x), int(y)), 2, (0, 255, 0), -1)  # 中心点：绿色实心
+        
+        # 可选：绘制十字线帮助定位
+        cv2.line(img_cv, 
+                (int(x - 20), int(y)), 
+                (int(x + 20), int(y)), 
+                (0, 255, 0), 1)  # 水平线
+        cv2.line(img_cv, 
+                (int(x), int(y - 20)), 
+                (int(x), int(y + 20)), 
+                (0, 255, 0), 1)  # 垂直线
+        
+        # 转换回 MaixPy 图像格式并保存
+        result_img = image.cv2image(img_cv, False, False)
+        result_img.save(filename)
+        
+        if logger:
+            logger.debug(f"校准图像已保存: {filename}")
+        
+        return filename
+    except Exception as e:
+        logger = logger_manager.logger
+        if logger:
+            logger.error(f"保存校准图像失败: {e}")
+            import traceback
+            logger.error(traceback.format_exc())
+        return None
 
 def detect_circle_v3(frame, laser_point=None):
     """检测图像中的靶心（优先清晰轮廓，其次黄色区域）- 返回椭圆参数版本