update distance estismate by laser, both distance value are uploaded

2026-01-12 18:06:04 +08:00
parent 708925ab41
commit 3c45fba0f5
5 changed files with 539 additions and 2052 deletions
--- a/app.yaml
+++ b/app.yaml
@@ -1,8 +1,20 @@
 id: t11
 name: t11
-version: 1.0.3
+version: 1.1.1
 author: t11
 icon: ''
 desc: t11
 files:
+  - app.yaml
+  - at_client.py
+  - config.py
+  - hardware.py
+  - laser_manager.py
+  - logger_manager.py
  - main.py
+  - network.py
+  - ota_manager.py
+  - power.py
+  - time_sync.py
+  - version.py
+  - vision.py
--- a/config.py
+++ b/config.py
@@ -50,6 +50,8 @@ ADC_LASER_THRESHOLD = 3000
 MODULE_ADDR = 0x00
 LASER_ON_CMD = bytes([0xAA, MODULE_ADDR, 0x01, 0xBE, 0x00, 0x01, 0x00, 0x01, 0xC1])
 LASER_OFF_CMD = bytes([0xAA, MODULE_ADDR, 0x01, 0xBE, 0x00, 0x01, 0x00, 0x00, 0xC0])
+DISTANCE_QUERY_CMD = bytes([0xAA, MODULE_ADDR, 0x00, 0x20, 0x00, 0x01, 0x00, 0x00, 0x21])  # 激光测距查询命令
+DISTANCE_RESPONSE_LEN = 13  # 激光测距响应数据长度（字节）
 DEFAULT_LASER_POINT = (640, 480)  # 默认激光中心点

 # ==================== 视觉检测配置 ====================
--- a/laser_manager.py
+++ b/laser_manager.py
@@ -6,6 +6,7 @@
 """
 import json
 import os
+import binascii
 from maix import time, camera
 import threading
 import config
@@ -31,7 +32,7 @@ class LaserManager:
        self._calibration_result = None
        self._calibration_lock = threading.Lock()
        self._laser_point = None
-        
+        self._laser_turned_on = False
        self._initialized = True
    
    # ==================== 状态访问（只读属性）====================
@@ -107,20 +108,23 @@ class LaserManager:
        if logger:
            logger.info(f"[LASER] 写入字节数: {written}")
        
-        time.sleep_ms(50)  # 增加等待时间，让模块有时间响应
-        
+        return None
+
+        # TODO: 暂时去掉这个等待
        # 读取回包
-        resp = hardware_manager.distance_serial.read(20)
-        if resp:
-            if logger:
-                logger.info(f"[LASER] 收到回包 ({len(resp)}字节): {resp.hex()}")
-            if resp == config.LASER_ON_CMD:
-                if logger:
-                    logger.info("✅ 激光开启指令已确认")
-        else:
-            if logger:
-                logger.warning("🔇 无回包（可能正常或模块不支持回包）")
-        return resp
+        # print("before read:", time.ticks_ms())
+        # resp = hardware_manager.distance_serial.read(len=20,timeout=10)
+        # print("after read:", time.ticks_ms())
+        # if resp:
+        #     if logger:
+        #         logger.info(f"[LASER] 收到回包 ({len(resp)}字节): {resp.hex()}")
+        #     if resp == config.LASER_ON_CMD:
+        #         if logger:
+        #             logger.info("✅ 激光开启指令已确认")
+        # else:
+        #     if logger:
+        #         logger.warning("🔇 无回包（可能正常或模块不支持回包）")
+        # return resp
    
    def turn_off_laser(self):
        """发送指令关闭激光"""
@@ -147,17 +151,18 @@ class LaserManager:
        if logger:
            logger.info(f"[LASER] 写入字节数: {written}")
        
-        time.sleep_ms(50)  # 增加等待时间
        
        # 读取回包
-        resp = hardware_manager.distance_serial.read(20)
-        if resp:
-            if logger:
-                logger.info(f"[LASER] 收到回包 ({len(resp)}字节): {resp.hex()}")
-        else:
-            if logger:
-                logger.warning("🔇 无回包")
-        return resp
+        # resp = hardware_manager.distance_serial.read(20)
+        # if resp:
+        #     if logger:
+        #         logger.info(f"[LASER] 收到回包 ({len(resp)}字节): {resp.hex()}")
+        # else:
+        #     if logger:
+        #         logger.warning("🔇 无回包")
+        # return resp
+        # 不用读回包
+        return None 
    
    def flash_laser(self, duration_ms=1000):
        """闪一下激光（用于射箭反馈）"""
@@ -223,6 +228,134 @@ class LaserManager:
            result = self._calibration_result
            self._calibration_result = None
            return result
+    
+    def parse_bcd_distance(self, bcd_bytes: bytes) -> float:
+        """将 4 字节 BCD 码转换为距离（米）"""
+        if len(bcd_bytes) != 4:
+            return 0.0
+        try:
+            hex_string = binascii.hexlify(bcd_bytes).decode()
+            distance_int = int(hex_string)
+            return distance_int / 1000.0
+        except Exception as e:
+            logger = logger_manager.logger
+            if logger:
+                logger.error(f"[LASER] BCD 解析失败: {e}")
+            return 0.0
+    
+    def read_distance_from_laser_sensor(self) -> float:
+        """发送测距指令并返回距离（米）"""
+        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
+        
+        try:
+            # 清空缓冲区
+            try:
+                hardware_manager.distance_serial.read(-1)
+            except:
+                pass
+            # 打开激光
+
+            self.turn_on_laser()
+            self._laser_turned_on = True
+            time.sleep_ms(40) # 需要一定时间让激光稳定
+            # 发送测距查询命令
+            hardware_manager.distance_serial.write(config.DISTANCE_QUERY_CMD)
+            # time.sleep_ms(500) # 测试结果：这里的等待没有用！
+            self.turn_off_laser()
+            self._laser_turned_on = False
+
+            # 这里的等待才是有效的！大概350ms能读到数据
+            # 循环读取响应，最多等待500ms
+            start_time = time.ticks_ms()
+            max_wait_ms = 500
+            response = None
+            
+            while True:
+                # 检查是否超时
+                elapsed_ms = time.ticks_diff(start_time,time.ticks_ms())
+                print("elapsed_ms:", elapsed_ms)
+                if elapsed_ms >= max_wait_ms:
+                    if logger:
+                        logger.warning(f"[LASER] 读取超时 ({elapsed_ms}ms)，未收到完整响应")
+                    return 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())
+                    if logger:
+                        logger.debug(f"[LASER] 收到响应 ({elapsed_ms}ms)")
+                    break
+                
+                # 如果还没超时，短暂等待后继续尝试
+                time.sleep_ms(10)  # 每次循环等待10ms，避免CPU占用过高
+            
+            # 验证响应格式
+            if response and len(response) == config.DISTANCE_RESPONSE_LEN:
+                if response[3] != 0x20:
+                    if response[0] == 0xEE:
+                        err_code = (response[7] << 8) | response[8]
+                        if logger:
+                            logger.warning(f"[LASER] 模块错误代码: {hex(err_code)}")
+                    return 0.0
+                
+                # 解析BCD码距离
+                bcd_bytes = response[6:10]
+                distance_value_m = self.parse_bcd_distance(bcd_bytes)
+                signal_quality = (response[10] << 8) | response[11]
+                
+                if logger:
+                    logger.debug(f"[LASER] 测距成功: {distance_value_m:.3f} m, 信号质量: {signal_quality}")
+                return distance_value_m
+            
+            if logger:
+                logger.warning(f"[LASER] 无效响应: {response.hex() if response else 'None'}")
+            return 0.0
+        except Exception as e:
+            logger = logger_manager.logger
+            if logger:
+                logger.error(f"[LASER] 读取激光测距失败: {e}")
+            return 0.0
+        
+    def quick_measure_distance(self) -> float:
+        """
+        快速激光测距：打开激光 → 测距 → 关闭激光
+        激光开启时间最小化（约500-600ms），尽量不让用户觉察到
+        返回距离（米），失败返回0.0
+        """
+        logger = logger_manager.logger
+        self._laser_turned_on = False
+        
+        try:
+
+            
+            # 等待激光稳定（最小延迟）
+            # time.sleep_ms(50)
+            
+            # 读取距离
+            distance_m = self.read_distance_from_laser_sensor()
+            
+            return distance_m
+        except Exception as e:
+            if logger:
+                logger.error(f"[LASER] 快速测距异常: {e}")
+            return 0.0
+        finally:
+            # 确保激光关闭
+            if self._laser_turned_on:
+                try:
+                    self.turn_off_laser()
+                except Exception as e:
+                    if logger:
+                        logger.error(f"[LASER] 关闭激光失败: {e}")


 # 创建全局单例实例
--- a/main.py
+++ b/main.py
--- a/vision.py
+++ b/vision.py
@@ -9,7 +9,6 @@ import numpy as np
 import os
 import math
 from maix import image
-import globals
 import config
 from logger_manager import logger_manager