ESP32-S3 摄像头图传：固件 + Slint 客户端 + 共享协议
两块 ESP32-S3-WROOM-1-N16R8 图传板（DVP 接口接 OV2640/OV3660/OV5640）， 向队伍客户端与裁判终端推 UDP 图传。固件是跑在 ESP-IDF 上的 Rust，用官方 esp32-camera 组件采集；客户端是 Rust + Slint 桌面程序。 三个 crate 各自独立构建： . firmware，交叉编译到 Xtensa protocol/ 线路格式与帧重组，两端共用，无 ESP-IDF 依赖，有单元测试 client/ Slint 桌面客户端 视频走 UDP 而非 TCP：实测传感器产出 22 fps，TCP MJPEG 只送出 1.9 fps， 且只占用链路能力的 1%——链路是空的，帧堵在队头阻塞后面等一个实时画面 等不起的重传计时器。改为独立数据报、不重传、缺片丢整帧后达到 20.8 fps。 发现是双向的：板子每 2 秒广播 Hello，客户端广播 Subscribe，板子据此定向 推流。板子身份由 MAC 派生，重启不变，所以客户端可以把绑定存进配置。 裁判端同时收两块板子（每个发送方一个重组器），队伍端绑定其中一块并只显示 自己那一边的血量。装甲板接口已预留（ArmorSource trait + ARMOR_PORT）， 当前无数据源，界面显示占位符。 vendor 了 esp32-camera 2.1.7：组件注册表的版本求解器找不到它，且需要打一处 补丁（见 components/esp32-camera/target/esp32s3/ll_cam.c）。 .gitignore 三条 target/ 都锚定在根目录，因为 protocol/ 与 client/ 各自也是 独立 crate；而 components/esp32-camera/target/ 是上游的按芯片分目录，里面是 源码，必须保留。
This commit is contained in:
Horldsence
@@ -0,0 +1,458 @@
|
||||
//! UDP 接收、发现、多路帧重组。
|
||||
//!
|
||||
//! # 多路
|
||||
//!
|
||||
//! 裁判终端同时收两块图传板，队伍终端也会收到两块（只是界面上只显示一块）。
|
||||
//! 所以重组状态**必须按发送方分开**——把两块板的帧塞进同一个重组器，会让它们
|
||||
//! 的 `frame_id` 互相顶掉，两路都出不来。
|
||||
//!
|
||||
//! # 发现与订阅
|
||||
//!
|
||||
//! 两个方向各司其职：
|
||||
//!
|
||||
//! - 板子每 2 秒广播一次 [`protocol::MsgType::Hello`]，客户端被动收到就知道有谁在线。
|
||||
//! - 客户端广播 [`protocol::MsgType::Subscribe`]，板子收到后开始定向推流。
|
||||
//!
|
||||
//! 板子靠**源地址**记住客户端，所以客户端只要周期性续订就能维持推流，不存在
|
||||
//! 任何超时淘汰的竞态。
|
||||
//!
|
||||
//! # 丢帧策略
|
||||
//!
|
||||
//! 缺了分片的帧整个丢弃、下一帧顶上，不重传也不尝试补洞。对实时画面这是对的：
|
||||
//! 等重传补齐时，那一帧早就不新鲜了。这个取舍的代价是丢帧率必须摆在界面上，否则
|
||||
//! "画面卡"没法区分是链路问题还是传感器问题。
|
||||
|
||||
use std::collections::HashMap;
|
||||
use std::io::ErrorKind;
|
||||
use std::net::{Ipv4Addr, SocketAddr, SocketAddrV4, UdpSocket};
|
||||
use std::sync::atomic::{AtomicBool, Ordering};
|
||||
use std::sync::{Arc, Mutex};
|
||||
use std::thread;
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
use protocol::{Fragment, FrameAssembler, Subscribe, SubscribeRole, DEFAULT_PORT, HEADER_LEN};
|
||||
|
||||
/// 接受的最大数据报。高于协议的理论上限（1427），留出余量，这样一个被截断的读只可能
|
||||
/// 来自网络，而不是来自过小的缓冲。
|
||||
const RECV_BUFFER: usize = 2048;
|
||||
|
||||
/// 重新广播订阅的间隔。
|
||||
///
|
||||
/// 比板子判定"客户端掉线"的超时（6 秒）短得多，所以偶发丢一两个包不会中断推流。
|
||||
const SUBSCRIBE_INTERVAL: Duration = Duration::from_millis(800);
|
||||
|
||||
/// 一张帧没收齐就放弃的时限。
|
||||
///
|
||||
/// 一帧在 22fps 下约 45ms 到，所以几百毫秒还没齐的永远不会齐了。
|
||||
const FRAME_TIMEOUT: Duration = Duration::from_millis(500);
|
||||
|
||||
/// 一块已知的图传板。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct CameraInfo {
|
||||
pub id: u32,
|
||||
pub addr: SocketAddrV4,
|
||||
pub firmware: u16,
|
||||
/// 最近一次收到它广播的时刻。
|
||||
pub last_seen: Instant,
|
||||
}
|
||||
|
||||
impl CameraInfo {
|
||||
/// 多久没有广播了。
|
||||
pub fn silence(&self) -> Duration {
|
||||
self.last_seen.elapsed()
|
||||
}
|
||||
|
||||
/// 板子在线吗？
|
||||
///
|
||||
/// 阈值是广播间隔的数倍，所以偶发丢包不会让界面上的在线状态闪��。
|
||||
pub fn is_online(&self) -> bool {
|
||||
self.silence() < Duration::from_secs(7)
|
||||
}
|
||||
}
|
||||
|
||||
/// 一块板的运行时统计。
|
||||
#[derive(Debug, Clone, Copy, Default)]
|
||||
pub struct SourceStats {
|
||||
pub frames_shown: u64,
|
||||
/// 因分片缺失被放弃的帧。
|
||||
pub frames_dropped: u64,
|
||||
pub datagrams: u64,
|
||||
pub last_frame_len: usize,
|
||||
pub width: u16,
|
||||
pub height: u16,
|
||||
/// 上一帧到这一帧之间的间隔。
|
||||
pub last_interval: Option<Duration>,
|
||||
last_shown_at: Option<Instant>,
|
||||
}
|
||||
|
||||
impl SourceStats {
|
||||
/// 丢弃的帧占已收到帧的比例。
|
||||
pub fn loss_percent(&self) -> f32 {
|
||||
let total = self.frames_shown + self.frames_dropped;
|
||||
if total == 0 {
|
||||
0.0
|
||||
} else {
|
||||
self.frames_dropped as f32 * 100.0 / total as f32
|
||||
}
|
||||
}
|
||||
|
||||
fn on_frame(&mut self, width: u16, height: u16, len: usize) {
|
||||
self.last_interval = self.last_shown_at.map(|t| t.elapsed());
|
||||
self.last_shown_at = Some(Instant::now());
|
||||
self.frames_shown += 1;
|
||||
self.width = width;
|
||||
self.height = height;
|
||||
self.last_frame_len = len;
|
||||
}
|
||||
}
|
||||
|
||||
/// 每一路的最新帧，等着被解码。
|
||||
///
|
||||
/// 每路只保留最新的一张：在 22fps 下把每一帧都排队意味着内存里压着几十张 JPEG，
|
||||
/// 然后解码那些几毫秒后就被下一帧顶掉的画面——而这正是这条链路要消除的延迟。
|
||||
#[derive(Default)]
|
||||
pub struct LatestFrames {
|
||||
inner: Mutex<HashMap<u32, protocol::Frame>>,
|
||||
}
|
||||
|
||||
impl LatestFrames {
|
||||
pub fn put(&self, frame: protocol::Frame) {
|
||||
self.inner
|
||||
.lock()
|
||||
.expect("latest-frames mutex poisoned")
|
||||
.insert(frame.sender_id, frame);
|
||||
}
|
||||
|
||||
/// 取走某一路的待处理帧。
|
||||
pub fn take(&self, sender_id: u32) -> Option<protocol::Frame> {
|
||||
self.inner
|
||||
.lock()
|
||||
.expect("latest-frames mutex poisoned")
|
||||
.remove(&sender_id)
|
||||
}
|
||||
|
||||
/// 列出所有有待处理帧的板子 id。
|
||||
pub fn pending_senders(&self) -> Vec<u32> {
|
||||
self.inner
|
||||
.lock()
|
||||
.expect("latest-frames mutex poisoned")
|
||||
.keys()
|
||||
.copied()
|
||||
.collect()
|
||||
}
|
||||
}
|
||||
|
||||
/// 所有板子的共享状态，跨线程只读。
|
||||
pub struct Discovery {
|
||||
cameras: Mutex<HashMap<u32, CameraInfo>>,
|
||||
stats: Mutex<HashMap<u32, SourceStats>>,
|
||||
}
|
||||
|
||||
impl Discovery {
|
||||
fn new() -> Self {
|
||||
Self {
|
||||
cameras: Mutex::new(HashMap::new()),
|
||||
stats: Mutex::new(HashMap::new()),
|
||||
}
|
||||
}
|
||||
|
||||
/// 当前已知的板子，按 id 排序，让界面上���列表顺序稳定。
|
||||
pub fn cameras(&self) -> Vec<CameraInfo> {
|
||||
let mut list: Vec<CameraInfo> = self
|
||||
.cameras
|
||||
.lock()
|
||||
.expect("cameras mutex poisoned")
|
||||
.values()
|
||||
.cloned()
|
||||
.collect();
|
||||
|
||||
list.sort_by_key(|c| c.id);
|
||||
list
|
||||
}
|
||||
|
||||
/// 在线且刚刚广播过的板子。
|
||||
pub fn online_cameras(&self) -> Vec<CameraInfo> {
|
||||
self.cameras().into_iter().filter(CameraInfo::is_online).collect()
|
||||
}
|
||||
|
||||
/// 某一路的统计。
|
||||
pub fn stats(&self, sender_id: u32) -> SourceStats {
|
||||
self.stats
|
||||
.lock()
|
||||
.expect("stats mutex poisoned")
|
||||
.get(&sender_id)
|
||||
.copied()
|
||||
.unwrap_or_default()
|
||||
}
|
||||
|
||||
fn note_hello(&self, id: u32, addr: SocketAddrV4, firmware: u16) {
|
||||
let mut cameras = self.cameras.lock().expect("cameras mutex poisoned");
|
||||
|
||||
cameras
|
||||
.entry(id)
|
||||
.and_modify(|c| {
|
||||
c.addr = addr;
|
||||
c.last_seen = Instant::now();
|
||||
c.firmware = firmware;
|
||||
})
|
||||
.or_insert(CameraInfo {
|
||||
id,
|
||||
addr,
|
||||
firmware,
|
||||
last_seen: Instant::now(),
|
||||
});
|
||||
}
|
||||
|
||||
fn note_frame(&self, id: u32, width: u16, height: u16, len: usize) {
|
||||
let mut stats = self.stats.lock().expect("stats mutex poisoned");
|
||||
stats.entry(id).or_default().on_frame(width, height, len);
|
||||
}
|
||||
|
||||
fn count(&self, id: u32, f: impl FnOnce(&mut SourceStats)) {
|
||||
let mut stats = self.stats.lock().expect("stats mutex poisoned");
|
||||
f(stats.entry(id).or_default());
|
||||
}
|
||||
|
||||
/// 超过 `max_idle` 没有广播的板子从列表里移除。
|
||||
///
|
||||
/// 界面上不会留下一堆永远离线的板子。
|
||||
pub fn forget_silent_cameras(&self, max_idle: Duration) {
|
||||
self.cameras
|
||||
.lock()
|
||||
.expect("cameras mutex poisoned")
|
||||
.retain(|_, c| c.silence() < max_idle);
|
||||
}
|
||||
}
|
||||
|
||||
/// 客户端身份。
|
||||
///
|
||||
/// 常量而非 MAC 派生：队伍客户端和裁判终端各自唯一，进程内也不需要区分自己，
|
||||
/// 而一个确定的常量让日志更好读。
|
||||
pub const CLIENT_ID: u32 = 0x0C11_0001;
|
||||
|
||||
/// 接收器。
|
||||
///
|
||||
/// socket 在这里创建而不是在接收线程里，这样"端口被占用"会立刻报错，而不是被线程
|
||||
/// 吞掉、表现为一个永远不出画面的空窗口。
|
||||
pub struct Receiver {
|
||||
discovery: Arc<Discovery>,
|
||||
frames: Arc<LatestFrames>,
|
||||
role: SubscribeRole,
|
||||
camera: Ipv4Addr,
|
||||
stop: Arc<AtomicBool>,
|
||||
}
|
||||
|
||||
impl Receiver {
|
||||
/// 绑定视频端口并准备好发现。
|
||||
///
|
||||
/// 绑定固定端口（而非临时端口）是必须的：板子是从订阅的**源端口**学到推流目标的，
|
||||
/// 临时端口也能工作，但固定端口让三台客户端行为一致，也方便抓包排查。
|
||||
pub fn start(camera: Ipv4Addr, role: SubscribeRole) -> std::io::Result<Self> {
|
||||
let socket =
|
||||
UdpSocket::bind(SocketAddrV4::new(Ipv4Addr::UNSPECIFIED, DEFAULT_PORT))?;
|
||||
socket.set_read_timeout(Some(Duration::from_millis(100)))?;
|
||||
|
||||
let broadcast = socket.set_broadcast(true).is_ok();
|
||||
if !broadcast {
|
||||
log::warn!("could not enable broadcast on the socket");
|
||||
}
|
||||
|
||||
Ok(Self {
|
||||
discovery: Arc::new(Discovery::new()),
|
||||
frames: Arc::new(LatestFrames::default()),
|
||||
role,
|
||||
camera,
|
||||
stop: Arc::new(AtomicBool::new(false)),
|
||||
})
|
||||
}
|
||||
|
||||
pub fn discovery(&self) -> Arc<Discovery> {
|
||||
Arc::clone(&self.discovery)
|
||||
}
|
||||
|
||||
pub fn frames(&self) -> Arc<LatestFrames> {
|
||||
Arc::clone(&self.frames)
|
||||
}
|
||||
|
||||
/// 运行接收循环，直到 [`Receiver::shutdown`]。
|
||||
///
|
||||
/// 阻塞，所以放在单独的线程上。
|
||||
pub fn run(&self, socket: UdpSocket) {
|
||||
let target = SocketAddrV4::new(self.camera, DEFAULT_PORT);
|
||||
let subscribe = Subscribe {
|
||||
role: self.role,
|
||||
client_id: CLIENT_ID,
|
||||
};
|
||||
let subscribe_bytes = subscribe.encode();
|
||||
|
||||
// 每个发送方一个重组器：两块板的 frame_id 各自独立，混在一起会互相顶掉。
|
||||
let mut assemblers: HashMap<u32, FrameAssembler> = HashMap::new();
|
||||
let mut buf = [0u8; RECV_BUFFER];
|
||||
let mut last_subscribe = Instant::now() - SUBSCRIBE_INTERVAL;
|
||||
let mut last_sweep = Instant::now();
|
||||
|
||||
log::info!(
|
||||
"listening on UDP {DEFAULT_PORT}, announcing {:?} to {target}",
|
||||
self.role
|
||||
);
|
||||
|
||||
while !self.stop.load(Ordering::Relaxed) {
|
||||
// 周期性续订就是保活：板子在 6 秒收不到就认为我们走了。
|
||||
if last_subscribe.elapsed() >= SUBSCRIBE_INTERVAL {
|
||||
if let Err(err) = socket.send_to(&subscribe_bytes, target) {
|
||||
log::warn!("could not subscribe at {target}: {err}");
|
||||
}
|
||||
last_subscribe = Instant::now();
|
||||
}
|
||||
|
||||
match socket.recv_from(&mut buf) {
|
||||
Ok((n, from)) => {
|
||||
self.handle_datagram(&buf[..n], from, &mut assemblers);
|
||||
}
|
||||
Err(err)
|
||||
if err.kind() == ErrorKind::WouldBlock
|
||||
|| err.kind() == ErrorKind::TimedOut =>
|
||||
{
|
||||
// 帧之间的常态。读超时正是让退出能及时响应的原因。
|
||||
}
|
||||
Err(err) => {
|
||||
log::warn!("recv failed: {err}");
|
||||
thread::sleep(Duration::from_millis(100));
|
||||
}
|
||||
}
|
||||
|
||||
// 放弃等太久的半截帧。否则板子在帧中途死掉时，重组器会一直留着它。
|
||||
for (id, asm) in assemblers.iter_mut() {
|
||||
if asm.expire().is_some() {
|
||||
self.discovery.count(*id, |s| s.frames_dropped += 1);
|
||||
}
|
||||
}
|
||||
|
||||
if last_sweep.elapsed() >= Duration::from_secs(5) {
|
||||
self.discovery.forget_silent_cameras(Duration::from_secs(60));
|
||||
last_sweep = Instant::now();
|
||||
}
|
||||
}
|
||||
|
||||
log::info!("receive loop stopped");
|
||||
}
|
||||
|
||||
/// 处理一个数据报。
|
||||
fn handle_datagram(
|
||||
&self,
|
||||
bytes: &[u8],
|
||||
from: std::net::SocketAddr,
|
||||
assemblers: &mut HashMap<u32, FrameAssembler>,
|
||||
) {
|
||||
if bytes.len() < HEADER_LEN {
|
||||
return;
|
||||
}
|
||||
|
||||
// 板子的在线通告。注意这里不看来源端口：广播来的源端口没有意义，身份由
|
||||
// 头里的 sender_id 决定。
|
||||
if let Some(hello) = protocol::Hello::decode(bytes) {
|
||||
if let SocketAddr::V4(v4) = from {
|
||||
let is_new = !self.discovery.cameras().iter().any(|c| c.id == hello.sender_id);
|
||||
self.discovery.note_hello(hello.sender_id, v4, hello.firmware);
|
||||
|
||||
if is_new {
|
||||
log::info!(
|
||||
"found camera 0x{:08X} at {v4}",
|
||||
hello.sender_id
|
||||
);
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
let Some(frag) = Fragment::decode(bytes) else {
|
||||
// 既不是 Hello 也不是合法分片：将来的其它消息，或一次损坏。
|
||||
return;
|
||||
};
|
||||
|
||||
let id = frag.header.sender_id;
|
||||
|
||||
match assemblers
|
||||
.entry(id)
|
||||
.or_insert_with(|| FrameAssembler::new(FRAME_TIMEOUT))
|
||||
.push(frag)
|
||||
{
|
||||
Ok(frame) => {
|
||||
self.discovery.count(id, |s| s.datagrams += 1);
|
||||
self.discovery
|
||||
.note_frame(id, frame.width, frame.height, frame.data.len());
|
||||
self.frames.put(frame);
|
||||
}
|
||||
Err(protocol::DropReason::Superseded) => {
|
||||
// 被更新的帧追上了。这不是丢帧：半截的那张只是被超越，在健康链路上
|
||||
// 这是帧被放弃的**正常**方式。
|
||||
self.discovery.count(id, |s| s.datagrams += 1);
|
||||
}
|
||||
Err(protocol::DropReason::TimedOut) => {
|
||||
self.discovery.count(id, |s| {
|
||||
s.datagrams += 1;
|
||||
s.frames_dropped += 1;
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn shutdown(&self) {
|
||||
self.stop.store(true, Ordering::Relaxed);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/// 解码后的画面。
|
||||
pub struct DecodedFrame {
|
||||
pub rgba: Vec<u8>,
|
||||
pub width: u32,
|
||||
pub height: u32,
|
||||
}
|
||||
|
||||
/// 解码一张 JPEG。
|
||||
///
|
||||
/// 返回 `None` 表示字节不是可解码的图像——协议层挡不住这种：它只检查分片结构，
|
||||
/// 不检查 JPEG 本身是否有效。
|
||||
pub fn decode_frame(jpeg: &[u8]) -> Option<DecodedFrame> {
|
||||
let img = image::load_from_memory_with_format(jpeg, image::ImageFormat::Jpeg).ok()?;
|
||||
let rgba = img.to_rgba8();
|
||||
|
||||
Some(DecodedFrame {
|
||||
width: rgba.width(),
|
||||
height: rgba.height(),
|
||||
rgba: rgba.into_raw(),
|
||||
})
|
||||
}
|
||||
|
||||
/// 装甲板血量的来源。
|
||||
///
|
||||
/// **预留，当前没有实现也没有数据源。** 血量显示为占位符。
|
||||
///
|
||||
/// 接口和端口都已就位（[`protocol::ARMOR_PORT`]、[`protocol::ArmorHealth`]）：装甲板
|
||||
/// 接上以后往那个端口发消息即可，裁判界面与其它代码都不用改。
|
||||
pub trait ArmorSource: Send + Sync {
|
||||
/// 某一边的血量。
|
||||
///
|
||||
/// 返回 4 个装甲板的当前血量，按 index 排列。没有数据时返回 `None`，界面据此
|
||||
/// 显示占位符而不是 0——0 是一个看起来像真值的假值。
|
||||
fn health(&self, team: u8) -> Option<[u16; 4]>;
|
||||
}
|
||||
|
||||
/// 一边的装甲板数量。
|
||||
///
|
||||
/// RoboMaster 规则是每队 4 块。界面按这个数量画格子，所以数据还没接上时也能看到
|
||||
/// 位置——空着比画 0 个格子更让人以为布局坏了。
|
||||
pub const ARMOR_BOARDS: usize = 4;
|
||||
|
||||
/// 目前唯一的实现：什么都不提供。
|
||||
///
|
||||
/// 存在的意义是让界面代码从一开始就对着一个真实接口开发，而不是等装甲板就绪后再改
|
||||
/// 一次结构。
|
||||
pub struct NoArmorYet;
|
||||
|
||||
impl ArmorSource for NoArmorYet {
|
||||
fn health(&self, _team: u8) -> Option<[u16; 4]> {
|
||||
None
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user