470 lines
16 KiB
Python
470 lines
16 KiB
Python
'''
|
||
主题：MAIXCAM 云台
|
||
作者：Tommy
|
||
日期：2025年-12月-21日
|
||
'''
|
||
from maix import camera, display, image, nn, app, sys
|
||
from maix import uart, pinmap, time, err, gpio, network
|
||
import paho.mqtt.client as mqtt
|
||
import asyncio
|
||
import gc
|
||
import traceback
|
||
|
||
|
||
DEVICE_ID = "XXX"
|
||
|
||
# 两套WiFi/MQTT配置
|
||
WIFI_CONFIGS = [
|
||
{
|
||
"SSID": "X",
|
||
"PASSWORD": "Y",
|
||
"MQTT_BROKER": "Z",
|
||
},
|
||
]
|
||
|
||
MQTT_PORT = 1883
|
||
|
||
# 发送的主题
|
||
MQTT_PUB_TOPIC_IMAGE = "hangtianyuan/head_video" # 图像发布主题
|
||
|
||
# 订阅的主题
|
||
MQTT_SUB_TOPIC_TEXT = "hangtianyuan/text" # 文本指令订阅主题
|
||
MQTT_SUB_TOPIC_MOVE = "hangtianyuan/set_move" # 舵机移动指令订阅主题
|
||
# 批量订阅
|
||
MQTT_SUB_TOPICS = [
|
||
(MQTT_SUB_TOPIC_TEXT, 0),
|
||
(MQTT_SUB_TOPIC_MOVE, 0),
|
||
]
|
||
|
||
# LED配置
|
||
LED_PIN_NAME = "A14"
|
||
LED_GPIO_NAME = "GPIOA14"
|
||
BLINK_INTERVAL = 500 # LED闪烁间隔(ms)
|
||
|
||
# 人脸追踪参数
|
||
kp_x = 0.05 # 水平方向 0.04
|
||
kp_y = 0.07 # 垂直方向
|
||
angle_threshold = 1
|
||
s1_range = [0, 180]
|
||
s2_range = [0, 180]
|
||
no_face_threshold = 30
|
||
|
||
# 滤波缓存
|
||
last_dev_x = 0.0
|
||
last_dev_y = 0.0
|
||
filter_alpha = 0.85
|
||
|
||
# ------------------------------
|
||
# 全局状态变量（新增track_mode控制跟踪开关）
|
||
# ------------------------------
|
||
mqtt_connected = False
|
||
wifi_connected = False
|
||
current_wifi_idx = 0
|
||
led = None
|
||
mqtt_client = None
|
||
previous_millis = 0
|
||
send_frame_interval = 8
|
||
frame_count = 0
|
||
track_mode = 1 # 0:关闭追踪，1:人脸跟踪模式
|
||
serial_dev = None # 新增：舵机串口全局变量
|
||
|
||
# ------------------------------
|
||
# 工具函数
|
||
# ------------------------------
|
||
def constrain(val, min_val, max_val):
|
||
return min(max_val, max(min_val, val))
|
||
|
||
# ------------------------------
|
||
# LED控制
|
||
# ------------------------------
|
||
def init_led():
|
||
"""初始化LED引脚"""
|
||
global led
|
||
try:
|
||
err.check_raise(
|
||
pinmap.set_pin_function(LED_PIN_NAME, LED_GPIO_NAME),
|
||
f"设置LED引脚{LED_PIN_NAME}功能失败"
|
||
)
|
||
led = gpio.GPIO(LED_GPIO_NAME, gpio.Mode.OUT)
|
||
led.value(0) # 初始熄灭
|
||
print(f"LED初始化完成：引脚{LED_PIN_NAME}（{LED_GPIO_NAME}）")
|
||
except Exception as e:
|
||
print(f"LED初始化失败: {e}")
|
||
led = None
|
||
|
||
def update_led():
|
||
"""更新LED状态：MQTT未连接闪烁，连接成功常亮"""
|
||
global previous_millis, mqtt_connected
|
||
if led is None:
|
||
return
|
||
|
||
current_millis = time.time_ms()
|
||
if not mqtt_connected:
|
||
if current_millis - previous_millis >= BLINK_INTERVAL:
|
||
previous_millis = current_millis
|
||
led.toggle()
|
||
else:
|
||
if led.value() != 1:
|
||
led.value(1)
|
||
|
||
# ------------------------------
|
||
# WiFi连接（异步）
|
||
# ------------------------------
|
||
async def connect_wifi_async():
|
||
"""异步尝试连接WiFi（自动切换两套配置）"""
|
||
global wifi_connected, current_wifi_idx
|
||
w = network.wifi.Wifi()
|
||
|
||
wifi_connected = False
|
||
current_wifi_idx = 0
|
||
|
||
while current_wifi_idx < len(WIFI_CONFIGS) and not wifi_connected:
|
||
config = WIFI_CONFIGS[current_wifi_idx]
|
||
ssid = config["SSID"]
|
||
password = config["PASSWORD"]
|
||
|
||
print(f"[WiFi] 尝试连接 {ssid} (第{current_wifi_idx+1}套)")
|
||
try:
|
||
e = w.connect(ssid, password, wait=True, timeout=10)
|
||
err.check_raise(e, f"WiFi连接失败")
|
||
wifi_connected = True
|
||
ip = w.get_ip()
|
||
print(f"[WiFi] 连接成功！IP: {ip}")
|
||
except Exception as e:
|
||
print(f"[WiFi] {ssid} 连接失败: {e}")
|
||
current_wifi_idx += 1
|
||
await asyncio.sleep(1)
|
||
|
||
if not wifi_connected:
|
||
print("[WiFi] 所有配置连接失败，无联网状态")
|
||
|
||
# ------------------------------
|
||
# MQTT回调函数（新增track_mode控制逻辑）
|
||
# ------------------------------
|
||
def on_mqtt_connect(client, userdata, flags, rc):
|
||
"""MQTT连接回调"""
|
||
global mqtt_connected
|
||
if rc == 0:
|
||
mqtt_connected = True
|
||
print("[MQTT] 连接成功")
|
||
# 订阅主题（如有）
|
||
if MQTT_SUB_TOPICS:
|
||
client.subscribe(MQTT_SUB_TOPICS)
|
||
print(f"[MQTT] 已订阅主题: {MQTT_SUB_TOPICS}")
|
||
else:
|
||
mqtt_connected = False
|
||
print(f"[MQTT] 连接失败，错误码: {rc}")
|
||
|
||
def on_mqtt_message(client, userdata, msg):
|
||
global track_mode, serial_dev
|
||
if msg.topic == MQTT_SUB_TOPIC_TEXT:
|
||
payload = msg.payload.decode().strip() # 去除首尾空格，提高容错
|
||
print(f"文本指令: {payload}")
|
||
# 解析跟踪模式指令
|
||
if payload.lower() == "track_off":
|
||
track_mode = 0
|
||
print("[跟踪模式] 已关闭人脸跟踪，停止绘制人脸框和追踪")
|
||
elif payload.lower() == "track_face":
|
||
track_mode = 1
|
||
print("[跟踪模式] 已开启人脸跟踪，开始绘制人脸框和追踪")
|
||
elif payload.lower() == "track_hand":
|
||
track_mode = 2
|
||
print("[跟踪模式] 已开启手部跟踪，开始绘制手部框和追踪")
|
||
elif msg.topic == MQTT_SUB_TOPIC_MOVE:
|
||
payload = msg.payload.decode().strip()
|
||
# 修复：判空保护 + 使用全局serial_dev
|
||
if serial_dev is not None:
|
||
try:
|
||
serial_dev.write_str(payload)
|
||
print(f"运动指令已发送: {payload}")
|
||
except Exception as e:
|
||
print(f"发送运动指令失败: {e} | 指令内容: {payload}")
|
||
else:
|
||
print(f"运动指令: {payload}，但串口未初始化")
|
||
|
||
def on_mqtt_disconnect(client, userdata, rc):
|
||
"""MQTT断开回调"""
|
||
global mqtt_connected
|
||
mqtt_connected = False
|
||
if rc != 0:
|
||
print(f"[MQTT] 意外断开，错误码: {rc}")
|
||
|
||
# ------------------------------
|
||
# MQTT初始化（异步）
|
||
# ------------------------------
|
||
async def init_mqtt_async():
|
||
"""异步初始化MQTT（依赖WiFi，自动重试）"""
|
||
global mqtt_client, mqtt_connected, current_wifi_idx
|
||
mqtt_connected = False
|
||
mqtt_client = None
|
||
|
||
# 等待WiFi连接（最多20秒）
|
||
wait_time = 0
|
||
while not app.need_exit() and not wifi_connected and wait_time < 20:
|
||
await asyncio.sleep(1)
|
||
wait_time += 1
|
||
|
||
if app.need_exit() or not wifi_connected:
|
||
print("[MQTT] WiFi未连接，跳过MQTT初始化")
|
||
return
|
||
|
||
# 兜底索引
|
||
if current_wifi_idx >= len(WIFI_CONFIGS):
|
||
current_wifi_idx = 0
|
||
current_broker = WIFI_CONFIGS[current_wifi_idx]["MQTT_BROKER"]
|
||
|
||
try:
|
||
mqtt_client = mqtt.Client(DEVICE_ID)
|
||
mqtt_client.on_connect = on_mqtt_connect
|
||
mqtt_client.on_message = on_mqtt_message
|
||
mqtt_client.on_disconnect = on_mqtt_disconnect
|
||
|
||
# 重试连接
|
||
retry_count = 0
|
||
while not app.need_exit() and not mqtt_connected and retry_count < 5:
|
||
try:
|
||
print(f"[MQTT] 尝试连接 {current_broker}:{MQTT_PORT} (重试{retry_count+1}/5)")
|
||
mqtt_client.connect(current_broker, MQTT_PORT, 60)
|
||
mqtt_client.loop_start()
|
||
|
||
# 等待连接结果
|
||
t = time.time()
|
||
while not mqtt_connected and time.time() - t < 5:
|
||
await asyncio.sleep(0.1)
|
||
|
||
if mqtt_connected:
|
||
break
|
||
else:
|
||
mqtt_client.disconnect()
|
||
retry_count += 1
|
||
await asyncio.sleep(3)
|
||
except Exception as e:
|
||
print(f"[MQTT] 连接异常: {e}")
|
||
retry_count += 1
|
||
await asyncio.sleep(3)
|
||
|
||
if not mqtt_connected:
|
||
print("[MQTT] 多次重试后仍无法连接")
|
||
except Exception as e:
|
||
print(f"[MQTT] 初始化失败: {e}")
|
||
mqtt_client = None
|
||
mqtt_connected = False
|
||
|
||
# ------------------------------
|
||
# 图像MQTT发送
|
||
# ------------------------------
|
||
def send_image_via_mqtt(img):
|
||
"""发送图像到MQTT（JPEG压缩）"""
|
||
global mqtt_connected, mqtt_client
|
||
if not mqtt_connected or mqtt_client is None:
|
||
return False
|
||
|
||
try:
|
||
jpeg_data = img.to_jpeg(quality=85).to_bytes()
|
||
# 二进制发送（高效）
|
||
mqtt_client.publish(MQTT_PUB_TOPIC_IMAGE, jpeg_data)
|
||
gc.collect()
|
||
return True
|
||
except Exception as e:
|
||
print(f"[MQTT] 图像发送失败: {e}")
|
||
try:
|
||
if mqtt_client:
|
||
mqtt_client.reconnect()
|
||
except:
|
||
mqtt_connected = False
|
||
return False
|
||
|
||
# ------------------------------
|
||
# 系统初始化
|
||
# ------------------------------
|
||
def init_system():
|
||
|
||
"""初始化相机、舵机、串口、LED"""
|
||
global serial_dev # 声明使用全局serial_dev
|
||
|
||
"""初始化相机、舵机、串口、LED"""
|
||
init_led()
|
||
|
||
# 人脸检测器
|
||
detector = nn.YOLOv8(model="/root/models/yolov8n_face.mud", dual_buff=True)
|
||
img_width = detector.input_width()
|
||
img_height = detector.input_height()
|
||
print(f"[系统] 图像尺寸: {img_width}x{img_height}")
|
||
image_center = (img_width // 2, img_height // 2)
|
||
|
||
# 相机/显示器
|
||
cam = camera.Camera(img_width, img_height, detector.input_format())
|
||
disp = display.Display()
|
||
|
||
# 串口（舵机）
|
||
pin_function = {"A19": "UART1_TX", "A18": "UART1_RX"}
|
||
device = "/dev/ttyS1"
|
||
for pin, func in pin_function.items():
|
||
err.check_raise(pinmap.set_pin_function(pin, func),
|
||
f"Failed set pin{pin} function to {func}")
|
||
serial_dev = uart.UART(device, 115200)
|
||
|
||
# 舵机初始角度
|
||
current_s1, current_s2 = 90, 90
|
||
initial_cmd = f"S1:{current_s1},S2:{current_s2};"
|
||
serial_dev.write_str(initial_cmd)
|
||
print(f"[舵机] 初始角度: {initial_cmd}")
|
||
time.sleep(1)
|
||
|
||
return {
|
||
"detector": detector, "cam": cam, "disp": disp, "serial_dev": serial_dev,
|
||
"img_width": img_width, "img_height": img_height, "image_center": image_center,
|
||
"current_s1": current_s1, "current_s2": current_s2
|
||
}
|
||
|
||
# ------------------------------
|
||
# 人脸检测处理（新增track_mode判断，关闭时不绘制人脸框）
|
||
# ------------------------------
|
||
def process_faces(img, objs):
|
||
global track_mode
|
||
# 跟踪模式关闭时，直接返回无脸坐标，不绘制任何框
|
||
if not track_mode :
|
||
return (-1, -1, -1)
|
||
|
||
max_face = None
|
||
max_width = 0
|
||
|
||
if objs:
|
||
for obj in objs:
|
||
if obj.w > max_width:
|
||
max_width = obj.w
|
||
max_face = obj
|
||
|
||
# 绘制人脸框（仅跟踪模式开启时执行）
|
||
for obj in objs:
|
||
color = image.COLOR_GREEN if obj == max_face else image.COLOR_RED
|
||
img.draw_rect(obj.x, obj.y, obj.w, obj.h, color=color, thickness=2)
|
||
|
||
if max_face:
|
||
center_x = int(max_face.x + max_face.w / 2)
|
||
center_y = int(max_face.y + max_face.h / 2)
|
||
img.draw_keypoints([center_x, center_y], image.COLOR_GREEN, size=3)
|
||
return (center_x, center_y, int(max_face.w))
|
||
else:
|
||
return (-1, -1, -1)
|
||
|
||
# ------------------------------
|
||
# 人脸追踪
|
||
# ------------------------------
|
||
def track_face(center, image_center, current_angles, serial_dev):
|
||
global last_dev_x, last_dev_y
|
||
center_x, center_y = center
|
||
current_s1, current_s2 = current_angles
|
||
|
||
# 偏差计算+滤波
|
||
deviation_x = center_x - image_center[0]
|
||
deviation_y = center_y - image_center[1]
|
||
smoothed_x = filter_alpha * last_dev_x + (1 - filter_alpha) * deviation_x
|
||
smoothed_y = filter_alpha * last_dev_y + (1 - filter_alpha) * deviation_y
|
||
last_dev_x, last_dev_y = smoothed_x, smoothed_y
|
||
|
||
# 角度计算+限制
|
||
# 金属舵机
|
||
# new_s1 = constrain(int(current_s1 + smoothed_x * kp_x), s1_range[0], s1_range[1])
|
||
# new_s2 = constrain(int(current_s2 + smoothed_y * kp_y), s2_range[0], s2_range[1])
|
||
# 塑料舵机
|
||
new_s1 = constrain(int(current_s1 - smoothed_x * kp_x), s1_range[0], s1_range[1])
|
||
new_s2 = constrain(int(current_s2 - smoothed_y * kp_y), s2_range[0], s2_range[1])
|
||
|
||
# 发送舵机指令
|
||
if abs(new_s1 - current_s1) > angle_threshold or abs(new_s2 - current_s2) > angle_threshold:
|
||
cmd = f"S1:{new_s1},S2:{new_s2};"
|
||
serial_dev.write_str(cmd)
|
||
return (new_s1, new_s2, cmd)
|
||
else:
|
||
return (current_s1, current_s2, None)
|
||
|
||
# ------------------------------
|
||
# 主循环（新增track_mode判断和状态显示）
|
||
# ------------------------------
|
||
async def main_loop_async(resources):
|
||
"""异步主循环"""
|
||
detector = resources["detector"]
|
||
cam = resources["cam"]
|
||
disp = resources["disp"]
|
||
serial_dev = resources["serial_dev"]
|
||
img_width = resources["img_width"]
|
||
img_height = resources["img_height"]
|
||
image_center = resources["image_center"]
|
||
current_s1, current_s2 = resources["current_s1"], resources["current_s2"]
|
||
|
||
# 启动网络异步任务
|
||
asyncio.create_task(connect_wifi_async())
|
||
asyncio.create_task(init_mqtt_async())
|
||
|
||
# 主循环
|
||
global frame_count, mqtt_connected, wifi_connected, track_mode
|
||
while not app.need_exit():
|
||
# 图像读取+人脸检测
|
||
img = cam.read().flip(image.FlipDir.Y)
|
||
objs = detector.detect(img, conf_th=0.4, iou_th=0.45)
|
||
center_x, center_y, face_width = process_faces(img, objs)
|
||
|
||
# 绘制基础状态
|
||
img.draw_string(20, 20, f"Center: ({center_x}, {center_y})", image.COLOR_GREEN, 1.0)
|
||
img.draw_string(20, 40, f"Max Width: {face_width}", image.COLOR_YELLOW, 1.0)
|
||
|
||
# 绘制跟踪模式状态（直观显示当前模式）
|
||
track_status = "ON" if track_mode else "OFF"
|
||
track_color = image.COLOR_GREEN if track_mode else image.COLOR_RED
|
||
img.draw_string(20, 100, f"Track Mode: {track_status}", track_color, 1.0)
|
||
|
||
# 人脸追踪（仅跟踪模式开启时执行）
|
||
if track_mode and center_x != -1 and center_y != -1:
|
||
new_s1, new_s2, cmd = track_face((center_x, center_y), image_center, (current_s1, current_s2), serial_dev)
|
||
current_s1, current_s2 = new_s1, new_s2
|
||
if cmd:
|
||
img.draw_string(20, 60, f"Tracking: {cmd}", image.COLOR_BLUE, 1.0)
|
||
elif track_mode and center_x == -1:
|
||
# 跟踪模式开启但无脸
|
||
img.draw_string(20, 60, "No face detected", image.COLOR_RED, 1.0)
|
||
else:
|
||
# 跟踪模式关闭
|
||
img.draw_string(20, 60, "Track Mode: OFF", image.COLOR_ORANGE, 1.0)
|
||
|
||
# 网络状态显示
|
||
net_status = f"WiFi: {'OK' if wifi_connected else 'NO'} | MQTT: {'OK' if mqtt_connected else 'NO'}"
|
||
img.draw_string(20, 80, net_status, image.COLOR_YELLOW, 1.0)
|
||
|
||
# LED更新+图像发送
|
||
update_led()
|
||
frame_count += 1
|
||
if frame_count >= send_frame_interval:
|
||
send_image_via_mqtt(img)
|
||
frame_count = 0
|
||
|
||
disp.show(img)
|
||
await asyncio.sleep(0.001)
|
||
|
||
# ------------------------------
|
||
# 程序入口
|
||
# ------------------------------
|
||
if __name__ == "__main__":
|
||
try:
|
||
system_resources = init_system()
|
||
asyncio.run(main_loop_async(system_resources))
|
||
except Exception as e:
|
||
print(f"[异常] 程序异常: {e}")
|
||
traceback.print_exc()
|
||
finally:
|
||
# 清理MQTT
|
||
if mqtt_client:
|
||
try:
|
||
mqtt_client.loop_stop()
|
||
mqtt_client.disconnect()
|
||
print("[MQTT] 客户端已断开")
|
||
except Exception as e:
|
||
print(f"[MQTT] 断开失败: {e}")
|
||
# 清理LED
|
||
if led:
|
||
try:
|
||
led.value(0)
|
||
print("[LED] 已关闭")
|
||
except Exception as e:
|
||
print(f"[LED] 关闭失败: {e}")
|
||
print("[系统] 程序已退出") |