From a0019b8b0e61708864698f0d83abce8ec93de2f4 Mon Sep 17 00:00:00 2001 From: huangzhenwei2 <10934114+huangzhenwei2@user.noreply.gitee.com> Date: Mon, 12 Jan 2026 20:53:23 +0800 Subject: [PATCH] fix the laser point x,y --- laser_manager.py | 37 +++++++++++++++++++------------------ main.py | 12 ++++++++++-- vision.py | 2 +- 3 files changed, 30 insertions(+), 21 deletions(-) diff --git a/laser_manager.py b/laser_manager.py index adde0ed..5b40b8f 100644 --- a/laser_manager.py +++ b/laser_manager.py @@ -243,15 +243,17 @@ class LaserManager: logger.error(f"[LASER] BCD 解析失败: {e}") return 0.0 - def read_distance_from_laser_sensor(self) -> float: - """发送测距指令并返回距离（米）""" + def read_distance_from_laser_sensor(self): + """发送测距指令并返回距离（米）和信号质量 + 返回: (distance_m, signal_quality) 元组，失败返回 (0.0, 0) + """ from hardware import hardware_manager logger = logger_manager.logger if hardware_manager.distance_serial is None: if logger: logger.error("[LASER] distance_serial 未初始化") - return 0.0 + return (0.0, 0) try: # 清空缓冲区 @@ -278,19 +280,19 @@ class LaserManager: while True: # 检查是否超时 - elapsed_ms = time.ticks_diff(start_time,time.ticks_ms()) - print("elapsed_ms:", elapsed_ms) + # 注意：使用 time.ticks_diff(start_time, time.ticks_ms()) 避免负数问题 + elapsed_ms = abs(time.ticks_diff(start_time, time.ticks_ms())) if elapsed_ms >= max_wait_ms: if logger: logger.warning(f"[LASER] 读取超时 ({elapsed_ms}ms)，未收到完整响应") - return 0.0 + return (0.0, 0) # 尝试读取数据 response = hardware_manager.distance_serial.read(config.DISTANCE_RESPONSE_LEN) # 如果读到完整数据，立即返回 if response and len(response) == config.DISTANCE_RESPONSE_LEN: - elapsed_ms = time.ticks_diff(start_time,time.ticks_ms()) + elapsed_ms = abs(time.ticks_diff(start_time, time.ticks_ms())) if logger: logger.debug(f"[LASER] 收到响应 ({elapsed_ms}ms)") break @@ -305,7 +307,7 @@ class LaserManager: err_code = (response[7] << 8) | response[8] if logger: logger.warning(f"[LASER] 模块错误代码: {hex(err_code)}") - return 0.0 + return (0.0, 0) # 解析BCD码距离 bcd_bytes = response[6:10] @@ -314,22 +316,22 @@ class LaserManager: if logger: logger.debug(f"[LASER] 测距成功: {distance_value_m:.3f} m, 信号质量: {signal_quality}") - return distance_value_m + return (distance_value_m, signal_quality) if logger: logger.warning(f"[LASER] 无效响应: {response.hex() if response else 'None'}") - return 0.0 + return (0.0, 0) except Exception as e: logger = logger_manager.logger if logger: logger.error(f"[LASER] 读取激光测距失败: {e}") - return 0.0 + return (0.0, 0) - def quick_measure_distance(self) -> float: + def quick_measure_distance(self): """ 快速激光测距：打开激光 → 测距 → 关闭激光 激光开启时间最小化（约500-600ms），尽量不让用户觉察到 - 返回距离（米），失败返回0.0 + 返回: (distance_m, signal_quality) 元组，失败返回 (0.0, 0) """ logger = logger_manager.logger self._laser_turned_on = False @@ -340,14 +342,13 @@ class LaserManager: # 等待激光稳定（最小延迟） # time.sleep_ms(50) - # 读取距离 - distance_m = self.read_distance_from_laser_sensor() - - return distance_m + # 读取距离和信号质量 + result = self.read_distance_from_laser_sensor() + return result except Exception as e: if logger: logger.error(f"[LASER] 快速测距异常: {e}") - return 0.0 + return (0.0, 0) finally: # 确保激光关闭 if self._laser_turned_on: diff --git a/main.py b/main.py index 5d3ad71..ade8f48 100644 --- a/main.py +++ b/main.py @@ -319,11 +319,18 @@ def cmd_str(): # 快速激光测距（激光一闪而过，约500-600ms） laser_distance_m = None + laser_signal_quality = 0 try: - laser_distance_m = laser_manager.quick_measure_distance() + result = laser_manager.quick_measure_distance() + if isinstance(result, tuple) and len(result) == 2: + laser_distance_m, laser_signal_quality = result + else: + # 向后兼容：如果返回的是单个值 + laser_distance_m = result if isinstance(result, (int, float)) else 0.0 + laser_signal_quality = 0 if logger: if laser_distance_m > 0: - logger.info(f"[MAIN] 激光测距成功: {laser_distance_m:.3f} m") + logger.info(f"[MAIN] 激光测距成功: {laser_distance_m:.3f} m, 信号质量: {laser_signal_quality}") else: logger.warning("[MAIN] 激光测距失败或返回0") except Exception as e: @@ -355,6 +362,7 @@ def cmd_str(): "r": 90.0, "d": round((distance_m or 0.0) * 100), # 视觉测距值（厘米） "d_laser": round((laser_distance_m or 0.0) * 100), # 激光测距值（厘米） + "d_laser_quality": laser_signal_quality, # 激光测距信号质量 "m": method if method else "no_target", "adc": adc_val } diff --git a/vision.py b/vision.py index 585de40..dc38585 100644 --- a/vision.py +++ b/vision.py @@ -209,7 +209,7 @@ def compute_laser_position(circle_center, laser_point, radius, method): if not all([circle_center, radius, method]): return None, None cx, cy = circle_center - lx, ly = laser_point + lx, ly = 320, 230 # 根据检测方法动态调整靶心物理半径（简化模型） circle_r = (radius / 4.0) * 20.0 if method == "模糊" else (68 / 16.0) * 20.0 dx = lx - cx