fix the laser point x,y
This commit is contained in:
@@ -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:
|
||||
|
||||
Reference in New Issue
Block a user