Adolfo Reyna
573 lines
24 KiB
Python
573 lines
24 KiB
Python
"""CircuitPython Video Stream Server utility.
|
|
|
|
Listens for incoming TCP/UDP video frames and draws them centered and cropped on the display.
|
|
"""
|
|
|
|
import time
|
|
|
|
class VideoStreamServer:
|
|
def __init__(self, display, pool, tcp_port=8081, udp_port=8082, color_port=8083, color_udp_port=8084):
|
|
self.display = display
|
|
self.pool = pool
|
|
self.tcp_port = tcp_port
|
|
self.udp_port = udp_port
|
|
self.color_port = color_port
|
|
self.color_udp_port = color_udp_port
|
|
|
|
# Sockets
|
|
self.tcp_server = None
|
|
self.tcp_client = None
|
|
self.udp_sock = None
|
|
self.color_server = None
|
|
self.color_client = None
|
|
self.color_udp_sock = None
|
|
|
|
# State
|
|
self.active = False
|
|
self.last_packet_time = 0
|
|
self.timeout_s = 3.0
|
|
|
|
# Frame buffering
|
|
self.buffer = bytearray(15000)
|
|
self.view = memoryview(self.buffer)
|
|
self.tcp_bytes_received = 0
|
|
|
|
# UDP Reassembly
|
|
self.udp_temp_buffer = bytearray(1002)
|
|
self.current_frame_id = -1
|
|
self.chunks_received = 0
|
|
self.color_chunks_mask = 0
|
|
|
|
# Color buffering (320x240 RGB565 is 153,600 bytes)
|
|
self.color_buffer = bytearray(153600)
|
|
self.color_view = memoryview(self.color_buffer)
|
|
self.color_bytes_received = 0
|
|
self.color_header = bytearray(16)
|
|
self.color_header_received = 0
|
|
self.color_payload_len = 0
|
|
self.color_x = 0
|
|
self.color_y = 0
|
|
self.color_w = 0
|
|
self.color_h = 0
|
|
|
|
# Stats
|
|
self.debug = False
|
|
self.frames_drawn = 0
|
|
self.udp_packets_received = 0
|
|
self.udp_frames_complete = 0
|
|
self.dropped_udp_frames = 0
|
|
self.last_draw_ms = 0
|
|
self.last_fps = 0.0
|
|
|
|
# FPS Calculation
|
|
self.fps_start_time = time.monotonic()
|
|
self.fps_frame_count = 0
|
|
|
|
def start(self):
|
|
"""Initializes TCP and UDP sockets."""
|
|
# 1. Start TCP Server (Mono)
|
|
try:
|
|
self.tcp_server = self.pool.socket(self.pool.AF_INET, self.pool.SOCK_STREAM)
|
|
try:
|
|
self.tcp_server.setsockopt(self.pool.SOL_SOCKET, self.pool.SO_REUSEADDR, 1)
|
|
except:
|
|
pass
|
|
self.tcp_server.bind(("", self.tcp_port))
|
|
self.tcp_server.listen(1)
|
|
self.tcp_server.setblocking(False)
|
|
print(f"Video TCP Stream server listening on port {self.tcp_port}...")
|
|
except Exception as e:
|
|
print(f"Failed to start TCP stream server: {e}")
|
|
|
|
# 2. Start UDP Server (Mono)
|
|
try:
|
|
self.udp_sock = self.pool.socket(self.pool.AF_INET, self.pool.SOCK_DGRAM)
|
|
try:
|
|
self.udp_sock.setsockopt(self.pool.SOL_SOCKET, self.pool.SO_REUSEADDR, 1)
|
|
except:
|
|
pass
|
|
self.udp_sock.bind(("", self.udp_port))
|
|
self.udp_sock.setblocking(False)
|
|
print(f"Video UDP Stream responder listening on port {self.udp_port}...")
|
|
except Exception as e:
|
|
print(f"Failed to start UDP stream server: {e}")
|
|
|
|
# 3. Start Color TCP Server
|
|
try:
|
|
self.color_server = self.pool.socket(self.pool.AF_INET, self.pool.SOCK_STREAM)
|
|
try:
|
|
self.color_server.setsockopt(self.pool.SOL_SOCKET, self.pool.SO_REUSEADDR, 1)
|
|
except:
|
|
pass
|
|
self.color_server.bind(("", self.color_port))
|
|
self.color_server.listen(1)
|
|
self.color_server.setblocking(False)
|
|
print(f"Video Color TCP Stream server listening on port {self.color_port}...")
|
|
except Exception as e:
|
|
print(f"Failed to start Color TCP server: {e}")
|
|
|
|
# 4. Start Color UDP Server
|
|
try:
|
|
self.color_udp_sock = self.pool.socket(self.pool.AF_INET, self.pool.SOCK_DGRAM)
|
|
try:
|
|
self.color_udp_sock.setsockopt(self.pool.SOL_SOCKET, self.pool.SO_REUSEADDR, 1)
|
|
except:
|
|
pass
|
|
self.color_udp_sock.bind(("", self.color_udp_port))
|
|
self.color_udp_sock.setblocking(False)
|
|
print(f"Video Color UDP Stream responder listening on port {self.color_udp_port}...")
|
|
except Exception as e:
|
|
print(f"Failed to start Color UDP stream server: {e}")
|
|
|
|
def restart_tcp_server(self):
|
|
print("Restarting TCP Stream Server...")
|
|
self.close_tcp_client()
|
|
if self.tcp_server:
|
|
try:
|
|
self.tcp_server.close()
|
|
except:
|
|
pass
|
|
self.tcp_server = None
|
|
time.sleep(0.1)
|
|
try:
|
|
self.tcp_server = self.pool.socket(self.pool.AF_INET, self.pool.SOCK_STREAM)
|
|
self.tcp_server.bind(("", self.tcp_port))
|
|
self.tcp_server.listen(1)
|
|
self.tcp_server.setblocking(False)
|
|
except Exception as e:
|
|
print(f"Restart TCP Server failed: {e}")
|
|
|
|
def restart_udp_sock(self):
|
|
print("Restarting UDP Stream Socket...")
|
|
if self.udp_sock:
|
|
try:
|
|
self.udp_sock.close()
|
|
except:
|
|
pass
|
|
self.udp_sock = None
|
|
time.sleep(0.1)
|
|
try:
|
|
self.udp_sock = self.pool.socket(self.pool.AF_INET, self.pool.SOCK_DGRAM)
|
|
self.udp_sock.bind(("", self.udp_port))
|
|
self.udp_sock.setblocking(False)
|
|
except Exception as e:
|
|
print(f"Restart UDP Socket failed: {e}")
|
|
|
|
def restart_color_server(self):
|
|
print("Restarting Color TCP Stream Server...")
|
|
self.close_color_client()
|
|
if self.color_server:
|
|
try:
|
|
self.color_server.close()
|
|
except:
|
|
pass
|
|
self.color_server = None
|
|
time.sleep(0.1)
|
|
try:
|
|
self.color_server = self.pool.socket(self.pool.AF_INET, self.pool.SOCK_STREAM)
|
|
self.color_server.bind(("", self.color_port))
|
|
self.color_server.listen(1)
|
|
self.color_server.setblocking(False)
|
|
except Exception as e:
|
|
print(f"Restart Color TCP Server failed: {e}")
|
|
|
|
def restart_color_udp_sock(self):
|
|
print("Restarting Color UDP Stream Socket...")
|
|
if self.color_udp_sock:
|
|
try:
|
|
self.color_udp_sock.close()
|
|
except:
|
|
pass
|
|
self.color_udp_sock = None
|
|
time.sleep(0.1)
|
|
try:
|
|
self.color_udp_sock = self.pool.socket(self.pool.AF_INET, self.pool.SOCK_DGRAM)
|
|
self.color_udp_sock.bind(("", self.color_udp_port))
|
|
self.color_udp_sock.setblocking(False)
|
|
except Exception as e:
|
|
print(f"Restart Color UDP Socket failed: {e}")
|
|
|
|
def update(self):
|
|
"""Non-blocking socket check for streaming updates."""
|
|
now = time.monotonic()
|
|
|
|
# Check Stream Active Timeout
|
|
if self.active and (now - self.last_packet_time) > self.timeout_s:
|
|
print("Video stream timed out. Returning to dashboard.")
|
|
self.active = False
|
|
self.close_tcp_client()
|
|
self.close_color_client()
|
|
|
|
# Calculate FPS periodically
|
|
fps_elapsed = now - self.fps_start_time
|
|
if fps_elapsed >= 2.0:
|
|
self.last_fps = self.fps_frame_count / fps_elapsed
|
|
self.fps_frame_count = 0
|
|
self.fps_start_time = now
|
|
|
|
# 1. Handle UDP reassembly (Mono)
|
|
if self.udp_sock:
|
|
while True:
|
|
try:
|
|
# recv_into returns number of bytes read
|
|
n = self.udp_sock.recv_into(self.udp_temp_buffer)
|
|
if n == 0:
|
|
break
|
|
|
|
self.udp_packets_received += 1
|
|
frame_id = self.udp_temp_buffer[0]
|
|
chunk_idx = self.udp_temp_buffer[1]
|
|
|
|
if chunk_idx < 15:
|
|
self.active = True
|
|
self.last_packet_time = now
|
|
if frame_id != self.current_frame_id:
|
|
if self.chunks_received != 0:
|
|
self.dropped_udp_frames += 1
|
|
self.current_frame_id = frame_id
|
|
self.chunks_received = 0
|
|
|
|
# Copy payload to self.buffer
|
|
start_offset = chunk_idx * 1000
|
|
self.buffer[start_offset : start_offset + 1000] = self.udp_temp_buffer[2:1002]
|
|
self.chunks_received |= (1 << chunk_idx)
|
|
|
|
if self.chunks_received == 0x7FFF:
|
|
self.udp_frames_complete += 1
|
|
self.active = True
|
|
self.last_packet_time = now
|
|
self._draw_frame()
|
|
self.chunks_received = 0
|
|
except OSError as e:
|
|
import errno
|
|
err = getattr(e, 'errno', None)
|
|
if err is None and e.args:
|
|
err = e.args[0]
|
|
ewouldblock = getattr(errno, 'EWOULDBLOCK', errno.EAGAIN)
|
|
if err in (errno.EAGAIN, ewouldblock) or err is None:
|
|
break
|
|
print(f"UDP Socket error: {e}")
|
|
self.restart_udp_sock()
|
|
break
|
|
|
|
# 1B. Handle UDP reassembly (Color)
|
|
if self.color_udp_sock:
|
|
while True:
|
|
try:
|
|
n = self.color_udp_sock.recv_into(self.udp_temp_buffer)
|
|
if n == 0:
|
|
break
|
|
|
|
self.udp_packets_received += 1
|
|
frame_id = self.udp_temp_buffer[0]
|
|
chunk_idx = self.udp_temp_buffer[1]
|
|
|
|
if chunk_idx < 154:
|
|
self.active = True
|
|
self.last_packet_time = now
|
|
if frame_id != self.current_frame_id:
|
|
self.current_frame_id = frame_id
|
|
self.color_chunks_mask = 0
|
|
|
|
# Copy payload to self.color_buffer
|
|
start_offset = chunk_idx * 1000
|
|
if start_offset + 1000 <= 153600:
|
|
self.color_buffer[start_offset : start_offset + 1000] = self.udp_temp_buffer[2:1002]
|
|
self.color_chunks_mask |= (1 << chunk_idx)
|
|
|
|
if self.color_chunks_mask == 0x3FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF:
|
|
self.udp_frames_complete += 1
|
|
self.active = True
|
|
self.last_packet_time = now
|
|
self.color_x = 0
|
|
self.color_y = 0
|
|
self.color_w = 320
|
|
self.color_h = 240
|
|
self.color_payload_len = 153600
|
|
self._draw_color_frame()
|
|
self.color_chunks_mask = 0
|
|
except OSError as e:
|
|
import errno
|
|
err = getattr(e, 'errno', None)
|
|
if err is None and e.args:
|
|
err = e.args[0]
|
|
ewouldblock = getattr(errno, 'EWOULDBLOCK', errno.EAGAIN)
|
|
if err in (errno.EAGAIN, ewouldblock) or err is None:
|
|
break
|
|
print(f"Color UDP Socket error: {e}")
|
|
self.restart_color_udp_sock()
|
|
break
|
|
|
|
# 2. Handle TCP stream (Mono)
|
|
if self.tcp_server:
|
|
if self.tcp_client is None:
|
|
try:
|
|
self.tcp_client, addr = self.tcp_server.accept()
|
|
self.tcp_client.setblocking(False)
|
|
self.tcp_bytes_received = 0
|
|
self.active = True
|
|
self.last_packet_time = now
|
|
print(f"TCP Stream client connected from: {addr}")
|
|
except OSError as e:
|
|
import errno
|
|
err = getattr(e, 'errno', None)
|
|
if err is None and e.args:
|
|
err = e.args[0]
|
|
ewouldblock = getattr(errno, 'EWOULDBLOCK', errno.EAGAIN)
|
|
if err not in (errno.EAGAIN, ewouldblock) and err is not None:
|
|
print(f"TCP Accept error: {e}")
|
|
self.restart_tcp_server()
|
|
|
|
if self.tcp_client is not None:
|
|
retries = 0
|
|
while self.tcp_bytes_received < 15000:
|
|
remaining = 15000 - self.tcp_bytes_received
|
|
slice_view = self.view[self.tcp_bytes_received : self.tcp_bytes_received + remaining]
|
|
try:
|
|
n = self.tcp_client.recv_into(slice_view)
|
|
if n > 0:
|
|
self.tcp_bytes_received += n
|
|
self.last_packet_time = now
|
|
self.active = True
|
|
retries = 0
|
|
elif n == 0:
|
|
print("TCP Stream client disconnected.")
|
|
self.close_tcp_client()
|
|
break
|
|
except OSError as e:
|
|
import errno
|
|
err = getattr(e, 'errno', None)
|
|
if err is None and e.args:
|
|
err = e.args[0]
|
|
ewouldblock = getattr(errno, 'EWOULDBLOCK', errno.EAGAIN)
|
|
if err in (errno.EAGAIN, ewouldblock) or err is None:
|
|
retries += 1
|
|
if retries > 15:
|
|
break
|
|
time.sleep(0.001)
|
|
else:
|
|
print(f"TCP Stream recv error: {e}")
|
|
self.close_tcp_client()
|
|
break
|
|
|
|
if self.tcp_bytes_received == 15000:
|
|
self.tcp_bytes_received = 0
|
|
self._draw_frame()
|
|
|
|
# 3. Handle Color TCP stream
|
|
if self.color_server:
|
|
if self.color_client is None:
|
|
try:
|
|
self.color_client, addr = self.color_server.accept()
|
|
self.color_client.setblocking(False)
|
|
self.color_bytes_received = 0
|
|
self.color_header_received = 0
|
|
self.color_payload_len = 0
|
|
self.active = True
|
|
self.last_packet_time = now
|
|
print(f"Color TCP Stream client connected from: {addr}")
|
|
except OSError as e:
|
|
import errno
|
|
err = getattr(e, 'errno', None)
|
|
if err is None and e.args:
|
|
err = e.args[0]
|
|
ewouldblock = getattr(errno, 'EWOULDBLOCK', errno.EAGAIN)
|
|
if err not in (errno.EAGAIN, ewouldblock) and err is not None:
|
|
print(f"Color TCP Accept error: {e}")
|
|
self.restart_color_server()
|
|
|
|
if self.color_client is not None:
|
|
try:
|
|
# Read header (16 bytes)
|
|
if self.color_header_received < 16:
|
|
start_h = time.monotonic()
|
|
while self.color_header_received < 16:
|
|
if (time.monotonic() - start_h) > 0.100:
|
|
break
|
|
remaining_h = 16 - self.color_header_received
|
|
slice_h = memoryview(self.color_header)[self.color_header_received : self.color_header_received + remaining_h]
|
|
n = self.color_client.recv_into(slice_h)
|
|
if n > 0:
|
|
self.color_header_received += n
|
|
elif n == 0:
|
|
self.close_color_client()
|
|
return
|
|
|
|
if self.color_header_received == 16:
|
|
# Detect version byte at index 4
|
|
version = self.color_header[4]
|
|
if version == 1:
|
|
# stream_color.py format: sig (4B), version (1B), format (1B), width (2B), height (2B), payload_len (4B), reserved (2B)
|
|
self.color_x = 0
|
|
self.color_y = 0
|
|
self.color_w = (self.color_header[6] << 8) | self.color_header[7]
|
|
self.color_h = (self.color_header[8] << 8) | self.color_header[9]
|
|
self.color_payload_len = (self.color_header[10] << 24) | (self.color_header[11] << 16) | (self.color_header[12] << 8) | self.color_header[13]
|
|
else:
|
|
# iPhone app format: sig (4B), x (2B), y (2B), width (2B), height (2B), payload_len (4B)
|
|
self.color_x = (self.color_header[4] << 8) | self.color_header[5]
|
|
self.color_y = (self.color_header[6] << 8) | self.color_header[7]
|
|
self.color_w = (self.color_header[8] << 8) | self.color_header[9]
|
|
self.color_h = (self.color_header[10] << 8) | self.color_header[11]
|
|
self.color_payload_len = (self.color_header[12] << 24) | (self.color_header[13] << 16) | (self.color_header[14] << 8) | self.color_header[15]
|
|
|
|
# Safety check:
|
|
if self.color_payload_len > len(self.color_buffer):
|
|
print(f"Warning: Color payload length {self.color_payload_len} exceeds preallocated buffer {len(self.color_buffer)}. Closing connection.")
|
|
self.close_color_client()
|
|
return
|
|
|
|
self.color_bytes_received = 0
|
|
|
|
# Read payload
|
|
if self.color_header_received == 16 and self.color_payload_len > 0:
|
|
retries = 0
|
|
while self.color_bytes_received < self.color_payload_len:
|
|
remaining_p = self.color_payload_len - self.color_bytes_received
|
|
slice_p = self.color_view[self.color_bytes_received : self.color_bytes_received + remaining_p]
|
|
try:
|
|
n = self.color_client.recv_into(slice_p)
|
|
if n > 0:
|
|
self.color_bytes_received += n
|
|
self.last_packet_time = now
|
|
self.active = True
|
|
retries = 0
|
|
elif n == 0:
|
|
self.close_color_client()
|
|
break
|
|
except OSError as e:
|
|
import errno
|
|
err = getattr(e, 'errno', None)
|
|
if err is None and e.args:
|
|
err = e.args[0]
|
|
ewouldblock = getattr(errno, 'EWOULDBLOCK', errno.EAGAIN)
|
|
if err in (errno.EAGAIN, ewouldblock) or err is None:
|
|
retries += 1
|
|
if retries > 25: # max 25ms wait total per frame
|
|
break
|
|
time.sleep(0.001)
|
|
else:
|
|
print(f"Color TCP Stream recv error during payload: {e}")
|
|
self.close_color_client()
|
|
break
|
|
|
|
if self.color_bytes_received == self.color_payload_len:
|
|
self._draw_color_frame()
|
|
self.color_header_received = 0
|
|
self.color_payload_len = 0
|
|
self.color_bytes_received = 0
|
|
except OSError as e:
|
|
import errno
|
|
err = getattr(e, 'errno', None)
|
|
if err is None and e.args:
|
|
err = e.args[0]
|
|
ewouldblock = getattr(errno, 'EWOULDBLOCK', errno.EAGAIN)
|
|
if err not in (errno.EAGAIN, ewouldblock) and err is not None:
|
|
print(f"Color TCP Stream recv error: {e}")
|
|
self.close_color_client()
|
|
|
|
def rlcd_to_mono_cp(self, rlcd_buf, canvas_buf, width, height):
|
|
for i in range(len(canvas_buf)):
|
|
canvas_buf[i] = 0
|
|
|
|
dx = (400 - width) // 2
|
|
dy = (300 - height) // 2
|
|
width_bytes = width // 8
|
|
|
|
for index in range(15000):
|
|
val = rlcd_buf[index]
|
|
if val == 0:
|
|
continue
|
|
|
|
byte_x = index // 75
|
|
block_y = index % 75
|
|
x_base = 2 * byte_x
|
|
y_base = 299 - 4 * block_y
|
|
|
|
for local_y in range(4):
|
|
for local_x in range(2):
|
|
bit = 7 - (local_y * 2 + local_x)
|
|
if val & (1 << bit):
|
|
x = x_base + local_x
|
|
y = y_base - local_y
|
|
screen_x = x - dx
|
|
screen_y = y - dy
|
|
|
|
if 0 <= screen_x < width and 0 <= screen_y < height:
|
|
byte_idx = screen_y * width_bytes + (screen_x >> 3)
|
|
bit_idx = 7 - (screen_x & 7)
|
|
canvas_buf[byte_idx] |= (1 << bit_idx)
|
|
|
|
def _draw_frame(self):
|
|
draw_start = time.monotonic()
|
|
|
|
# Check if RLCD display vs standard ILI9341 display
|
|
disp_name = self.display.__class__.__name__
|
|
if disp_name == "RLCD":
|
|
# Direct SPI write commands for RLCD layout
|
|
self.display.write_cmd(0x2A)
|
|
self.display.write_data([0x12, 0x2A])
|
|
self.display.write_cmd(0x2B)
|
|
self.display.write_data([0x00, 0xC7])
|
|
self.display.write_cmd(0x2C)
|
|
self.display.write_data(self.buffer)
|
|
else:
|
|
# Map 400x300 RLCD buffer into the ILI9341 320x240 canvas buffer
|
|
self.rlcd_to_mono_cp(self.buffer, self.display.canvas_buffer, self.display.width, self.display.height)
|
|
self.display.show()
|
|
|
|
self.last_draw_ms = int((time.monotonic() - draw_start) * 1000)
|
|
self.frames_drawn += 1
|
|
self.fps_frame_count += 1
|
|
|
|
def _draw_color_frame(self):
|
|
draw_start = time.monotonic()
|
|
if hasattr(self.display, "draw_rgb565"):
|
|
self.display.draw_rgb565(
|
|
self.color_x,
|
|
self.color_y,
|
|
self.color_w,
|
|
self.color_h,
|
|
self.color_view[:self.color_payload_len],
|
|
sync_canvas=False,
|
|
)
|
|
else:
|
|
# Fallback if no raw RGB565 method is exposed (e.g. standard RLCD)
|
|
pass
|
|
|
|
self.last_draw_ms = int((time.monotonic() - draw_start) * 1000)
|
|
self.frames_drawn += 1
|
|
self.fps_frame_count += 1
|
|
|
|
def get_stats(self):
|
|
return {
|
|
"frames_drawn": self.frames_drawn,
|
|
"tcp_bytes_received": self.tcp_bytes_received,
|
|
"udp_packets_received": self.udp_packets_received,
|
|
"udp_frames_complete": self.udp_frames_complete,
|
|
"dropped_udp_frames": self.dropped_udp_frames,
|
|
"last_draw_ms": self.last_draw_ms,
|
|
"last_fps": round(self.last_fps, 1),
|
|
"active": self.active,
|
|
}
|
|
|
|
def close_tcp_client(self):
|
|
if self.tcp_client:
|
|
try:
|
|
self.tcp_client.close()
|
|
except:
|
|
pass
|
|
self.tcp_client = None
|
|
self.tcp_bytes_received = 0
|
|
|
|
def close_color_client(self):
|
|
if self.color_client:
|
|
try:
|
|
self.color_client.close()
|
|
except:
|
|
pass
|
|
self.color_client = None
|
|
self.color_bytes_received = 0
|
|
self.color_header_received = 0
|
|
self.color_payload_len = 0
|