246 lines
7.6 KiB
Python
246 lines
7.6 KiB
Python
#!/usr/bin/env python3
|
|
# -*- coding: utf-8 -*-
|
|
"""
|
|
M01激光测距模块测试脚本 - 修正版
|
|
基于文档中的完整命令示例
|
|
"""
|
|
|
|
from maix import uart, pinmap, time
|
|
import binascii
|
|
|
|
# ==================== 配置 ====================
|
|
UART_PORT = "/dev/ttyS1"
|
|
BAUDRATE = 9600
|
|
|
|
# 初始化串口
|
|
try:
|
|
pinmap.set_pin_function("A18", "UART1_RX")
|
|
pinmap.set_pin_function("A19", "UART1_TX")
|
|
laser_uart = uart.UART(UART_PORT, BAUDRATE)
|
|
print("✅ 硬件初始化完成")
|
|
except Exception as e:
|
|
print(f"❌ 初始化失败: {e}")
|
|
exit(1)
|
|
|
|
# ==================== 根据文档的完整命令集 ====================
|
|
# 1. 激光开关（文档2.3.10，已验证可用）
|
|
LASER_ON_CMD = bytes([0xAA, 0x00, 0x01, 0xBE, 0x00, 0x01, 0x00, 0x01, 0xC1])
|
|
LASER_OFF_CMD = bytes([0xAA, 0x00, 0x01, 0xBE, 0x00, 0x01, 0x00, 0x00, 0xC0])
|
|
|
|
# 2. 尝试不同的测距命令格式
|
|
TEST_COMMANDS = [
|
|
# 格式1：文档2.3.12的单次测量（您测试失败的）
|
|
{
|
|
"name": "单次测量 (0x0020)",
|
|
"cmd": bytes([0xAA, 0x00, 0x00, 0x20, 0x00, 0x01, 0x00, 0x00, 0x21]),
|
|
"desc": "文档2.3.12 示例命令"
|
|
},
|
|
# 格式2：文档2.3.7的读取测量结果
|
|
{
|
|
"name": "读取测量结果 (0x0022)",
|
|
"cmd": bytes([0xAA, 0x80, 0x00, 0x22, 0xA2]),
|
|
"desc": "文档2.3.7 读取测量结果"
|
|
},
|
|
# 格式3：文档2.3.13的快速测量
|
|
{
|
|
"name": "快速测量 (0x0022带数据)",
|
|
"cmd": bytes([0xAA, 0x00, 0x00, 0x22, 0x00, 0x01, 0x00, 0x00, 0x23]),
|
|
"desc": "文档2.3.13 快速测量"
|
|
},
|
|
# 格式4：连续测量模式
|
|
{
|
|
"name": "连续测量模式 (0x0021)",
|
|
"cmd": bytes([0xAA, 0x00, 0x00, 0x21, 0x00, 0x01, 0x00, 0x00, 0x22]),
|
|
"desc": "文档2.3.14 连续测量"
|
|
}
|
|
]
|
|
|
|
def clear_buffer():
|
|
"""清空串口缓冲区"""
|
|
try:
|
|
data = laser_uart.read(-1)
|
|
if data:
|
|
print(f"清空: {len(data)}字节")
|
|
except:
|
|
pass
|
|
|
|
def send_and_wait(cmd, name, wait_time=2000):
|
|
"""发送命令并等待响应"""
|
|
print(f"\n📤 发送: {name}")
|
|
print(f" 命令: {cmd.hex()}")
|
|
|
|
clear_buffer()
|
|
|
|
try:
|
|
laser_uart.write(cmd)
|
|
print(f" 已发送 {len(cmd)} 字节")
|
|
except Exception as e:
|
|
print(f" ❌ 发送失败: {e}")
|
|
return None
|
|
|
|
# 等待响应
|
|
start_time = time.ticks_ms()
|
|
response = b""
|
|
|
|
while time.ticks_ms() - start_time < wait_time:
|
|
try:
|
|
chunk = laser_uart.read(1)
|
|
if chunk:
|
|
response += chunk
|
|
# 完整响应通常是9或13字节
|
|
if len(response) >= 9:
|
|
# 检查是否完整帧
|
|
if response[0] in [0xAA, 0xEE]:
|
|
if len(response) >= 13: # 测距完整响应
|
|
break
|
|
elif response[0] == 0xEE: # 错误响应
|
|
break
|
|
except:
|
|
break
|
|
|
|
time.sleep_ms(10)
|
|
|
|
if response:
|
|
print(f" 📥 响应: {response.hex()}")
|
|
print(f" 长度: {len(response)} 字节")
|
|
|
|
# 解析错误码
|
|
if response[0] == 0xEE and len(response) >= 9:
|
|
err_code = (response[7] << 8) | response[8]
|
|
error_mapping = {
|
|
0x0000: "无错误",
|
|
0x0001: "硬件错误",
|
|
0x0002: "无输出数据",
|
|
0x0003: "反射信号太弱",
|
|
0x0004: "反射信号太强",
|
|
0x0005: "温度太高(>40℃)",
|
|
0x0006: "温度太低(<-10℃)",
|
|
0x0007: "电源电压低(<2.5V)",
|
|
0x0008: "超出量程",
|
|
0x0009: "读通讯错误",
|
|
0x000A: "写通讯错误",
|
|
0x000B: "地址错误"
|
|
}
|
|
err_msg = error_mapping.get(err_code, f"未知错误: 0x{err_code:04X}")
|
|
print(f" ❌ 模块错误: {err_msg}")
|
|
else:
|
|
print(" ⚠️ 无响应")
|
|
|
|
return response
|
|
|
|
def parse_distance_data(response):
|
|
"""解析距离数据"""
|
|
if not response or len(response) < 13:
|
|
return None
|
|
|
|
if response[0] != 0xAA or response[3] not in [0x20, 0x21, 0x22]:
|
|
return None
|
|
|
|
# 解析4字节BCD码
|
|
bcd_bytes = response[6:10]
|
|
distance_int = 0
|
|
|
|
for byte in bcd_bytes:
|
|
high = (byte >> 4) & 0x0F
|
|
low = byte & 0x0F
|
|
|
|
if high > 9 or low > 9:
|
|
return None
|
|
|
|
distance_int = distance_int * 100 + high * 10 + low
|
|
|
|
distance_m = distance_int / 1000.0
|
|
|
|
# 信号质量
|
|
signal = 0
|
|
if len(response) >= 12:
|
|
signal = (response[10] << 8) | response[11]
|
|
|
|
return {
|
|
'meters': distance_m,
|
|
'millimeters': distance_m * 1000,
|
|
'signal': signal,
|
|
'raw': response.hex()
|
|
}
|
|
|
|
# ==================== 主测试 ====================
|
|
print("\n" + "="*50)
|
|
print("M01激光测距模块详细测试")
|
|
print("="*50)
|
|
|
|
try:
|
|
# 1. 测试基本连接
|
|
print("\n1. 测试模块连接...")
|
|
version_cmd = bytes([0xAA, 0x80, 0x00, 0x0A, 0x8A])
|
|
resp = send_and_wait(version_cmd, "读取硬件版本")
|
|
|
|
if resp and resp[0] == 0xAA and resp[3] == 0x0A:
|
|
print(f"✅ 模块正常，版本: {resp[6]:02X}{resp[7]:02X}")
|
|
else:
|
|
print("❌ 模块连接测试失败")
|
|
exit(1)
|
|
|
|
# 2. 开启激光
|
|
print("\n2. 开启激光...")
|
|
resp = send_and_wait(LASER_ON_CMD, "开启激光", 1000)
|
|
if resp and resp.hex() == "aa0001be00010001c1":
|
|
print("✅ 激光已开启")
|
|
|
|
print(" 等待激光稳定...")
|
|
time.sleep(2) # 重要等待时间
|
|
|
|
# 3. 尝试不同的测距命令
|
|
print("\n3. 测试不同测距命令...")
|
|
|
|
for i, test_cmd in enumerate(TEST_COMMANDS):
|
|
print(f"\n{'='*30}")
|
|
print(f"测试 {i+1}: {test_cmd['name']}")
|
|
print(f"{test_cmd['desc']}")
|
|
print(f"{'='*30}")
|
|
|
|
resp = send_and_wait(test_cmd['cmd'], test_cmd['name'], 3000)
|
|
|
|
if resp:
|
|
if resp[0] == 0xAA and len(resp) >= 13:
|
|
result = parse_distance_data(resp)
|
|
if result:
|
|
print(f"✅ 测距成功!")
|
|
print(f" 距离: {result['meters']:.3f} m")
|
|
print(f" 距离: {result['millimeters']:.1f} mm")
|
|
print(f" 信号质量: {result['signal']}")
|
|
break
|
|
else:
|
|
print("❌ 无法解析距离数据")
|
|
elif resp[0] == 0xEE:
|
|
print("❌ 命令执行错误")
|
|
else:
|
|
print("❌ 无效响应格式")
|
|
else:
|
|
print("❌ 无响应")
|
|
|
|
time.sleep(1) # 命令间间隔
|
|
|
|
# 4. 关闭激光
|
|
print("\n4. 关闭激光...")
|
|
send_and_wait(LASER_OFF_CMD, "关闭激光", 1000)
|
|
|
|
print("\n" + "="*50)
|
|
print("🏁 测试完成")
|
|
print("="*50)
|
|
|
|
print("\n📋 测试总结：")
|
|
print("1. 模块通信: ✅ 正常")
|
|
print("2. 激光控制: ✅ 正常")
|
|
print("3. 测距功能: ❌ 有问题")
|
|
print("\n建议：")
|
|
print("1. 检查激光是否实际发光（在暗处观察红点）")
|
|
print("2. 确保测量目标在有效范围内（0.2-60米）")
|
|
print("3. 确保目标有足够反射率（白色平面最佳）")
|
|
print("4. 如果所有测距命令都返回ERR_ADDR，可能是固件版本问题")
|
|
|
|
except KeyboardInterrupt:
|
|
print("\n\n🛑 用户中断")
|
|
laser_uart.write(LASER_OFF_CMD)
|
|
print("✅ 已发送关闭指令")
|
|
except Exception as e:
|
|
print(f"\n❌ 测试出错: {e}") |