826 lines
29 KiB
Python
826 lines
29 KiB
Python
#!/usr/bin/env python3
|
||
# -*- coding: utf-8 -*-
|
||
"""
|
||
激光射击系统主程序（激光测距版）
|
||
功能：目标检测、激光校准、4G TCP 通信、OTA 升级、M01 激光测距、INA226 电量监测
|
||
平台：MaixPy (Sipeed MAIX)
|
||
作者：ZZH
|
||
最后更新：2025-11-21
|
||
"""
|
||
|
||
from maix import camera, display, image, app, time, key, uart, pinmap, i2c, network, err
|
||
import cv2
|
||
import numpy as np
|
||
import json
|
||
import struct
|
||
import re
|
||
from maix.peripheral import adc
|
||
import _thread
|
||
import os
|
||
import requests
|
||
import socket
|
||
import binascii
|
||
|
||
# ==============================
|
||
# 全局配置
|
||
# ==============================
|
||
# OTA 升级地址（建议后续改为动态下发）
|
||
url = "https://static.shelingxingqiu.com/shoot/202511031031/main.py"
|
||
local_filename = "/maixapp/apps/t11/main.py"
|
||
|
||
DEVICE_ID = None
|
||
PASSWORD = None
|
||
SERVER_IP = "www.shelingxingqiu.com"
|
||
SERVER_PORT = 50005
|
||
HEARTBEAT_INTERVAL = 2 # 心跳间隔（秒）
|
||
|
||
CONFIG_FILE = "/root/laser_config.json"
|
||
DEFAULT_POINT = (640, 480) # 图像中心点
|
||
laser_point = DEFAULT_POINT
|
||
|
||
# HTTP API（当前未使用，保留备用）
|
||
URL = "http://ws.shelingxingqiu.com"
|
||
API_PATH = "/home/shoot/device_fire/arrow/fire"
|
||
|
||
# UART 设备初始化
|
||
uart4g = uart.UART("/dev/ttyS2", 115200) # 4G 模块（TCP 透传）
|
||
distance_serial = uart.UART("/dev/ttyS1", 9600) # M01 激光测距模块
|
||
|
||
# 消息类型常量
|
||
MSG_TYPE_LOGIN_REQ = 1 # 登录请求
|
||
MSG_TYPE_STATUS = 2 # 状态上报
|
||
MSG_TYPE_HEARTBEAT = 4 # 心跳包
|
||
# 引脚功能映射
|
||
pinmap.set_pin_function("A18", "UART1_RX")
|
||
pinmap.set_pin_function("A19", "UART1_TX")
|
||
pinmap.set_pin_function("A29", "UART2_RX")
|
||
pinmap.set_pin_function("A28", "UART2_TX")
|
||
pinmap.set_pin_function("P18", "I2C1_SCL")
|
||
pinmap.set_pin_function("P21", "I2C1_SDA")
|
||
# pinmap.set_pin_function("A15", "I2C5_SCL")
|
||
# pinmap.set_pin_function("A27", "I2C5_SDA")#ota升级要修改的
|
||
# ADC 触发阈值（用于检测扳机/激光触发）
|
||
ADC_TRIGGER_THRESHOLD = 3000
|
||
ADC_LASER_THRESHOLD = 3000
|
||
# 显示参数
|
||
color = image.Color(255, 100, 0) # 橙色十字线
|
||
thickness = 1
|
||
length = 2
|
||
|
||
# ADC 扳机触发阈值（0~4095）
|
||
ADC_TRIGGER_THRESHOLD = 3000
|
||
|
||
# I2C 电源监测（INA226）
|
||
adc_obj = adc.ADC(0, adc.RES_BIT_12)
|
||
bus = i2c.I2C(1, i2c.Mode.MASTER)
|
||
# bus = i2c.I2C(5, i2c.Mode.MASTER)#ota升级总线
|
||
INA226_ADDR = 0x40
|
||
REG_CONFIGURATION = 0x00
|
||
REG_BUS_VOLTAGE = 0x02
|
||
REG_CALIBRATION = 0x05
|
||
CALIBRATION_VALUE = 0x1400
|
||
|
||
# M01 激光模块指令
|
||
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
|
||
|
||
# TCP / 线程状态
|
||
tcp_connected = False
|
||
send_queue = []
|
||
update_thread_started = False # 防止重复 OTA
|
||
send_queue_lock = _thread.allocate_lock()
|
||
laser_calibration_data_lock = _thread.allocate_lock()
|
||
laser_calibration_active = False
|
||
laser_calibration_result = None
|
||
|
||
|
||
# ==============================
|
||
# 网络工具函数
|
||
# ==============================
|
||
|
||
def is_server_reachable(host, port=80, timeout=5):
|
||
"""检查能否连接到指定主机和端口（用于 OTA 前网络检测）"""
|
||
try:
|
||
addr_info = socket.getaddrinfo(host, port)[0]
|
||
s = socket.socket(addr_info[0], addr_info[1], addr_info[2])
|
||
s.settimeout(timeout)
|
||
s.connect(addr_info[-1])
|
||
s.close()
|
||
return True
|
||
except Exception as e:
|
||
print(f"[NET] 无法连接 {host}:{port} - {e}")
|
||
return False
|
||
|
||
|
||
def download_file(url, filename):
|
||
"""
|
||
从指定 URL 下载文件并保存为 UTF-8 文本。
|
||
注意：此操作会覆盖本地 main.py！
|
||
"""
|
||
try:
|
||
print(f"[OTA] 正在从 {url} 下载文件...")
|
||
response = requests.get(url, timeout=10) # ⏱️ 防止卡死
|
||
response.raise_for_status()
|
||
response.encoding = 'utf-8'
|
||
with open(filename, 'w', encoding='utf-8') as file:
|
||
file.write(response.text)
|
||
return f"下载成功！文件已保存为: {filename}"
|
||
except requests.exceptions.RequestException as e:
|
||
return f"下载失败！网络请求错误: {e}"
|
||
except OSError as e:
|
||
return f"下载失败！文件写入错误: {e}"
|
||
except Exception as e:
|
||
return f"下载失败！发生未知错误: {e}"
|
||
|
||
|
||
def connect_wifi(ssid, password):
|
||
"""
|
||
连接 Wi-Fi 并持久化凭证到 /boot/ 目录，使设备重启后自动连接。
|
||
返回 (ip, error) 元组。
|
||
"""
|
||
conf_path = "/etc/wpa_supplicant.conf"
|
||
ssid_file = "/boot/wifi.ssid"
|
||
pass_file = "/boot/wifi.pass"
|
||
|
||
try:
|
||
# 生成 wpa_supplicant 配置
|
||
net_conf = os.popen(f'wpa_passphrase "{ssid}" "{password}"').read()
|
||
if "network={" not in net_conf:
|
||
return None, "Failed to generate wpa config"
|
||
|
||
# 写入运行时配置
|
||
with open(conf_path, "w") as f:
|
||
f.write("ctrl_interface=/var/run/wpa_supplicant\n")
|
||
f.write("update_config=1\n\n")
|
||
f.write(net_conf)
|
||
|
||
# 持久化保存（供开机脚本读取）
|
||
with open(ssid_file, "w") as f:
|
||
f.write(ssid.strip())
|
||
with open(pass_file, "w") as f:
|
||
f.write(password.strip())
|
||
|
||
# 重启 Wi-Fi 服务
|
||
os.system("/etc/init.d/S30wifi restart")
|
||
|
||
# 等待获取 IP（最多 20 秒）
|
||
for _ in range(20):
|
||
ip = os.popen("ifconfig wlan0 2>/dev/null | grep 'inet ' | awk '{print $2}'").read().strip()
|
||
if ip:
|
||
return ip, None
|
||
time.sleep(1)
|
||
|
||
return None, "Timeout: No IP obtained"
|
||
|
||
except Exception as e:
|
||
return None, f"Exception: {str(e)}"
|
||
|
||
def direct_ota_download():
|
||
"""
|
||
直接执行 OTA 下载（假设已有网络）
|
||
用于 cmd=7 触发
|
||
"""
|
||
global update_thread_started
|
||
try:
|
||
# 再次确认网络可达（可选但推荐）
|
||
from urllib.parse import urlparse
|
||
parsed_url = urlparse(url)
|
||
host = parsed_url.hostname
|
||
port = parsed_url.port or (443 if parsed_url.scheme == 'https' else 80)
|
||
|
||
if not is_server_reachable(host, port, timeout=8):
|
||
safe_enqueue({"result": "ota_failed", "reason": f"无法连接 {host}:{port}"}, MSG_TYPE_STATUS)
|
||
return
|
||
|
||
print(f"[OTA] 开始直接下载固件...")
|
||
result_msg = download_file(url, local_filename)
|
||
print(f"[OTA] {result_msg}")
|
||
safe_enqueue({"result": result_msg}, MSG_TYPE_STATUS)
|
||
|
||
except Exception as e:
|
||
error_msg = f"OTA 异常: {str(e)}"
|
||
print(error_msg)
|
||
safe_enqueue({"result": "ota_failed", "reason": error_msg}, MSG_TYPE_STATUS)
|
||
finally:
|
||
update_thread_started = False # 允许下次 OTA
|
||
|
||
|
||
def handle_wifi_and_update(ssid, password):
|
||
"""
|
||
OTA 更新线程入口。
|
||
注意：必须在 finally 中重置 update_thread_started！
|
||
"""
|
||
global update_thread_started
|
||
try:
|
||
ip, error = connect_wifi(ssid, password)
|
||
if error:
|
||
safe_enqueue({"result": "wifi_failed", "error": error}, MSG_TYPE_STATUS)
|
||
return
|
||
|
||
safe_enqueue({"result": "wifi_connected", "ip": ip}, MSG_TYPE_STATUS)
|
||
|
||
from urllib.parse import urlparse
|
||
parsed_url = urlparse(url)
|
||
host = parsed_url.hostname
|
||
port = parsed_url.port or (443 if parsed_url.scheme == 'https' else 80)
|
||
|
||
if not is_server_reachable(host, port, timeout=8):
|
||
err_msg = f"网络不通：无法连接 {host}:{port}"
|
||
safe_enqueue({"result": err_msg}, MSG_TYPE_STATUS)
|
||
return
|
||
|
||
print(f"[OTA] 已确认可访问 {host}:{port}，开始下载...")
|
||
try:
|
||
cs = download_file(url, local_filename)
|
||
except Exception as e:
|
||
cs = f"下载失败: {str(e)}"
|
||
print(cs)
|
||
safe_enqueue({"result": cs}, MSG_TYPE_STATUS)
|
||
|
||
finally:
|
||
# ✅ 关键修复：允许下次 OTA
|
||
update_thread_started = False
|
||
print("[UPDATE] OTA 线程执行完毕，标志已重置。")
|
||
|
||
|
||
# ==============================
|
||
# 工具函数
|
||
# ==============================
|
||
|
||
def read_device_id():
|
||
"""从 /device_key 读取设备唯一 ID"""
|
||
try:
|
||
with open("/device_key", "r") as f:
|
||
device_id = f.read().strip()
|
||
if device_id:
|
||
print(f"[INFO] 从 /device_key 读取到 DEVICE_ID: {device_id}")
|
||
return device_id
|
||
else:
|
||
raise ValueError("文件为空")
|
||
except Exception as e:
|
||
print(f"[ERROR] 无法读取 /device_key: {e}")
|
||
return "DEFAULT_DEVICE_ID"
|
||
|
||
|
||
def safe_enqueue(data_dict, msg_type=MSG_TYPE_STATUS):
|
||
"""线程安全地将消息加入发送队列"""
|
||
global send_queue, send_queue_lock
|
||
with send_queue_lock:
|
||
send_queue.append((msg_type, data_dict))
|
||
|
||
|
||
def at(cmd, wait="OK", timeout=2000):
|
||
"""向 4G 模块发送 AT 指令并等待响应"""
|
||
if cmd:
|
||
uart4g.write((cmd + "\r\n").encode())
|
||
t0 = time.ticks_ms()
|
||
buf = b""
|
||
while time.ticks_ms() - t0 < timeout:
|
||
data = uart4g.read()
|
||
if data:
|
||
buf += data
|
||
if wait.encode() in buf:
|
||
return buf.decode(errors="ignore")
|
||
return buf.decode(errors="ignore")
|
||
|
||
|
||
def make_packet(msg_type: int, body_dict: dict) -> bytes:
|
||
"""构造二进制数据包：[body_len][msg_type][checksum][body]"""
|
||
body = json.dumps(body_dict, ensure_ascii=False).encode('utf-8')
|
||
body_len = len(body)
|
||
checksum = body_len + msg_type
|
||
header = struct.pack(">III", body_len, msg_type, checksum)
|
||
return header + body
|
||
|
||
|
||
def parse_packet(data: bytes):
|
||
"""解析二进制数据包"""
|
||
if len(data) < 12:
|
||
return None, None
|
||
body_len, msg_type, checksum = struct.unpack(">III", data[:12])
|
||
body = data[12:12 + body_len]
|
||
try:
|
||
# ✅ 显式指定 UTF-8 编码
|
||
return msg_type, json.loads(body.decode('utf-8'))
|
||
except Exception as e:
|
||
print(f"[ERROR] 解析包体失败: {e}")
|
||
return msg_type, {"raw": body.decode('utf-8', errors='ignore')}
|
||
|
||
|
||
def tcp_send_raw(data: bytes, max_retries=2) -> bool:
|
||
"""通过 4G 模块发送原始 TCP 数据（仅在 tcp_main 线程调用）"""
|
||
global tcp_connected
|
||
if not tcp_connected:
|
||
return False
|
||
|
||
for attempt in range(max_retries):
|
||
cmd = f'AT+MIPSEND=0,{len(data)}'
|
||
if ">" not in at(cmd, ">", 1500):
|
||
time.sleep_ms(100)
|
||
continue
|
||
|
||
time.sleep_ms(10)
|
||
full = data + b"\x1A"
|
||
try:
|
||
sent = uart4g.write(full)
|
||
if sent != len(full):
|
||
time.sleep_ms(100)
|
||
continue
|
||
except:
|
||
time.sleep_ms(100)
|
||
continue
|
||
|
||
if "OK" in at("", "OK", 1000):
|
||
return True
|
||
time.sleep_ms(100)
|
||
|
||
return False
|
||
|
||
|
||
def load_laser_point():
|
||
"""从配置文件加载激光点坐标"""
|
||
global laser_point
|
||
try:
|
||
if "laser_config.json" in os.listdir("/root"):
|
||
with open(CONFIG_FILE, "r") as f:
|
||
data = json.load(f)
|
||
if isinstance(data, list) and len(data) == 2:
|
||
laser_point = (int(data[0]), int(data[1]))
|
||
print(f"[INFO] 加载激光点: {laser_point}")
|
||
else:
|
||
raise ValueError
|
||
else:
|
||
laser_point = DEFAULT_POINT
|
||
except:
|
||
laser_point = DEFAULT_POINT
|
||
|
||
|
||
def save_laser_point(point):
|
||
"""保存激光点坐标到文件"""
|
||
global laser_point
|
||
try:
|
||
with open(CONFIG_FILE, "w") as f:
|
||
json.dump([point[0], point[1]], f)
|
||
laser_point = point
|
||
except:
|
||
pass
|
||
|
||
|
||
def turn_on_laser():
|
||
"""发送激光开启指令"""
|
||
distance_serial.write(LASER_ON_CMD)
|
||
time.sleep_ms(10)
|
||
resp = distance_serial.read(20)
|
||
if resp:
|
||
if resp == LASER_ON_CMD:
|
||
print("✅ 激光指令已确认")
|
||
else:
|
||
print("🔇 无回包（正常或模块不支持）")
|
||
return resp
|
||
|
||
|
||
# ==============================
|
||
# M01 激光测距模块
|
||
# ==============================
|
||
|
||
def parse_bcd_distance(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:
|
||
print(f"[ERROR] BCD 解析失败: {e}")
|
||
return 0.0
|
||
|
||
|
||
def read_distance_from_laser_sensor():
|
||
"""发送测距指令并返回距离（米）"""
|
||
global distance_serial
|
||
try:
|
||
distance_serial.read() # 清空缓冲区
|
||
distance_serial.write(DISTANCE_QUERY_CMD)
|
||
time.sleep_ms(500)
|
||
response = distance_serial.read(DISTANCE_RESPONSE_LEN)
|
||
|
||
if response and len(response) == DISTANCE_RESPONSE_LEN:
|
||
if response[3] != 0x20:
|
||
if response[0] == 0xEE:
|
||
err_code = (response[7] << 8) | response[8]
|
||
print(f"[LASER] 模块错误代码: {hex(err_code)}")
|
||
return 0.0
|
||
|
||
bcd_bytes = response[6:10]
|
||
distance_value_m = parse_bcd_distance(bcd_bytes)
|
||
signal_quality = (response[10] << 8) | response[11]
|
||
print(f"[LASER] 测距成功: {distance_value_m:.3f} m, 信号质量: {signal_quality}")
|
||
return distance_value_m
|
||
|
||
print(f"[LASER] 无效响应: {response.hex() if response else 'None'}")
|
||
return 0.0
|
||
except Exception as e:
|
||
print(f"[ERROR] 读取激光测距失败: {e}")
|
||
return 0.0
|
||
|
||
|
||
# ==============================
|
||
# 激光点校准
|
||
# ==============================
|
||
|
||
def find_red_laser(frame, threshold=150):
|
||
"""在图像中查找最亮的红色点（简单 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 calibrate_laser_position():
|
||
"""拍摄一帧并识别激光点位置"""
|
||
time.sleep_ms(80)
|
||
cam = camera.Camera(640, 480)
|
||
frame = cam.read()
|
||
pos = find_red_laser(frame)
|
||
if pos:
|
||
save_laser_point(pos)
|
||
return pos
|
||
return None
|
||
|
||
|
||
# ==============================
|
||
# 电量监测（INA226）
|
||
# ==============================
|
||
|
||
def write_register(reg, value):
|
||
data = [(value >> 8) & 0xFF, value & 0xFF]
|
||
bus.writeto_mem(INA226_ADDR, reg, bytes(data))
|
||
|
||
|
||
def read_register(reg):
|
||
data = bus.readfrom_mem(INA226_ADDR, reg, 2)
|
||
return (data[0] << 8) | data[1]
|
||
|
||
|
||
def init_ina226():
|
||
write_register(REG_CONFIGURATION, 0x4527)
|
||
write_register(REG_CALIBRATION, CALIBRATION_VALUE)
|
||
|
||
|
||
def get_bus_voltage():
|
||
raw = read_register(REG_BUS_VOLTAGE)
|
||
return raw * 1.25 / 1000
|
||
|
||
|
||
def voltage_to_percent(voltage):
|
||
points = [
|
||
(4.20, 100), (4.10, 95), (4.05, 85), (4.00, 75), (3.95, 65),
|
||
(3.90, 55), (3.85, 45), (3.80, 35), (3.75, 25), (3.70, 15),
|
||
(3.65, 5), (3.60, 0)
|
||
]
|
||
if voltage >= points[0][0]: return 100
|
||
if voltage <= points[-1][0]: return 0
|
||
for i in range(len(points) - 1):
|
||
v1, p1 = points[i]; v2, p2 = points[i + 1]
|
||
if v2 <= voltage <= v1:
|
||
ratio = (voltage - v1) / (v2 - v1)
|
||
percent = p1 + (p2 - p1) * ratio
|
||
return max(0, min(100, int(round(percent))))
|
||
return 0
|
||
|
||
|
||
# ==============================
|
||
# 目标检测
|
||
# ==============================
|
||
|
||
def detect_circle(frame):
|
||
"""检测靶心圆（清晰/模糊两种模式）"""
|
||
img_cv = image.image2cv(frame, False, False)
|
||
gray = cv2.cvtColor(img_cv, cv2.COLOR_BGR2GRAY)
|
||
blurred = cv2.GaussianBlur(gray, (5, 5), 0)
|
||
edged = cv2.Canny(blurred, 50, 150)
|
||
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5))
|
||
ceroded = cv2.erode(cv2.dilate(edged, kernel), kernel)
|
||
|
||
contours, _ = cv2.findContours(ceroded, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
|
||
best_center = best_radius = method = None
|
||
|
||
for cnt in contours:
|
||
area = cv2.contourArea(cnt)
|
||
perimeter = cv2.arcLength(cnt, True)
|
||
if perimeter < 100 or area < 100: continue
|
||
circularity = 4 * np.pi * area / (perimeter ** 2)
|
||
if circularity > 0.75 and len(cnt) >= 5:
|
||
center, axes, angle = cv2.fitEllipse(cnt)
|
||
radius = (axes[0] + axes[1]) / 4
|
||
best_center = (int(center[0]), int(center[1]))
|
||
best_radius = int(radius)
|
||
method = "清晰"
|
||
break
|
||
|
||
if not best_center:
|
||
hsv = cv2.cvtColor(img_cv, cv2.COLOR_BGR2HSV)
|
||
h, s, v = cv2.split(hsv)
|
||
s = np.clip(s * 2, 0, 255).astype(np.uint8)
|
||
hsv = cv2.merge((h, s, v))
|
||
lower_yellow = np.array([7, 80, 0])
|
||
upper_yellow = np.array([32, 255, 182])
|
||
mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
|
||
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5))
|
||
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
|
||
mask = cv2.morphologyEx(mask, cv2.MORPH_DILATE, kernel)
|
||
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||
if contours:
|
||
largest = max(contours, key=cv2.contourArea)
|
||
if cv2.contourArea(largest) > 50:
|
||
(x, y), radius = cv2.minEnclosingCircle(largest)
|
||
best_center = (int(x), int(y))
|
||
best_radius = int(radius)
|
||
method = "模糊"
|
||
|
||
result_img = image.cv2image(img_cv, False, False)
|
||
return result_img, best_center, best_radius, method, best_radius
|
||
|
||
|
||
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
|
||
# 根据检测模式估算实际半径（单位：像素 → 厘米）
|
||
circle_r_cm = (radius / 4.0) * 20.0 if method == "模糊" else (68 / 16.0) * 20.0
|
||
dx = lx - cx
|
||
dy = ly - cy
|
||
scale = circle_r_cm / radius if radius != 0 else 1.0
|
||
return dx * scale, -dy * scale
|
||
|
||
|
||
# ==============================
|
||
# TCP 通信主线程
|
||
# ==============================
|
||
|
||
def connect_server():
|
||
"""连接服务器（通过 4G 模块 AT 指令）"""
|
||
global tcp_connected
|
||
if tcp_connected:
|
||
return True
|
||
print("正在连接服务器...")
|
||
at("AT+MIPCLOSE=0", "OK", 1000)
|
||
res = at(f'AT+MIPOPEN=0,"TCP","{SERVER_IP}",{SERVER_PORT}', "+MIPOPEN", 8000)
|
||
if "+MIPOPEN: 0,0" in res:
|
||
tcp_connected = True
|
||
return True
|
||
return False
|
||
|
||
|
||
def tcp_main():
|
||
"""TCP 通信主循环（独立线程）"""
|
||
global tcp_connected, send_queue, laser_calibration_active, laser_calibration_result,update_thread_started
|
||
|
||
while not app.need_exit():
|
||
if not connect_server():
|
||
time.sleep_ms(5000)
|
||
continue
|
||
|
||
login_data = {"deviceId": DEVICE_ID, "password": PASSWORD}
|
||
if not tcp_send_raw(make_packet(MSG_TYPE_LOGIN_REQ, login_data)):
|
||
tcp_connected = False
|
||
time.sleep_ms(2000)
|
||
continue
|
||
|
||
print("➡️ 登录包已发送，等待确认...")
|
||
logged_in = False
|
||
last_heartbeat_ack_time = time.ticks_ms()
|
||
last_heartbeat_send_time = time.ticks_ms()
|
||
rx_buf = b""
|
||
|
||
while True:
|
||
data = uart4g.read()
|
||
if data:
|
||
rx_buf += data
|
||
while b'+MIPURC: "rtcp"' in rx_buf:
|
||
try:
|
||
match = re.search(b'\+MIPURC: "rtcp",0,(\d+),(.+)', rx_buf, re.DOTALL)
|
||
if match:
|
||
payload_len = int(match.group(1))
|
||
payload = match.group(2)[:payload_len]
|
||
msg_type, body = parse_packet(payload)
|
||
|
||
if not logged_in and msg_type == MSG_TYPE_LOGIN_REQ:
|
||
if body and body.get("cmd") == 1 and body.get("data") == "登录成功":
|
||
logged_in = True
|
||
last_heartbeat_ack_time = time.ticks_ms()
|
||
print("✅ 登录成功")
|
||
else:
|
||
break
|
||
|
||
elif logged_in and msg_type == MSG_TYPE_HEARTBEAT:
|
||
last_heartbeat_ack_time = time.ticks_ms()
|
||
print("✅ 收到心跳确认")
|
||
|
||
elif logged_in and isinstance(body, dict):
|
||
inner_data = body.get("data", {})
|
||
if isinstance(inner_data, dict) and "cmd" in inner_data:
|
||
inner_cmd = inner_data["cmd"]
|
||
if inner_cmd == 2:
|
||
turn_on_laser()
|
||
time.sleep_ms(100)
|
||
laser_calibration_active = True
|
||
safe_enqueue({"result": "calibrating"}, MSG_TYPE_STATUS)
|
||
elif inner_cmd == 3:
|
||
distance_serial.write(LASER_OFF_CMD)
|
||
laser_calibration_active = False
|
||
safe_enqueue({"result": "laser_off"}, MSG_TYPE_STATUS)
|
||
elif inner_cmd == 4:
|
||
voltage = get_bus_voltage()
|
||
battery_percent = voltage_to_percent(voltage)
|
||
battery_data = {"battery": battery_percent, "voltage": round(voltage, 3)}
|
||
safe_enqueue(battery_data, MSG_TYPE_STATUS)
|
||
elif inner_cmd == 5:
|
||
ssid = inner_data.get("ssid")
|
||
password = inner_data.get("password")
|
||
if not ssid or not password:
|
||
safe_enqueue({"result": "missing_ssid_or_password"}, MSG_TYPE_STATUS)
|
||
else:
|
||
# global update_thread_started
|
||
if not update_thread_started:
|
||
update_thread_started = True
|
||
_thread.start_new_thread(handle_wifi_and_update, (ssid, password))
|
||
else:
|
||
safe_enqueue({"result": "update_already_started"}, MSG_TYPE_STATUS)
|
||
elif inner_cmd == 6:
|
||
try:
|
||
ip = os.popen("ifconfig wlan0 2>/dev/null | grep 'inet ' | awk '{print $2}'").read().strip()
|
||
ip = ip if ip else "no_ip"
|
||
except:
|
||
ip = "error_getting_ip"
|
||
safe_enqueue({"result": "current_ip", "ip": ip}, MSG_TYPE_STATUS)
|
||
|
||
elif inner_cmd == 7:
|
||
# global update_thread_started
|
||
if update_thread_started:
|
||
safe_enqueue({"result": "update_already_started"}, MSG_TYPE_STATUS)
|
||
continue
|
||
|
||
# 实时检查是否有 IP
|
||
try:
|
||
ip = os.popen("ifconfig wlan0 2>/dev/null | grep 'inet ' | awk '{print $2}'").read().strip()
|
||
except:
|
||
ip = None
|
||
|
||
if not ip:
|
||
safe_enqueue({"result": "ota_rejected", "reason": "no_wifi_ip"}, MSG_TYPE_STATUS)
|
||
else:
|
||
# 启动纯下载线程
|
||
update_thread_started = True
|
||
_thread.start_new_thread(direct_ota_download, ())
|
||
rx_buf = rx_buf[match.end():]
|
||
else:
|
||
break
|
||
except Exception as e:
|
||
print(f"[ERROR] 解析/处理数据包失败: {e}")
|
||
rx_buf = b""
|
||
break
|
||
|
||
# 发送队列处理
|
||
msg_type = None
|
||
if logged_in:
|
||
with send_queue_lock:
|
||
if send_queue:
|
||
msg_type, data_dict = send_queue.pop(0)
|
||
if msg_type is not None:
|
||
pkt = make_packet(msg_type, data_dict)
|
||
if not tcp_send_raw(pkt):
|
||
print("💔 发送失败，断开重连")
|
||
break
|
||
|
||
# 校准结果上报
|
||
if logged_in:
|
||
x = y = None
|
||
with laser_calibration_data_lock:
|
||
if laser_calibration_result is not None:
|
||
x, y = laser_calibration_result
|
||
laser_calibration_result = None
|
||
if x is not None:
|
||
safe_enqueue({"result": "ok", "x": x, "y": y}, MSG_TYPE_STATUS)
|
||
|
||
# 心跳机制
|
||
current_time = time.ticks_ms()
|
||
if logged_in and current_time - last_heartbeat_send_time > HEARTBEAT_INTERVAL * 1000:
|
||
if not tcp_send_raw(make_packet(MSG_TYPE_HEARTBEAT, {"t": int(time.time())})):
|
||
print("💔 心跳发送失败")
|
||
break
|
||
last_heartbeat_send_time = current_time
|
||
|
||
if logged_in and current_time - last_heartbeat_ack_time > 6000:
|
||
print("⏰ 6秒无心跳ACK，重连")
|
||
break
|
||
|
||
time.sleep_ms(50)
|
||
|
||
tcp_connected = False
|
||
time.sleep_ms(2000)
|
||
|
||
|
||
def laser_calibration_worker():
|
||
"""后台激光校准线程"""
|
||
global laser_calibration_active, laser_calibration_result
|
||
while True:
|
||
if laser_calibration_active:
|
||
result = calibrate_laser_position()
|
||
if result and len(result) == 2:
|
||
with laser_calibration_data_lock:
|
||
laser_calibration_result = result
|
||
laser_calibration_active = False
|
||
print(f"✅ 后台校准成功: {result}")
|
||
else:
|
||
time.sleep_ms(80)
|
||
else:
|
||
time.sleep_ms(50)
|
||
|
||
|
||
# ==============================
|
||
# 主程序入口
|
||
# ==============================
|
||
|
||
def cmd_str():
|
||
global DEVICE_ID, PASSWORD
|
||
DEVICE_ID = read_device_id()
|
||
PASSWORD = DEVICE_ID + "."
|
||
|
||
photo_dir = "/root/phot"
|
||
if photo_dir not in os.listdir("/root"):
|
||
try:
|
||
os.mkdir(photo_dir)
|
||
except:
|
||
pass
|
||
|
||
init_ina226()
|
||
load_laser_point()
|
||
|
||
disp = display.Display()
|
||
cam = camera.Camera(640, 480)
|
||
|
||
_thread.start_new_thread(tcp_main, ())
|
||
_thread.start_new_thread(laser_calibration_worker, ())
|
||
|
||
print("系统准备完成...")
|
||
|
||
while not app.need_exit():
|
||
if adc_obj.read() > ADC_TRIGGER_THRESHOLD:
|
||
time.sleep_ms(60)
|
||
frame = cam.read()
|
||
|
||
x, y = laser_point
|
||
frame.draw_line(int(x - length), int(y), int(x + length), int(y), color, thickness)
|
||
frame.draw_line(int(x), int(y - length), int(x), int(y + length), color, thickness)
|
||
frame.draw_circle(int(x), int(y), 1, color, thickness)
|
||
|
||
result_img, center, radius, method, _ = detect_circle(frame)
|
||
disp.show(result_img)
|
||
|
||
dx, dy = compute_laser_position(center, (x, y), radius, method)
|
||
distance_m = read_distance_from_laser_sensor()
|
||
voltage = get_bus_voltage()
|
||
battery_percent = voltage_to_percent(voltage)
|
||
|
||
try:
|
||
jpg_count = len([f for f in os.listdir(photo_dir) if f.endswith('.jpg')])
|
||
filename = f"{photo_dir}/{int(x)}_{int(y)}_{round((distance_m or 0.0) * 100)}_{method}_{jpg_count:04d}.jpg"
|
||
result_img.save(filename, quality=70)
|
||
except Exception as e:
|
||
print(f"❌ 保存照片失败: {e}")
|
||
|
||
inner_data = {
|
||
"x": float(dx) if dx is not None else 200.0,
|
||
"y": float(dy) if dy is not None else 200.0,
|
||
"r": 90.0,
|
||
"d": round((distance_m or 0.0) * 100),
|
||
"m": method
|
||
}
|
||
report_data = {"cmd": 1, "data": inner_data}
|
||
safe_enqueue(report_data, MSG_TYPE_STATUS)
|
||
|
||
time.sleep_ms(100)
|
||
else:
|
||
disp.show(cam.read())
|
||
time.sleep_ms(50)
|
||
|
||
|
||
if __name__ == "__main__":
|
||
cmd_str() |