Migrate MCP and utilities to CircuitPython, including color GIF and volume fixes

This commit is contained in:
Adolfo Reyna
2026-06-22 10:13:10 -04:00
parent 2863f21459
commit 8f871e499e
53 changed files with 4239 additions and 108 deletions
+572
View File
@@ -0,0 +1,572 @@
"""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